Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

main.cpp@6:4ddd68260f76, 2014-05-13 (annotated)

Committer:
MatteoT
Date:
Tue May 13 22:56:44 2014 +0000
Revision:
6:4ddd68260f76
Parent:
5:33abcc31b0aa
Child:
7:cda17cffec3c
experiments (magic networking send/receive working)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MatteoT 2:7439607ccd51 1 #include "math.h"
MatteoT 0:22b18227d29e 2 #include "mbed.h"
MatteoT 2:7439607ccd51 3 #include "rtos.h"
MatteoT 0:22b18227d29e 4 #include "esc.h"
MatteoT 5:33abcc31b0aa 5 #include "FreeIMU.h"
MatteoT 3:5e7476bca25f 6 #include <string>
MatteoT 3:5e7476bca25f 7
MatteoT 3:5e7476bca25f 8
MatteoT 3:5e7476bca25f 9 //Author: Matteo Terruzzi
MatteoT 3:5e7476bca25f 10
MatteoT 3:5e7476bca25f 11 //Target differential of time in seconds, for the various program cycles:
MatteoT 5:33abcc31b0aa 12 #define TARGET_IMU_DT 0.002 //500Hz on main thread, every step of the cycle.
MatteoT 5:33abcc31b0aa 13 #define TARGET_MAIN_DT 0.016 //62.5Hz on main thread, every 8 steps + 7 step delay.
MatteoT 5:33abcc31b0aa 14 #define TARGET_TX_DT 0.050 //20Hz on other thread. Exchanged with main every 24 steps + 21 step delay (dt=48ms, 20.8333Hz)
MatteoT 5:33abcc31b0aa 15 #define TARGET_RX_DT 0.050 //waited with previous: this dt is only measured, not waited.
MatteoT 3:5e7476bca25f 16
MatteoT 3:5e7476bca25f 17 //For the various dt, "average" is computed using a sort of low-pass filter of the actual value.
MatteoT 3:5e7476bca25f 18 //The formula (recurrence equation) is:
MatteoT 3:5e7476bca25f 19 // a(t) = x(t) * k - a(t-1) * k with a(0) = 0
MatteoT 3:5e7476bca25f 20 //where:
MatteoT 3:5e7476bca25f 21 // x(t) is the actual value at the time t;
MatteoT 3:5e7476bca25f 22 // a(t) is the average at the timepoint t;
MatteoT 3:5e7476bca25f 23 // a(t-1) is the average at the previous timepoint;
MatteoT 3:5e7476bca25f 24 // k is AVERAGE_DT_K_GAIN (macro defined below).
MatteoT 3:5e7476bca25f 25 //
MatteoT 3:5e7476bca25f 26 //So, here is the meaning of AVERAGE_DT_K_GAIN, or k: the higher it is, the higher is the cut-frequency.
MatteoT 3:5e7476bca25f 27 //If k == 1, then a(t) == x(t).
MatteoT 3:5e7476bca25f 28 //If k == 0, then a(t) == target value (the actual value is totaly ignored).
MatteoT 3:5e7476bca25f 29 //A good value may be k = 1/8 = 0.125.
MatteoT 3:5e7476bca25f 30 #define AVERAGE_DT_K_GAIN 0.0125
MatteoT 3:5e7476bca25f 31
MatteoT 3:5e7476bca25f 32 //Blinking status leds
MatteoT 5:33abcc31b0aa 33 #define LED_IMU LED1
MatteoT 5:33abcc31b0aa 34 #define LED_ESC LED2
MatteoT 3:5e7476bca25f 35 #define LED_TXRX LED3
MatteoT 5:33abcc31b0aa 36 #define LED_RX LED4
MatteoT 3:5e7476bca25f 37
MatteoT 5:33abcc31b0aa 38 #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); }
MatteoT 5:33abcc31b0aa 39 #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); }
MatteoT 5:33abcc31b0aa 40 #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);}
MatteoT 3:5e7476bca25f 41
MatteoT 2:7439607ccd51 42 #include "state.h"
MatteoT 6:4ddd68260f76 43 #include "TXRX_magic.h"
MatteoT 0:22b18227d29e 44
MatteoT 5:33abcc31b0aa 45 FreeIMU IMU_imu;
MatteoT 5:33abcc31b0aa 46
MatteoT 0:22b18227d29e 47 int main()
MatteoT 0:22b18227d29e 48 {
MatteoT 3:5e7476bca25f 49
MatteoT 3:5e7476bca25f 50
MatteoT 3:5e7476bca25f 51 //INITIALIZATION
MatteoT 3:5e7476bca25f 52 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 53
MatteoT 0:22b18227d29e 54 //Setup ESCs
MatteoT 5:33abcc31b0aa 55 ESC esc1(p23); esc1=0;
MatteoT 5:33abcc31b0aa 56 ESC esc2(p24); esc1=0;
MatteoT 5:33abcc31b0aa 57 ESC esc3(p25); esc1=0;
MatteoT 5:33abcc31b0aa 58 ESC esc4(p26); esc1=0;
MatteoT 2:7439607ccd51 59
MatteoT 2:7439607ccd51 60 //Setup status leds
MatteoT 5:33abcc31b0aa 61 DigitalOut led_imu(LED_IMU); led_imu = 0;
MatteoT 5:33abcc31b0aa 62 DigitalOut led_esc(LED_ESC); led_esc = 0;
MatteoT 5:33abcc31b0aa 63 DigitalOut led_txrx(LED_TXRX); led_txrx = 0;
MatteoT 5:33abcc31b0aa 64 FAST_FLASH_ON(led_esc,5);
MatteoT 0:22b18227d29e 65
MatteoT 3:5e7476bca25f 66 //Setup remote control
MatteoT 5:33abcc31b0aa 67 FAST_FLASH_OFF(led_txrx,2);
MatteoT 5:33abcc31b0aa 68 Thread TXRX_thread(TXRX_thread_routine);
MatteoT 6:4ddd68260f76 69 Thread::wait(6000);
MatteoT 5:33abcc31b0aa 70 FAST_FLASH_ON(led_txrx,5);
MatteoT 5:33abcc31b0aa 71
MatteoT 5:33abcc31b0aa 72 //Setup IMU
MatteoT 5:33abcc31b0aa 73 Thread::wait(2000);
MatteoT 5:33abcc31b0aa 74 FAST_FLASH_OFF(led_imu,2);
MatteoT 5:33abcc31b0aa 75 IMU_imu.init(true);
MatteoT 6:4ddd68260f76 76 Thread::wait(2000);
MatteoT 5:33abcc31b0aa 77 FAST_FLASH_ON(led_imu,5);
MatteoT 0:22b18227d29e 78
MatteoT 2:7439607ccd51 79 //Setup state
MatteoT 2:7439607ccd51 80 QuadState mainQuadState; mainQuadState.reset();
MatteoT 2:7439607ccd51 81 QuadState rxQuadState; rxQuadState.reset();
MatteoT 2:7439607ccd51 82 QuadState txQuadState; txQuadState.reset();
MatteoT 3:5e7476bca25f 83
MatteoT 3:5e7476bca25f 84
MatteoT 2:7439607ccd51 85
MatteoT 3:5e7476bca25f 86 //MAIN CONTROL CYCLE PREPARATION
MatteoT 3:5e7476bca25f 87 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 88
MatteoT 3:5e7476bca25f 89 unsigned short int step = 0;
MatteoT 3:5e7476bca25f 90
MatteoT 3:5e7476bca25f 91 //Just a moment... Let the other threads begin!
MatteoT 3:5e7476bca25f 92 Thread::yield();
MatteoT 6:4ddd68260f76 93 Thread::wait((TARGET_MAIN_DT + TARGET_IMU_DT + TARGET_TX_DT + TARGET_RX_DT) * 10.0);
MatteoT 3:5e7476bca25f 94 //just made sure the other threads had time to begin.
MatteoT 3:5e7476bca25f 95
MatteoT 3:5e7476bca25f 96
MatteoT 5:33abcc31b0aa 97 //Some flashing animation
MatteoT 5:33abcc31b0aa 98 for(int i=100;i>10;i=(75*i)/100){
MatteoT 5:33abcc31b0aa 99 led_txrx = 0;led_esc = 0;led_imu = 0; Thread::wait(i);
MatteoT 5:33abcc31b0aa 100 led_txrx = 1;led_esc = 1;led_imu = 1; Thread::wait(i);
MatteoT 5:33abcc31b0aa 101 }
MatteoT 5:33abcc31b0aa 102
MatteoT 6:4ddd68260f76 103 //Setup timers
MatteoT 6:4ddd68260f76 104 Timer dt_timer; dt_timer.reset(); dt_timer.start();
MatteoT 6:4ddd68260f76 105 Timer IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start();
MatteoT 6:4ddd68260f76 106 Thread::wait(TARGET_IMU_DT);
MatteoT 6:4ddd68260f76 107
MatteoT 5:33abcc31b0aa 108
MatteoT 3:5e7476bca25f 109
MatteoT 3:5e7476bca25f 110 //MAIN CONTROL CYCLE
MatteoT 3:5e7476bca25f 111 //===================================================================================
MatteoT 2:7439607ccd51 112 while(1)
MatteoT 2:7439607ccd51 113 {
MatteoT 5:33abcc31b0aa 114
MatteoT 5:33abcc31b0aa 115
MatteoT 5:33abcc31b0aa 116 //measure the time for IMU cycle
MatteoT 6:4ddd68260f76 117 Thread::wait(1);
MatteoT 3:5e7476bca25f 118 QUAD_STATE_READ_ACTUAL_DT(mainQuadState,imu,IMU_dt_timer)
MatteoT 3:5e7476bca25f 119 QUAD_STATE_WAIT_DT_TARGET(mainQuadState.actual_imu_dt,mainQuadState.target_imu_dt)
MatteoT 3:5e7476bca25f 120 QUAD_STATE_UPDATE_DT(mainQuadState,imu,IMU_dt_timer)
MatteoT 5:33abcc31b0aa 121
MatteoT 5:33abcc31b0aa 122
MatteoT 5:33abcc31b0aa 123 if(step % 8 != 7){//see step%8==7; prevent double sensor refresh
MatteoT 5:33abcc31b0aa 124
MatteoT 5:33abcc31b0aa 125
MatteoT 5:33abcc31b0aa 126 //IMU INPUT
MatteoT 5:33abcc31b0aa 127 //-------------------------------------------------------------------------------
MatteoT 5:33abcc31b0aa 128
MatteoT 5:33abcc31b0aa 129 led_imu = 1;
MatteoT 5:33abcc31b0aa 130 IMU_imu.getQ(NULL);
MatteoT 5:33abcc31b0aa 131 led_imu = 0;
MatteoT 3:5e7476bca25f 132
MatteoT 3:5e7476bca25f 133
MatteoT 5:33abcc31b0aa 134 }//end of 7/8 steps
MatteoT 5:33abcc31b0aa 135 if(step % 8 == 7){
MatteoT 5:33abcc31b0aa 136
MatteoT 3:5e7476bca25f 137
MatteoT 5:33abcc31b0aa 138 //IMU INPUT AND ANGLES COMPUTATION
MatteoT 5:33abcc31b0aa 139 //-------------------------------------------------------------------------------
MatteoT 5:33abcc31b0aa 140
MatteoT 3:5e7476bca25f 141 {
MatteoT 5:33abcc31b0aa 142 float ypr[3];
MatteoT 5:33abcc31b0aa 143 led_imu = 1;
MatteoT 5:33abcc31b0aa 144 IMU_imu.getYawPitchRoll(ypr);
MatteoT 5:33abcc31b0aa 145 mainQuadState.estimated_position_z = IMU_imu.getBaroAlt();
MatteoT 5:33abcc31b0aa 146 IMU_imu.baro->getTemperature();
MatteoT 6:4ddd68260f76 147 mainQuadState.estimated_rotation_y = ypr[0]; //yaw = yaw
MatteoT 6:4ddd68260f76 148 mainQuadState.estimated_rotation_p = ypr[2]; //pitch = roll
MatteoT 6:4ddd68260f76 149 mainQuadState.estimated_rotation_r = ypr[1]; //roll = pitch
MatteoT 5:33abcc31b0aa 150 led_imu = 0;
MatteoT 2:7439607ccd51 151 }
MatteoT 3:5e7476bca25f 152 if(mainQuadState.actual_imu_dt >= 100.0 * mainQuadState.target_imu_dt)
MatteoT 3:5e7476bca25f 153 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
MatteoT 3:5e7476bca25f 154
MatteoT 3:5e7476bca25f 155
MatteoT 3:5e7476bca25f 156 //ELABORATION
MatteoT 3:5e7476bca25f 157 //-------------------------------------------------------------------------------
MatteoT 5:33abcc31b0aa 158
MatteoT 5:33abcc31b0aa 159 //Measure the time for main control cycle
MatteoT 3:5e7476bca25f 160 QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
MatteoT 0:22b18227d29e 161
MatteoT 5:33abcc31b0aa 162 //Elaborate inputs to determinate outputs
MatteoT 5:33abcc31b0aa 163 led_esc = 1;
MatteoT 3:5e7476bca25f 164
MatteoT 4:46f3c3dd5977 165 {//Roll PID
MatteoT 4:46f3c3dd5977 166 float previous = mainQuadState.pid_rotation_r_error;
MatteoT 4:46f3c3dd5977 167 mainQuadState.pid_rotation_r_error = mainQuadState.target_rotation_r - mainQuadState.estimated_rotation_r;
MatteoT 4:46f3c3dd5977 168 mainQuadState.pid_rotation_r_error_integrative += mainQuadState.pid_rotation_r_error;
MatteoT 4:46f3c3dd5977 169 mainQuadState.pid_rotation_r_error_derivative = mainQuadState.pid_rotation_r_error - previous;
MatteoT 4:46f3c3dd5977 170 mainQuadState.pid_rotation_r_out = mainQuadState.pid_rotation_r_kP * mainQuadState.pid_rotation_r_error
MatteoT 4:46f3c3dd5977 171 + mainQuadState.pid_rotation_r_kI * mainQuadState.pid_rotation_r_error_integrative
MatteoT 4:46f3c3dd5977 172 + mainQuadState.pid_rotation_r_kD * mainQuadState.pid_rotation_r_error_derivative;
MatteoT 4:46f3c3dd5977 173 if(mainQuadState.pid_rotation_r_error_integrative > 100) mainQuadState.pid_rotation_r_error_integrative = 100;
MatteoT 5:33abcc31b0aa 174 else if(mainQuadState.pid_rotation_r_error_integrative < -100) mainQuadState.pid_rotation_r_error_integrative = -100;
MatteoT 4:46f3c3dd5977 175 }{//Pitch PID
MatteoT 4:46f3c3dd5977 176 float previous = mainQuadState.pid_rotation_p_error;
MatteoT 4:46f3c3dd5977 177 mainQuadState.pid_rotation_p_error = mainQuadState.target_rotation_p - mainQuadState.estimated_rotation_p;
MatteoT 4:46f3c3dd5977 178 mainQuadState.pid_rotation_p_error_integrative += mainQuadState.pid_rotation_p_error;
MatteoT 4:46f3c3dd5977 179 mainQuadState.pid_rotation_p_error_derivative = mainQuadState.pid_rotation_p_error - previous;
MatteoT 4:46f3c3dd5977 180 mainQuadState.pid_rotation_p_out = mainQuadState.pid_rotation_p_kP * mainQuadState.pid_rotation_p_error
MatteoT 4:46f3c3dd5977 181 + mainQuadState.pid_rotation_p_kI * mainQuadState.pid_rotation_p_error_integrative
MatteoT 4:46f3c3dd5977 182 + mainQuadState.pid_rotation_p_kD * mainQuadState.pid_rotation_p_error_derivative;
MatteoT 4:46f3c3dd5977 183 if(mainQuadState.pid_rotation_p_error_integrative > 100) mainQuadState.pid_rotation_p_error_integrative = 100;
MatteoT 5:33abcc31b0aa 184 else if(mainQuadState.pid_rotation_p_error_integrative < -100) mainQuadState.pid_rotation_p_error_integrative = -100;
MatteoT 4:46f3c3dd5977 185 }{//Yaw PID
MatteoT 4:46f3c3dd5977 186 float previous = mainQuadState.pid_rotation_y_error;
MatteoT 4:46f3c3dd5977 187 mainQuadState.pid_rotation_y_error = mainQuadState.target_rotation_y - mainQuadState.estimated_rotation_y;
MatteoT 4:46f3c3dd5977 188 mainQuadState.pid_rotation_y_error_integrative += mainQuadState.pid_rotation_y_error;
MatteoT 4:46f3c3dd5977 189 mainQuadState.pid_rotation_y_error_derivative = mainQuadState.pid_rotation_y_error - previous;
MatteoT 4:46f3c3dd5977 190 mainQuadState.pid_rotation_y_out = mainQuadState.pid_rotation_y_kP * mainQuadState.pid_rotation_y_error
MatteoT 4:46f3c3dd5977 191 + mainQuadState.pid_rotation_y_kI * mainQuadState.pid_rotation_y_error_integrative
MatteoT 4:46f3c3dd5977 192 + mainQuadState.pid_rotation_y_kD * mainQuadState.pid_rotation_y_error_derivative;
MatteoT 4:46f3c3dd5977 193 if(mainQuadState.pid_rotation_y_error_integrative > 100) mainQuadState.pid_rotation_y_error_integrative = 100;
MatteoT 5:33abcc31b0aa 194 else if(mainQuadState.pid_rotation_y_error_integrative < -100) mainQuadState.pid_rotation_y_error_integrative = -100;
MatteoT 4:46f3c3dd5977 195 }
MatteoT 4:46f3c3dd5977 196 //Compute actual throttle values
MatteoT 3:5e7476bca25f 197 if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0
MatteoT 3:5e7476bca25f 198 || rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0){
MatteoT 2:7439607ccd51 199 mainQuadState.actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 200 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 201 mainQuadState.actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 202 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 203 }else{
MatteoT 3:5e7476bca25f 204 mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
MatteoT 3:5e7476bca25f 205 mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
MatteoT 2:7439607ccd51 206 mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
MatteoT 2:7439607ccd51 207 mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
MatteoT 2:7439607ccd51 208
MatteoT 4:46f3c3dd5977 209 mainQuadState.reference_throttle_1 = rxQuadState.reference_throttle_1;
MatteoT 4:46f3c3dd5977 210 mainQuadState.reference_throttle_2 = rxQuadState.reference_throttle_2;
MatteoT 4:46f3c3dd5977 211 mainQuadState.reference_throttle_3 = rxQuadState.reference_throttle_3;
MatteoT 4:46f3c3dd5977 212 mainQuadState.reference_throttle_4 = rxQuadState.reference_throttle_4;
MatteoT 2:7439607ccd51 213
MatteoT 4:46f3c3dd5977 214 if(mainQuadState.reference_throttle_1 <= -1000)
MatteoT 4:46f3c3dd5977 215 mainQuadState.actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 216 else{
MatteoT 4:46f3c3dd5977 217 float out = mainQuadState.reference_throttle_1; //NOTE: here we will have full throttle calcs..
MatteoT 6:4ddd68260f76 218 out += mainQuadState.pid_rotation_r_out;
MatteoT 6:4ddd68260f76 219 out += mainQuadState.pid_rotation_p_out;
MatteoT 6:4ddd68260f76 220 out += mainQuadState.pid_rotation_y_out;
MatteoT 2:7439607ccd51 221 mainQuadState.actual_throttle_1 = out;
MatteoT 2:7439607ccd51 222 }
MatteoT 3:5e7476bca25f 223
MatteoT 4:46f3c3dd5977 224 if(mainQuadState.reference_throttle_2 <= -1000)
MatteoT 4:46f3c3dd5977 225 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 226 else{
MatteoT 4:46f3c3dd5977 227 float out = mainQuadState.reference_throttle_2;
MatteoT 6:4ddd68260f76 228 out += mainQuadState.pid_rotation_r_out;
MatteoT 6:4ddd68260f76 229 out -= mainQuadState.pid_rotation_p_out;
MatteoT 6:4ddd68260f76 230 out -= mainQuadState.pid_rotation_y_out;
MatteoT 4:46f3c3dd5977 231 mainQuadState.actual_throttle_2 = out;
MatteoT 2:7439607ccd51 232 }
MatteoT 4:46f3c3dd5977 233
MatteoT 4:46f3c3dd5977 234 if(mainQuadState.reference_throttle_3 <= -1000)
MatteoT 4:46f3c3dd5977 235 mainQuadState.actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 236 else{
MatteoT 4:46f3c3dd5977 237 float out = mainQuadState.reference_throttle_3;
MatteoT 6:4ddd68260f76 238 out -= mainQuadState.pid_rotation_r_out;
MatteoT 6:4ddd68260f76 239 out -= mainQuadState.pid_rotation_p_out;
MatteoT 6:4ddd68260f76 240 out += mainQuadState.pid_rotation_y_out;
MatteoT 2:7439607ccd51 241 mainQuadState.actual_throttle_3 = out;
MatteoT 2:7439607ccd51 242 }
MatteoT 4:46f3c3dd5977 243
MatteoT 4:46f3c3dd5977 244 if(mainQuadState.reference_throttle_4 <= -1000)
MatteoT 4:46f3c3dd5977 245 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 246 else{
MatteoT 4:46f3c3dd5977 247 float out = mainQuadState.reference_throttle_4;
MatteoT 6:4ddd68260f76 248 out -= mainQuadState.pid_rotation_r_out;
MatteoT 6:4ddd68260f76 249 out += mainQuadState.pid_rotation_p_out;
MatteoT 6:4ddd68260f76 250 out -= mainQuadState.pid_rotation_y_out;
MatteoT 4:46f3c3dd5977 251 mainQuadState.actual_throttle_4 = out;
MatteoT 2:7439607ccd51 252 }
MatteoT 2:7439607ccd51 253 }
MatteoT 0:22b18227d29e 254
MatteoT 3:5e7476bca25f 255
MatteoT 3:5e7476bca25f 256 //OUTPUT
MatteoT 3:5e7476bca25f 257 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 258
MatteoT 3:5e7476bca25f 259 //set new ESC values.
MatteoT 2:7439607ccd51 260 esc1 = mainQuadState.actual_throttle_1;
MatteoT 2:7439607ccd51 261 esc2 = mainQuadState.actual_throttle_2;
MatteoT 2:7439607ccd51 262 esc3 = mainQuadState.actual_throttle_3;
MatteoT 2:7439607ccd51 263 esc4 = mainQuadState.actual_throttle_4;
MatteoT 0:22b18227d29e 264
MatteoT 3:5e7476bca25f 265 //effectively output to ESCs
MatteoT 1:acb2e0f9d1bc 266 esc1();
MatteoT 1:acb2e0f9d1bc 267 esc2();
MatteoT 1:acb2e0f9d1bc 268 esc3();
MatteoT 1:acb2e0f9d1bc 269 esc4();
MatteoT 3:5e7476bca25f 270 led_esc = 0;
MatteoT 3:5e7476bca25f 271
MatteoT 5:33abcc31b0aa 272
MatteoT 5:33abcc31b0aa 273 } //end of 1/8 steps
MatteoT 5:33abcc31b0aa 274 if(step % 24 == 21){
MatteoT 2:7439607ccd51 275
MatteoT 2:7439607ccd51 276
MatteoT 5:33abcc31b0aa 277 //REMOTE INPUT/OUTPUT EXCHANGE
MatteoT 5:33abcc31b0aa 278 //-------------------------------------------------------------------------------
MatteoT 5:33abcc31b0aa 279 led_txrx = 1;
MatteoT 5:33abcc31b0aa 280 txQuadState = mainQuadState; //send info back
MatteoT 5:33abcc31b0aa 281 txQuadState.timestamp = time(NULL);
MatteoT 5:33abcc31b0aa 282 if(TXRX_stateExchange(txQuadState,rxQuadState)){
MatteoT 5:33abcc31b0aa 283 mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
MatteoT 5:33abcc31b0aa 284 mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
MatteoT 5:33abcc31b0aa 285 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER;
MatteoT 5:33abcc31b0aa 286 if(mainQuadState.actual_rx_dt >= 100.0 * mainQuadState.target_rx_dt)
MatteoT 5:33abcc31b0aa 287 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
MatteoT 5:33abcc31b0aa 288 }
MatteoT 5:33abcc31b0aa 289 led_txrx = 0;
MatteoT 5:33abcc31b0aa 290
MatteoT 5:33abcc31b0aa 291
MatteoT 5:33abcc31b0aa 292 }//end of 1/24 steps
MatteoT 5:33abcc31b0aa 293
MatteoT 2:7439607ccd51 294
MatteoT 3:5e7476bca25f 295 ++step;
MatteoT 3:5e7476bca25f 296 }//End of main control loop
MatteoT 2:7439607ccd51 297
MatteoT 5:33abcc31b0aa 298
MatteoT 3:5e7476bca25f 299 //It must not get here.
MatteoT 2:7439607ccd51 300 return 0;
MatteoT 0:22b18227d29e 301 }