Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

state.h@5:33abcc31b0aa, 2014-05-12 (annotated)

Committer:
MatteoT
Date:
Mon May 12 22:58:06 2014 +0000
Revision:
5:33abcc31b0aa
Parent:
4:46f3c3dd5977
Child:
6:4ddd68260f76
experiments (txrx and imu working)

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