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 ypr[3];
00009   float alt;
00010   float temp;
00011   int time;
00012 } imu_t;
00013 
00014 FreeIMU imu;
00015 Mail<imu_t, 4> imu_queue;
00016 int i = 0;
00017 Timer t,t2;
00018 
00019 void getIMUdata(void const *n){
00020     i++;
00021     if (i%20==0){
00022         float *q = (float *)imu_queue.alloc();
00023         t.reset();
00024         t.start();
00025         imu.getYawPitchRoll(q);
00026         t.stop();
00027         ((imu_t*)q)->time = t.read_us();
00028         ((imu_t*)q)->alt = imu.getBaroAlt();
00029         ((imu_t*)q)->temp = imu.baro->getTemperature();
00030         imu_queue.put((imu_t*)q);
00031     }else{
00032         imu.getQ(NULL);
00033     }
00034 }
00035 
00036 int main() {
00037     pc.baud(115200);
00038     imu.init(true);
00039     RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)0);
00040     IMUTimer.start(2);
00041     t2.start();
00042     int16_t raw_values[9];
00043     
00044     Thread::wait(10);
00045     
00046     pc.printf("Init\r\n");
00047     
00048     while (true) {
00049         t2.reset();
00050         osEvent evt = imu_queue.get();
00051         if (evt.status == osEventMail) {
00052             imu_t *obj = (imu_t*)evt.value.p;
00053             pc.printf("Y: %f, P: %f, R: %f, Alt: %f, Temp: %fC\r\n", obj->ypr[0], obj->ypr[1], obj->ypr[2], obj->alt, obj->temp);
00054             imu_queue.free(obj);
00055         }
00056     }
00057 }