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@4:145d1eb8a15d, 2018-03-28 (annotated)

Committer:
tyftyftyf
Date:
Wed Mar 28 21:07:33 2018 +0000
Revision:
4:145d1eb8a15d
Parent:
3:cc1415081d36
Child:
5:d21e84d6f5a1
wip

Who changed what in which revision?

UserRevisionLine numberNew 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 4:145d1eb8a15d 70 // #define AUTO_OUTPUT
tyftyftyf 1:622d7b18a1c0 71
tyftyftyf 0:1b3e1eec2577 72 int main()
tyftyftyf 0:1b3e1eec2577 73 {
tyftyftyf 4:145d1eb8a15d 74 RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)0);
tyftyftyf 0:1b3e1eec2577 75 pc.baud(115200);
tyftyftyf 4:145d1eb8a15d 76 // pc.printf("123456789012345678901234567890123456789012345678901234567890\r\n");
tyftyftyf 0:1b3e1eec2577 77 imu.init(true);
tyftyftyf 4:145d1eb8a15d 78 // pc.printf("fresfgsdgfsgdfgsdg123456789012345fdsgsdgd678901234567890123456789012345678901234567890\r\n");
tyftyftyf 0:1b3e1eec2577 79 pc.baud(115200);
tyftyftyf 4:145d1eb8a15d 80
tyftyftyf 4:145d1eb8a15d 81 // start data collection
tyftyftyf 0:1b3e1eec2577 82 IMUTimer.start(5);
tyftyftyf 0:1b3e1eec2577 83
tyftyftyf 0:1b3e1eec2577 84 while(true) {
tyftyftyf 0:1b3e1eec2577 85 int16_t raw_values[9];
tyftyftyf 0:1b3e1eec2577 86
tyftyftyf 3:cc1415081d36 87 Thread::wait(5);
tyftyftyf 1:622d7b18a1c0 88 char cmd = 'q';
tyftyftyf 1:622d7b18a1c0 89 #ifndef AUTO_OUTPUT
tyftyftyf 1:622d7b18a1c0 90 cmd = pc.getc();
tyftyftyf 1:622d7b18a1c0 91 #endif
tyftyftyf 0:1b3e1eec2577 92
tyftyftyf 0:1b3e1eec2577 93 if(cmd=='v') {
tyftyftyf 0:1b3e1eec2577 94 char str[256];
tyftyftyf 0:1b3e1eec2577 95 sprintf(str, "FreeIMU library by %s, FREQ:%s, LIB_VERSION: %s, IMU: %s", FREEIMU_DEVELOPER, 0x10000000, FREEIMU_LIB_VERSION, 0);
tyftyftyf 0:1b3e1eec2577 96 pc.printf("%s", str);
tyftyftyf 3:cc1415081d36 97 pc.printf("\r\n");
tyftyftyf 0:1b3e1eec2577 98 }
tyftyftyf 0:1b3e1eec2577 99
tyftyftyf 0:1b3e1eec2577 100 if (cmd=='q') {
tyftyftyf 1:622d7b18a1c0 101 uint8_t count = 32;
tyftyftyf 1:622d7b18a1c0 102 #ifndef AUTO_OUTPUT
tyftyftyf 1:622d7b18a1c0 103 count = pc.getc();
tyftyftyf 1:622d7b18a1c0 104 #endif
tyftyftyf 0:1b3e1eec2577 105 for(uint8_t i=0; i<count; i++) {
tyftyftyf 0:1b3e1eec2577 106 osEvent evt = imu_queue.get();
tyftyftyf 0:1b3e1eec2577 107 if (evt.status == osEventMail) {
tyftyftyf 0:1b3e1eec2577 108 imu_t *obj = (imu_t*)evt.value.p;
tyftyftyf 0:1b3e1eec2577 109 serialPrintFloatArr(obj->q, 4);
tyftyftyf 0:1b3e1eec2577 110 imu_queue.free(obj);
tyftyftyf 0:1b3e1eec2577 111 pc.printf("\r\n");
tyftyftyf 0:1b3e1eec2577 112 }
tyftyftyf 0:1b3e1eec2577 113 Thread::yield();
tyftyftyf 0:1b3e1eec2577 114 }
tyftyftyf 0:1b3e1eec2577 115 }
tyftyftyf 3:cc1415081d36 116
tyftyftyf 3:cc1415081d36 117 if(cmd=='o') {
tyftyftyf 3:cc1415081d36 118 uint8_t count = pc.getc();
tyftyftyf 3:cc1415081d36 119 float raw_valuesf[9];
tyftyftyf 3:cc1415081d36 120 for(uint8_t i=0; i<count; i++) {
tyftyftyf 3:cc1415081d36 121
tyftyftyf 3:cc1415081d36 122 imu.getValues(raw_valuesf);
tyftyftyf 3:cc1415081d36 123
tyftyftyf 3:cc1415081d36 124 pc.printf("%f, %f, %f", raw_valuesf[3], raw_valuesf[4], raw_valuesf[5]);
tyftyftyf 3:cc1415081d36 125
tyftyftyf 3:cc1415081d36 126 pc.printf("\r\n");
tyftyftyf 3:cc1415081d36 127 Thread::wait(4);
tyftyftyf 3:cc1415081d36 128 }
tyftyftyf 3:cc1415081d36 129 }
tyftyftyf 0:1b3e1eec2577 130
tyftyftyf 0:1b3e1eec2577 131 if(cmd=='b') {
tyftyftyf 0:1b3e1eec2577 132 uint8_t count = pc.getc();
tyftyftyf 0:1b3e1eec2577 133 for(uint8_t i=0; i<count; i++) {
tyftyftyf 0:1b3e1eec2577 134
tyftyftyf 2:ba8ce2dbc778 135 imu.accgyro->getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
tyftyftyf 0:1b3e1eec2577 136
tyftyftyf 0:1b3e1eec2577 137 writeArr(raw_values, 6, sizeof(int16_t)); // writes accelerometer and gyro values
tyftyftyf 0:1b3e1eec2577 138
tyftyftyf 2:ba8ce2dbc778 139 imu.magn->getValues(&raw_values[0], &raw_values[1], &raw_values[2]);
tyftyftyf 4:145d1eb8a15d 140 writeArr(raw_values, 3, sizeof(int16_t)); // write magnetometer values
tyftyftyf 0:1b3e1eec2577 141
tyftyftyf 0:1b3e1eec2577 142 pc.printf("\r\n");
tyftyftyf 3:cc1415081d36 143 Thread::wait(4);
tyftyftyf 0:1b3e1eec2577 144 }
tyftyftyf 0:1b3e1eec2577 145 }
tyftyftyf 0:1b3e1eec2577 146 }
tyftyftyf 0:1b3e1eec2577 147
tyftyftyf 0:1b3e1eec2577 148 }