IMU.h

00001 /*typedef struct {
00002     float * ypr;
00003     float alt;
00004     float temp;
00005     int time;
00006 } IMU_Ejection;
00007 
00008 typedef struct {
00009     double target_imu_dt;
00010     double actual_imu_dt;
00011     double average_imu_dt;
00012     double average_imu_dt_k;
00013     
00014     void reset(){
00015         target_imu_dt = TARGET_IMU_DT;
00016         actual_imu_dt = TARGET_IMU_DT;
00017         average_imu_dt = TARGET_IMU_DT;
00018         average_imu_dt_k = AVERAGE_DT_K_GAIN;
00019     }
00020 } IMU_QuadStateTimes;
00021 
00022 //////////////////////////////////////////////////////////////
00023 
00024 FreeIMU              IMU_imu;
00025 unsigned int         IMU_step;
00026 Mutex                IMU_ejectionMutex;
00027 IMU_Ejection         IMU_eject;
00028 IMU_QuadStateTimes   IMU_quadState;
00029 Timer                IMU_dt_timer;
00030 DigitalOut           IMU_led (LED_IMU);
00031 //RtosTimer          IMU_timer; //moved to main thread stack
00032 
00033 void IMU_init(){
00034     IMU_led = 1;
00035     IMU_step = 0;
00036     IMU_imu.init(true);
00037     IMU_quadState.reset();
00038 }
00039 
00040 void IMU_update (void const * param){
00041     ++IMU_step;
00042     if(IMU_step%10 == 0){
00043         //get results to be ejected.
00044         IMU_ejectionMutex.lock();
00045         
00046             IMU_led = 1;
00047             IMU_imu.getYawPitchRoll(IMU_eject.ypr);
00048             IMU_eject.alt = IMU_imu.getBaroAlt();
00049             IMU_eject.temp = IMU_imu.baro->getTemperature();
00050             QUAD_STATE_UPDATE_DT(IMU_quadState,imu,IMU_dt_timer)
00051             IMU_led = 0;
00052 
00053         IMU_ejectionMutex.unlock();
00054     }else{
00055         IMU_imu.getQ(NULL);
00056     }
00057 }
00058 
00059 void IMU_injectStateTo (QuadState & state){
00060     IMU_ejectionMutex.lock();
00061     
00062         state.estimated_rotation_y = IMU_eject.ypr[0];
00063         state.estimated_rotation_p = IMU_eject.ypr[1];
00064         state.estimated_rotation_r = IMU_eject.ypr[2];
00065         
00066         state.target_imu_dt = IMU_quadState.target_imu_dt;
00067         state.actual_imu_dt = IMU_quadState.actual_imu_dt;
00068         state.average_imu_dt = IMU_quadState.average_imu_dt;
00069         state.average_imu_dt_k = IMU_quadState.average_imu_dt_k;
00070 
00071     IMU_ejectionMutex.unlock();
00072 } */