Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

IMU.h@7:cda17cffec3c, 2014-05-24 (annotated)

Committer:
MatteoT
Date:
Sat May 24 17:42:03 2014 +0000
Revision:
7:cda17cffec3c
Parent:
5:33abcc31b0aa 
experiments (going to first launch)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MatteoT 5:33abcc31b0aa 1 /*typedef struct {
MatteoT 5:33abcc31b0aa 2 float * ypr;
MatteoT 5:33abcc31b0aa 3 float alt;
MatteoT 5:33abcc31b0aa 4 float temp;
MatteoT 5:33abcc31b0aa 5 int time;
MatteoT 5:33abcc31b0aa 6 } IMU_Ejection;
MatteoT 5:33abcc31b0aa 7
MatteoT 5:33abcc31b0aa 8 typedef struct {
MatteoT 5:33abcc31b0aa 9 double target_imu_dt;
MatteoT 5:33abcc31b0aa 10 double actual_imu_dt;
MatteoT 5:33abcc31b0aa 11 double average_imu_dt;
MatteoT 5:33abcc31b0aa 12 double average_imu_dt_k;
MatteoT 5:33abcc31b0aa 13
MatteoT 5:33abcc31b0aa 14 void reset(){
MatteoT 5:33abcc31b0aa 15 target_imu_dt = TARGET_IMU_DT;
MatteoT 5:33abcc31b0aa 16 actual_imu_dt = TARGET_IMU_DT;
MatteoT 5:33abcc31b0aa 17 average_imu_dt = TARGET_IMU_DT;
MatteoT 5:33abcc31b0aa 18 average_imu_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 5:33abcc31b0aa 19 }
MatteoT 5:33abcc31b0aa 20 } IMU_QuadStateTimes;
MatteoT 5:33abcc31b0aa 21
MatteoT 5:33abcc31b0aa 22 //////////////////////////////////////////////////////////////
MatteoT 5:33abcc31b0aa 23
MatteoT 5:33abcc31b0aa 24 FreeIMU IMU_imu;
MatteoT 5:33abcc31b0aa 25 unsigned int IMU_step;
MatteoT 5:33abcc31b0aa 26 Mutex IMU_ejectionMutex;
MatteoT 5:33abcc31b0aa 27 IMU_Ejection IMU_eject;
MatteoT 5:33abcc31b0aa 28 IMU_QuadStateTimes IMU_quadState;
MatteoT 5:33abcc31b0aa 29 Timer IMU_dt_timer;
MatteoT 5:33abcc31b0aa 30 DigitalOut IMU_led (LED_IMU);
MatteoT 5:33abcc31b0aa 31 //RtosTimer IMU_timer; //moved to main thread stack
MatteoT 5:33abcc31b0aa 32
MatteoT 5:33abcc31b0aa 33 void IMU_init(){
MatteoT 5:33abcc31b0aa 34 IMU_led = 1;
MatteoT 5:33abcc31b0aa 35 IMU_step = 0;
MatteoT 5:33abcc31b0aa 36 IMU_imu.init(true);
MatteoT 5:33abcc31b0aa 37 IMU_quadState.reset();
MatteoT 5:33abcc31b0aa 38 }
MatteoT 5:33abcc31b0aa 39
MatteoT 5:33abcc31b0aa 40 void IMU_update (void const * param){
MatteoT 5:33abcc31b0aa 41 ++IMU_step;
MatteoT 5:33abcc31b0aa 42 if(IMU_step%10 == 0){
MatteoT 5:33abcc31b0aa 43 //get results to be ejected.
MatteoT 5:33abcc31b0aa 44 IMU_ejectionMutex.lock();
MatteoT 5:33abcc31b0aa 45
MatteoT 5:33abcc31b0aa 46 IMU_led = 1;
MatteoT 5:33abcc31b0aa 47 IMU_imu.getYawPitchRoll(IMU_eject.ypr);
MatteoT 5:33abcc31b0aa 48 IMU_eject.alt = IMU_imu.getBaroAlt();
MatteoT 5:33abcc31b0aa 49 IMU_eject.temp = IMU_imu.baro->getTemperature();
MatteoT 5:33abcc31b0aa 50 QUAD_STATE_UPDATE_DT(IMU_quadState,imu,IMU_dt_timer)
MatteoT 5:33abcc31b0aa 51 IMU_led = 0;
MatteoT 5:33abcc31b0aa 52
MatteoT 5:33abcc31b0aa 53 IMU_ejectionMutex.unlock();
MatteoT 5:33abcc31b0aa 54 }else{
MatteoT 5:33abcc31b0aa 55 IMU_imu.getQ(NULL);
MatteoT 5:33abcc31b0aa 56 }
MatteoT 5:33abcc31b0aa 57 }
MatteoT 5:33abcc31b0aa 58
MatteoT 5:33abcc31b0aa 59 void IMU_injectStateTo (QuadState & state){
MatteoT 5:33abcc31b0aa 60 IMU_ejectionMutex.lock();
MatteoT 5:33abcc31b0aa 61
MatteoT 5:33abcc31b0aa 62 state.estimated_rotation_y = IMU_eject.ypr[0];
MatteoT 5:33abcc31b0aa 63 state.estimated_rotation_p = IMU_eject.ypr[1];
MatteoT 5:33abcc31b0aa 64 state.estimated_rotation_r = IMU_eject.ypr[2];
MatteoT 5:33abcc31b0aa 65
MatteoT 5:33abcc31b0aa 66 state.target_imu_dt = IMU_quadState.target_imu_dt;
MatteoT 5:33abcc31b0aa 67 state.actual_imu_dt = IMU_quadState.actual_imu_dt;
MatteoT 5:33abcc31b0aa 68 state.average_imu_dt = IMU_quadState.average_imu_dt;
MatteoT 5:33abcc31b0aa 69 state.average_imu_dt_k = IMU_quadState.average_imu_dt_k;
MatteoT 5:33abcc31b0aa 70
MatteoT 5:33abcc31b0aa 71 IMU_ejectionMutex.unlock();
MatteoT 5:33abcc31b0aa 72 } */