Matteo Terruzzi
/
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
Embed:
(wiki syntax)
Show/hide line numbers
state.h
00001 #ifndef QUAD_STATE__H 00002 #define QUAD_STATE__H 00003 00004 #include <stdint.h> 00005 00006 #ifndef QUAD_STATE_NO_JSON 00007 #endif 00008 00009 00010 //this struct must be the same for all the programs communicating with the Quad. 00011 struct QuadState; 00012 struct QuadState { //double[76] 00013 00014 double reference_throttle_1; 00015 double reference_throttle_2; 00016 double reference_throttle_3; 00017 double reference_throttle_4; 00018 00019 double actual_throttle_1; 00020 double actual_throttle_2; 00021 double actual_throttle_3; 00022 double actual_throttle_4; 00023 00024 double pid_rotation_r_error; 00025 double pid_rotation_r_error_integrative; 00026 double pid_rotation_r_error_derivative; 00027 double pid_rotation_r_kP; 00028 double pid_rotation_r_kI; 00029 double pid_rotation_r_kD; 00030 double pid_rotation_r_out; 00031 00032 double pid_rotation_p_error; 00033 double pid_rotation_p_error_integrative; 00034 double pid_rotation_p_error_derivative; 00035 double pid_rotation_p_kP; 00036 double pid_rotation_p_kI; 00037 double pid_rotation_p_kD; 00038 double pid_rotation_p_out; 00039 00040 double pid_rotation_y_error; 00041 double pid_rotation_y_error_integrative; 00042 double pid_rotation_y_error_derivative; 00043 double pid_rotation_y_kP; 00044 double pid_rotation_y_kI; 00045 double pid_rotation_y_kD; 00046 double pid_rotation_y_out; 00047 00048 double target_main_dt; 00049 double actual_main_dt; 00050 double average_main_dt; 00051 double average_main_dt_k; 00052 00053 double target_tx_dt; 00054 double actual_tx_dt; 00055 double average_tx_dt; 00056 double average_tx_dt_k; 00057 00058 double target_rx_dt; 00059 double actual_rx_dt; 00060 double average_rx_dt; 00061 double average_rx_dt_k; 00062 00063 double target_imu_dt; 00064 double actual_imu_dt; 00065 double average_imu_dt; 00066 double average_imu_dt_k; 00067 00068 double estimated_altitude; 00069 double estimated_rotation_q1; 00070 double estimated_rotation_q2; 00071 double estimated_rotation_q3; 00072 double estimated_rotation_q4; 00073 00074 double estimated_position_x; 00075 double estimated_position_y; 00076 double estimated_position_z; 00077 double estimated_rotation_y; 00078 double estimated_rotation_p; 00079 double estimated_rotation_r; 00080 00081 double sensor_accel_x; 00082 double sensor_accel_y; 00083 double sensor_accel_z; 00084 double sensor_gyro_r; 00085 double sensor_gyro_p; 00086 double sensor_gyro_y; 00087 00088 double target_position_x; //see _isRequired members 00089 double target_position_y; 00090 double target_position_z; 00091 double target_rotation_y; 00092 double target_rotation_p; 00093 double target_rotation_r; 00094 00095 double target_position_x_isRequired; //1=true; 0=false; error otherwise 00096 double target_position_y_isRequired; 00097 double target_position_z_isRequired; 00098 double target_rotation_y_isRequired; 00099 double target_rotation_p_isRequired; 00100 double target_rotation_r_isRequired; 00101 00102 double timestamp; 00103 00104 double ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER; //1=OK; 0=EmergencyStop; error otherwise 00105 00106 00107 00108 static int length () { return 76; } 00109 00110 void reset (){ 00111 00112 reference_throttle_1 = -1000; 00113 reference_throttle_2 = -1000; 00114 reference_throttle_3 = -1000; 00115 reference_throttle_4 = -1000; 00116 00117 actual_throttle_1 = 0; 00118 actual_throttle_2 = 0; 00119 actual_throttle_3 = 0; 00120 actual_throttle_4 = 0; 00121 00122 pid_rotation_r_error = 0; 00123 pid_rotation_r_error_integrative = 0; 00124 pid_rotation_r_error_derivative = 0; 00125 pid_rotation_r_kP = 0.3; 00126 pid_rotation_r_kI = 0; 00127 pid_rotation_r_kD = 1.2; 00128 pid_rotation_r_out = 0; 00129 00130 pid_rotation_p_error = 0; 00131 pid_rotation_p_error_integrative = 0; 00132 pid_rotation_p_error_derivative = 0; 00133 pid_rotation_p_kP = 0.3; 00134 pid_rotation_p_kI = 0; 00135 pid_rotation_p_kD = 1.2; 00136 pid_rotation_p_out = 0; 00137 00138 pid_rotation_y_error = 0; 00139 pid_rotation_y_error_integrative = 0; 00140 pid_rotation_y_error_derivative = 0; 00141 pid_rotation_y_kP = 0.3; 00142 pid_rotation_y_kI = 0; 00143 pid_rotation_y_kD = 0.1; 00144 pid_rotation_y_out = 0; 00145 00146 target_main_dt = TARGET_MAIN_DT; 00147 actual_main_dt = TARGET_MAIN_DT; 00148 average_main_dt = TARGET_MAIN_DT; 00149 average_main_dt_k = AVERAGE_DT_K_GAIN; 00150 00151 target_tx_dt = TARGET_TX_DT; 00152 actual_tx_dt = TARGET_TX_DT; 00153 average_tx_dt = TARGET_TX_DT; 00154 average_tx_dt_k = AVERAGE_DT_K_GAIN; 00155 00156 target_rx_dt = TARGET_RX_DT; 00157 actual_rx_dt = TARGET_RX_DT; 00158 average_rx_dt = TARGET_RX_DT; 00159 average_rx_dt_k = AVERAGE_DT_K_GAIN; 00160 00161 target_imu_dt = TARGET_IMU_DT; 00162 actual_imu_dt = TARGET_IMU_DT; 00163 average_imu_dt = TARGET_IMU_DT; 00164 average_imu_dt_k = AVERAGE_DT_K_GAIN; 00165 00166 estimated_altitude = 0; 00167 estimated_rotation_q1 = 0; 00168 estimated_rotation_q2 = 0; 00169 estimated_rotation_q3 = 0; 00170 estimated_rotation_q4 = 0; 00171 00172 estimated_position_x = 0; 00173 estimated_position_y = 0; 00174 estimated_position_z = 0; 00175 estimated_rotation_y = 0; 00176 estimated_rotation_p = 0; 00177 estimated_rotation_r = 0; 00178 00179 sensor_accel_x = 0; 00180 sensor_accel_y = 0; 00181 sensor_accel_z = 0; 00182 sensor_gyro_r = 0; 00183 sensor_gyro_p = 0; 00184 sensor_gyro_y = 0; 00185 00186 target_position_x = 0; 00187 target_position_y = 0; 00188 target_position_z = 0; 00189 target_rotation_y = 0; 00190 target_rotation_p = 0; 00191 target_rotation_r = 0; 00192 00193 target_position_x_isRequired = 0; 00194 target_position_y_isRequired = 0; 00195 target_position_z_isRequired = 0; 00196 target_rotation_y_isRequired = 0; 00197 target_rotation_p_isRequired = 0; 00198 target_rotation_r_isRequired = 0; 00199 00200 timestamp = 0; 00201 00202 ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; 00203 } 00204 00205 #ifndef QUAD_STATE_NO_JSON 00206 /** Sets the QuadState by parsing a JSON string. 00207 * @return number of setted members. 00208 */ 00209 int setFromJSON(const char* JSON_frame){ 00210 /*int setted = 0; 00211 picojson::value json; 00212 picojson::parse(json, JSON_frame, JSON_frame + strlen(JSON_frame)); 00213 00214 unsigned int i=0; 00215 00216 //#define JSON_SET(var) { \ 00217 picojson::value j = json.get(i++); \ 00218 if(j.is<double>()){ \ 00219 var = j.get<double>(); \ 00220 ++setted; \ 00221 } \ 00222 } 00223 */ 00224 00225 #define JSON_SET(var) , & var 00226 00227 return sscanf(JSON_frame, "[%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf]" 00228 00229 JSON_SET(reference_throttle_1) 00230 JSON_SET(reference_throttle_2) 00231 JSON_SET(reference_throttle_3) 00232 JSON_SET(reference_throttle_4) 00233 00234 JSON_SET(actual_throttle_1) 00235 JSON_SET(actual_throttle_2) 00236 JSON_SET(actual_throttle_3) 00237 JSON_SET(actual_throttle_4) 00238 00239 JSON_SET(pid_rotation_r_error) 00240 JSON_SET(pid_rotation_r_error_integrative) 00241 JSON_SET(pid_rotation_r_error_derivative) 00242 JSON_SET(pid_rotation_r_kP) 00243 JSON_SET(pid_rotation_r_kI) 00244 JSON_SET(pid_rotation_r_kD) 00245 JSON_SET(pid_rotation_r_out) 00246 00247 JSON_SET(pid_rotation_p_error) 00248 JSON_SET(pid_rotation_p_error_integrative) 00249 JSON_SET(pid_rotation_p_error_derivative) 00250 JSON_SET(pid_rotation_p_kP) 00251 JSON_SET(pid_rotation_p_kI) 00252 JSON_SET(pid_rotation_p_kD) 00253 JSON_SET(pid_rotation_p_out) 00254 00255 JSON_SET(pid_rotation_y_error) 00256 JSON_SET(pid_rotation_y_error_integrative) 00257 JSON_SET(pid_rotation_y_error_derivative) 00258 JSON_SET(pid_rotation_y_kP) 00259 JSON_SET(pid_rotation_y_kI) 00260 JSON_SET(pid_rotation_y_kD) 00261 JSON_SET(pid_rotation_y_out) 00262 00263 JSON_SET(target_main_dt) 00264 JSON_SET(actual_main_dt) 00265 JSON_SET(average_main_dt) 00266 JSON_SET(average_main_dt_k) 00267 00268 JSON_SET(target_tx_dt) 00269 JSON_SET(actual_tx_dt) 00270 JSON_SET(average_tx_dt) 00271 JSON_SET(average_tx_dt_k) 00272 00273 JSON_SET(target_rx_dt) 00274 JSON_SET(actual_rx_dt) 00275 JSON_SET(average_rx_dt) 00276 JSON_SET(average_rx_dt_k) 00277 00278 JSON_SET(target_imu_dt) 00279 JSON_SET(actual_imu_dt) 00280 JSON_SET(average_imu_dt) 00281 JSON_SET(average_imu_dt_k) 00282 00283 JSON_SET(estimated_altitude) 00284 JSON_SET(estimated_rotation_q1) 00285 JSON_SET(estimated_rotation_q2) 00286 JSON_SET(estimated_rotation_q3) 00287 JSON_SET(estimated_rotation_q4) 00288 00289 JSON_SET(estimated_position_x) 00290 JSON_SET(estimated_position_y) 00291 JSON_SET(estimated_position_z) 00292 JSON_SET(estimated_rotation_y) 00293 JSON_SET(estimated_rotation_p) 00294 JSON_SET(estimated_rotation_r) 00295 00296 JSON_SET(sensor_accel_x) 00297 JSON_SET(sensor_accel_y) 00298 JSON_SET(sensor_accel_z) 00299 JSON_SET(sensor_gyro_r) 00300 JSON_SET(sensor_gyro_p) 00301 JSON_SET(sensor_gyro_y) 00302 00303 JSON_SET(target_position_x) 00304 JSON_SET(target_position_y) 00305 JSON_SET(target_position_z) 00306 JSON_SET(target_rotation_y) 00307 JSON_SET(target_rotation_p) 00308 JSON_SET(target_rotation_r) 00309 00310 JSON_SET(target_position_x_isRequired) 00311 JSON_SET(target_position_y_isRequired) 00312 JSON_SET(target_position_z_isRequired) 00313 JSON_SET(target_rotation_y_isRequired) 00314 JSON_SET(target_rotation_p_isRequired) 00315 JSON_SET(target_rotation_r_isRequired) 00316 00317 JSON_SET(timestamp) 00318 00319 JSON_SET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER) 00320 00321 ); 00322 00323 #undef JSON_SET 00324 00325 //return 76; 00326 } 00327 00328 /** Prints the QuadState in JSON format on the specified buffer. 00329 * @return total number of characters written. On failure, a negative number is returned. 00330 */ 00331 int getJSON(char * buffer) const{ 00332 00333 //picojson::array array_vector; 00334 00335 //#define JSON_GET(var) array_vector.push_back(picojson::value(var)); 00336 #define JSON_GET(var) ,var 00337 00338 return sprintf(buffer, "[%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lf,%lg] \r\n" 00339 00340 JSON_GET(reference_throttle_1) 00341 JSON_GET(reference_throttle_2) 00342 JSON_GET(reference_throttle_3) 00343 JSON_GET(reference_throttle_4) 00344 00345 JSON_GET(actual_throttle_1) 00346 JSON_GET(actual_throttle_2) 00347 JSON_GET(actual_throttle_3) 00348 JSON_GET(actual_throttle_4) 00349 00350 JSON_GET(pid_rotation_r_error) 00351 JSON_GET(pid_rotation_r_error_integrative) 00352 JSON_GET(pid_rotation_r_error_derivative) 00353 JSON_GET(pid_rotation_r_kP) 00354 JSON_GET(pid_rotation_r_kI) 00355 JSON_GET(pid_rotation_r_kD) 00356 JSON_GET(pid_rotation_r_out) 00357 00358 JSON_GET(pid_rotation_p_error) 00359 JSON_GET(pid_rotation_p_error_integrative) 00360 JSON_GET(pid_rotation_p_error_derivative) 00361 JSON_GET(pid_rotation_p_kP) 00362 JSON_GET(pid_rotation_p_kI) 00363 JSON_GET(pid_rotation_p_kD) 00364 JSON_GET(pid_rotation_p_out) 00365 00366 JSON_GET(pid_rotation_y_error) 00367 JSON_GET(pid_rotation_y_error_integrative) 00368 JSON_GET(pid_rotation_y_error_derivative) 00369 JSON_GET(pid_rotation_y_kP) 00370 JSON_GET(pid_rotation_y_kI) 00371 JSON_GET(pid_rotation_y_kD) 00372 JSON_GET(pid_rotation_y_out) 00373 00374 JSON_GET(target_main_dt) 00375 JSON_GET(actual_main_dt) 00376 JSON_GET(average_main_dt) 00377 JSON_GET(average_main_dt_k) 00378 00379 JSON_GET(target_tx_dt) 00380 JSON_GET(actual_tx_dt) 00381 JSON_GET(average_tx_dt) 00382 JSON_GET(average_tx_dt_k) 00383 00384 JSON_GET(target_rx_dt) 00385 JSON_GET(actual_rx_dt) 00386 JSON_GET(average_rx_dt) 00387 JSON_GET(average_rx_dt_k) 00388 00389 JSON_GET(target_imu_dt) 00390 JSON_GET(actual_imu_dt) 00391 JSON_GET(average_imu_dt) 00392 JSON_GET(average_imu_dt_k) 00393 00394 JSON_GET(estimated_altitude) 00395 JSON_GET(estimated_rotation_q1) 00396 JSON_GET(estimated_rotation_q2) 00397 JSON_GET(estimated_rotation_q3) 00398 JSON_GET(estimated_rotation_q4) 00399 00400 JSON_GET(estimated_position_x) 00401 JSON_GET(estimated_position_y) 00402 JSON_GET(estimated_position_z) 00403 JSON_GET(estimated_rotation_y) 00404 JSON_GET(estimated_rotation_p) 00405 JSON_GET(estimated_rotation_r) 00406 00407 JSON_GET(sensor_accel_x) 00408 JSON_GET(sensor_accel_y) 00409 JSON_GET(sensor_accel_z) 00410 JSON_GET(sensor_gyro_r) 00411 JSON_GET(sensor_gyro_p) 00412 JSON_GET(sensor_gyro_y) 00413 00414 JSON_GET(target_position_x) 00415 JSON_GET(target_position_y) 00416 JSON_GET(target_position_z) 00417 JSON_GET(target_rotation_y) 00418 JSON_GET(target_rotation_p) 00419 JSON_GET(target_rotation_r) 00420 00421 JSON_GET(target_position_x_isRequired) 00422 JSON_GET(target_position_y_isRequired) 00423 JSON_GET(target_position_z_isRequired) 00424 JSON_GET(target_rotation_y_isRequired) 00425 JSON_GET(target_rotation_p_isRequired) 00426 JSON_GET(target_rotation_r_isRequired) 00427 00428 JSON_GET(timestamp) 00429 00430 JSON_GET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER) 00431 00432 ); 00433 00434 #undef JSON_GET 00435 00436 //return picojson::value(array_vector).serialize(); 00437 } 00438 #endif 00439 00440 }; 00441 00442 00443 ///Calc dt in seconds. 00444 #define QUAD_STATE_READ_ACTUAL_DT(state, what, timer) { \ 00445 state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us(); \ 00446 } 00447 00448 ///Sleep until the setted DT is reached. 00449 #define QUAD_STATE_WAIT_DT_TARGET(actual, target) { \ 00450 /*if(target-actual > 0.001*target && target-actual > 0.00001){*/ \ 00451 Thread::wait((double) 1000.0*target-1000.0*actual); \ 00452 /*} else*/ \ 00453 /*Thread::yield();*/ \ 00454 } 00455 00456 ///Calc actual dt and update average 00457 #define QUAD_STATE_UPDATE_DT(state, what, timer) { \ 00458 state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us(); \ 00459 timer.reset(); \ 00460 state.average_ ## what ## _dt *= 0.9999999999999999 - state.average_ ## what ## _dt_k; \ 00461 state.average_ ## what ## _dt += state.actual_ ## what ## _dt * state.average_ ## what ## _dt_k; \ 00462 } 00463 00464 00465 /* 00466 //Calc dt in seconds 00467 #define DT_READ() mainQuadState.actual_throttle_dt = (float) (1.0/1000000.0) * dt_t.read_us() 00468 DT_READ(); 00469 if(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt > MAIN_CYCLE_TARGET_DT/10){ 00470 //if the dt is very small 00471 //sleep until the setted DT is reached. 00472 Thread::wait(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt); 00473 DT_READ(); 00474 } 00475 dt_timer.reset(); 00476 00477 //Calc average dt 00478 mainQuadState.average_throttle_dt -= mainQuadState.average_throttle_dt * mainQuadState.average_throttle_dt_k; 00479 mainQuadState.average_throttle_dt += mainQuadState.actual_throttle_dt * mainQuadState.average_throttle_dt_k; 00480 */ 00481 00482 00483 00484 00485 #endif //QUAD_STATE__H
Generated on Wed Jul 13 2022 10:41:19 by
1.7.2