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?
User | Revision | Line number | New 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 | } */ |