Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
Diff: main.cpp
- Revision:
- 6:4ddd68260f76
- Parent:
- 5:33abcc31b0aa
- Child:
- 7:cda17cffec3c
--- a/main.cpp Mon May 12 22:58:06 2014 +0000 +++ b/main.cpp Tue May 13 22:56:44 2014 +0000 @@ -40,7 +40,7 @@ #define SLOW_FLASH_ON(led,times) for(int i=0;i<times;++i){Thread::wait(250); led = 0; Thread::wait(500); led = 1; Thread::wait(250);} #include "state.h" -#include "TXRX_nomagic.h" +#include "TXRX_magic.h" FreeIMU IMU_imu; @@ -66,14 +66,14 @@ //Setup remote control FAST_FLASH_OFF(led_txrx,2); Thread TXRX_thread(TXRX_thread_routine); - Thread::wait(8000); + Thread::wait(6000); FAST_FLASH_ON(led_txrx,5); //Setup IMU Thread::wait(2000); FAST_FLASH_OFF(led_imu,2); IMU_imu.init(true); - Thread::wait(8000); + Thread::wait(2000); FAST_FLASH_ON(led_imu,5); //Setup state @@ -90,22 +90,21 @@ //Just a moment... Let the other threads begin! Thread::yield(); - Thread::wait(1000.0 * (TARGET_MAIN_DT + TARGET_IMU_DT + TARGET_TX_DT + TARGET_RX_DT) * 10.0); + Thread::wait((TARGET_MAIN_DT + TARGET_IMU_DT + TARGET_TX_DT + TARGET_RX_DT) * 10.0); //just made sure the other threads had time to begin. - //Setup timers - Timer dt_timer; dt_timer.reset(); dt_timer.start(); - Timer IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start(); - Thread::wait(1000.0 * TARGET_IMU_DT); - - //Some flashing animation for(int i=100;i>10;i=(75*i)/100){ led_txrx = 0;led_esc = 0;led_imu = 0; Thread::wait(i); led_txrx = 1;led_esc = 1;led_imu = 1; Thread::wait(i); } + //Setup timers + Timer dt_timer; dt_timer.reset(); dt_timer.start(); + Timer IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start(); + Thread::wait(TARGET_IMU_DT); + //MAIN CONTROL CYCLE @@ -115,10 +114,10 @@ //measure the time for IMU cycle + Thread::wait(1); QUAD_STATE_READ_ACTUAL_DT(mainQuadState,imu,IMU_dt_timer) QUAD_STATE_WAIT_DT_TARGET(mainQuadState.actual_imu_dt,mainQuadState.target_imu_dt) QUAD_STATE_UPDATE_DT(mainQuadState,imu,IMU_dt_timer) - wait_us(1000); if(step % 8 != 7){//see step%8==7; prevent double sensor refresh @@ -145,9 +144,9 @@ IMU_imu.getYawPitchRoll(ypr); mainQuadState.estimated_position_z = IMU_imu.getBaroAlt(); IMU_imu.baro->getTemperature(); - mainQuadState.estimated_rotation_y = ypr[0]; - mainQuadState.estimated_rotation_p = ypr[1]; - mainQuadState.estimated_rotation_r = ypr[2]; + mainQuadState.estimated_rotation_y = ypr[0]; //yaw = yaw + mainQuadState.estimated_rotation_p = ypr[2]; //pitch = roll + mainQuadState.estimated_rotation_r = ypr[1]; //roll = pitch led_imu = 0; } if(mainQuadState.actual_imu_dt >= 100.0 * mainQuadState.target_imu_dt) @@ -216,7 +215,9 @@ mainQuadState.actual_throttle_1 = 0; else{ float out = mainQuadState.reference_throttle_1; //NOTE: here we will have full throttle calcs.. - out -= mainQuadState.pid_rotation_r_out; + out += mainQuadState.pid_rotation_r_out; + out += mainQuadState.pid_rotation_p_out; + out += mainQuadState.pid_rotation_y_out; mainQuadState.actual_throttle_1 = out; } @@ -224,6 +225,9 @@ mainQuadState.actual_throttle_2 = 0; else{ float out = mainQuadState.reference_throttle_2; + out += mainQuadState.pid_rotation_r_out; + out -= mainQuadState.pid_rotation_p_out; + out -= mainQuadState.pid_rotation_y_out; mainQuadState.actual_throttle_2 = out; } @@ -231,7 +235,9 @@ mainQuadState.actual_throttle_3 = 0; else{ float out = mainQuadState.reference_throttle_3; - out += mainQuadState.pid_rotation_r_out; + out -= mainQuadState.pid_rotation_r_out; + out -= mainQuadState.pid_rotation_p_out; + out += mainQuadState.pid_rotation_y_out; mainQuadState.actual_throttle_3 = out; } @@ -239,6 +245,9 @@ mainQuadState.actual_throttle_4 = 0; else{ float out = mainQuadState.reference_throttle_4; + out -= mainQuadState.pid_rotation_r_out; + out += mainQuadState.pid_rotation_p_out; + out -= mainQuadState.pid_rotation_y_out; mainQuadState.actual_throttle_4 = out; } }