Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
IMU.h
- Committer:
- MatteoT
- Date:
- 2014-05-24
- Revision:
- 7:cda17cffec3c
- Parent:
- 5:33abcc31b0aa
File content as of revision 7:cda17cffec3c:
/*typedef struct { float * ypr; float alt; float temp; int time; } IMU_Ejection; typedef struct { double target_imu_dt; double actual_imu_dt; double average_imu_dt; double average_imu_dt_k; void reset(){ target_imu_dt = TARGET_IMU_DT; actual_imu_dt = TARGET_IMU_DT; average_imu_dt = TARGET_IMU_DT; average_imu_dt_k = AVERAGE_DT_K_GAIN; } } IMU_QuadStateTimes; ////////////////////////////////////////////////////////////// FreeIMU IMU_imu; unsigned int IMU_step; Mutex IMU_ejectionMutex; IMU_Ejection IMU_eject; IMU_QuadStateTimes IMU_quadState; Timer IMU_dt_timer; DigitalOut IMU_led (LED_IMU); //RtosTimer IMU_timer; //moved to main thread stack void IMU_init(){ IMU_led = 1; IMU_step = 0; IMU_imu.init(true); IMU_quadState.reset(); } void IMU_update (void const * param){ ++IMU_step; if(IMU_step%10 == 0){ //get results to be ejected. IMU_ejectionMutex.lock(); IMU_led = 1; IMU_imu.getYawPitchRoll(IMU_eject.ypr); IMU_eject.alt = IMU_imu.getBaroAlt(); IMU_eject.temp = IMU_imu.baro->getTemperature(); QUAD_STATE_UPDATE_DT(IMU_quadState,imu,IMU_dt_timer) IMU_led = 0; IMU_ejectionMutex.unlock(); }else{ IMU_imu.getQ(NULL); } } void IMU_injectStateTo (QuadState & state){ IMU_ejectionMutex.lock(); state.estimated_rotation_y = IMU_eject.ypr[0]; state.estimated_rotation_p = IMU_eject.ypr[1]; state.estimated_rotation_r = IMU_eject.ypr[2]; state.target_imu_dt = IMU_quadState.target_imu_dt; state.actual_imu_dt = IMU_quadState.actual_imu_dt; state.average_imu_dt = IMU_quadState.average_imu_dt; state.average_imu_dt_k = IMU_quadState.average_imu_dt_k; IMU_ejectionMutex.unlock(); } */