Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
state.h@7:cda17cffec3c, 2014-05-24 (annotated)
- Committer:
- MatteoT
- Date:
- Sat May 24 17:42:03 2014 +0000
- Revision:
- 7:cda17cffec3c
- Parent:
- 6:4ddd68260f76
experiments (going to first launch)
Who changed what in which revision?
User | Revision | Line number | New 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 | 6:4ddd68260f76 | 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 |