Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

main.cpp@3:5e7476bca25f, 2013-10-27 (annotated)

Committer:
MatteoT
Date:
Sun Oct 27 01:05:35 2013 +0000
Revision:
3:5e7476bca25f
Parent:
2:7439607ccd51
Child:
4:46f3c3dd5977
Now able to read QuadState in JSON from UDP broadcast

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 3:5e7476bca25f 4 #include "EthernetInterface.h"
MatteoT 0:22b18227d29e 5 #include "esc.h"
MatteoT 3:5e7476bca25f 6 #include "PID.h"
MatteoT 3:5e7476bca25f 7 #include "MPU6050.h"
MatteoT 3:5e7476bca25f 8 #include "IMUfilter.h"
MatteoT 3:5e7476bca25f 9 #include <string>
MatteoT 3:5e7476bca25f 10
MatteoT 3:5e7476bca25f 11
MatteoT 3:5e7476bca25f 12 //Author: Matteo Terruzzi
MatteoT 3:5e7476bca25f 13
MatteoT 3:5e7476bca25f 14 //Target differential of time in seconds, for the various program cycles:
MatteoT 3:5e7476bca25f 15 //timings as for main cycle:
MatteoT 3:5e7476bca25f 16 #define TARGET_IMU_DT 0.002 //every 1 step: 100%
MatteoT 3:5e7476bca25f 17 #define TARGET_MAIN_DT 0.016 //every 8 steps: 12.5% + 2 step start delay
MatteoT 3:5e7476bca25f 18 #define TARGET_TX_DT 0.040 //every 20 steps: 5% + 1 step start delay
MatteoT 3:5e7476bca25f 19 #define TARGET_RX_DT 0.040 //with previous
MatteoT 3:5e7476bca25f 20
MatteoT 3:5e7476bca25f 21 //For the various dt, "average" is computed using a sort of low-pass filter of the actual value.
MatteoT 3:5e7476bca25f 22 //The formula (recurrence equation) is:
MatteoT 3:5e7476bca25f 23 // a(t) = x(t) * k - a(t-1) * k with a(0) = 0
MatteoT 3:5e7476bca25f 24 //where:
MatteoT 3:5e7476bca25f 25 // x(t) is the actual value at the time t;
MatteoT 3:5e7476bca25f 26 // a(t) is the average at the timepoint t;
MatteoT 3:5e7476bca25f 27 // a(t-1) is the average at the previous timepoint;
MatteoT 3:5e7476bca25f 28 // k is AVERAGE_DT_K_GAIN (macro defined below).
MatteoT 3:5e7476bca25f 29 //
MatteoT 3:5e7476bca25f 30 //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 31 //If k == 1, then a(t) == x(t).
MatteoT 3:5e7476bca25f 32 //If k == 0, then a(t) == target value (the actual value is totaly ignored).
MatteoT 3:5e7476bca25f 33 //A good value may be k = 1/8 = 0.125.
MatteoT 3:5e7476bca25f 34 #define AVERAGE_DT_K_GAIN 0.0125
MatteoT 3:5e7476bca25f 35
MatteoT 3:5e7476bca25f 36 //Blinking status leds
MatteoT 3:5e7476bca25f 37 #define LED_ESC LED1
MatteoT 3:5e7476bca25f 38 #define LED_IMU LED2
MatteoT 3:5e7476bca25f 39 #define LED_TXRX LED3
MatteoT 3:5e7476bca25f 40 #define LED_RX LED4
MatteoT 3:5e7476bca25f 41
MatteoT 3:5e7476bca25f 42
MatteoT 2:7439607ccd51 43 #include "state.h"
MatteoT 2:7439607ccd51 44 #include "TXRX.h"
MatteoT 0:22b18227d29e 45
MatteoT 0:22b18227d29e 46 int main()
MatteoT 0:22b18227d29e 47 {
MatteoT 3:5e7476bca25f 48
MatteoT 3:5e7476bca25f 49 Serial pc(USBTX,USBRX);
MatteoT 3:5e7476bca25f 50
MatteoT 3:5e7476bca25f 51
MatteoT 3:5e7476bca25f 52 //INITIALIZATION
MatteoT 3:5e7476bca25f 53 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 54
MatteoT 0:22b18227d29e 55 //Setup ESCs
MatteoT 2:7439607ccd51 56 ESC esc1(p23);
MatteoT 2:7439607ccd51 57 ESC esc2(p24);
MatteoT 2:7439607ccd51 58 ESC esc3(p25);
MatteoT 2:7439607ccd51 59 ESC esc4(p26);
MatteoT 2:7439607ccd51 60
MatteoT 2:7439607ccd51 61 //Setup status leds
MatteoT 3:5e7476bca25f 62 DigitalOut led_esc(LED_ESC); led_esc = 1; //flip at esc update (fixed 0 means that the ESCs was not been initiated--the program is stucked before this point)
MatteoT 3:5e7476bca25f 63 DigitalOut led_imu(LED_IMU); led_imu = 0; //flip at imu computations done (fixed 0 means that the imu sensors don't respond)
MatteoT 3:5e7476bca25f 64 DigitalOut led_txrx(LED_TXRX); led_txrx = 0; //flip at remote signal exchange done (fixed 0 means that the TXRX_thread has not been started)
MatteoT 2:7439607ccd51 65
MatteoT 2:7439607ccd51 66 //Setup Ethernet
MatteoT 3:5e7476bca25f 67 EthernetInterface interface;
MatteoT 3:5e7476bca25f 68 interface.init("192.168.1.16","255.255.255.0","192.168.1.1");
MatteoT 3:5e7476bca25f 69 interface.connect();
MatteoT 0:22b18227d29e 70
MatteoT 3:5e7476bca25f 71 //Setup remote control
MatteoT 2:7439607ccd51 72 Thread TXRX_thread (TXRX_thread_routine);
MatteoT 3:5e7476bca25f 73 led_txrx = 1;
MatteoT 0:22b18227d29e 74
MatteoT 2:7439607ccd51 75 //Setup state
MatteoT 2:7439607ccd51 76 QuadState mainQuadState; mainQuadState.reset();
MatteoT 2:7439607ccd51 77 QuadState rxQuadState; rxQuadState.reset();
MatteoT 2:7439607ccd51 78 QuadState txQuadState; txQuadState.reset();
MatteoT 3:5e7476bca25f 79
MatteoT 3:5e7476bca25f 80 //Setup FreeIMU
MatteoT 3:5e7476bca25f 81 IMUfilter IMU_filter(TARGET_IMU_DT,40);
MatteoT 3:5e7476bca25f 82 MPU6050 IMU_sensor(p28, p27);
MatteoT 3:5e7476bca25f 83 IMU_sensor.setGyroRange(MPU6050_GYRO_RANGE_250);
MatteoT 3:5e7476bca25f 84 IMU_sensor.setAcceleroRange(MPU6050_ACCELERO_RANGE_4G);
MatteoT 3:5e7476bca25f 85 led_imu = 1;
MatteoT 3:5e7476bca25f 86
MatteoT 3:5e7476bca25f 87
MatteoT 3:5e7476bca25f 88
MatteoT 2:7439607ccd51 89
MatteoT 3:5e7476bca25f 90 //MAIN CONTROL CYCLE PREPARATION
MatteoT 3:5e7476bca25f 91 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 92
MatteoT 3:5e7476bca25f 93 unsigned short int step = 0;
MatteoT 3:5e7476bca25f 94
MatteoT 3:5e7476bca25f 95 //Just a moment... Let the other threads begin!
MatteoT 3:5e7476bca25f 96 Thread::wait(2000 /*ms*/);
MatteoT 3:5e7476bca25f 97 Thread::yield();
MatteoT 3:5e7476bca25f 98 //just made sure the other threads had time to begin.
MatteoT 3:5e7476bca25f 99
MatteoT 3:5e7476bca25f 100
MatteoT 3:5e7476bca25f 101 //Setup timers
MatteoT 3:5e7476bca25f 102 Timer IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start();
MatteoT 3:5e7476bca25f 103 Timer dt_timer; dt_timer.reset(); dt_timer.start();
MatteoT 3:5e7476bca25f 104 Thread::wait(1000.0 * min(mainQuadState.target_imu_dt,mainQuadState.target_main_dt));
MatteoT 2:7439607ccd51 105
MatteoT 2:7439607ccd51 106 //pid
MatteoT 2:7439607ccd51 107 PID targetRoll_pid(0.1,0.0001,0.5, 0.020);
MatteoT 2:7439607ccd51 108 targetRoll_pid.setInputLimits(-3.15/4,3.15/4);
MatteoT 2:7439607ccd51 109 targetRoll_pid.setOutputLimits(0,0.5);
MatteoT 3:5e7476bca25f 110
MatteoT 3:5e7476bca25f 111
MatteoT 3:5e7476bca25f 112
MatteoT 3:5e7476bca25f 113 //MAIN CONTROL CYCLE
MatteoT 3:5e7476bca25f 114 //===================================================================================
MatteoT 2:7439607ccd51 115 while(1)
MatteoT 2:7439607ccd51 116 {
MatteoT 3:5e7476bca25f 117
MatteoT 3:5e7476bca25f 118
MatteoT 3:5e7476bca25f 119 //IMU INPUT
MatteoT 3:5e7476bca25f 120 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 121
MatteoT 3:5e7476bca25f 122 //Measure time
MatteoT 3:5e7476bca25f 123 QUAD_STATE_READ_ACTUAL_DT(mainQuadState,imu,IMU_dt_timer)
MatteoT 3:5e7476bca25f 124 QUAD_STATE_WAIT_DT_TARGET(mainQuadState.actual_imu_dt,mainQuadState.target_imu_dt)
MatteoT 3:5e7476bca25f 125 QUAD_STATE_UPDATE_DT(mainQuadState,imu,IMU_dt_timer)
MatteoT 3:5e7476bca25f 126
MatteoT 3:5e7476bca25f 127
MatteoT 3:5e7476bca25f 128 led_imu = 1;
MatteoT 3:5e7476bca25f 129
MatteoT 3:5e7476bca25f 130 //Retrieve inertial values from sensors
MatteoT 3:5e7476bca25f 131 {
MatteoT 3:5e7476bca25f 132 float a[3], g[3];
MatteoT 3:5e7476bca25f 133 IMU_sensor.getAccelero(a);
MatteoT 3:5e7476bca25f 134 IMU_sensor.getGyro(g);
MatteoT 3:5e7476bca25f 135
MatteoT 3:5e7476bca25f 136 //Compute IMU filters
MatteoT 3:5e7476bca25f 137 IMU_filter.updateFilter(g[0], g[1], g[2], a[0], a[1], a[2]);
MatteoT 2:7439607ccd51 138 }
MatteoT 3:5e7476bca25f 139 if(mainQuadState.actual_imu_dt >= 100.0 * mainQuadState.target_imu_dt)
MatteoT 3:5e7476bca25f 140 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
MatteoT 3:5e7476bca25f 141
MatteoT 3:5e7476bca25f 142
MatteoT 3:5e7476bca25f 143 led_imu = 0;
MatteoT 3:5e7476bca25f 144
MatteoT 3:5e7476bca25f 145
MatteoT 3:5e7476bca25f 146
MatteoT 3:5e7476bca25f 147
MatteoT 3:5e7476bca25f 148 if(step % 20 == 2){
MatteoT 3:5e7476bca25f 149 //5% frequency; step%20==2
MatteoT 3:5e7476bca25f 150
MatteoT 3:5e7476bca25f 151
MatteoT 3:5e7476bca25f 152
MatteoT 3:5e7476bca25f 153 //REMOTE INPUT
MatteoT 3:5e7476bca25f 154 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 155 led_txrx = 1;
MatteoT 3:5e7476bca25f 156
MatteoT 3:5e7476bca25f 157 //Remote signal exchange
MatteoT 3:5e7476bca25f 158 txQuadState = mainQuadState; //send info back
MatteoT 3:5e7476bca25f 159 if(TXRX_stateExchange(txQuadState,rxQuadState)){
MatteoT 3:5e7476bca25f 160 mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
MatteoT 3:5e7476bca25f 161 mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
MatteoT 3:5e7476bca25f 162 if(mainQuadState.actual_rx_dt >= 100.0 * mainQuadState.target_rx_dt)
MatteoT 3:5e7476bca25f 163 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
MatteoT 3:5e7476bca25f 164 }
MatteoT 3:5e7476bca25f 165
MatteoT 3:5e7476bca25f 166
MatteoT 3:5e7476bca25f 167 /*if(step % 640 == 2){
MatteoT 2:7439607ccd51 168
MatteoT 3:5e7476bca25f 169 //Other tasks
MatteoT 3:5e7476bca25f 170
MatteoT 3:5e7476bca25f 171 if(pc.writeable()){
MatteoT 3:5e7476bca25f 172 pc.printf("%s \r\n", mainQuadState.getJSON().c_str()); //the message is long => it slows down a lot!
MatteoT 3:5e7476bca25f 173 }
MatteoT 3:5e7476bca25f 174 }*/
MatteoT 3:5e7476bca25f 175
MatteoT 3:5e7476bca25f 176 led_txrx = 0;
MatteoT 3:5e7476bca25f 177
MatteoT 3:5e7476bca25f 178
MatteoT 3:5e7476bca25f 179
MatteoT 3:5e7476bca25f 180 //end of 5% frequency; step%20==2
MatteoT 3:5e7476bca25f 181 }
MatteoT 3:5e7476bca25f 182 if(step % 8 == 3){
MatteoT 3:5e7476bca25f 183 //12.5% frequency; step%8==3
MatteoT 3:5e7476bca25f 184
MatteoT 2:7439607ccd51 185
MatteoT 3:5e7476bca25f 186 led_esc = 1;
MatteoT 3:5e7476bca25f 187
MatteoT 3:5e7476bca25f 188
MatteoT 3:5e7476bca25f 189 //ELABORATION
MatteoT 3:5e7476bca25f 190 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 191
MatteoT 3:5e7476bca25f 192 //Measure time
MatteoT 3:5e7476bca25f 193 QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
MatteoT 0:22b18227d29e 194
MatteoT 3:5e7476bca25f 195 //Estimate rotation
MatteoT 3:5e7476bca25f 196 IMU_filter.computeEuler();
MatteoT 3:5e7476bca25f 197 mainQuadState.estimated_rotation_y = IMU_filter.getYaw();
MatteoT 3:5e7476bca25f 198 mainQuadState.estimated_rotation_p = IMU_filter.getPitch();
MatteoT 3:5e7476bca25f 199 mainQuadState.estimated_rotation_r = IMU_filter.getRoll();
MatteoT 3:5e7476bca25f 200
MatteoT 3:5e7476bca25f 201 //Elaborate inputs to determinate outputs
MatteoT 3:5e7476bca25f 202 if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0
MatteoT 3:5e7476bca25f 203 || rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0){
MatteoT 2:7439607ccd51 204 mainQuadState.actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 205 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 206 mainQuadState.actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 207 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 208 }else{
MatteoT 3:5e7476bca25f 209 mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
MatteoT 3:5e7476bca25f 210 mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
MatteoT 2:7439607ccd51 211 mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
MatteoT 2:7439607ccd51 212 mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
MatteoT 2:7439607ccd51 213
MatteoT 2:7439607ccd51 214
MatteoT 2:7439607ccd51 215 float roll = 0;
MatteoT 2:7439607ccd51 216 if(mainQuadState.target_rotation_r_isRequired){
MatteoT 2:7439607ccd51 217 targetRoll_pid.setSetPoint(mainQuadState.target_rotation_r);
MatteoT 3:5e7476bca25f 218 targetRoll_pid.setInterval(mainQuadState.average_main_dt);
MatteoT 2:7439607ccd51 219 targetRoll_pid.setProcessValue(mainQuadState.estimated_rotation_r);
MatteoT 2:7439607ccd51 220 roll = targetRoll_pid.compute();
MatteoT 2:7439607ccd51 221 }else{
MatteoT 2:7439607ccd51 222 targetRoll_pid.setSetPoint(mainQuadState.estimated_rotation_r);
MatteoT 2:7439607ccd51 223 }
MatteoT 2:7439607ccd51 224
MatteoT 2:7439607ccd51 225 if(rxQuadState.requested_throttle_1 >= 0 && rxQuadState.requested_throttle_1 <=1)
MatteoT 2:7439607ccd51 226 mainQuadState.actual_throttle_1 = rxQuadState.requested_throttle_1;
MatteoT 2:7439607ccd51 227 else{
MatteoT 2:7439607ccd51 228 float out = 0.5; //NOTE: here we will have full throttle calcs..
MatteoT 2:7439607ccd51 229 out -= roll;
MatteoT 2:7439607ccd51 230 mainQuadState.actual_throttle_1 = out;
MatteoT 2:7439607ccd51 231 }
MatteoT 3:5e7476bca25f 232
MatteoT 2:7439607ccd51 233 if(rxQuadState.requested_throttle_2 >= 0 && rxQuadState.requested_throttle_2 <=1)
MatteoT 2:7439607ccd51 234 mainQuadState.actual_throttle_2 = rxQuadState.requested_throttle_2;
MatteoT 2:7439607ccd51 235 else{
MatteoT 2:7439607ccd51 236 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 237 }
MatteoT 2:7439607ccd51 238 if(rxQuadState.requested_throttle_3 >= 0 && rxQuadState.requested_throttle_3 <=1)
MatteoT 2:7439607ccd51 239 mainQuadState.actual_throttle_3 = rxQuadState.requested_throttle_3;
MatteoT 2:7439607ccd51 240 else{
MatteoT 2:7439607ccd51 241 float out = 0.5;
MatteoT 2:7439607ccd51 242 out += roll;
MatteoT 2:7439607ccd51 243 mainQuadState.actual_throttle_3 = out;
MatteoT 2:7439607ccd51 244 }
MatteoT 2:7439607ccd51 245 if(rxQuadState.requested_throttle_4 >= 0 && rxQuadState.requested_throttle_4 <=1)
MatteoT 2:7439607ccd51 246 mainQuadState.actual_throttle_4 = rxQuadState.requested_throttle_4;
MatteoT 2:7439607ccd51 247 else{
MatteoT 2:7439607ccd51 248 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 249 }
MatteoT 2:7439607ccd51 250 }
MatteoT 0:22b18227d29e 251
MatteoT 3:5e7476bca25f 252
MatteoT 3:5e7476bca25f 253
MatteoT 3:5e7476bca25f 254 //OUTPUT
MatteoT 3:5e7476bca25f 255 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 256
MatteoT 3:5e7476bca25f 257 //set new ESC values.
MatteoT 2:7439607ccd51 258 esc1 = mainQuadState.actual_throttle_1;
MatteoT 2:7439607ccd51 259 esc2 = mainQuadState.actual_throttle_2;
MatteoT 2:7439607ccd51 260 esc3 = mainQuadState.actual_throttle_3;
MatteoT 2:7439607ccd51 261 esc4 = mainQuadState.actual_throttle_4;
MatteoT 0:22b18227d29e 262
MatteoT 3:5e7476bca25f 263 //effectively output to ESCs
MatteoT 1:acb2e0f9d1bc 264 esc1();
MatteoT 1:acb2e0f9d1bc 265 esc2();
MatteoT 1:acb2e0f9d1bc 266 esc3();
MatteoT 1:acb2e0f9d1bc 267 esc4();
MatteoT 0:22b18227d29e 268
MatteoT 2:7439607ccd51 269
MatteoT 2:7439607ccd51 270
MatteoT 3:5e7476bca25f 271 led_esc = 0;
MatteoT 3:5e7476bca25f 272
MatteoT 3:5e7476bca25f 273 //end of 12.5% frequency; step%8==3
MatteoT 3:5e7476bca25f 274 }
MatteoT 2:7439607ccd51 275
MatteoT 2:7439607ccd51 276
MatteoT 2:7439607ccd51 277
MatteoT 3:5e7476bca25f 278 ++step;
MatteoT 3:5e7476bca25f 279 }//End of main control loop
MatteoT 2:7439607ccd51 280
MatteoT 3:5e7476bca25f 281 //It must not get here.
MatteoT 2:7439607ccd51 282 return 0;
MatteoT 0:22b18227d29e 283 }