Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
main.cpp
- Committer:
- MatteoT
- Date:
- 2014-05-13
- Revision:
- 6:4ddd68260f76
- Parent:
- 5:33abcc31b0aa
- Child:
- 7:cda17cffec3c
File content as of revision 6:4ddd68260f76:
#include "math.h" #include "mbed.h" #include "rtos.h" #include "esc.h" #include "FreeIMU.h" #include <string> //Author: Matteo Terruzzi //Target differential of time in seconds, for the various program cycles: #define TARGET_IMU_DT 0.002 //500Hz on main thread, every step of the cycle. #define TARGET_MAIN_DT 0.016 //62.5Hz on main thread, every 8 steps + 7 step delay. #define TARGET_TX_DT 0.050 //20Hz on other thread. Exchanged with main every 24 steps + 21 step delay (dt=48ms, 20.8333Hz) #define TARGET_RX_DT 0.050 //waited with previous: this dt is only measured, not waited. //For the various dt, "average" is computed using a sort of low-pass filter of the actual value. //The formula (recurrence equation) is: // a(t) = x(t) * k - a(t-1) * k with a(0) = 0 //where: // x(t) is the actual value at the time t; // a(t) is the average at the timepoint t; // a(t-1) is the average at the previous timepoint; // k is AVERAGE_DT_K_GAIN (macro defined below). // //So, here is the meaning of AVERAGE_DT_K_GAIN, or k: the higher it is, the higher is the cut-frequency. //If k == 1, then a(t) == x(t). //If k == 0, then a(t) == target value (the actual value is totaly ignored). //A good value may be k = 1/8 = 0.125. #define AVERAGE_DT_K_GAIN 0.0125 //Blinking status leds #define LED_IMU LED1 #define LED_ESC LED2 #define LED_TXRX LED3 #define LED_RX LED4 #define FAST_FLASH_ON(led,times) for(int i=0;i<times;++i){Thread::wait(25); led = 0; Thread::wait(50); led = 1; Thread::wait(25); } #define FAST_FLASH_OFF(led,times) for(int i=0;i<times;++i){Thread::wait(25); led = 1; Thread::wait(50); led = 0; Thread::wait(25); } #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_magic.h" FreeIMU IMU_imu; int main() { //INITIALIZATION //------------------------------------------------------------------------------- //Setup ESCs ESC esc1(p23); esc1=0; ESC esc2(p24); esc1=0; ESC esc3(p25); esc1=0; ESC esc4(p26); esc1=0; //Setup status leds DigitalOut led_imu(LED_IMU); led_imu = 0; DigitalOut led_esc(LED_ESC); led_esc = 0; DigitalOut led_txrx(LED_TXRX); led_txrx = 0; FAST_FLASH_ON(led_esc,5); //Setup remote control FAST_FLASH_OFF(led_txrx,2); Thread TXRX_thread(TXRX_thread_routine); 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(2000); FAST_FLASH_ON(led_imu,5); //Setup state QuadState mainQuadState; mainQuadState.reset(); QuadState rxQuadState; rxQuadState.reset(); QuadState txQuadState; txQuadState.reset(); //MAIN CONTROL CYCLE PREPARATION //------------------------------------------------------------------------------- unsigned short int step = 0; //Just a moment... Let the other threads begin! Thread::yield(); 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. //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 //=================================================================================== while(1) { //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) if(step % 8 != 7){//see step%8==7; prevent double sensor refresh //IMU INPUT //------------------------------------------------------------------------------- led_imu = 1; IMU_imu.getQ(NULL); led_imu = 0; }//end of 7/8 steps if(step % 8 == 7){ //IMU INPUT AND ANGLES COMPUTATION //------------------------------------------------------------------------------- { float ypr[3]; led_imu = 1; IMU_imu.getYawPitchRoll(ypr); mainQuadState.estimated_position_z = IMU_imu.getBaroAlt(); IMU_imu.baro->getTemperature(); 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) mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!! //ELABORATION //------------------------------------------------------------------------------- //Measure the time for main control cycle QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer) //Elaborate inputs to determinate outputs led_esc = 1; {//Roll PID float previous = mainQuadState.pid_rotation_r_error; mainQuadState.pid_rotation_r_error = mainQuadState.target_rotation_r - mainQuadState.estimated_rotation_r; mainQuadState.pid_rotation_r_error_integrative += mainQuadState.pid_rotation_r_error; mainQuadState.pid_rotation_r_error_derivative = mainQuadState.pid_rotation_r_error - previous; mainQuadState.pid_rotation_r_out = mainQuadState.pid_rotation_r_kP * mainQuadState.pid_rotation_r_error + mainQuadState.pid_rotation_r_kI * mainQuadState.pid_rotation_r_error_integrative + mainQuadState.pid_rotation_r_kD * mainQuadState.pid_rotation_r_error_derivative; if(mainQuadState.pid_rotation_r_error_integrative > 100) mainQuadState.pid_rotation_r_error_integrative = 100; else if(mainQuadState.pid_rotation_r_error_integrative < -100) mainQuadState.pid_rotation_r_error_integrative = -100; }{//Pitch PID float previous = mainQuadState.pid_rotation_p_error; mainQuadState.pid_rotation_p_error = mainQuadState.target_rotation_p - mainQuadState.estimated_rotation_p; mainQuadState.pid_rotation_p_error_integrative += mainQuadState.pid_rotation_p_error; mainQuadState.pid_rotation_p_error_derivative = mainQuadState.pid_rotation_p_error - previous; mainQuadState.pid_rotation_p_out = mainQuadState.pid_rotation_p_kP * mainQuadState.pid_rotation_p_error + mainQuadState.pid_rotation_p_kI * mainQuadState.pid_rotation_p_error_integrative + mainQuadState.pid_rotation_p_kD * mainQuadState.pid_rotation_p_error_derivative; if(mainQuadState.pid_rotation_p_error_integrative > 100) mainQuadState.pid_rotation_p_error_integrative = 100; else if(mainQuadState.pid_rotation_p_error_integrative < -100) mainQuadState.pid_rotation_p_error_integrative = -100; }{//Yaw PID float previous = mainQuadState.pid_rotation_y_error; mainQuadState.pid_rotation_y_error = mainQuadState.target_rotation_y - mainQuadState.estimated_rotation_y; mainQuadState.pid_rotation_y_error_integrative += mainQuadState.pid_rotation_y_error; mainQuadState.pid_rotation_y_error_derivative = mainQuadState.pid_rotation_y_error - previous; mainQuadState.pid_rotation_y_out = mainQuadState.pid_rotation_y_kP * mainQuadState.pid_rotation_y_error + mainQuadState.pid_rotation_y_kI * mainQuadState.pid_rotation_y_error_integrative + mainQuadState.pid_rotation_y_kD * mainQuadState.pid_rotation_y_error_derivative; if(mainQuadState.pid_rotation_y_error_integrative > 100) mainQuadState.pid_rotation_y_error_integrative = 100; else if(mainQuadState.pid_rotation_y_error_integrative < -100) mainQuadState.pid_rotation_y_error_integrative = -100; } //Compute actual throttle values if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0 || rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0){ mainQuadState.actual_throttle_1 = 0; mainQuadState.actual_throttle_2 = 0; mainQuadState.actual_throttle_3 = 0; mainQuadState.actual_throttle_4 = 0; }else{ mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt; mainQuadState.average_rx_dt = rxQuadState.average_rx_dt; mainQuadState.target_rotation_r = rxQuadState.target_rotation_r; mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired; mainQuadState.reference_throttle_1 = rxQuadState.reference_throttle_1; mainQuadState.reference_throttle_2 = rxQuadState.reference_throttle_2; mainQuadState.reference_throttle_3 = rxQuadState.reference_throttle_3; mainQuadState.reference_throttle_4 = rxQuadState.reference_throttle_4; if(mainQuadState.reference_throttle_1 <= -1000) 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_p_out; out += mainQuadState.pid_rotation_y_out; mainQuadState.actual_throttle_1 = out; } if(mainQuadState.reference_throttle_2 <= -1000) 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; } if(mainQuadState.reference_throttle_3 <= -1000) mainQuadState.actual_throttle_3 = 0; else{ float out = mainQuadState.reference_throttle_3; out -= mainQuadState.pid_rotation_r_out; out -= mainQuadState.pid_rotation_p_out; out += mainQuadState.pid_rotation_y_out; mainQuadState.actual_throttle_3 = out; } if(mainQuadState.reference_throttle_4 <= -1000) 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; } } //OUTPUT //------------------------------------------------------------------------------- //set new ESC values. esc1 = mainQuadState.actual_throttle_1; esc2 = mainQuadState.actual_throttle_2; esc3 = mainQuadState.actual_throttle_3; esc4 = mainQuadState.actual_throttle_4; //effectively output to ESCs esc1(); esc2(); esc3(); esc4(); led_esc = 0; } //end of 1/8 steps if(step % 24 == 21){ //REMOTE INPUT/OUTPUT EXCHANGE //------------------------------------------------------------------------------- led_txrx = 1; txQuadState = mainQuadState; //send info back txQuadState.timestamp = time(NULL); if(TXRX_stateExchange(txQuadState,rxQuadState)){ mainQuadState.average_rx_dt = rxQuadState.average_rx_dt; mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt; mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER; if(mainQuadState.actual_rx_dt >= 100.0 * mainQuadState.target_rx_dt) mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!! } led_txrx = 0; }//end of 1/24 steps ++step; }//End of main control loop //It must not get here. return 0; }