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
Diff: main.cpp
- Revision:
- 0:1b3e1eec2577
- Child:
- 1:622d7b18a1c0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Dec 23 08:46:06 2013 +0000 @@ -0,0 +1,122 @@ +#include "mbed.h" +#include "rtos.h" +#include "FreeIMU.h" + +Serial pc(USBTX, USBRX); // tx, rx + +typedef struct { + float q[4]; +} imu_t; + +FreeIMU imu; +Mail<imu_t, 8> imu_queue; +int i = 0; + +void getIMUdata(void const *n) +{ + i++; + if (i%4==0) { + float *q = (float *)imu_queue.alloc(); + imu.getQ(q); + imu_queue.put((imu_t*)q); + } else { + imu.getQ(NULL); + } +} + +// thanks to Francesco Ferrara and the Simplo project for the following code! +void writeVar(void * val, uint8_t type_bytes) +{ + char * addr=(char *)(val); + for(uint8_t i=0; i<type_bytes; i++) { + //Serial1.write(addr[i]); + pc.putc(addr[i]); + } +} + +void writeArr(void * varr, uint8_t arr_length, uint8_t type_bytes) +{ + char * arr = (char*) varr; + for(uint8_t i=0; i<arr_length; i++) { + writeVar(&arr[i * type_bytes], type_bytes); + } +} + +void serialFloatPrint(float f) +{ + char * b = (char *) &f; + for(int i=0; i<4; i++) { + + char b1 = (b[i] >> 4) & 0x0f; + char b2 = (b[i] & 0x0f); + + char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10; + char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10; + + pc.putc(c1); + pc.putc(c2); + } +} + +void serialPrintFloatArr(float * arr, int length) +{ + for(int i=0; i<length; i++) { + serialFloatPrint(arr[i]); + pc.putc(','); + } +} + +int main() +{ + pc.baud(115200); + imu.init(true); + pc.baud(115200); + RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)0); + IMUTimer.start(5); + + while(true) { + int16_t raw_values[9]; + + Thread::wait(10); + char cmd = 'q';//pc.getc(); + + if(cmd=='v') { + char str[256]; + sprintf(str, "FreeIMU library by %s, FREQ:%s, LIB_VERSION: %s, IMU: %s", FREEIMU_DEVELOPER, 0x10000000, FREEIMU_LIB_VERSION, 0); + pc.printf("%s", str); + pc.printf("\n"); + } + + if (cmd=='q') { + uint8_t count = 32;//pc.getc(); + + for(uint8_t i=0; i<count; i++) { + osEvent evt = imu_queue.get(); + if (evt.status == osEventMail) { + imu_t *obj = (imu_t*)evt.value.p; + serialPrintFloatArr(obj->q, 4); + imu_queue.free(obj); + pc.printf("\r\n"); + } + Thread::yield(); + } + } + + if(cmd=='b') { + uint8_t count = pc.getc(); + for(uint8_t i=0; i<count; i++) { + + imu.accgyro.getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]); + + writeArr(raw_values, 6, sizeof(int16_t)); // writes accelerometer and gyro values + + imu.magn.getValues(&raw_values[0], &raw_values[1], &raw_values[2]); + writeArr(raw_values, 3, sizeof(int16_t)); + + pc.printf("\r\n"); + wait_us(4000); + } + } + } + +}