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@1:622d7b18a1c0, 2013-12-23 (annotated)

Committer:
tyftyftyf
Date:
Mon Dec 23 09:18:24 2013 +0000
Revision:
1:622d7b18a1c0
Parent:
0:1b3e1eec2577
Child:
2:ba8ce2dbc778
Fix processing error

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 1:622d7b18a1c0 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 0:1b3e1eec2577 83 Thread::wait(10);
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 0:1b3e1eec2577 93 pc.printf("\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 0:1b3e1eec2577 112
tyftyftyf 0:1b3e1eec2577 113 if(cmd=='b') {
tyftyftyf 0:1b3e1eec2577 114 uint8_t count = pc.getc();
tyftyftyf 0:1b3e1eec2577 115 for(uint8_t i=0; i<count; i++) {
tyftyftyf 0:1b3e1eec2577 116
tyftyftyf 0:1b3e1eec2577 117 imu.accgyro.getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
tyftyftyf 0:1b3e1eec2577 118
tyftyftyf 0:1b3e1eec2577 119 writeArr(raw_values, 6, sizeof(int16_t)); // writes accelerometer and gyro values
tyftyftyf 0:1b3e1eec2577 120
tyftyftyf 0:1b3e1eec2577 121 imu.magn.getValues(&raw_values[0], &raw_values[1], &raw_values[2]);
tyftyftyf 0:1b3e1eec2577 122 writeArr(raw_values, 3, sizeof(int16_t));
tyftyftyf 0:1b3e1eec2577 123
tyftyftyf 0:1b3e1eec2577 124 pc.printf("\r\n");
tyftyftyf 0:1b3e1eec2577 125 wait_us(4000);
tyftyftyf 0:1b3e1eec2577 126 }
tyftyftyf 0:1b3e1eec2577 127 }
tyftyftyf 0:1b3e1eec2577 128 }
tyftyftyf 0:1b3e1eec2577 129
tyftyftyf 0:1b3e1eec2577 130 }