main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "FreeIMU.h"
00004 
00005 Serial pc(USBTX, USBRX); // tx, rx
00006 
00007 typedef struct {
00008     float q[4];
00009 } imu_t;
00010 
00011 FreeIMU imu;
00012 Mail<imu_t, 8> imu_queue;
00013 int i = 0;
00014 
00015 void getIMUdata(void const *n)
00016 {
00017     i++;
00018     if (i%4==0) {
00019         float *q = (float *)imu_queue.alloc();
00020         imu.getQ(q);
00021         imu_queue.put((imu_t*)q);
00022     } else {
00023         imu.getQ(NULL);
00024     }
00025 }
00026 
00027 // thanks to Francesco Ferrara and the Simplo project for the following code!
00028 void writeVar(void * val, uint8_t type_bytes)
00029 {
00030     char * addr=(char *)(val);
00031     for(uint8_t i=0; i<type_bytes; i++) {
00032         //Serial1.write(addr[i]);
00033         pc.putc(addr[i]);
00034     }
00035 }
00036 
00037 void writeArr(void * varr, uint8_t arr_length, uint8_t type_bytes)
00038 {
00039     char * arr = (char*) varr;
00040     for(uint8_t i=0; i<arr_length; i++) {
00041         writeVar(&arr[i * type_bytes], type_bytes);
00042     }
00043 }
00044 
00045 void serialFloatPrint(float f)
00046 {
00047     char * b = (char *) &f;
00048     for(int i=0; i<4; i++) {
00049 
00050         char b1 = (b[i] >> 4) & 0x0f;
00051         char b2 = (b[i] & 0x0f);
00052 
00053         char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
00054         char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
00055 
00056         pc.putc(c1);
00057         pc.putc(c2);
00058     }
00059 }
00060 
00061 void serialPrintFloatArr(float * arr, int length)
00062 {
00063     for(int i=0; i<length; i++) {
00064         serialFloatPrint(arr[i]);
00065         pc.putc(',');
00066     }
00067 }
00068 
00069 //if the Processing program cannot accept serial inputs, uncomment this to send quaternion information automatically
00070 // #define AUTO_OUTPUT
00071 
00072 int main()
00073 {
00074     RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)0);
00075     pc.baud(115200);
00076     // pc.printf("123456789012345678901234567890123456789012345678901234567890\r\n");
00077     imu.init(true);
00078     // pc.printf("fresfgsdgfsgdfgsdg123456789012345fdsgsdgd678901234567890123456789012345678901234567890\r\n");
00079     pc.baud(115200);
00080     
00081     // start data collection
00082     IMUTimer.start(5);
00083 
00084     while(true) {
00085         int16_t raw_values[9];
00086 
00087         Thread::wait(5);
00088         char cmd = 'q';
00089 #ifndef AUTO_OUTPUT
00090         cmd = pc.getc();
00091 #endif
00092 
00093         if(cmd=='v') {
00094             char str[256];
00095             sprintf(str, "FreeIMU library by %s, FREQ:%d, LIB_VERSION: %s, IMU: %d", FREEIMU_DEVELOPER, 0x10000000, FREEIMU_LIB_VERSION, 0);
00096             pc.printf("%s", str);
00097             pc.printf("\r\n");
00098         }
00099 
00100         if (cmd=='q') {
00101             uint8_t count = 32;
00102 #ifndef AUTO_OUTPUT
00103             count = pc.getc();
00104 #endif
00105             for(uint8_t i=0; i<count; i++) {
00106                 osEvent evt = imu_queue.get();
00107                 if (evt.status == osEventMail) {
00108                     imu_t *obj = (imu_t*)evt.value.p;
00109                     serialPrintFloatArr(obj->q, 4);
00110                     imu_queue.free(obj);
00111                     pc.printf("\r\n");
00112                 }
00113                 Thread::yield();
00114             }
00115         }
00116         
00117         if(cmd=='o') {
00118             uint8_t count = pc.getc();
00119             float raw_valuesf[9];
00120             for(uint8_t i=0; i<count; i++) {
00121 
00122                 imu.getValues(raw_valuesf);
00123 
00124                 pc.printf("%f, %f, %f", raw_valuesf[3], raw_valuesf[4], raw_valuesf[5]);
00125 
00126                 pc.printf("\r\n");
00127                 Thread::wait(4);
00128             }
00129         }
00130 
00131         if(cmd=='b') {
00132             uint8_t count = pc.getc();
00133             for(uint8_t i=0; i<count; i++) {
00134 
00135                 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]);
00136 
00137                 writeArr(raw_values, 6, sizeof(int16_t)); // writes accelerometer and gyro values
00138 
00139                 writeArr(&raw_values[6], 3, sizeof(int16_t)); // write magnetometer values
00140 
00141                 pc.printf("\r\n");
00142                 Thread::wait(4);
00143             }
00144         }
00145     }
00146 
00147 }