Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

state.h@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 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 4:46f3c3dd5977 14
MatteoT 4:46f3c3dd5977 15 double reference_throttle_1;
MatteoT 4:46f3c3dd5977 16 double reference_throttle_2;
MatteoT 4:46f3c3dd5977 17 double reference_throttle_3;
MatteoT 4:46f3c3dd5977 18 double reference_throttle_4;
MatteoT 2:7439607ccd51 19
MatteoT 3:5e7476bca25f 20 double actual_throttle_1;
MatteoT 3:5e7476bca25f 21 double actual_throttle_2;
MatteoT 3:5e7476bca25f 22 double actual_throttle_3;
MatteoT 3:5e7476bca25f 23 double actual_throttle_4;
MatteoT 4:46f3c3dd5977 24
MatteoT 4:46f3c3dd5977 25 double pid_rotation_r_error;
MatteoT 4:46f3c3dd5977 26 double pid_rotation_r_error_integrative;
MatteoT 4:46f3c3dd5977 27 double pid_rotation_r_error_derivative;
MatteoT 4:46f3c3dd5977 28 double pid_rotation_r_kP;
MatteoT 4:46f3c3dd5977 29 double pid_rotation_r_kI;
MatteoT 4:46f3c3dd5977 30 double pid_rotation_r_kD;
MatteoT 4:46f3c3dd5977 31 double pid_rotation_r_out;
MatteoT 4:46f3c3dd5977 32
MatteoT 4:46f3c3dd5977 33 double pid_rotation_p_error;
MatteoT 4:46f3c3dd5977 34 double pid_rotation_p_error_integrative;
MatteoT 4:46f3c3dd5977 35 double pid_rotation_p_error_derivative;
MatteoT 4:46f3c3dd5977 36 double pid_rotation_p_kP;
MatteoT 4:46f3c3dd5977 37 double pid_rotation_p_kI;
MatteoT 4:46f3c3dd5977 38 double pid_rotation_p_kD;
MatteoT 4:46f3c3dd5977 39 double pid_rotation_p_out;
MatteoT 4:46f3c3dd5977 40
MatteoT 4:46f3c3dd5977 41 double pid_rotation_y_error;
MatteoT 4:46f3c3dd5977 42 double pid_rotation_y_error_integrative;
MatteoT 4:46f3c3dd5977 43 double pid_rotation_y_error_derivative;
MatteoT 4:46f3c3dd5977 44 double pid_rotation_y_kP;
MatteoT 4:46f3c3dd5977 45 double pid_rotation_y_kI;
MatteoT 4:46f3c3dd5977 46 double pid_rotation_y_kD;
MatteoT 4:46f3c3dd5977 47 double pid_rotation_y_out;
MatteoT 4:46f3c3dd5977 48
MatteoT 3:5e7476bca25f 49 double target_main_dt;
MatteoT 3:5e7476bca25f 50 double actual_main_dt;
MatteoT 3:5e7476bca25f 51 double average_main_dt;
MatteoT 3:5e7476bca25f 52 double average_main_dt_k;
MatteoT 4:46f3c3dd5977 53
MatteoT 3:5e7476bca25f 54 double target_tx_dt;
MatteoT 3:5e7476bca25f 55 double actual_tx_dt;
MatteoT 3:5e7476bca25f 56 double average_tx_dt;
MatteoT 3:5e7476bca25f 57 double average_tx_dt_k;
MatteoT 4:46f3c3dd5977 58
MatteoT 3:5e7476bca25f 59 double target_rx_dt;
MatteoT 3:5e7476bca25f 60 double actual_rx_dt;
MatteoT 3:5e7476bca25f 61 double average_rx_dt;
MatteoT 3:5e7476bca25f 62 double average_rx_dt_k;
MatteoT 4:46f3c3dd5977 63
MatteoT 3:5e7476bca25f 64 double target_imu_dt;
MatteoT 3:5e7476bca25f 65 double actual_imu_dt;
MatteoT 3:5e7476bca25f 66 double average_imu_dt;
MatteoT 3:5e7476bca25f 67 double average_imu_dt_k;
MatteoT 4:46f3c3dd5977 68
MatteoT 3:5e7476bca25f 69 double estimated_altitude;
MatteoT 3:5e7476bca25f 70 double estimated_rotation_q1;
MatteoT 3:5e7476bca25f 71 double estimated_rotation_q2;
MatteoT 3:5e7476bca25f 72 double estimated_rotation_q3;
MatteoT 3:5e7476bca25f 73 double estimated_rotation_q4;
MatteoT 4:46f3c3dd5977 74
MatteoT 3:5e7476bca25f 75 double estimated_position_x;
MatteoT 3:5e7476bca25f 76 double estimated_position_y;
MatteoT 3:5e7476bca25f 77 double estimated_position_z;
MatteoT 3:5e7476bca25f 78 double estimated_rotation_y;
MatteoT 3:5e7476bca25f 79 double estimated_rotation_p;
MatteoT 3:5e7476bca25f 80 double estimated_rotation_r;
MatteoT 4:46f3c3dd5977 81
MatteoT 4:46f3c3dd5977 82 double sensor_accel_x;
MatteoT 4:46f3c3dd5977 83 double sensor_accel_y;
MatteoT 4:46f3c3dd5977 84 double sensor_accel_z;
MatteoT 4:46f3c3dd5977 85 double sensor_gyro_r;
MatteoT 4:46f3c3dd5977 86 double sensor_gyro_p;
MatteoT 4:46f3c3dd5977 87 double sensor_gyro_y;
MatteoT 4:46f3c3dd5977 88
MatteoT 3:5e7476bca25f 89 double target_position_x; //see _isRequired members
MatteoT 3:5e7476bca25f 90 double target_position_y;
MatteoT 3:5e7476bca25f 91 double target_position_z;
MatteoT 3:5e7476bca25f 92 double target_rotation_y;
MatteoT 3:5e7476bca25f 93 double target_rotation_p;
MatteoT 3:5e7476bca25f 94 double target_rotation_r;
MatteoT 4:46f3c3dd5977 95
MatteoT 3:5e7476bca25f 96 double target_position_x_isRequired; //1=true; 0=false; error otherwise
MatteoT 3:5e7476bca25f 97 double target_position_y_isRequired;
MatteoT 3:5e7476bca25f 98 double target_position_z_isRequired;
MatteoT 3:5e7476bca25f 99 double target_rotation_y_isRequired;
MatteoT 3:5e7476bca25f 100 double target_rotation_p_isRequired;
MatteoT 3:5e7476bca25f 101 double target_rotation_r_isRequired;
MatteoT 4:46f3c3dd5977 102
MatteoT 4:46f3c3dd5977 103 double timestamp;
MatteoT 4:46f3c3dd5977 104
MatteoT 3:5e7476bca25f 105 double ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER; //1=OK; 0=EmergencyStop; error otherwise
MatteoT 4:46f3c3dd5977 106
MatteoT 4:46f3c3dd5977 107
MatteoT 2:7439607ccd51 108 void reset (){
MatteoT 4:46f3c3dd5977 109
MatteoT 4:46f3c3dd5977 110 reference_throttle_1 = -1000;
MatteoT 4:46f3c3dd5977 111 reference_throttle_2 = -1000;
MatteoT 4:46f3c3dd5977 112 reference_throttle_3 = -1000;
MatteoT 4:46f3c3dd5977 113 reference_throttle_4 = -1000;
MatteoT 2:7439607ccd51 114
MatteoT 2:7439607ccd51 115 actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 116 actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 117 actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 118 actual_throttle_4 = 0;
MatteoT 4:46f3c3dd5977 119
MatteoT 4:46f3c3dd5977 120 pid_rotation_r_error = 0;
MatteoT 4:46f3c3dd5977 121 pid_rotation_r_error_integrative = 0;
MatteoT 4:46f3c3dd5977 122 pid_rotation_r_error_derivative = 0;
MatteoT 4:46f3c3dd5977 123 pid_rotation_r_kP = 0.3;
MatteoT 4:46f3c3dd5977 124 pid_rotation_r_kI = 0;
MatteoT 4:46f3c3dd5977 125 pid_rotation_r_kD = 1.2;
MatteoT 4:46f3c3dd5977 126 pid_rotation_r_out = 0;
MatteoT 4:46f3c3dd5977 127
MatteoT 4:46f3c3dd5977 128 pid_rotation_p_error = 0;
MatteoT 4:46f3c3dd5977 129 pid_rotation_p_error_integrative = 0;
MatteoT 4:46f3c3dd5977 130 pid_rotation_p_error_derivative = 0;
MatteoT 4:46f3c3dd5977 131 pid_rotation_p_kP = 0.3;
MatteoT 4:46f3c3dd5977 132 pid_rotation_p_kI = 0;
MatteoT 4:46f3c3dd5977 133 pid_rotation_p_kD = 1.2;
MatteoT 4:46f3c3dd5977 134 pid_rotation_p_out = 0;
MatteoT 4:46f3c3dd5977 135
MatteoT 4:46f3c3dd5977 136 pid_rotation_y_error = 0;
MatteoT 4:46f3c3dd5977 137 pid_rotation_y_error_integrative = 0;
MatteoT 4:46f3c3dd5977 138 pid_rotation_y_error_derivative = 0;
MatteoT 4:46f3c3dd5977 139 pid_rotation_y_kP = 0.3;
MatteoT 4:46f3c3dd5977 140 pid_rotation_y_kI = 0;
MatteoT 4:46f3c3dd5977 141 pid_rotation_y_kD = 0.1;
MatteoT 4:46f3c3dd5977 142 pid_rotation_y_out = 0;
MatteoT 4:46f3c3dd5977 143
MatteoT 3:5e7476bca25f 144 target_main_dt = TARGET_MAIN_DT;
MatteoT 3:5e7476bca25f 145 actual_main_dt = TARGET_MAIN_DT;
MatteoT 3:5e7476bca25f 146 average_main_dt = TARGET_MAIN_DT;
MatteoT 3:5e7476bca25f 147 average_main_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 4:46f3c3dd5977 148
MatteoT 3:5e7476bca25f 149 target_tx_dt = TARGET_TX_DT;
MatteoT 3:5e7476bca25f 150 actual_tx_dt = TARGET_TX_DT;
MatteoT 3:5e7476bca25f 151 average_tx_dt = TARGET_TX_DT;
MatteoT 3:5e7476bca25f 152 average_tx_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 4:46f3c3dd5977 153
MatteoT 3:5e7476bca25f 154 target_rx_dt = TARGET_RX_DT;
MatteoT 3:5e7476bca25f 155 actual_rx_dt = TARGET_RX_DT;
MatteoT 3:5e7476bca25f 156 average_rx_dt = TARGET_RX_DT;
MatteoT 3:5e7476bca25f 157 average_rx_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 4:46f3c3dd5977 158
MatteoT 3:5e7476bca25f 159 target_imu_dt = TARGET_IMU_DT;
MatteoT 3:5e7476bca25f 160 actual_imu_dt = TARGET_IMU_DT;
MatteoT 3:5e7476bca25f 161 average_imu_dt = TARGET_IMU_DT;
MatteoT 3:5e7476bca25f 162 average_imu_dt_k = AVERAGE_DT_K_GAIN;
MatteoT 4:46f3c3dd5977 163
MatteoT 2:7439607ccd51 164 estimated_altitude = 0;
MatteoT 2:7439607ccd51 165 estimated_rotation_q1 = 0;
MatteoT 2:7439607ccd51 166 estimated_rotation_q2 = 0;
MatteoT 2:7439607ccd51 167 estimated_rotation_q3 = 0;
MatteoT 2:7439607ccd51 168 estimated_rotation_q4 = 0;
MatteoT 4:46f3c3dd5977 169
MatteoT 2:7439607ccd51 170 estimated_position_x = 0;
MatteoT 2:7439607ccd51 171 estimated_position_y = 0;
MatteoT 2:7439607ccd51 172 estimated_position_z = 0;
MatteoT 2:7439607ccd51 173 estimated_rotation_y = 0;
MatteoT 2:7439607ccd51 174 estimated_rotation_p = 0;
MatteoT 2:7439607ccd51 175 estimated_rotation_r = 0;
MatteoT 4:46f3c3dd5977 176
MatteoT 4:46f3c3dd5977 177 sensor_accel_x = 0;
MatteoT 4:46f3c3dd5977 178 sensor_accel_y = 0;
MatteoT 4:46f3c3dd5977 179 sensor_accel_z = 0;
MatteoT 4:46f3c3dd5977 180 sensor_gyro_r = 0;
MatteoT 4:46f3c3dd5977 181 sensor_gyro_p = 0;
MatteoT 4:46f3c3dd5977 182 sensor_gyro_y = 0;
MatteoT 4:46f3c3dd5977 183
MatteoT 2:7439607ccd51 184 target_position_x = 0;
MatteoT 2:7439607ccd51 185 target_position_y = 0;
MatteoT 2:7439607ccd51 186 target_position_z = 0;
MatteoT 2:7439607ccd51 187 target_rotation_y = 0;
MatteoT 2:7439607ccd51 188 target_rotation_p = 0;
MatteoT 2:7439607ccd51 189 target_rotation_r = 0;
MatteoT 4:46f3c3dd5977 190
MatteoT 3:5e7476bca25f 191 target_position_x_isRequired = 0;
MatteoT 3:5e7476bca25f 192 target_position_y_isRequired = 0;
MatteoT 3:5e7476bca25f 193 target_position_z_isRequired = 0;
MatteoT 3:5e7476bca25f 194 target_rotation_y_isRequired = 0;
MatteoT 3:5e7476bca25f 195 target_rotation_p_isRequired = 0;
MatteoT 3:5e7476bca25f 196 target_rotation_r_isRequired = 0;
MatteoT 4:46f3c3dd5977 197
MatteoT 4:46f3c3dd5977 198 timestamp = 0;
MatteoT 4:46f3c3dd5977 199
MatteoT 4:46f3c3dd5977 200 ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0;
MatteoT 3:5e7476bca25f 201 }
MatteoT 4:46f3c3dd5977 202
MatteoT 3:5e7476bca25f 203 #ifndef QUAD_STATE_NO_JSON
MatteoT 3:5e7476bca25f 204 /**
MatteoT 3:5e7476bca25f 205 * @return number of setted members.
MatteoT 3:5e7476bca25f 206 */
MatteoT 3:5e7476bca25f 207 int setFromJSON(const char* JSON_frame){
MatteoT 3:5e7476bca25f 208 int setted = 0;
MatteoT 3:5e7476bca25f 209 picojson::value json;
MatteoT 3:5e7476bca25f 210 picojson::parse(json, JSON_frame, JSON_frame + strlen(JSON_frame));
MatteoT 4:46f3c3dd5977 211
MatteoT 3:5e7476bca25f 212 unsigned int i=0;
MatteoT 3:5e7476bca25f 213
MatteoT 3:5e7476bca25f 214 #define JSON_SET(var) { \
MatteoT 3:5e7476bca25f 215 picojson::value j = json.get(i++); \
MatteoT 3:5e7476bca25f 216 if(j.is<double>()){ \
MatteoT 3:5e7476bca25f 217 var = j.get<double>(); \
MatteoT 3:5e7476bca25f 218 ++setted; \
MatteoT 3:5e7476bca25f 219 } \
MatteoT 3:5e7476bca25f 220 }
MatteoT 4:46f3c3dd5977 221
MatteoT 4:46f3c3dd5977 222 JSON_SET(reference_throttle_1)
MatteoT 4:46f3c3dd5977 223 JSON_SET(reference_throttle_2)
MatteoT 4:46f3c3dd5977 224 JSON_SET(reference_throttle_3)
MatteoT 4:46f3c3dd5977 225 JSON_SET(reference_throttle_4)
MatteoT 3:5e7476bca25f 226
MatteoT 3:5e7476bca25f 227 JSON_SET(actual_throttle_1)
MatteoT 3:5e7476bca25f 228 JSON_SET(actual_throttle_2)
MatteoT 3:5e7476bca25f 229 JSON_SET(actual_throttle_3)
MatteoT 3:5e7476bca25f 230 JSON_SET(actual_throttle_4)
MatteoT 4:46f3c3dd5977 231
MatteoT 4:46f3c3dd5977 232 JSON_SET(pid_rotation_r_error)
MatteoT 4:46f3c3dd5977 233 JSON_SET(pid_rotation_r_error_integrative)
MatteoT 4:46f3c3dd5977 234 JSON_SET(pid_rotation_r_error_derivative)
MatteoT 4:46f3c3dd5977 235 JSON_SET(pid_rotation_r_kP)
MatteoT 4:46f3c3dd5977 236 JSON_SET(pid_rotation_r_kI)
MatteoT 4:46f3c3dd5977 237 JSON_SET(pid_rotation_r_kD)
MatteoT 4:46f3c3dd5977 238 JSON_SET(pid_rotation_r_out)
MatteoT 4:46f3c3dd5977 239
MatteoT 4:46f3c3dd5977 240 JSON_SET(pid_rotation_p_error)
MatteoT 4:46f3c3dd5977 241 JSON_SET(pid_rotation_p_error_integrative)
MatteoT 4:46f3c3dd5977 242 JSON_SET(pid_rotation_p_error_derivative)
MatteoT 4:46f3c3dd5977 243 JSON_SET(pid_rotation_p_kP)
MatteoT 4:46f3c3dd5977 244 JSON_SET(pid_rotation_p_kI)
MatteoT 4:46f3c3dd5977 245 JSON_SET(pid_rotation_p_kD)
MatteoT 4:46f3c3dd5977 246 JSON_SET(pid_rotation_p_out)
MatteoT 4:46f3c3dd5977 247
MatteoT 4:46f3c3dd5977 248 JSON_SET(pid_rotation_y_error)
MatteoT 4:46f3c3dd5977 249 JSON_SET(pid_rotation_y_error_integrative)
MatteoT 4:46f3c3dd5977 250 JSON_SET(pid_rotation_y_error_derivative)
MatteoT 4:46f3c3dd5977 251 JSON_SET(pid_rotation_y_kP)
MatteoT 4:46f3c3dd5977 252 JSON_SET(pid_rotation_y_kI)
MatteoT 4:46f3c3dd5977 253 JSON_SET(pid_rotation_y_kD)
MatteoT 4:46f3c3dd5977 254 JSON_SET(pid_rotation_y_out)
MatteoT 4:46f3c3dd5977 255
MatteoT 3:5e7476bca25f 256 JSON_SET(target_main_dt)
MatteoT 3:5e7476bca25f 257 JSON_SET(actual_main_dt)
MatteoT 3:5e7476bca25f 258 JSON_SET(average_main_dt)
MatteoT 3:5e7476bca25f 259 JSON_SET(average_main_dt_k)
MatteoT 4:46f3c3dd5977 260
MatteoT 3:5e7476bca25f 261 JSON_SET(target_tx_dt)
MatteoT 3:5e7476bca25f 262 JSON_SET(actual_tx_dt)
MatteoT 3:5e7476bca25f 263 JSON_SET(average_tx_dt)
MatteoT 3:5e7476bca25f 264 JSON_SET(average_tx_dt_k)
MatteoT 4:46f3c3dd5977 265
MatteoT 3:5e7476bca25f 266 JSON_SET(target_rx_dt)
MatteoT 3:5e7476bca25f 267 JSON_SET(actual_rx_dt)
MatteoT 3:5e7476bca25f 268 JSON_SET(average_rx_dt)
MatteoT 3:5e7476bca25f 269 JSON_SET(average_rx_dt_k)
MatteoT 4:46f3c3dd5977 270
MatteoT 3:5e7476bca25f 271 JSON_SET(target_imu_dt)
MatteoT 3:5e7476bca25f 272 JSON_SET(actual_imu_dt)
MatteoT 3:5e7476bca25f 273 JSON_SET(average_imu_dt)
MatteoT 3:5e7476bca25f 274 JSON_SET(average_imu_dt_k)
MatteoT 4:46f3c3dd5977 275
MatteoT 3:5e7476bca25f 276 JSON_SET(estimated_altitude)
MatteoT 3:5e7476bca25f 277 JSON_SET(estimated_rotation_q1)
MatteoT 3:5e7476bca25f 278 JSON_SET(estimated_rotation_q2)
MatteoT 3:5e7476bca25f 279 JSON_SET(estimated_rotation_q3)
MatteoT 3:5e7476bca25f 280 JSON_SET(estimated_rotation_q4)
MatteoT 4:46f3c3dd5977 281
MatteoT 3:5e7476bca25f 282 JSON_SET(estimated_position_x)
MatteoT 3:5e7476bca25f 283 JSON_SET(estimated_position_y)
MatteoT 3:5e7476bca25f 284 JSON_SET(estimated_position_z)
MatteoT 3:5e7476bca25f 285 JSON_SET(estimated_rotation_y)
MatteoT 3:5e7476bca25f 286 JSON_SET(estimated_rotation_p)
MatteoT 3:5e7476bca25f 287 JSON_SET(estimated_rotation_r)
MatteoT 4:46f3c3dd5977 288
MatteoT 4:46f3c3dd5977 289 JSON_SET(sensor_accel_x)
MatteoT 4:46f3c3dd5977 290 JSON_SET(sensor_accel_y)
MatteoT 4:46f3c3dd5977 291 JSON_SET(sensor_accel_z)
MatteoT 4:46f3c3dd5977 292 JSON_SET(sensor_gyro_r)
MatteoT 4:46f3c3dd5977 293 JSON_SET(sensor_gyro_p)
MatteoT 4:46f3c3dd5977 294 JSON_SET(sensor_gyro_y)
MatteoT 4:46f3c3dd5977 295
MatteoT 3:5e7476bca25f 296 JSON_SET(target_position_x)
MatteoT 3:5e7476bca25f 297 JSON_SET(target_position_y)
MatteoT 3:5e7476bca25f 298 JSON_SET(target_position_z)
MatteoT 3:5e7476bca25f 299 JSON_SET(target_rotation_y)
MatteoT 3:5e7476bca25f 300 JSON_SET(target_rotation_p)
MatteoT 3:5e7476bca25f 301 JSON_SET(target_rotation_r)
MatteoT 4:46f3c3dd5977 302
MatteoT 3:5e7476bca25f 303 JSON_SET(target_position_x_isRequired)
MatteoT 3:5e7476bca25f 304 JSON_SET(target_position_y_isRequired)
MatteoT 3:5e7476bca25f 305 JSON_SET(target_position_z_isRequired)
MatteoT 3:5e7476bca25f 306 JSON_SET(target_rotation_y_isRequired)
MatteoT 3:5e7476bca25f 307 JSON_SET(target_rotation_p_isRequired)
MatteoT 3:5e7476bca25f 308 JSON_SET(target_rotation_r_isRequired)
MatteoT 4:46f3c3dd5977 309
MatteoT 4:46f3c3dd5977 310 JSON_SET(timestamp)
MatteoT 4:46f3c3dd5977 311
MatteoT 3:5e7476bca25f 312 JSON_SET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER)
MatteoT 4:46f3c3dd5977 313
MatteoT 3:5e7476bca25f 314 #undef JSON_SET
MatteoT 4:46f3c3dd5977 315
MatteoT 3:5e7476bca25f 316 return setted;
MatteoT 2:7439607ccd51 317 }
MatteoT 4:46f3c3dd5977 318
MatteoT 3:5e7476bca25f 319 std::string getJSON() const{
MatteoT 4:46f3c3dd5977 320
MatteoT 3:5e7476bca25f 321 picojson::array array_vector;
MatteoT 4:46f3c3dd5977 322
MatteoT 3:5e7476bca25f 323 #define JSON_GET(var) array_vector.push_back(picojson::value(var));
MatteoT 3:5e7476bca25f 324
MatteoT 4:46f3c3dd5977 325
MatteoT 4:46f3c3dd5977 326 JSON_GET(reference_throttle_1)
MatteoT 4:46f3c3dd5977 327 JSON_GET(reference_throttle_2)
MatteoT 4:46f3c3dd5977 328 JSON_GET(reference_throttle_3)
MatteoT 4:46f3c3dd5977 329 JSON_GET(reference_throttle_4)
MatteoT 3:5e7476bca25f 330
MatteoT 3:5e7476bca25f 331 JSON_GET(actual_throttle_1)
MatteoT 3:5e7476bca25f 332 JSON_GET(actual_throttle_2)
MatteoT 3:5e7476bca25f 333 JSON_GET(actual_throttle_3)
MatteoT 3:5e7476bca25f 334 JSON_GET(actual_throttle_4)
MatteoT 4:46f3c3dd5977 335
MatteoT 4:46f3c3dd5977 336 JSON_GET(pid_rotation_r_error)
MatteoT 4:46f3c3dd5977 337 JSON_GET(pid_rotation_r_error_integrative)
MatteoT 4:46f3c3dd5977 338 JSON_GET(pid_rotation_r_error_derivative)
MatteoT 4:46f3c3dd5977 339 JSON_GET(pid_rotation_r_kP)
MatteoT 4:46f3c3dd5977 340 JSON_GET(pid_rotation_r_kI)
MatteoT 4:46f3c3dd5977 341 JSON_GET(pid_rotation_r_kD)
MatteoT 4:46f3c3dd5977 342 JSON_GET(pid_rotation_r_out)
MatteoT 4:46f3c3dd5977 343
MatteoT 4:46f3c3dd5977 344 JSON_GET(pid_rotation_p_error)
MatteoT 4:46f3c3dd5977 345 JSON_GET(pid_rotation_p_error_integrative)
MatteoT 4:46f3c3dd5977 346 JSON_GET(pid_rotation_p_error_derivative)
MatteoT 4:46f3c3dd5977 347 JSON_GET(pid_rotation_p_kP)
MatteoT 4:46f3c3dd5977 348 JSON_GET(pid_rotation_p_kI)
MatteoT 4:46f3c3dd5977 349 JSON_GET(pid_rotation_p_kD)
MatteoT 4:46f3c3dd5977 350 JSON_GET(pid_rotation_p_out)
MatteoT 4:46f3c3dd5977 351
MatteoT 4:46f3c3dd5977 352 JSON_GET(pid_rotation_y_error)
MatteoT 4:46f3c3dd5977 353 JSON_GET(pid_rotation_y_error_integrative)
MatteoT 4:46f3c3dd5977 354 JSON_GET(pid_rotation_y_error_derivative)
MatteoT 4:46f3c3dd5977 355 JSON_GET(pid_rotation_y_kP)
MatteoT 4:46f3c3dd5977 356 JSON_GET(pid_rotation_y_kI)
MatteoT 4:46f3c3dd5977 357 JSON_GET(pid_rotation_y_kD)
MatteoT 4:46f3c3dd5977 358 JSON_GET(pid_rotation_y_out)
MatteoT 4:46f3c3dd5977 359
MatteoT 3:5e7476bca25f 360 JSON_GET(target_main_dt)
MatteoT 3:5e7476bca25f 361 JSON_GET(actual_main_dt)
MatteoT 3:5e7476bca25f 362 JSON_GET(average_main_dt)
MatteoT 3:5e7476bca25f 363 JSON_GET(average_main_dt_k)
MatteoT 4:46f3c3dd5977 364
MatteoT 3:5e7476bca25f 365 JSON_GET(target_tx_dt)
MatteoT 3:5e7476bca25f 366 JSON_GET(actual_tx_dt)
MatteoT 3:5e7476bca25f 367 JSON_GET(average_tx_dt)
MatteoT 3:5e7476bca25f 368 JSON_GET(average_tx_dt_k)
MatteoT 4:46f3c3dd5977 369
MatteoT 3:5e7476bca25f 370 JSON_GET(target_rx_dt)
MatteoT 3:5e7476bca25f 371 JSON_GET(actual_rx_dt)
MatteoT 3:5e7476bca25f 372 JSON_GET(average_rx_dt)
MatteoT 3:5e7476bca25f 373 JSON_GET(average_rx_dt_k)
MatteoT 4:46f3c3dd5977 374
MatteoT 3:5e7476bca25f 375 JSON_GET(target_imu_dt)
MatteoT 3:5e7476bca25f 376 JSON_GET(actual_imu_dt)
MatteoT 3:5e7476bca25f 377 JSON_GET(average_imu_dt)
MatteoT 3:5e7476bca25f 378 JSON_GET(average_imu_dt_k)
MatteoT 4:46f3c3dd5977 379
MatteoT 3:5e7476bca25f 380 JSON_GET(estimated_altitude)
MatteoT 3:5e7476bca25f 381 JSON_GET(estimated_rotation_q1)
MatteoT 3:5e7476bca25f 382 JSON_GET(estimated_rotation_q2)
MatteoT 3:5e7476bca25f 383 JSON_GET(estimated_rotation_q3)
MatteoT 3:5e7476bca25f 384 JSON_GET(estimated_rotation_q4)
MatteoT 4:46f3c3dd5977 385
MatteoT 3:5e7476bca25f 386 JSON_GET(estimated_position_x)
MatteoT 3:5e7476bca25f 387 JSON_GET(estimated_position_y)
MatteoT 3:5e7476bca25f 388 JSON_GET(estimated_position_z)
MatteoT 3:5e7476bca25f 389 JSON_GET(estimated_rotation_y)
MatteoT 3:5e7476bca25f 390 JSON_GET(estimated_rotation_p)
MatteoT 3:5e7476bca25f 391 JSON_GET(estimated_rotation_r)
MatteoT 4:46f3c3dd5977 392
MatteoT 4:46f3c3dd5977 393 JSON_GET(sensor_accel_x)
MatteoT 4:46f3c3dd5977 394 JSON_GET(sensor_accel_y)
MatteoT 4:46f3c3dd5977 395 JSON_GET(sensor_accel_z)
MatteoT 4:46f3c3dd5977 396 JSON_GET(sensor_gyro_r)
MatteoT 4:46f3c3dd5977 397 JSON_GET(sensor_gyro_p)
MatteoT 4:46f3c3dd5977 398 JSON_GET(sensor_gyro_y)
MatteoT 4:46f3c3dd5977 399
MatteoT 3:5e7476bca25f 400 JSON_GET(target_position_x)
MatteoT 3:5e7476bca25f 401 JSON_GET(target_position_y)
MatteoT 3:5e7476bca25f 402 JSON_GET(target_position_z)
MatteoT 3:5e7476bca25f 403 JSON_GET(target_rotation_y)
MatteoT 3:5e7476bca25f 404 JSON_GET(target_rotation_p)
MatteoT 3:5e7476bca25f 405 JSON_GET(target_rotation_r)
MatteoT 4:46f3c3dd5977 406
MatteoT 3:5e7476bca25f 407 JSON_GET(target_position_x_isRequired)
MatteoT 3:5e7476bca25f 408 JSON_GET(target_position_y_isRequired)
MatteoT 3:5e7476bca25f 409 JSON_GET(target_position_z_isRequired)
MatteoT 3:5e7476bca25f 410 JSON_GET(target_rotation_y_isRequired)
MatteoT 3:5e7476bca25f 411 JSON_GET(target_rotation_p_isRequired)
MatteoT 3:5e7476bca25f 412 JSON_GET(target_rotation_r_isRequired)
MatteoT 4:46f3c3dd5977 413
MatteoT 4:46f3c3dd5977 414 JSON_GET(timestamp)
MatteoT 4:46f3c3dd5977 415
MatteoT 3:5e7476bca25f 416 JSON_GET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER)
MatteoT 4:46f3c3dd5977 417
MatteoT 3:5e7476bca25f 418 #undef JSON_GET
MatteoT 4:46f3c3dd5977 419
MatteoT 3:5e7476bca25f 420 return picojson::value(array_vector).serialize();
MatteoT 3:5e7476bca25f 421 }
MatteoT 3:5e7476bca25f 422 #endif
MatteoT 3:5e7476bca25f 423
MatteoT 2:7439607ccd51 424 };
MatteoT 2:7439607ccd51 425
MatteoT 3:5e7476bca25f 426
MatteoT 3:5e7476bca25f 427 ///Calc dt in seconds.
MatteoT 3:5e7476bca25f 428 #define QUAD_STATE_READ_ACTUAL_DT(state, what, timer) { \
MatteoT 3:5e7476bca25f 429 state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us(); \
MatteoT 3:5e7476bca25f 430 }
MatteoT 3:5e7476bca25f 431
MatteoT 3:5e7476bca25f 432 ///Sleep until the setted DT is reached.
MatteoT 3:5e7476bca25f 433 #define QUAD_STATE_WAIT_DT_TARGET(actual, target) { \
MatteoT 3:5e7476bca25f 434 if(target-actual > 0.01*target && target-actual > 0.0001){ \
MatteoT 3:5e7476bca25f 435 Thread::wait((uint32_t) (1000.0*target-1000.0*actual)); \
MatteoT 3:5e7476bca25f 436 } else \
MatteoT 3:5e7476bca25f 437 Thread::yield(); \
MatteoT 3:5e7476bca25f 438 }
MatteoT 3:5e7476bca25f 439
MatteoT 3:5e7476bca25f 440 ///Calc actual dt and update average
MatteoT 3:5e7476bca25f 441 #define QUAD_STATE_UPDATE_DT(state, what, timer) { \
MatteoT 3:5e7476bca25f 442 state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us(); \
MatteoT 3:5e7476bca25f 443 timer.reset(); \
MatteoT 3:5e7476bca25f 444 state.average_ ## what ## _dt *= 0.9999999999999999 - state.average_ ## what ## _dt_k; \
MatteoT 3:5e7476bca25f 445 state.average_ ## what ## _dt += state.actual_ ## what ## _dt * state.average_ ## what ## _dt_k; \
MatteoT 3:5e7476bca25f 446 }
MatteoT 3:5e7476bca25f 447
MatteoT 3:5e7476bca25f 448
MatteoT 3:5e7476bca25f 449 /*
MatteoT 3:5e7476bca25f 450 //Calc dt in seconds
MatteoT 3:5e7476bca25f 451 #define DT_READ() mainQuadState.actual_throttle_dt = (float) (1.0/1000000.0) * dt_t.read_us()
MatteoT 3:5e7476bca25f 452 DT_READ();
MatteoT 3:5e7476bca25f 453 if(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt > MAIN_CYCLE_TARGET_DT/10){
MatteoT 3:5e7476bca25f 454 //if the dt is very small
MatteoT 3:5e7476bca25f 455 //sleep until the setted DT is reached.
MatteoT 3:5e7476bca25f 456 Thread::wait(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt);
MatteoT 3:5e7476bca25f 457 DT_READ();
MatteoT 3:5e7476bca25f 458 }
MatteoT 3:5e7476bca25f 459 dt_timer.reset();
MatteoT 4:46f3c3dd5977 460
MatteoT 3:5e7476bca25f 461 //Calc average dt
MatteoT 3:5e7476bca25f 462 mainQuadState.average_throttle_dt -= mainQuadState.average_throttle_dt * mainQuadState.average_throttle_dt_k;
MatteoT 3:5e7476bca25f 463 mainQuadState.average_throttle_dt += mainQuadState.actual_throttle_dt * mainQuadState.average_throttle_dt_k;
MatteoT 3:5e7476bca25f 464 */
MatteoT 3:5e7476bca25f 465
MatteoT 3:5e7476bca25f 466
MatteoT 3:5e7476bca25f 467
MatteoT 3:5e7476bca25f 468
MatteoT 3:5e7476bca25f 469 #endif //QUAD_STATE__H