Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

IMU.h@2:7439607ccd51, 2013-10-15 (annotated)

Committer:
MatteoT
Date:
Tue Oct 15 18:30:46 2013 +0000
Revision:
2:7439607ccd51
IMU experiments with serial communication

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MatteoT 2:7439607ccd51 1 #include "mbed.h"
MatteoT 2:7439607ccd51 2 #include "rtos.h"
MatteoT 2:7439607ccd51 3 #include "MPU6050.h"
MatteoT 2:7439607ccd51 4 #include "IMUfilter.h"
MatteoT 2:7439607ccd51 5
MatteoT 2:7439607ccd51 6 #ifndef IMU_H
MatteoT 2:7439607ccd51 7 #define IMU_H
MatteoT 2:7439607ccd51 8
MatteoT 2:7439607ccd51 9 Mutex IMU_mutex;
MatteoT 2:7439607ccd51 10 const float IMU_dt = 0.010;
MatteoT 2:7439607ccd51 11 IMUfilter IMU_filter(IMU_dt,40);
MatteoT 2:7439607ccd51 12
MatteoT 2:7439607ccd51 13 void IMU_thread_routine (void const *args){
MatteoT 2:7439607ccd51 14 const uint32_t wait_time_ms = (uint32_t)((float)1000.0 * IMU_dt);
MatteoT 2:7439607ccd51 15 DigitalOut led2(LED2);
MatteoT 2:7439607ccd51 16 led2 = 0;
MatteoT 2:7439607ccd51 17 MPU6050 IMU_sensor(p28, p27); //NOTE: set here I2C pin!!!
MatteoT 2:7439607ccd51 18 IMU_sensor.setGyroRange(MPU6050_GYRO_RANGE_250);
MatteoT 2:7439607ccd51 19 IMU_sensor.setAcceleroRange(MPU6050_ACCELERO_RANGE_4G);
MatteoT 2:7439607ccd51 20
MatteoT 2:7439607ccd51 21 led2 = 1;
MatteoT 2:7439607ccd51 22 while(true){
MatteoT 2:7439607ccd51 23 float a[3], g[3];
MatteoT 2:7439607ccd51 24
MatteoT 2:7439607ccd51 25 IMU_sensor.getAccelero(a);
MatteoT 2:7439607ccd51 26 IMU_sensor.getGyro(g);
MatteoT 2:7439607ccd51 27
MatteoT 2:7439607ccd51 28 IMU_mutex.lock();
MatteoT 2:7439607ccd51 29 IMU_filter.updateFilter(g[0], g[1], g[2], a[0], a[1], a[2]);
MatteoT 2:7439607ccd51 30 IMU_mutex.unlock();
MatteoT 2:7439607ccd51 31
MatteoT 2:7439607ccd51 32 led2 = !led2;
MatteoT 2:7439607ccd51 33
MatteoT 2:7439607ccd51 34 Thread::wait(wait_time_ms);
MatteoT 2:7439607ccd51 35 }
MatteoT 2:7439607ccd51 36 }
MatteoT 2:7439607ccd51 37
MatteoT 2:7439607ccd51 38
MatteoT 2:7439607ccd51 39 inline void IMU_filter_getYawPitchRoll (float &y, float &p, float &r){
MatteoT 2:7439607ccd51 40 IMU_mutex.lock();
MatteoT 2:7439607ccd51 41 IMU_filter.computeEuler();
MatteoT 2:7439607ccd51 42 y = IMU_filter.getYaw();
MatteoT 2:7439607ccd51 43 p = IMU_filter.getPitch();
MatteoT 2:7439607ccd51 44 r = IMU_filter.getRoll();
MatteoT 2:7439607ccd51 45 IMU_mutex.unlock();
MatteoT 2:7439607ccd51 46 }
MatteoT 2:7439607ccd51 47
MatteoT 2:7439607ccd51 48
MatteoT 2:7439607ccd51 49 #endif