FreeIMU_serial - Calibration program for my port of FreeIMU librar…

Users » tyftyftyf » Code » FreeIMU_serial

Yifei Teng / Mbed 2 deprecated FreeIMU_serial

Calibration program for my port of FreeIMU library. Use it with the FreeIMU Cube program or the python calibration program.

Dependencies: FreeIMU mbed-rtos mbed

main.cpp@3:cc1415081d36, 2014-01-10 (annotated)

Committer:: tyftyftyf
Date:: Fri Jan 10 05:52:20 2014 +0000
Revision:: 3:cc1415081d36
Parent:: 2:ba8ce2dbc778
Child:: 4:145d1eb8a15d

Who changed what in which revision?

User	Revision	Line number	New contents of line
tyftyftyf	0:1b3e1eec2577	1	#include "mbed.h"
tyftyftyf	0:1b3e1eec2577	2	#include "rtos.h"
tyftyftyf	0:1b3e1eec2577	3	#include "FreeIMU.h"
tyftyftyf	0:1b3e1eec2577	4
tyftyftyf	0:1b3e1eec2577	5	Serial pc(USBTX, USBRX); // tx, rx
tyftyftyf	0:1b3e1eec2577	6
tyftyftyf	0:1b3e1eec2577	7	typedef struct {
tyftyftyf	0:1b3e1eec2577	8	float q[4];
tyftyftyf	0:1b3e1eec2577	9	} imu_t;
tyftyftyf	0:1b3e1eec2577	10
tyftyftyf	0:1b3e1eec2577	11	FreeIMU imu;
tyftyftyf	0:1b3e1eec2577	12	Mail<imu_t, 8> imu_queue;
tyftyftyf	0:1b3e1eec2577	13	int i = 0;
tyftyftyf	0:1b3e1eec2577	14
tyftyftyf	0:1b3e1eec2577	15	void getIMUdata(void const *n)
tyftyftyf	0:1b3e1eec2577	16	{
tyftyftyf	0:1b3e1eec2577	17	i++;
tyftyftyf	0:1b3e1eec2577	18	if (i%4==0) {
tyftyftyf	0:1b3e1eec2577	19	float q = (float )imu_queue.alloc();
tyftyftyf	0:1b3e1eec2577	20	imu.getQ(q);
tyftyftyf	0:1b3e1eec2577	21	imu_queue.put((imu_t*)q);
tyftyftyf	0:1b3e1eec2577	22	} else {
tyftyftyf	0:1b3e1eec2577	23	imu.getQ(NULL);
tyftyftyf	0:1b3e1eec2577	24	}
tyftyftyf	0:1b3e1eec2577	25	}
tyftyftyf	0:1b3e1eec2577	26
tyftyftyf	0:1b3e1eec2577	27	// thanks to Francesco Ferrara and the Simplo project for the following code!
tyftyftyf	0:1b3e1eec2577	28	void writeVar(void * val, uint8_t type_bytes)
tyftyftyf	0:1b3e1eec2577	29	{
tyftyftyf	0:1b3e1eec2577	30	char * addr=(char *)(val);
tyftyftyf	0:1b3e1eec2577	31	for(uint8_t i=0; i<type_bytes; i++) {
tyftyftyf	0:1b3e1eec2577	32	//Serial1.write(addr[i]);
tyftyftyf	0:1b3e1eec2577	33	pc.putc(addr[i]);
tyftyftyf	0:1b3e1eec2577	34	}
tyftyftyf	0:1b3e1eec2577	35	}
tyftyftyf	0:1b3e1eec2577	36
tyftyftyf	0:1b3e1eec2577	37	void writeArr(void * varr, uint8_t arr_length, uint8_t type_bytes)
tyftyftyf	0:1b3e1eec2577	38	{
tyftyftyf	0:1b3e1eec2577	39	char * arr = (char*) varr;
tyftyftyf	0:1b3e1eec2577	40	for(uint8_t i=0; i<arr_length; i++) {
tyftyftyf	0:1b3e1eec2577	41	writeVar(&arr[i * type_bytes], type_bytes);
tyftyftyf	0:1b3e1eec2577	42	}
tyftyftyf	0:1b3e1eec2577	43	}
tyftyftyf	0:1b3e1eec2577	44
tyftyftyf	0:1b3e1eec2577	45	void serialFloatPrint(float f)
tyftyftyf	0:1b3e1eec2577	46	{
tyftyftyf	0:1b3e1eec2577	47	char * b = (char *) &f;
tyftyftyf	0:1b3e1eec2577	48	for(int i=0; i<4; i++) {
tyftyftyf	0:1b3e1eec2577	49
tyftyftyf	0:1b3e1eec2577	50	char b1 = (b[i] >> 4) & 0x0f;
tyftyftyf	0:1b3e1eec2577	51	char b2 = (b[i] & 0x0f);
tyftyftyf	0:1b3e1eec2577	52
tyftyftyf	0:1b3e1eec2577	53	char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
tyftyftyf	0:1b3e1eec2577	54	char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
tyftyftyf	0:1b3e1eec2577	55
tyftyftyf	0:1b3e1eec2577	56	pc.putc(c1);
tyftyftyf	0:1b3e1eec2577	57	pc.putc(c2);
tyftyftyf	0:1b3e1eec2577	58	}
tyftyftyf	0:1b3e1eec2577	59	}
tyftyftyf	0:1b3e1eec2577	60
tyftyftyf	0:1b3e1eec2577	61	void serialPrintFloatArr(float * arr, int length)
tyftyftyf	0:1b3e1eec2577	62	{
tyftyftyf	0:1b3e1eec2577	63	for(int i=0; i<length; i++) {
tyftyftyf	0:1b3e1eec2577	64	serialFloatPrint(arr[i]);
tyftyftyf	0:1b3e1eec2577	65	pc.putc(',');
tyftyftyf	0:1b3e1eec2577	66	}
tyftyftyf	0:1b3e1eec2577	67	}
tyftyftyf	0:1b3e1eec2577	68
tyftyftyf	1:622d7b18a1c0	69	//if the Processing program cannot accept serial inputs, uncomment this to send quaternion information automatically
tyftyftyf	2:ba8ce2dbc778	70	//#define AUTO_OUTPUT
tyftyftyf	1:622d7b18a1c0	71
tyftyftyf	0:1b3e1eec2577	72	int main()
tyftyftyf	0:1b3e1eec2577	73	{
tyftyftyf	0:1b3e1eec2577	74	pc.baud(115200);
tyftyftyf	0:1b3e1eec2577	75	imu.init(true);
tyftyftyf	0:1b3e1eec2577	76	pc.baud(115200);
tyftyftyf	0:1b3e1eec2577	77	RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)0);
tyftyftyf	0:1b3e1eec2577	78	IMUTimer.start(5);
tyftyftyf	0:1b3e1eec2577	79
tyftyftyf	0:1b3e1eec2577	80	while(true) {
tyftyftyf	0:1b3e1eec2577	81	int16_t raw_values[9];
tyftyftyf	0:1b3e1eec2577	82
tyftyftyf	3:cc1415081d36	83	Thread::wait(5);
tyftyftyf	1:622d7b18a1c0	84	char cmd = 'q';
tyftyftyf	1:622d7b18a1c0	85	#ifndef AUTO_OUTPUT
tyftyftyf	1:622d7b18a1c0	86	cmd = pc.getc();
tyftyftyf	1:622d7b18a1c0	87	#endif
tyftyftyf	0:1b3e1eec2577	88
tyftyftyf	0:1b3e1eec2577	89	if(cmd=='v') {
tyftyftyf	0:1b3e1eec2577	90	char str[256];
tyftyftyf	0:1b3e1eec2577	91	sprintf(str, "FreeIMU library by %s, FREQ:%s, LIB_VERSION: %s, IMU: %s", FREEIMU_DEVELOPER, 0x10000000, FREEIMU_LIB_VERSION, 0);
tyftyftyf	0:1b3e1eec2577	92	pc.printf("%s", str);
tyftyftyf	3:cc1415081d36	93	pc.printf("\r\n");
tyftyftyf	0:1b3e1eec2577	94	}
tyftyftyf	0:1b3e1eec2577	95
tyftyftyf	0:1b3e1eec2577	96	if (cmd=='q') {
tyftyftyf	1:622d7b18a1c0	97	uint8_t count = 32;
tyftyftyf	1:622d7b18a1c0	98	#ifndef AUTO_OUTPUT
tyftyftyf	1:622d7b18a1c0	99	count = pc.getc();
tyftyftyf	1:622d7b18a1c0	100	#endif
tyftyftyf	0:1b3e1eec2577	101	for(uint8_t i=0; i<count; i++) {
tyftyftyf	0:1b3e1eec2577	102	osEvent evt = imu_queue.get();
tyftyftyf	0:1b3e1eec2577	103	if (evt.status == osEventMail) {
tyftyftyf	0:1b3e1eec2577	104	imu_t obj = (imu_t)evt.value.p;
tyftyftyf	0:1b3e1eec2577	105	serialPrintFloatArr(obj->q, 4);
tyftyftyf	0:1b3e1eec2577	106	imu_queue.free(obj);
tyftyftyf	0:1b3e1eec2577	107	pc.printf("\r\n");
tyftyftyf	0:1b3e1eec2577	108	}
tyftyftyf	0:1b3e1eec2577	109	Thread::yield();
tyftyftyf	0:1b3e1eec2577	110	}
tyftyftyf	0:1b3e1eec2577	111	}
tyftyftyf	3:cc1415081d36	112
tyftyftyf	3:cc1415081d36	113	if(cmd=='o') {
tyftyftyf	3:cc1415081d36	114	uint8_t count = pc.getc();
tyftyftyf	3:cc1415081d36	115	float raw_valuesf[9];
tyftyftyf	3:cc1415081d36	116	for(uint8_t i=0; i<count; i++) {
tyftyftyf	3:cc1415081d36	117
tyftyftyf	3:cc1415081d36	118	imu.getValues(raw_valuesf);
tyftyftyf	3:cc1415081d36	119
tyftyftyf	3:cc1415081d36	120	pc.printf("%f, %f, %f", raw_valuesf[3], raw_valuesf[4], raw_valuesf[5]);
tyftyftyf	3:cc1415081d36	121
tyftyftyf	3:cc1415081d36	122	pc.printf("\r\n");
tyftyftyf	3:cc1415081d36	123	Thread::wait(4);
tyftyftyf	3:cc1415081d36	124	}
tyftyftyf	3:cc1415081d36	125	}
tyftyftyf	0:1b3e1eec2577	126
tyftyftyf	0:1b3e1eec2577	127	if(cmd=='b') {
tyftyftyf	0:1b3e1eec2577	128	uint8_t count = pc.getc();
tyftyftyf	0:1b3e1eec2577	129	for(uint8_t i=0; i<count; i++) {
tyftyftyf	0:1b3e1eec2577	130
tyftyftyf	2:ba8ce2dbc778	131	imu.accgyro->getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
tyftyftyf	0:1b3e1eec2577	132
tyftyftyf	0:1b3e1eec2577	133	writeArr(raw_values, 6, sizeof(int16_t)); // writes accelerometer and gyro values
tyftyftyf	0:1b3e1eec2577	134
tyftyftyf	2:ba8ce2dbc778	135	imu.magn->getValues(&raw_values[0], &raw_values[1], &raw_values[2]);
tyftyftyf	0:1b3e1eec2577	136	writeArr(raw_values, 3, sizeof(int16_t));
tyftyftyf	0:1b3e1eec2577	137
tyftyftyf	0:1b3e1eec2577	138	pc.printf("\r\n");
tyftyftyf	3:cc1415081d36	139	Thread::wait(4);
tyftyftyf	0:1b3e1eec2577	140	}
tyftyftyf	0:1b3e1eec2577	141	}
tyftyftyf	0:1b3e1eec2577	142	}
tyftyftyf	0:1b3e1eec2577	143
tyftyftyf	0:1b3e1eec2577	144	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	23 Dec 2013
Imports:	42
Forks:	0
Commits:	8
Dependents:	0
Dependencies:	3
Followers:	3

main.cpp@3:cc1415081d36, 2014-01-10 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning