Yifei Teng / Mbed 2 deprecated FreeIMU_demo

FreeIMU demo program for mbed

Dependencies:   FreeIMU mbed-rtos mbed

main.cpp@3:39f5e2c1af36, 2018-03-29 (annotated)

Committer:
tyftyftyf
Date:
Thu Mar 29 22:35:55 2018 +0000
Revision:
3:39f5e2c1af36
Parent:
0:e1b8c014805d 
parameter tuning

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 3:39f5e2c1af36 21 if (i%20==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 3:39f5e2c1af36 37 pc.baud(115200);
tyftyftyf 0:e1b8c014805d 38 imu.init(true);
tyftyftyf 0:e1b8c014805d 39 RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)0);
tyftyftyf 0:e1b8c014805d 40 IMUTimer.start(2);
tyftyftyf 0:e1b8c014805d 41 t2.start();
tyftyftyf 3:39f5e2c1af36 42 int16_t raw_values[9];
tyftyftyf 3:39f5e2c1af36 43
tyftyftyf 3:39f5e2c1af36 44 Thread::wait(10);
tyftyftyf 3:39f5e2c1af36 45
tyftyftyf 3:39f5e2c1af36 46 pc.printf("Init\r\n");
tyftyftyf 0:e1b8c014805d 47
tyftyftyf 0:e1b8c014805d 48 while (true) {
tyftyftyf 0:e1b8c014805d 49 t2.reset();
tyftyftyf 0:e1b8c014805d 50 osEvent evt = imu_queue.get();
tyftyftyf 0:e1b8c014805d 51 if (evt.status == osEventMail) {
tyftyftyf 0:e1b8c014805d 52 imu_t *obj = (imu_t*)evt.value.p;
tyftyftyf 0:e1b8c014805d 53 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 54 imu_queue.free(obj);
tyftyftyf 0:e1b8c014805d 55 }
tyftyftyf 0:e1b8c014805d 56 }
tyftyftyf 0:e1b8c014805d 57 }