Yifei Teng / Mbed 2 deprecated FreeIMU_demo

FreeIMU demo program for mbed

Dependencies:   FreeIMU mbed-rtos mbed

main.cpp@0:e1b8c014805d, 2013-11-29 (annotated)

Committer:
tyftyftyf
Date:
Fri Nov 29 15:34:11 2013 +0000
Revision:
0:e1b8c014805d
Child:
3:39f5e2c1af36
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tyftyftyf 0:e1b8c014805d 1 #include "mbed.h"
tyftyftyf 0:e1b8c014805d 2 #include "rtos.h"
tyftyftyf 0:e1b8c014805d 3 #include "FreeIMU.h"
tyftyftyf 0:e1b8c014805d 4
tyftyftyf 0:e1b8c014805d 5 Serial pc(USBTX, USBRX); // tx, rx
tyftyftyf 0:e1b8c014805d 6
tyftyftyf 0:e1b8c014805d 7 typedef struct {
tyftyftyf 0:e1b8c014805d 8 float ypr[3];
tyftyftyf 0:e1b8c014805d 9 float alt;
tyftyftyf 0:e1b8c014805d 10 float temp;
tyftyftyf 0:e1b8c014805d 11 int time;
tyftyftyf 0:e1b8c014805d 12 } imu_t;
tyftyftyf 0:e1b8c014805d 13
tyftyftyf 0:e1b8c014805d 14 FreeIMU imu;
tyftyftyf 0:e1b8c014805d 15 Mail<imu_t, 4> imu_queue;
tyftyftyf 0:e1b8c014805d 16 int i = 0;
tyftyftyf 0:e1b8c014805d 17 Timer t,t2;
tyftyftyf 0:e1b8c014805d 18
tyftyftyf 0:e1b8c014805d 19 void getIMUdata(void const *n){
tyftyftyf 0:e1b8c014805d 20 i++;
tyftyftyf 0:e1b8c014805d 21 if (i%10==0){
tyftyftyf 0:e1b8c014805d 22 float *q = (float *)imu_queue.alloc();
tyftyftyf 0:e1b8c014805d 23 t.reset();
tyftyftyf 0:e1b8c014805d 24 t.start();
tyftyftyf 0:e1b8c014805d 25 imu.getYawPitchRoll(q);
tyftyftyf 0:e1b8c014805d 26 t.stop();
tyftyftyf 0:e1b8c014805d 27 ((imu_t*)q)->time = t.read_us();
tyftyftyf 0:e1b8c014805d 28 ((imu_t*)q)->alt = imu.getBaroAlt();
tyftyftyf 0:e1b8c014805d 29 ((imu_t*)q)->temp = imu.baro->getTemperature();
tyftyftyf 0:e1b8c014805d 30 imu_queue.put((imu_t*)q);
tyftyftyf 0:e1b8c014805d 31 }else{
tyftyftyf 0:e1b8c014805d 32 imu.getQ(NULL);
tyftyftyf 0:e1b8c014805d 33 }
tyftyftyf 0:e1b8c014805d 34 }
tyftyftyf 0:e1b8c014805d 35
tyftyftyf 0:e1b8c014805d 36 int main() {
tyftyftyf 0:e1b8c014805d 37 imu.init(true);
tyftyftyf 0:e1b8c014805d 38 pc.baud(115200);
tyftyftyf 0:e1b8c014805d 39 RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)0);
tyftyftyf 0:e1b8c014805d 40 IMUTimer.start(2);
tyftyftyf 0:e1b8c014805d 41 t2.start();
tyftyftyf 0:e1b8c014805d 42
tyftyftyf 0:e1b8c014805d 43 while (true) {
tyftyftyf 0:e1b8c014805d 44 t2.reset();
tyftyftyf 0:e1b8c014805d 45 osEvent evt = imu_queue.get();
tyftyftyf 0:e1b8c014805d 46 if (evt.status == osEventMail) {
tyftyftyf 0:e1b8c014805d 47 imu_t *obj = (imu_t*)evt.value.p;
tyftyftyf 0:e1b8c014805d 48 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);
tyftyftyf 0:e1b8c014805d 49 imu_queue.free(obj);
tyftyftyf 0:e1b8c014805d 50 pc.printf("\r\n");
tyftyftyf 0:e1b8c014805d 51 }
tyftyftyf 0:e1b8c014805d 52 }
tyftyftyf 0:e1b8c014805d 53 }