
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@7:2f2e18fdc164, 2018-03-28 (annotated)
- Committer:
- tyftyftyf
- Date:
- Wed Mar 28 22:07:42 2018 +0000
- Revision:
- 7:2f2e18fdc164
- Parent:
- 5:d21e84d6f5a1
wip
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 | 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 | 5:d21e84d6f5a1 | 95 | sprintf(str, "FreeIMU library by %s, FREQ:%d, LIB_VERSION: %s, IMU: %d", 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 | 5:d21e84d6f5a1 | 135 | imu.accgyro->getMotion9(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5], &raw_values[6], &raw_values[7], &raw_values[8]); |
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 | 5:d21e84d6f5a1 | 139 | writeArr(&raw_values[6], 3, sizeof(int16_t)); // write magnetometer values |
tyftyftyf | 0:1b3e1eec2577 | 140 | |
tyftyftyf | 0:1b3e1eec2577 | 141 | pc.printf("\r\n"); |
tyftyftyf | 3:cc1415081d36 | 142 | Thread::wait(4); |
tyftyftyf | 0:1b3e1eec2577 | 143 | } |
tyftyftyf | 0:1b3e1eec2577 | 144 | } |
tyftyftyf | 0:1b3e1eec2577 | 145 | } |
tyftyftyf | 0:1b3e1eec2577 | 146 | |
tyftyftyf | 0:1b3e1eec2577 | 147 | } |