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@4:46f3c3dd5977, 2014-05-03 (annotated)

Committer:
MatteoT
Date:
Sat May 03 13:22:30 2014 +0000
Revision:
4:46f3c3dd5977
Parent:
3:5e7476bca25f
Child:
5:33abcc31b0aa
experiments

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 4:46f3c3dd5977 44 #include "IMU.h"
MatteoT 2:7439607ccd51 45 #include "TXRX.h"
MatteoT 0:22b18227d29e 46
MatteoT 0:22b18227d29e 47 int main()
MatteoT 0:22b18227d29e 48 {
MatteoT 3:5e7476bca25f 49
MatteoT 3:5e7476bca25f 50 Serial pc(USBTX,USBRX);
MatteoT 3:5e7476bca25f 51
MatteoT 3:5e7476bca25f 52
MatteoT 3:5e7476bca25f 53 //INITIALIZATION
MatteoT 3:5e7476bca25f 54 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 55
MatteoT 0:22b18227d29e 56 //Setup ESCs
MatteoT 2:7439607ccd51 57 ESC esc1(p23);
MatteoT 2:7439607ccd51 58 ESC esc2(p24);
MatteoT 2:7439607ccd51 59 ESC esc3(p25);
MatteoT 2:7439607ccd51 60 ESC esc4(p26);
MatteoT 2:7439607ccd51 61
MatteoT 2:7439607ccd51 62 //Setup status leds
MatteoT 3:5e7476bca25f 63 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 64 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 65 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 66
MatteoT 2:7439607ccd51 67 //Setup Ethernet
MatteoT 3:5e7476bca25f 68 EthernetInterface interface;
MatteoT 4:46f3c3dd5977 69 interface.init("192.168.2.16","255.255.255.0","192.168.2.1");
MatteoT 3:5e7476bca25f 70 interface.connect();
MatteoT 0:22b18227d29e 71
MatteoT 3:5e7476bca25f 72 //Setup remote control
MatteoT 2:7439607ccd51 73 Thread TXRX_thread (TXRX_thread_routine);
MatteoT 3:5e7476bca25f 74 led_txrx = 1;
MatteoT 0:22b18227d29e 75
MatteoT 2:7439607ccd51 76 //Setup state
MatteoT 2:7439607ccd51 77 QuadState mainQuadState; mainQuadState.reset();
MatteoT 2:7439607ccd51 78 QuadState rxQuadState; rxQuadState.reset();
MatteoT 2:7439607ccd51 79 QuadState txQuadState; txQuadState.reset();
MatteoT 3:5e7476bca25f 80
MatteoT 3:5e7476bca25f 81 //Setup FreeIMU
MatteoT 3:5e7476bca25f 82 IMUfilter IMU_filter(TARGET_IMU_DT,40);
MatteoT 3:5e7476bca25f 83 MPU6050 IMU_sensor(p28, p27);
MatteoT 3:5e7476bca25f 84 IMU_sensor.setGyroRange(MPU6050_GYRO_RANGE_250);
MatteoT 3:5e7476bca25f 85 IMU_sensor.setAcceleroRange(MPU6050_ACCELERO_RANGE_4G);
MatteoT 3:5e7476bca25f 86 led_imu = 1;
MatteoT 3:5e7476bca25f 87
MatteoT 3:5e7476bca25f 88
MatteoT 3:5e7476bca25f 89
MatteoT 2:7439607ccd51 90
MatteoT 3:5e7476bca25f 91 //MAIN CONTROL CYCLE PREPARATION
MatteoT 3:5e7476bca25f 92 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 93
MatteoT 3:5e7476bca25f 94 unsigned short int step = 0;
MatteoT 3:5e7476bca25f 95
MatteoT 3:5e7476bca25f 96 //Just a moment... Let the other threads begin!
MatteoT 3:5e7476bca25f 97 Thread::wait(2000 /*ms*/);
MatteoT 3:5e7476bca25f 98 Thread::yield();
MatteoT 3:5e7476bca25f 99 //just made sure the other threads had time to begin.
MatteoT 3:5e7476bca25f 100
MatteoT 3:5e7476bca25f 101
MatteoT 3:5e7476bca25f 102 //Setup timers
MatteoT 3:5e7476bca25f 103 Timer IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start();
MatteoT 3:5e7476bca25f 104 Timer dt_timer; dt_timer.reset(); dt_timer.start();
MatteoT 3:5e7476bca25f 105 Thread::wait(1000.0 * min(mainQuadState.target_imu_dt,mainQuadState.target_main_dt));
MatteoT 2:7439607ccd51 106
MatteoT 2:7439607ccd51 107 //pid
MatteoT 4:46f3c3dd5977 108 /*PID targetRoll_pid(0.1,0.0001,0.5, 0.020);
MatteoT 2:7439607ccd51 109 targetRoll_pid.setInputLimits(-3.15/4,3.15/4);
MatteoT 4:46f3c3dd5977 110 targetRoll_pid.setOutputLimits(0,0.5);*/
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 4:46f3c3dd5977 122 //Measure the time and sleep to correct the interval
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 4:46f3c3dd5977 135
MatteoT 4:46f3c3dd5977 136 mainQuadState.sensor_accel_x += a[0]/4 - mainQuadState.sensor_accel_x/4;
MatteoT 4:46f3c3dd5977 137 mainQuadState.sensor_accel_y += a[1]/4 - mainQuadState.sensor_accel_y/4;
MatteoT 4:46f3c3dd5977 138 mainQuadState.sensor_accel_z += a[2]/4 - mainQuadState.sensor_accel_z/4;
MatteoT 4:46f3c3dd5977 139 mainQuadState.sensor_gyro_r = g[0];
MatteoT 4:46f3c3dd5977 140 mainQuadState.sensor_gyro_p = g[1];
MatteoT 4:46f3c3dd5977 141 mainQuadState.sensor_gyro_y = g[2];
MatteoT 4:46f3c3dd5977 142
MatteoT 3:5e7476bca25f 143 //Compute IMU filters
MatteoT 3:5e7476bca25f 144 IMU_filter.updateFilter(g[0], g[1], g[2], a[0], a[1], a[2]);
MatteoT 2:7439607ccd51 145 }
MatteoT 3:5e7476bca25f 146 if(mainQuadState.actual_imu_dt >= 100.0 * mainQuadState.target_imu_dt)
MatteoT 3:5e7476bca25f 147 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
MatteoT 3:5e7476bca25f 148
MatteoT 3:5e7476bca25f 149 led_imu = 0;
MatteoT 3:5e7476bca25f 150
MatteoT 3:5e7476bca25f 151
MatteoT 3:5e7476bca25f 152
MatteoT 3:5e7476bca25f 153
MatteoT 3:5e7476bca25f 154 if(step % 20 == 2){
MatteoT 3:5e7476bca25f 155 //5% frequency; step%20==2
MatteoT 3:5e7476bca25f 156
MatteoT 3:5e7476bca25f 157
MatteoT 3:5e7476bca25f 158
MatteoT 3:5e7476bca25f 159 //REMOTE INPUT
MatteoT 3:5e7476bca25f 160 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 161 led_txrx = 1;
MatteoT 3:5e7476bca25f 162
MatteoT 3:5e7476bca25f 163 //Remote signal exchange
MatteoT 3:5e7476bca25f 164 txQuadState = mainQuadState; //send info back
MatteoT 4:46f3c3dd5977 165 txQuadState.timestamp = time(NULL);
MatteoT 3:5e7476bca25f 166 if(TXRX_stateExchange(txQuadState,rxQuadState)){
MatteoT 3:5e7476bca25f 167 mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
MatteoT 3:5e7476bca25f 168 mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
MatteoT 4:46f3c3dd5977 169 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER;
MatteoT 3:5e7476bca25f 170 if(mainQuadState.actual_rx_dt >= 100.0 * mainQuadState.target_rx_dt)
MatteoT 3:5e7476bca25f 171 mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
MatteoT 3:5e7476bca25f 172 }
MatteoT 3:5e7476bca25f 173
MatteoT 3:5e7476bca25f 174
MatteoT 3:5e7476bca25f 175 /*if(step % 640 == 2){
MatteoT 2:7439607ccd51 176
MatteoT 3:5e7476bca25f 177 //Other tasks
MatteoT 3:5e7476bca25f 178
MatteoT 3:5e7476bca25f 179 if(pc.writeable()){
MatteoT 3:5e7476bca25f 180 pc.printf("%s \r\n", mainQuadState.getJSON().c_str()); //the message is long => it slows down a lot!
MatteoT 3:5e7476bca25f 181 }
MatteoT 3:5e7476bca25f 182 }*/
MatteoT 3:5e7476bca25f 183
MatteoT 3:5e7476bca25f 184 led_txrx = 0;
MatteoT 3:5e7476bca25f 185
MatteoT 3:5e7476bca25f 186
MatteoT 3:5e7476bca25f 187
MatteoT 3:5e7476bca25f 188 //end of 5% frequency; step%20==2
MatteoT 3:5e7476bca25f 189 }
MatteoT 3:5e7476bca25f 190 if(step % 8 == 3){
MatteoT 3:5e7476bca25f 191 //12.5% frequency; step%8==3
MatteoT 3:5e7476bca25f 192
MatteoT 2:7439607ccd51 193
MatteoT 3:5e7476bca25f 194 led_esc = 1;
MatteoT 3:5e7476bca25f 195
MatteoT 3:5e7476bca25f 196
MatteoT 3:5e7476bca25f 197 //ELABORATION
MatteoT 3:5e7476bca25f 198 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 199
MatteoT 4:46f3c3dd5977 200 //Measure the time
MatteoT 3:5e7476bca25f 201 QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
MatteoT 0:22b18227d29e 202
MatteoT 4:46f3c3dd5977 203 //Get estimated rotation
MatteoT 4:46f3c3dd5977 204 //IMU_filter.computeEuler();
MatteoT 4:46f3c3dd5977 205 //mainQuadState.estimated_rotation_y = IMU_filter.getYaw();
MatteoT 4:46f3c3dd5977 206 //mainQuadState.estimated_rotation_p = IMU_filter.getPitch();
MatteoT 4:46f3c3dd5977 207 //mainQuadState.estimated_rotation_r = IMU_filter.getRoll();
MatteoT 4:46f3c3dd5977 208 IMU_filter.getCorrectedEuler(
MatteoT 4:46f3c3dd5977 209 mainQuadState.estimated_rotation_r,
MatteoT 4:46f3c3dd5977 210 mainQuadState.estimated_rotation_p,
MatteoT 4:46f3c3dd5977 211 mainQuadState.estimated_rotation_y );
MatteoT 4:46f3c3dd5977 212 IMU_filter.getQuaternion(
MatteoT 4:46f3c3dd5977 213 mainQuadState.estimated_rotation_q1,
MatteoT 4:46f3c3dd5977 214 mainQuadState.estimated_rotation_q2,
MatteoT 4:46f3c3dd5977 215 mainQuadState.estimated_rotation_q3,
MatteoT 4:46f3c3dd5977 216 mainQuadState.estimated_rotation_q4);
MatteoT 3:5e7476bca25f 217
MatteoT 3:5e7476bca25f 218 //Elaborate inputs to determinate outputs
MatteoT 4:46f3c3dd5977 219 {//Roll PID
MatteoT 4:46f3c3dd5977 220 float previous = mainQuadState.pid_rotation_r_error;
MatteoT 4:46f3c3dd5977 221 mainQuadState.pid_rotation_r_error = mainQuadState.target_rotation_r - mainQuadState.estimated_rotation_r;
MatteoT 4:46f3c3dd5977 222 mainQuadState.pid_rotation_r_error_integrative += mainQuadState.pid_rotation_r_error;
MatteoT 4:46f3c3dd5977 223 mainQuadState.pid_rotation_r_error_derivative = mainQuadState.pid_rotation_r_error - previous;
MatteoT 4:46f3c3dd5977 224 mainQuadState.pid_rotation_r_out = mainQuadState.pid_rotation_r_kP * mainQuadState.pid_rotation_r_error
MatteoT 4:46f3c3dd5977 225 + mainQuadState.pid_rotation_r_kI * mainQuadState.pid_rotation_r_error_integrative
MatteoT 4:46f3c3dd5977 226 + mainQuadState.pid_rotation_r_kD * mainQuadState.pid_rotation_r_error_derivative;
MatteoT 4:46f3c3dd5977 227 if(mainQuadState.pid_rotation_r_error_integrative > 100) mainQuadState.pid_rotation_r_error_integrative = 100;
MatteoT 4:46f3c3dd5977 228 else if(mainQuadState.pid_rotation_r_error_integrative > -100) mainQuadState.pid_rotation_r_error_integrative = -100;
MatteoT 4:46f3c3dd5977 229 }{//Pitch PID
MatteoT 4:46f3c3dd5977 230 float previous = mainQuadState.pid_rotation_p_error;
MatteoT 4:46f3c3dd5977 231 mainQuadState.pid_rotation_p_error = mainQuadState.target_rotation_p - mainQuadState.estimated_rotation_p;
MatteoT 4:46f3c3dd5977 232 mainQuadState.pid_rotation_p_error_integrative += mainQuadState.pid_rotation_p_error;
MatteoT 4:46f3c3dd5977 233 mainQuadState.pid_rotation_p_error_derivative = mainQuadState.pid_rotation_p_error - previous;
MatteoT 4:46f3c3dd5977 234 mainQuadState.pid_rotation_p_out = mainQuadState.pid_rotation_p_kP * mainQuadState.pid_rotation_p_error
MatteoT 4:46f3c3dd5977 235 + mainQuadState.pid_rotation_p_kI * mainQuadState.pid_rotation_p_error_integrative
MatteoT 4:46f3c3dd5977 236 + mainQuadState.pid_rotation_p_kD * mainQuadState.pid_rotation_p_error_derivative;
MatteoT 4:46f3c3dd5977 237 if(mainQuadState.pid_rotation_p_error_integrative > 100) mainQuadState.pid_rotation_p_error_integrative = 100;
MatteoT 4:46f3c3dd5977 238 else if(mainQuadState.pid_rotation_p_error_integrative > -100) mainQuadState.pid_rotation_p_error_integrative = -100;
MatteoT 4:46f3c3dd5977 239 }{//Yaw PID
MatteoT 4:46f3c3dd5977 240 float previous = mainQuadState.pid_rotation_y_error;
MatteoT 4:46f3c3dd5977 241 mainQuadState.pid_rotation_y_error = mainQuadState.target_rotation_y - mainQuadState.estimated_rotation_y;
MatteoT 4:46f3c3dd5977 242 mainQuadState.pid_rotation_y_error_integrative += mainQuadState.pid_rotation_y_error;
MatteoT 4:46f3c3dd5977 243 mainQuadState.pid_rotation_y_error_derivative = mainQuadState.pid_rotation_y_error - previous;
MatteoT 4:46f3c3dd5977 244 mainQuadState.pid_rotation_y_out = mainQuadState.pid_rotation_y_kP * mainQuadState.pid_rotation_y_error
MatteoT 4:46f3c3dd5977 245 + mainQuadState.pid_rotation_y_kI * mainQuadState.pid_rotation_y_error_integrative
MatteoT 4:46f3c3dd5977 246 + mainQuadState.pid_rotation_y_kD * mainQuadState.pid_rotation_y_error_derivative;
MatteoT 4:46f3c3dd5977 247 if(mainQuadState.pid_rotation_y_error_integrative > 100) mainQuadState.pid_rotation_y_error_integrative = 100;
MatteoT 4:46f3c3dd5977 248 else if(mainQuadState.pid_rotation_y_error_integrative > -100) mainQuadState.pid_rotation_y_error_integrative = -100;
MatteoT 4:46f3c3dd5977 249 }
MatteoT 4:46f3c3dd5977 250 //Compute actual throttle values
MatteoT 3:5e7476bca25f 251 if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0
MatteoT 3:5e7476bca25f 252 || rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0){
MatteoT 2:7439607ccd51 253 mainQuadState.actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 254 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 255 mainQuadState.actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 256 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 257 }else{
MatteoT 3:5e7476bca25f 258 mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
MatteoT 3:5e7476bca25f 259 mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
MatteoT 2:7439607ccd51 260 mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
MatteoT 2:7439607ccd51 261 mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
MatteoT 2:7439607ccd51 262
MatteoT 4:46f3c3dd5977 263 mainQuadState.reference_throttle_1 = rxQuadState.reference_throttle_1;
MatteoT 4:46f3c3dd5977 264 mainQuadState.reference_throttle_2 = rxQuadState.reference_throttle_2;
MatteoT 4:46f3c3dd5977 265 mainQuadState.reference_throttle_3 = rxQuadState.reference_throttle_3;
MatteoT 4:46f3c3dd5977 266 mainQuadState.reference_throttle_4 = rxQuadState.reference_throttle_4;
MatteoT 2:7439607ccd51 267
MatteoT 4:46f3c3dd5977 268 /*float roll = 0;
MatteoT 2:7439607ccd51 269 if(mainQuadState.target_rotation_r_isRequired){
MatteoT 2:7439607ccd51 270 targetRoll_pid.setSetPoint(mainQuadState.target_rotation_r);
MatteoT 3:5e7476bca25f 271 targetRoll_pid.setInterval(mainQuadState.average_main_dt);
MatteoT 2:7439607ccd51 272 targetRoll_pid.setProcessValue(mainQuadState.estimated_rotation_r);
MatteoT 2:7439607ccd51 273 roll = targetRoll_pid.compute();
MatteoT 2:7439607ccd51 274 }else{
MatteoT 2:7439607ccd51 275 targetRoll_pid.setSetPoint(mainQuadState.estimated_rotation_r);
MatteoT 4:46f3c3dd5977 276 }*/
MatteoT 2:7439607ccd51 277
MatteoT 4:46f3c3dd5977 278 if(mainQuadState.reference_throttle_1 <= -1000)
MatteoT 4:46f3c3dd5977 279 mainQuadState.actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 280 else{
MatteoT 4:46f3c3dd5977 281 float out = mainQuadState.reference_throttle_1; //NOTE: here we will have full throttle calcs..
MatteoT 4:46f3c3dd5977 282 out -= mainQuadState.pid_rotation_r_out;
MatteoT 2:7439607ccd51 283 mainQuadState.actual_throttle_1 = out;
MatteoT 2:7439607ccd51 284 }
MatteoT 3:5e7476bca25f 285
MatteoT 4:46f3c3dd5977 286 if(mainQuadState.reference_throttle_2 <= -1000)
MatteoT 4:46f3c3dd5977 287 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 288 else{
MatteoT 4:46f3c3dd5977 289 float out = mainQuadState.reference_throttle_2;
MatteoT 4:46f3c3dd5977 290 mainQuadState.actual_throttle_2 = out;
MatteoT 2:7439607ccd51 291 }
MatteoT 4:46f3c3dd5977 292
MatteoT 4:46f3c3dd5977 293 if(mainQuadState.reference_throttle_3 <= -1000)
MatteoT 4:46f3c3dd5977 294 mainQuadState.actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 295 else{
MatteoT 4:46f3c3dd5977 296 float out = mainQuadState.reference_throttle_3;
MatteoT 4:46f3c3dd5977 297 out += mainQuadState.pid_rotation_r_out;
MatteoT 2:7439607ccd51 298 mainQuadState.actual_throttle_3 = out;
MatteoT 2:7439607ccd51 299 }
MatteoT 4:46f3c3dd5977 300
MatteoT 4:46f3c3dd5977 301 if(mainQuadState.reference_throttle_4 <= -1000)
MatteoT 4:46f3c3dd5977 302 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 303 else{
MatteoT 4:46f3c3dd5977 304 float out = mainQuadState.reference_throttle_4;
MatteoT 4:46f3c3dd5977 305 mainQuadState.actual_throttle_4 = out;
MatteoT 2:7439607ccd51 306 }
MatteoT 2:7439607ccd51 307 }
MatteoT 0:22b18227d29e 308
MatteoT 3:5e7476bca25f 309
MatteoT 3:5e7476bca25f 310
MatteoT 3:5e7476bca25f 311 //OUTPUT
MatteoT 3:5e7476bca25f 312 //-------------------------------------------------------------------------------
MatteoT 3:5e7476bca25f 313
MatteoT 3:5e7476bca25f 314 //set new ESC values.
MatteoT 2:7439607ccd51 315 esc1 = mainQuadState.actual_throttle_1;
MatteoT 2:7439607ccd51 316 esc2 = mainQuadState.actual_throttle_2;
MatteoT 2:7439607ccd51 317 esc3 = mainQuadState.actual_throttle_3;
MatteoT 2:7439607ccd51 318 esc4 = mainQuadState.actual_throttle_4;
MatteoT 0:22b18227d29e 319
MatteoT 3:5e7476bca25f 320 //effectively output to ESCs
MatteoT 1:acb2e0f9d1bc 321 esc1();
MatteoT 1:acb2e0f9d1bc 322 esc2();
MatteoT 1:acb2e0f9d1bc 323 esc3();
MatteoT 1:acb2e0f9d1bc 324 esc4();
MatteoT 0:22b18227d29e 325
MatteoT 2:7439607ccd51 326
MatteoT 2:7439607ccd51 327
MatteoT 3:5e7476bca25f 328 led_esc = 0;
MatteoT 3:5e7476bca25f 329
MatteoT 3:5e7476bca25f 330 //end of 12.5% frequency; step%8==3
MatteoT 3:5e7476bca25f 331 }
MatteoT 2:7439607ccd51 332
MatteoT 2:7439607ccd51 333
MatteoT 2:7439607ccd51 334
MatteoT 3:5e7476bca25f 335 ++step;
MatteoT 3:5e7476bca25f 336 }//End of main control loop
MatteoT 2:7439607ccd51 337
MatteoT 3:5e7476bca25f 338 //It must not get here.
MatteoT 2:7439607ccd51 339 return 0;
MatteoT 0:22b18227d29e 340 }