Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

state.h@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 3:5e7476bca25f 1 #ifndef QUAD_STATE__H
MatteoT 3:5e7476bca25f 2 #define QUAD_STATE__H
MatteoT 3:5e7476bca25f 3
MatteoT 2:7439607ccd51 4 #include <stdint.h>
MatteoT 2:7439607ccd51 5
MatteoT 3:5e7476bca25f 6 #ifndef QUAD_STATE_NO_JSON
MatteoT 3:5e7476bca25f 7 #include <picojson.h>
MatteoT 3:5e7476bca25f 8 #endif
MatteoT 3:5e7476bca25f 9
MatteoT 3:5e7476bca25f 10
MatteoT 2:7439607ccd51 11 //this struct must be the same for all the programs communicating with the Quad.
MatteoT 2:7439607ccd51 12 struct QuadState;
MatteoT 2:7439607ccd51 13 struct QuadState {
MatteoT 3:5e7476bca25f 14 double requested_throttle_1; //negative if not requested
MatteoT 3:5e7476bca25f 15 double requested_throttle_2;
MatteoT 3:5e7476bca25f 16 double requested_throttle_3;
MatteoT 3:5e7476bca25f 17 double requested_throttle_4;
MatteoT 2:7439607ccd51 18
MatteoT 3:5e7476bca25f 19 double actual_throttle_1;
MatteoT 3:5e7476bca25f 20 double actual_throttle_2;
MatteoT 3:5e7476bca25f 21 double actual_throttle_3;
MatteoT 3:5e7476bca25f 22 double actual_throttle_4;
MatteoT 2:7439607ccd51 23
MatteoT 3:5e7476bca25f 24 double target_main_dt;
MatteoT 3:5e7476bca25f 25 double actual_main_dt;
MatteoT 3:5e7476bca25f 26 double average_main_dt;
MatteoT 3:5e7476bca25f 27 double average_main_dt_k;
MatteoT 2:7439607ccd51 28
MatteoT 3:5e7476bca25f 29 double target_tx_dt;
MatteoT 3:5e7476bca25f 30 double actual_tx_dt;
MatteoT 3:5e7476bca25f 31 double average_tx_dt;
MatteoT 3:5e7476bca25f 32 double average_tx_dt_k;
MatteoT 2:7439607ccd51 33
MatteoT 3:5e7476bca25f 34 double target_rx_dt;
MatteoT 3:5e7476bca25f 35 double actual_rx_dt;
MatteoT 3:5e7476bca25f 36 double average_rx_dt;
MatteoT 3:5e7476bca25f 37 double average_rx_dt_k;
MatteoT 2:7439607ccd51 38
MatteoT 3:5e7476bca25f 39 double target_imu_dt;
MatteoT 3:5e7476bca25f 40 double actual_imu_dt;
MatteoT 3:5e7476bca25f 41 double average_imu_dt;
MatteoT 3:5e7476bca25f 42 double average_imu_dt_k;
MatteoT 3:5e7476bca25f 43
MatteoT 3:5e7476bca25f 44 double estimated_altitude;
MatteoT 3:5e7476bca25f 45 double estimated_rotation_q1;
MatteoT 3:5e7476bca25f 46 double estimated_rotation_q2;
MatteoT 3:5e7476bca25f 47 double estimated_rotation_q3;
MatteoT 3:5e7476bca25f 48 double estimated_rotation_q4;
MatteoT 2:7439607ccd51 49
MatteoT 3:5e7476bca25f 50 double estimated_position_x;
MatteoT 3:5e7476bca25f 51 double estimated_position_y;
MatteoT 3:5e7476bca25f 52 double estimated_position_z;
MatteoT 3:5e7476bca25f 53 double estimated_rotation_y;
MatteoT 3:5e7476bca25f 54 double estimated_rotation_p;
MatteoT 3:5e7476bca25f 55 double estimated_rotation_r;
MatteoT 2:7439607ccd51 56
MatteoT 3:5e7476bca25f 57 double target_position_x; //see _isRequired members
MatteoT 3:5e7476bca25f 58 double target_position_y;
MatteoT 3:5e7476bca25f 59 double target_position_z;
MatteoT 3:5e7476bca25f 60 double target_rotation_y;
MatteoT 3:5e7476bca25f 61 double target_rotation_p;
MatteoT 3:5e7476bca25f 62 double target_rotation_r;
MatteoT 2:7439607ccd51 63
MatteoT 3:5e7476bca25f 64 double target_position_x_isRequired; //1=true; 0=false; error otherwise
MatteoT 3:5e7476bca25f 65 double target_position_y_isRequired;
MatteoT 3:5e7476bca25f 66 double target_position_z_isRequired;
MatteoT 3:5e7476bca25f 67 double target_rotation_y_isRequired;
MatteoT 3:5e7476bca25f 68 double target_rotation_p_isRequired;
MatteoT 3:5e7476bca25f 69 double target_rotation_r_isRequired;
MatteoT 3:5e7476bca25f 70
MatteoT 3:5e7476bca25f 71 double ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER; //1=OK; 0=EmergencyStop; error otherwise
MatteoT 2:7439607ccd51 72
MatteoT 2:7439607ccd51 73
MatteoT 2:7439607ccd51 74 void reset (){
MatteoT 2:7439607ccd51 75
MatteoT 2:7439607ccd51 76 requested_throttle_1 = -1;
MatteoT 2:7439607ccd51 77 requested_throttle_2 = -1;
MatteoT 2:7439607ccd51 78 requested_throttle_3 = -1;
MatteoT 2:7439607ccd51 79 requested_throttle_4 = -1;
MatteoT 2:7439607ccd51 80
MatteoT 2:7439607ccd51 81 actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 82 actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 83 actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 84 actual_throttle_4 = 0;
MatteoT 3:5e7476bca25f 85
MatteoT 3:5e7476bca25f 86 target_main_dt = TARGET_MAIN_DT;
MatteoT 3:5e7476bca25f 87 actual_main_dt = TARGET_MAIN_DT;
MatteoT 3:5e7476bca25f 88 average_main_dt = TARGET_MAIN_DT;
MatteoT 3:5e7476bca25f 89 average_main_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 3:5e7476bca25f 90
MatteoT 3:5e7476bca25f 91 target_tx_dt = TARGET_TX_DT;
MatteoT 3:5e7476bca25f 92 actual_tx_dt = TARGET_TX_DT;
MatteoT 3:5e7476bca25f 93 average_tx_dt = TARGET_TX_DT;
MatteoT 3:5e7476bca25f 94 average_tx_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 3:5e7476bca25f 95
MatteoT 3:5e7476bca25f 96 target_rx_dt = TARGET_RX_DT;
MatteoT 3:5e7476bca25f 97 actual_rx_dt = TARGET_RX_DT;
MatteoT 3:5e7476bca25f 98 average_rx_dt = TARGET_RX_DT;
MatteoT 3:5e7476bca25f 99 average_rx_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 3:5e7476bca25f 100
MatteoT 3:5e7476bca25f 101 target_imu_dt = TARGET_IMU_DT;
MatteoT 3:5e7476bca25f 102 actual_imu_dt = TARGET_IMU_DT;
MatteoT 3:5e7476bca25f 103 average_imu_dt = TARGET_IMU_DT;
MatteoT 3:5e7476bca25f 104 average_imu_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 2:7439607ccd51 105
MatteoT 2:7439607ccd51 106 estimated_altitude = 0;
MatteoT 2:7439607ccd51 107 estimated_rotation_q1 = 0;
MatteoT 2:7439607ccd51 108 estimated_rotation_q2 = 0;
MatteoT 2:7439607ccd51 109 estimated_rotation_q3 = 0;
MatteoT 2:7439607ccd51 110 estimated_rotation_q4 = 0;
MatteoT 2:7439607ccd51 111
MatteoT 2:7439607ccd51 112 estimated_position_x = 0;
MatteoT 2:7439607ccd51 113 estimated_position_y = 0;
MatteoT 2:7439607ccd51 114 estimated_position_z = 0;
MatteoT 2:7439607ccd51 115 estimated_rotation_y = 0;
MatteoT 2:7439607ccd51 116 estimated_rotation_p = 0;
MatteoT 2:7439607ccd51 117 estimated_rotation_r = 0;
MatteoT 2:7439607ccd51 118
MatteoT 2:7439607ccd51 119 target_position_x = 0;
MatteoT 2:7439607ccd51 120 target_position_y = 0;
MatteoT 2:7439607ccd51 121 target_position_z = 0;
MatteoT 2:7439607ccd51 122 target_rotation_y = 0;
MatteoT 2:7439607ccd51 123 target_rotation_p = 0;
MatteoT 2:7439607ccd51 124 target_rotation_r = 0;
MatteoT 2:7439607ccd51 125
MatteoT 3:5e7476bca25f 126 target_position_x_isRequired = 0;
MatteoT 3:5e7476bca25f 127 target_position_y_isRequired = 0;
MatteoT 3:5e7476bca25f 128 target_position_z_isRequired = 0;
MatteoT 3:5e7476bca25f 129 target_rotation_y_isRequired = 0;
MatteoT 3:5e7476bca25f 130 target_rotation_p_isRequired = 0;
MatteoT 3:5e7476bca25f 131 target_rotation_r_isRequired = 0;
MatteoT 3:5e7476bca25f 132
MatteoT 3:5e7476bca25f 133 ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 1;
MatteoT 3:5e7476bca25f 134 }
MatteoT 3:5e7476bca25f 135
MatteoT 3:5e7476bca25f 136 #ifndef QUAD_STATE_NO_JSON
MatteoT 3:5e7476bca25f 137 /**
MatteoT 3:5e7476bca25f 138 * @return number of setted members.
MatteoT 3:5e7476bca25f 139 */
MatteoT 3:5e7476bca25f 140 int setFromJSON(const char* JSON_frame){
MatteoT 3:5e7476bca25f 141 int setted = 0;
MatteoT 3:5e7476bca25f 142 picojson::value json;
MatteoT 3:5e7476bca25f 143 picojson::parse(json, JSON_frame, JSON_frame + strlen(JSON_frame));
MatteoT 3:5e7476bca25f 144
MatteoT 3:5e7476bca25f 145 unsigned int i=0;
MatteoT 3:5e7476bca25f 146
MatteoT 3:5e7476bca25f 147 #define JSON_SET(var) { \
MatteoT 3:5e7476bca25f 148 picojson::value j = json.get(i++); \
MatteoT 3:5e7476bca25f 149 if(j.is<double>()){ \
MatteoT 3:5e7476bca25f 150 var = j.get<double>(); \
MatteoT 3:5e7476bca25f 151 ++setted; \
MatteoT 3:5e7476bca25f 152 } \
MatteoT 3:5e7476bca25f 153 }
MatteoT 2:7439607ccd51 154
MatteoT 3:5e7476bca25f 155 JSON_SET(requested_throttle_1)
MatteoT 3:5e7476bca25f 156 JSON_SET(requested_throttle_2)
MatteoT 3:5e7476bca25f 157 JSON_SET(requested_throttle_3)
MatteoT 3:5e7476bca25f 158 JSON_SET(requested_throttle_4)
MatteoT 3:5e7476bca25f 159
MatteoT 3:5e7476bca25f 160 JSON_SET(actual_throttle_1)
MatteoT 3:5e7476bca25f 161 JSON_SET(actual_throttle_2)
MatteoT 3:5e7476bca25f 162 JSON_SET(actual_throttle_3)
MatteoT 3:5e7476bca25f 163 JSON_SET(actual_throttle_4)
MatteoT 3:5e7476bca25f 164
MatteoT 3:5e7476bca25f 165 JSON_SET(target_main_dt)
MatteoT 3:5e7476bca25f 166 JSON_SET(actual_main_dt)
MatteoT 3:5e7476bca25f 167 JSON_SET(average_main_dt)
MatteoT 3:5e7476bca25f 168 JSON_SET(average_main_dt_k)
MatteoT 3:5e7476bca25f 169
MatteoT 3:5e7476bca25f 170 JSON_SET(target_tx_dt)
MatteoT 3:5e7476bca25f 171 JSON_SET(actual_tx_dt)
MatteoT 3:5e7476bca25f 172 JSON_SET(average_tx_dt)
MatteoT 3:5e7476bca25f 173 JSON_SET(average_tx_dt_k)
MatteoT 3:5e7476bca25f 174
MatteoT 3:5e7476bca25f 175 JSON_SET(target_rx_dt)
MatteoT 3:5e7476bca25f 176 JSON_SET(actual_rx_dt)
MatteoT 3:5e7476bca25f 177 JSON_SET(average_rx_dt)
MatteoT 3:5e7476bca25f 178 JSON_SET(average_rx_dt_k)
MatteoT 3:5e7476bca25f 179
MatteoT 3:5e7476bca25f 180 JSON_SET(target_imu_dt)
MatteoT 3:5e7476bca25f 181 JSON_SET(actual_imu_dt)
MatteoT 3:5e7476bca25f 182 JSON_SET(average_imu_dt)
MatteoT 3:5e7476bca25f 183 JSON_SET(average_imu_dt_k)
MatteoT 3:5e7476bca25f 184
MatteoT 3:5e7476bca25f 185 JSON_SET(estimated_altitude)
MatteoT 3:5e7476bca25f 186 JSON_SET(estimated_rotation_q1)
MatteoT 3:5e7476bca25f 187 JSON_SET(estimated_rotation_q2)
MatteoT 3:5e7476bca25f 188 JSON_SET(estimated_rotation_q3)
MatteoT 3:5e7476bca25f 189 JSON_SET(estimated_rotation_q4)
MatteoT 3:5e7476bca25f 190
MatteoT 3:5e7476bca25f 191 JSON_SET(estimated_position_x)
MatteoT 3:5e7476bca25f 192 JSON_SET(estimated_position_y)
MatteoT 3:5e7476bca25f 193 JSON_SET(estimated_position_z)
MatteoT 3:5e7476bca25f 194 JSON_SET(estimated_rotation_y)
MatteoT 3:5e7476bca25f 195 JSON_SET(estimated_rotation_p)
MatteoT 3:5e7476bca25f 196 JSON_SET(estimated_rotation_r)
MatteoT 3:5e7476bca25f 197
MatteoT 3:5e7476bca25f 198 JSON_SET(target_position_x)
MatteoT 3:5e7476bca25f 199 JSON_SET(target_position_y)
MatteoT 3:5e7476bca25f 200 JSON_SET(target_position_z)
MatteoT 3:5e7476bca25f 201 JSON_SET(target_rotation_y)
MatteoT 3:5e7476bca25f 202 JSON_SET(target_rotation_p)
MatteoT 3:5e7476bca25f 203 JSON_SET(target_rotation_r)
MatteoT 3:5e7476bca25f 204
MatteoT 3:5e7476bca25f 205 JSON_SET(target_position_x_isRequired)
MatteoT 3:5e7476bca25f 206 JSON_SET(target_position_y_isRequired)
MatteoT 3:5e7476bca25f 207 JSON_SET(target_position_z_isRequired)
MatteoT 3:5e7476bca25f 208 JSON_SET(target_rotation_y_isRequired)
MatteoT 3:5e7476bca25f 209 JSON_SET(target_rotation_p_isRequired)
MatteoT 3:5e7476bca25f 210 JSON_SET(target_rotation_r_isRequired)
MatteoT 3:5e7476bca25f 211
MatteoT 3:5e7476bca25f 212 JSON_SET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER)
MatteoT 3:5e7476bca25f 213
MatteoT 3:5e7476bca25f 214 #undef JSON_SET
MatteoT 3:5e7476bca25f 215
MatteoT 3:5e7476bca25f 216 return setted;
MatteoT 2:7439607ccd51 217 }
MatteoT 3:5e7476bca25f 218
MatteoT 3:5e7476bca25f 219 std::string getJSON() const{
MatteoT 3:5e7476bca25f 220
MatteoT 3:5e7476bca25f 221 picojson::array array_vector;
MatteoT 3:5e7476bca25f 222
MatteoT 3:5e7476bca25f 223 #define JSON_GET(var) array_vector.push_back(picojson::value(var));
MatteoT 3:5e7476bca25f 224
MatteoT 3:5e7476bca25f 225
MatteoT 3:5e7476bca25f 226 JSON_GET(requested_throttle_1)
MatteoT 3:5e7476bca25f 227 JSON_GET(requested_throttle_2)
MatteoT 3:5e7476bca25f 228 JSON_GET(requested_throttle_3)
MatteoT 3:5e7476bca25f 229 JSON_GET(requested_throttle_4)
MatteoT 3:5e7476bca25f 230
MatteoT 3:5e7476bca25f 231 JSON_GET(actual_throttle_1)
MatteoT 3:5e7476bca25f 232 JSON_GET(actual_throttle_2)
MatteoT 3:5e7476bca25f 233 JSON_GET(actual_throttle_3)
MatteoT 3:5e7476bca25f 234 JSON_GET(actual_throttle_4)
MatteoT 3:5e7476bca25f 235
MatteoT 3:5e7476bca25f 236 JSON_GET(target_main_dt)
MatteoT 3:5e7476bca25f 237 JSON_GET(actual_main_dt)
MatteoT 3:5e7476bca25f 238 JSON_GET(average_main_dt)
MatteoT 3:5e7476bca25f 239 JSON_GET(average_main_dt_k)
MatteoT 3:5e7476bca25f 240
MatteoT 3:5e7476bca25f 241 JSON_GET(target_tx_dt)
MatteoT 3:5e7476bca25f 242 JSON_GET(actual_tx_dt)
MatteoT 3:5e7476bca25f 243 JSON_GET(average_tx_dt)
MatteoT 3:5e7476bca25f 244 JSON_GET(average_tx_dt_k)
MatteoT 3:5e7476bca25f 245
MatteoT 3:5e7476bca25f 246 JSON_GET(target_rx_dt)
MatteoT 3:5e7476bca25f 247 JSON_GET(actual_rx_dt)
MatteoT 3:5e7476bca25f 248 JSON_GET(average_rx_dt)
MatteoT 3:5e7476bca25f 249 JSON_GET(average_rx_dt_k)
MatteoT 3:5e7476bca25f 250
MatteoT 3:5e7476bca25f 251 JSON_GET(target_imu_dt)
MatteoT 3:5e7476bca25f 252 JSON_GET(actual_imu_dt)
MatteoT 3:5e7476bca25f 253 JSON_GET(average_imu_dt)
MatteoT 3:5e7476bca25f 254 JSON_GET(average_imu_dt_k)
MatteoT 3:5e7476bca25f 255
MatteoT 3:5e7476bca25f 256 JSON_GET(estimated_altitude)
MatteoT 3:5e7476bca25f 257 JSON_GET(estimated_rotation_q1)
MatteoT 3:5e7476bca25f 258 JSON_GET(estimated_rotation_q2)
MatteoT 3:5e7476bca25f 259 JSON_GET(estimated_rotation_q3)
MatteoT 3:5e7476bca25f 260 JSON_GET(estimated_rotation_q4)
MatteoT 3:5e7476bca25f 261
MatteoT 3:5e7476bca25f 262 JSON_GET(estimated_position_x)
MatteoT 3:5e7476bca25f 263 JSON_GET(estimated_position_y)
MatteoT 3:5e7476bca25f 264 JSON_GET(estimated_position_z)
MatteoT 3:5e7476bca25f 265 JSON_GET(estimated_rotation_y)
MatteoT 3:5e7476bca25f 266 JSON_GET(estimated_rotation_p)
MatteoT 3:5e7476bca25f 267 JSON_GET(estimated_rotation_r)
MatteoT 3:5e7476bca25f 268
MatteoT 3:5e7476bca25f 269 JSON_GET(target_position_x)
MatteoT 3:5e7476bca25f 270 JSON_GET(target_position_y)
MatteoT 3:5e7476bca25f 271 JSON_GET(target_position_z)
MatteoT 3:5e7476bca25f 272 JSON_GET(target_rotation_y)
MatteoT 3:5e7476bca25f 273 JSON_GET(target_rotation_p)
MatteoT 3:5e7476bca25f 274 JSON_GET(target_rotation_r)
MatteoT 3:5e7476bca25f 275
MatteoT 3:5e7476bca25f 276 JSON_GET(target_position_x_isRequired)
MatteoT 3:5e7476bca25f 277 JSON_GET(target_position_y_isRequired)
MatteoT 3:5e7476bca25f 278 JSON_GET(target_position_z_isRequired)
MatteoT 3:5e7476bca25f 279 JSON_GET(target_rotation_y_isRequired)
MatteoT 3:5e7476bca25f 280 JSON_GET(target_rotation_p_isRequired)
MatteoT 3:5e7476bca25f 281 JSON_GET(target_rotation_r_isRequired)
MatteoT 3:5e7476bca25f 282
MatteoT 3:5e7476bca25f 283 JSON_GET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER)
MatteoT 3:5e7476bca25f 284
MatteoT 3:5e7476bca25f 285 #undef JSON_GET
MatteoT 3:5e7476bca25f 286
MatteoT 3:5e7476bca25f 287 return picojson::value(array_vector).serialize();
MatteoT 3:5e7476bca25f 288 }
MatteoT 3:5e7476bca25f 289 #endif
MatteoT 3:5e7476bca25f 290
MatteoT 2:7439607ccd51 291 };
MatteoT 2:7439607ccd51 292
MatteoT 3:5e7476bca25f 293
MatteoT 3:5e7476bca25f 294 ///Calc dt in seconds.
MatteoT 3:5e7476bca25f 295 #define QUAD_STATE_READ_ACTUAL_DT(state, what, timer) { \
MatteoT 3:5e7476bca25f 296 state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us(); \
MatteoT 3:5e7476bca25f 297 }
MatteoT 3:5e7476bca25f 298
MatteoT 3:5e7476bca25f 299 ///Sleep until the setted DT is reached.
MatteoT 3:5e7476bca25f 300 #define QUAD_STATE_WAIT_DT_TARGET(actual, target) { \
MatteoT 3:5e7476bca25f 301 if(target-actual > 0.01*target && target-actual > 0.0001){ \
MatteoT 3:5e7476bca25f 302 Thread::wait((uint32_t) (1000.0*target-1000.0*actual)); \
MatteoT 3:5e7476bca25f 303 } else \
MatteoT 3:5e7476bca25f 304 Thread::yield(); \
MatteoT 3:5e7476bca25f 305 }
MatteoT 3:5e7476bca25f 306
MatteoT 3:5e7476bca25f 307 ///Calc actual dt and update average
MatteoT 3:5e7476bca25f 308 #define QUAD_STATE_UPDATE_DT(state, what, timer) { \
MatteoT 3:5e7476bca25f 309 state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us(); \
MatteoT 3:5e7476bca25f 310 timer.reset(); \
MatteoT 3:5e7476bca25f 311 state.average_ ## what ## _dt *= 0.9999999999999999 - state.average_ ## what ## _dt_k; \
MatteoT 3:5e7476bca25f 312 state.average_ ## what ## _dt += state.actual_ ## what ## _dt * state.average_ ## what ## _dt_k; \
MatteoT 3:5e7476bca25f 313 }
MatteoT 3:5e7476bca25f 314
MatteoT 3:5e7476bca25f 315
MatteoT 3:5e7476bca25f 316 /*
MatteoT 3:5e7476bca25f 317 //Calc dt in seconds
MatteoT 3:5e7476bca25f 318 #define DT_READ() mainQuadState.actual_throttle_dt = (float) (1.0/1000000.0) * dt_t.read_us()
MatteoT 3:5e7476bca25f 319 DT_READ();
MatteoT 3:5e7476bca25f 320 if(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt > MAIN_CYCLE_TARGET_DT/10){
MatteoT 3:5e7476bca25f 321 //if the dt is very small
MatteoT 3:5e7476bca25f 322 //sleep until the setted DT is reached.
MatteoT 3:5e7476bca25f 323 Thread::wait(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt);
MatteoT 3:5e7476bca25f 324 DT_READ();
MatteoT 3:5e7476bca25f 325 }
MatteoT 3:5e7476bca25f 326 dt_timer.reset();
MatteoT 3:5e7476bca25f 327
MatteoT 3:5e7476bca25f 328 //Calc average dt
MatteoT 3:5e7476bca25f 329 mainQuadState.average_throttle_dt -= mainQuadState.average_throttle_dt * mainQuadState.average_throttle_dt_k;
MatteoT 3:5e7476bca25f 330 mainQuadState.average_throttle_dt += mainQuadState.actual_throttle_dt * mainQuadState.average_throttle_dt_k;
MatteoT 3:5e7476bca25f 331 */
MatteoT 3:5e7476bca25f 332
MatteoT 3:5e7476bca25f 333
MatteoT 3:5e7476bca25f 334
MatteoT 3:5e7476bca25f 335
MatteoT 3:5e7476bca25f 336 #endif //QUAD_STATE__H