Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ESC FreeIMU mbed-rtos mbed

main.cpp@5:33abcc31b0aa, 2014-05-12 (annotated)

Committer:
MatteoT
Date:
Mon May 12 22:58:06 2014 +0000
Revision:
5:33abcc31b0aa
Parent:
4:46f3c3dd5977
Child:
6:4ddd68260f76
experiments (txrx and imu 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 5:33abcc31b0aa 43 #include "TXRX_nomagic.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 5:33abcc31b0aa 69 Thread::wait(8000);
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 5:33abcc31b0aa 76 Thread::wait(8000);
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 5:33abcc31b0aa 93 Thread::wait(1000.0 * (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 3:5e7476bca25f 97 //Setup timers
MatteoT 3:5e7476bca25f 98 Timer dt_timer; dt_timer.reset(); dt_timer.start();
MatteoT 5:33abcc31b0aa 99 Timer IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start();
MatteoT 5:33abcc31b0aa 100 Thread::wait(1000.0 * TARGET_IMU_DT);
MatteoT 5:33abcc31b0aa 101
MatteoT 2:7439607ccd51 102
MatteoT 5:33abcc31b0aa 103 //Some flashing animation
MatteoT 5:33abcc31b0aa 104 for(int i=100;i>10;i=(75*i)/100){
MatteoT 5:33abcc31b0aa 105 led_txrx = 0;led_esc = 0;led_imu = 0; Thread::wait(i);
MatteoT 5:33abcc31b0aa 106 led_txrx = 1;led_esc = 1;led_imu = 1; Thread::wait(i);
MatteoT 5:33abcc31b0aa 107 }
MatteoT 5:33abcc31b0aa 108
MatteoT 5:33abcc31b0aa 109
MatteoT 3:5e7476bca25f 110
MatteoT 3:5e7476bca25f 111 //MAIN CONTROL CYCLE
MatteoT 3:5e7476bca25f 112 //===================================================================================
MatteoT 2:7439607ccd51 113 while(1)
MatteoT 2:7439607ccd51 114 {
MatteoT 5:33abcc31b0aa 115
MatteoT 5:33abcc31b0aa 116
MatteoT 5:33abcc31b0aa 117 //measure the time for IMU cycle
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 wait_us(1000);
MatteoT 5:33abcc31b0aa 122
MatteoT 5:33abcc31b0aa 123
MatteoT 5:33abcc31b0aa 124 if(step % 8 != 7){//see step%8==7; prevent double sensor refresh
MatteoT 5:33abcc31b0aa 125
MatteoT 5:33abcc31b0aa 126
MatteoT 5:33abcc31b0aa 127 //IMU INPUT
MatteoT 5:33abcc31b0aa 128 //-------------------------------------------------------------------------------
MatteoT 5:33abcc31b0aa 129
MatteoT 5:33abcc31b0aa 130 led_imu = 1;
MatteoT 5:33abcc31b0aa 131 IMU_imu.getQ(NULL);
MatteoT 5:33abcc31b0aa 132 led_imu = 0;
MatteoT 3:5e7476bca25f 133
MatteoT 3:5e7476bca25f 134
MatteoT 5:33abcc31b0aa 135 }//end of 7/8 steps
MatteoT 5:33abcc31b0aa 136 if(step % 8 == 7){
MatteoT 5:33abcc31b0aa 137
MatteoT 3:5e7476bca25f 138
MatteoT 5:33abcc31b0aa 139 //IMU INPUT AND ANGLES COMPUTATION
MatteoT 5:33abcc31b0aa 140 //-------------------------------------------------------------------------------
MatteoT 5:33abcc31b0aa 141
MatteoT 3:5e7476bca25f 142 {
MatteoT 5:33abcc31b0aa 143 float ypr[3];
MatteoT 5:33abcc31b0aa 144 led_imu = 1;
MatteoT 5:33abcc31b0aa 145 IMU_imu.getYawPitchRoll(ypr);
MatteoT 5:33abcc31b0aa 146 mainQuadState.estimated_position_z = IMU_imu.getBaroAlt();
MatteoT 5:33abcc31b0aa 147 IMU_imu.baro->getTemperature();
MatteoT 5:33abcc31b0aa 148 mainQuadState.estimated_rotation_y = ypr[0];
MatteoT 5:33abcc31b0aa 149 mainQuadState.estimated_rotation_p = ypr[1];
MatteoT 5:33abcc31b0aa 150 mainQuadState.estimated_rotation_r = ypr[2];
MatteoT 5:33abcc31b0aa 151 led_imu = 0;
MatteoT 2:7439607ccd51 152 }
MatteoT 3:5e7476bca25f 153 if(mainQuadState.actual_imu_dt >= 100.0 * mainQuadState.target_imu_dt)
MatteoT 3:5e7476bca25f 154 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
MatteoT 3:5e7476bca25f 155
MatteoT 3:5e7476bca25f 156
MatteoT 3:5e7476bca25f 157 //ELABORATION
MatteoT 3:5e7476bca25f 158 //-------------------------------------------------------------------------------
MatteoT 5:33abcc31b0aa 159
MatteoT 5:33abcc31b0aa 160 //Measure the time for main control cycle
MatteoT 3:5e7476bca25f 161 QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
MatteoT 0:22b18227d29e 162
MatteoT 5:33abcc31b0aa 163 //Elaborate inputs to determinate outputs
MatteoT 5:33abcc31b0aa 164 led_esc = 1;
MatteoT 3:5e7476bca25f 165
MatteoT 4:46f3c3dd5977 166 {//Roll PID
MatteoT 4:46f3c3dd5977 167 float previous = mainQuadState.pid_rotation_r_error;
MatteoT 4:46f3c3dd5977 168 mainQuadState.pid_rotation_r_error = mainQuadState.target_rotation_r - mainQuadState.estimated_rotation_r;
MatteoT 4:46f3c3dd5977 169 mainQuadState.pid_rotation_r_error_integrative += mainQuadState.pid_rotation_r_error;
MatteoT 4:46f3c3dd5977 170 mainQuadState.pid_rotation_r_error_derivative = mainQuadState.pid_rotation_r_error - previous;
MatteoT 4:46f3c3dd5977 171 mainQuadState.pid_rotation_r_out = mainQuadState.pid_rotation_r_kP * mainQuadState.pid_rotation_r_error
MatteoT 4:46f3c3dd5977 172 + mainQuadState.pid_rotation_r_kI * mainQuadState.pid_rotation_r_error_integrative
MatteoT 4:46f3c3dd5977 173 + mainQuadState.pid_rotation_r_kD * mainQuadState.pid_rotation_r_error_derivative;
MatteoT 4:46f3c3dd5977 174 if(mainQuadState.pid_rotation_r_error_integrative > 100) mainQuadState.pid_rotation_r_error_integrative = 100;
MatteoT 5:33abcc31b0aa 175 else if(mainQuadState.pid_rotation_r_error_integrative < -100) mainQuadState.pid_rotation_r_error_integrative = -100;
MatteoT 4:46f3c3dd5977 176 }{//Pitch PID
MatteoT 4:46f3c3dd5977 177 float previous = mainQuadState.pid_rotation_p_error;
MatteoT 4:46f3c3dd5977 178 mainQuadState.pid_rotation_p_error = mainQuadState.target_rotation_p - mainQuadState.estimated_rotation_p;
MatteoT 4:46f3c3dd5977 179 mainQuadState.pid_rotation_p_error_integrative += mainQuadState.pid_rotation_p_error;
MatteoT 4:46f3c3dd5977 180 mainQuadState.pid_rotation_p_error_derivative = mainQuadState.pid_rotation_p_error - previous;
MatteoT 4:46f3c3dd5977 181 mainQuadState.pid_rotation_p_out = mainQuadState.pid_rotation_p_kP * mainQuadState.pid_rotation_p_error
MatteoT 4:46f3c3dd5977 182 + mainQuadState.pid_rotation_p_kI * mainQuadState.pid_rotation_p_error_integrative
MatteoT 4:46f3c3dd5977 183 + mainQuadState.pid_rotation_p_kD * mainQuadState.pid_rotation_p_error_derivative;
MatteoT 4:46f3c3dd5977 184 if(mainQuadState.pid_rotation_p_error_integrative > 100) mainQuadState.pid_rotation_p_error_integrative = 100;
MatteoT 5:33abcc31b0aa 185 else if(mainQuadState.pid_rotation_p_error_integrative < -100) mainQuadState.pid_rotation_p_error_integrative = -100;
MatteoT 4:46f3c3dd5977 186 }{//Yaw PID
MatteoT 4:46f3c3dd5977 187 float previous = mainQuadState.pid_rotation_y_error;
MatteoT 4:46f3c3dd5977 188 mainQuadState.pid_rotation_y_error = mainQuadState.target_rotation_y - mainQuadState.estimated_rotation_y;
MatteoT 4:46f3c3dd5977 189 mainQuadState.pid_rotation_y_error_integrative += mainQuadState.pid_rotation_y_error;
MatteoT 4:46f3c3dd5977 190 mainQuadState.pid_rotation_y_error_derivative = mainQuadState.pid_rotation_y_error - previous;
MatteoT 4:46f3c3dd5977 191 mainQuadState.pid_rotation_y_out = mainQuadState.pid_rotation_y_kP * mainQuadState.pid_rotation_y_error
MatteoT 4:46f3c3dd5977 192 + mainQuadState.pid_rotation_y_kI * mainQuadState.pid_rotation_y_error_integrative
MatteoT 4:46f3c3dd5977 193 + mainQuadState.pid_rotation_y_kD * mainQuadState.pid_rotation_y_error_derivative;
MatteoT 4:46f3c3dd5977 194 if(mainQuadState.pid_rotation_y_error_integrative > 100) mainQuadState.pid_rotation_y_error_integrative = 100;
MatteoT 5:33abcc31b0aa 195 else if(mainQuadState.pid_rotation_y_error_integrative < -100) mainQuadState.pid_rotation_y_error_integrative = -100;
MatteoT 4:46f3c3dd5977 196 }
MatteoT 4:46f3c3dd5977 197 //Compute actual throttle values
MatteoT 3:5e7476bca25f 198 if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0
MatteoT 3:5e7476bca25f 199 || rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0){
MatteoT 2:7439607ccd51 200 mainQuadState.actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 201 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 202 mainQuadState.actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 203 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 204 }else{
MatteoT 3:5e7476bca25f 205 mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
MatteoT 3:5e7476bca25f 206 mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
MatteoT 2:7439607ccd51 207 mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
MatteoT 2:7439607ccd51 208 mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
MatteoT 2:7439607ccd51 209
MatteoT 4:46f3c3dd5977 210 mainQuadState.reference_throttle_1 = rxQuadState.reference_throttle_1;
MatteoT 4:46f3c3dd5977 211 mainQuadState.reference_throttle_2 = rxQuadState.reference_throttle_2;
MatteoT 4:46f3c3dd5977 212 mainQuadState.reference_throttle_3 = rxQuadState.reference_throttle_3;
MatteoT 4:46f3c3dd5977 213 mainQuadState.reference_throttle_4 = rxQuadState.reference_throttle_4;
MatteoT 2:7439607ccd51 214
MatteoT 4:46f3c3dd5977 215 if(mainQuadState.reference_throttle_1 <= -1000)
MatteoT 4:46f3c3dd5977 216 mainQuadState.actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 217 else{
MatteoT 4:46f3c3dd5977 218 float out = mainQuadState.reference_throttle_1; //NOTE: here we will have full throttle calcs..
MatteoT 4:46f3c3dd5977 219 out -= mainQuadState.pid_rotation_r_out;
MatteoT 2:7439607ccd51 220 mainQuadState.actual_throttle_1 = out;
MatteoT 2:7439607ccd51 221 }
MatteoT 3:5e7476bca25f 222
MatteoT 4:46f3c3dd5977 223 if(mainQuadState.reference_throttle_2 <= -1000)
MatteoT 4:46f3c3dd5977 224 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 225 else{
MatteoT 4:46f3c3dd5977 226 float out = mainQuadState.reference_throttle_2;
MatteoT 4:46f3c3dd5977 227 mainQuadState.actual_throttle_2 = out;
MatteoT 2:7439607ccd51 228 }
MatteoT 4:46f3c3dd5977 229
MatteoT 4:46f3c3dd5977 230 if(mainQuadState.reference_throttle_3 <= -1000)
MatteoT 4:46f3c3dd5977 231 mainQuadState.actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 232 else{
MatteoT 4:46f3c3dd5977 233 float out = mainQuadState.reference_throttle_3;
MatteoT 4:46f3c3dd5977 234 out += mainQuadState.pid_rotation_r_out;
MatteoT 2:7439607ccd51 235 mainQuadState.actual_throttle_3 = out;
MatteoT 2:7439607ccd51 236 }
MatteoT 4:46f3c3dd5977 237
MatteoT 4:46f3c3dd5977 238 if(mainQuadState.reference_throttle_4 <= -1000)
MatteoT 4:46f3c3dd5977 239 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 240 else{
MatteoT 4:46f3c3dd5977 241 float out = mainQuadState.reference_throttle_4;
MatteoT 4:46f3c3dd5977 242 mainQuadState.actual_throttle_4 = out;
MatteoT 2:7439607ccd51 243 }
MatteoT 2:7439607ccd51 244 }
MatteoT 0:22b18227d29e 245
MatteoT 3:5e7476bca25f 246
MatteoT 3:5e7476bca25f 247 //OUTPUT
MatteoT 3:5e7476bca25f 248 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 249
MatteoT 3:5e7476bca25f 250 //set new ESC values.
MatteoT 2:7439607ccd51 251 esc1 = mainQuadState.actual_throttle_1;
MatteoT 2:7439607ccd51 252 esc2 = mainQuadState.actual_throttle_2;
MatteoT 2:7439607ccd51 253 esc3 = mainQuadState.actual_throttle_3;
MatteoT 2:7439607ccd51 254 esc4 = mainQuadState.actual_throttle_4;
MatteoT 0:22b18227d29e 255
MatteoT 3:5e7476bca25f 256 //effectively output to ESCs
MatteoT 1:acb2e0f9d1bc 257 esc1();
MatteoT 1:acb2e0f9d1bc 258 esc2();
MatteoT 1:acb2e0f9d1bc 259 esc3();
MatteoT 1:acb2e0f9d1bc 260 esc4();
MatteoT 3:5e7476bca25f 261 led_esc = 0;
MatteoT 3:5e7476bca25f 262
MatteoT 5:33abcc31b0aa 263
MatteoT 5:33abcc31b0aa 264 } //end of 1/8 steps
MatteoT 5:33abcc31b0aa 265 if(step % 24 == 21){
MatteoT 2:7439607ccd51 266
MatteoT 2:7439607ccd51 267
MatteoT 5:33abcc31b0aa 268 //REMOTE INPUT/OUTPUT EXCHANGE
MatteoT 5:33abcc31b0aa 269 //-------------------------------------------------------------------------------
MatteoT 5:33abcc31b0aa 270 led_txrx = 1;
MatteoT 5:33abcc31b0aa 271 txQuadState = mainQuadState; //send info back
MatteoT 5:33abcc31b0aa 272 txQuadState.timestamp = time(NULL);
MatteoT 5:33abcc31b0aa 273 if(TXRX_stateExchange(txQuadState,rxQuadState)){
MatteoT 5:33abcc31b0aa 274 mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
MatteoT 5:33abcc31b0aa 275 mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
MatteoT 5:33abcc31b0aa 276 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER;
MatteoT 5:33abcc31b0aa 277 if(mainQuadState.actual_rx_dt >= 100.0 * mainQuadState.target_rx_dt)
MatteoT 5:33abcc31b0aa 278 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
MatteoT 5:33abcc31b0aa 279 }
MatteoT 5:33abcc31b0aa 280 led_txrx = 0;
MatteoT 5:33abcc31b0aa 281
MatteoT 5:33abcc31b0aa 282
MatteoT 5:33abcc31b0aa 283 }//end of 1/24 steps
MatteoT 5:33abcc31b0aa 284
MatteoT 2:7439607ccd51 285
MatteoT 3:5e7476bca25f 286 ++step;
MatteoT 3:5e7476bca25f 287 }//End of main control loop
MatteoT 2:7439607ccd51 288
MatteoT 5:33abcc31b0aa 289
MatteoT 3:5e7476bca25f 290 //It must not get here.
MatteoT 2:7439607ccd51 291 return 0;
MatteoT 0:22b18227d29e 292 }