Matteo Terruzzi
Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
Diff: state.h
- Revision:
- 3:5e7476bca25f
- Parent:
- 2:7439607ccd51
- Child:
- 4:46f3c3dd5977
--- a/state.h Tue Oct 15 18:30:46 2013 +0000
+++ b/state.h Sun Oct 27 01:05:35 2013 +0000
@@ -1,53 +1,74 @@
+#ifndef QUAD_STATE__H
+#define QUAD_STATE__H
+
#include <stdint.h>
+#ifndef QUAD_STATE_NO_JSON
+#include <picojson.h>
+#endif
+
+
//this struct must be the same for all the programs communicating with the Quad.
struct QuadState;
struct QuadState {
- float requested_throttle_1; //negative if not requested
- float requested_throttle_2;
- float requested_throttle_3;
- float requested_throttle_4;
+ double requested_throttle_1; //negative if not requested
+ double requested_throttle_2;
+ double requested_throttle_3;
+ double requested_throttle_4;
- float actual_throttle_1;
- float actual_throttle_2;
- float actual_throttle_3;
- float actual_throttle_4;
+ double actual_throttle_1;
+ double actual_throttle_2;
+ double actual_throttle_3;
+ double actual_throttle_4;
- float actual_throttle_dt;
- float average_throttle_dt;
- float average_throttle_dt_k;
+ double target_main_dt;
+ double actual_main_dt;
+ double average_main_dt;
+ double average_main_dt_k;
- float actual_rx_delta_ms;
- float actual_rx_wait_ms;
+ double target_tx_dt;
+ double actual_tx_dt;
+ double average_tx_dt;
+ double average_tx_dt_k;
- float estimated_altitude;
- float estimated_rotation_q1;
- float estimated_rotation_q2;
- float estimated_rotation_q3;
- float estimated_rotation_q4;
+ double target_rx_dt;
+ double actual_rx_dt;
+ double average_rx_dt;
+ double average_rx_dt_k;
- float estimated_position_x;
- float estimated_position_y;
- float estimated_position_z;
- float estimated_rotation_y;
- float estimated_rotation_p;
- float estimated_rotation_r;
+ double target_imu_dt;
+ double actual_imu_dt;
+ double average_imu_dt;
+ double average_imu_dt_k;
+
+ double estimated_altitude;
+ double estimated_rotation_q1;
+ double estimated_rotation_q2;
+ double estimated_rotation_q3;
+ double estimated_rotation_q4;
- float target_position_x; //see _isRequired members
- float target_position_y;
- float target_position_z;
- float target_rotation_y;
- float target_rotation_p;
- float target_rotation_r;
+ double estimated_position_x;
+ double estimated_position_y;
+ double estimated_position_z;
+ double estimated_rotation_y;
+ double estimated_rotation_p;
+ double estimated_rotation_r;
- bool target_position_x_isRequired;
- bool target_position_y_isRequired;
- bool target_position_z_isRequired;
- bool target_rotation_y_isRequired;
- bool target_rotation_p_isRequired;
- bool target_rotation_r_isRequired;
+ double target_position_x; //see _isRequired members
+ double target_position_y;
+ double target_position_z;
+ double target_rotation_y;
+ double target_rotation_p;
+ double target_rotation_r;
- bool ZERO_ALL_MOTORS_NOW__DANGER;
+ double target_position_x_isRequired; //1=true; 0=false; error otherwise
+ double target_position_y_isRequired;
+ double target_position_z_isRequired;
+ double target_rotation_y_isRequired;
+ double target_rotation_p_isRequired;
+ double target_rotation_r_isRequired;
+
+ double ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER; //1=OK; 0=EmergencyStop; error otherwise
void reset (){
@@ -61,13 +82,26 @@
actual_throttle_2 = 0;
actual_throttle_3 = 0;
actual_throttle_4 = 0;
-
- actual_throttle_dt = 0.01;
- average_throttle_dt = 0.01;
- average_throttle_dt_k = 0.0125;
-
- actual_rx_delta_ms = 0.1;
- actual_rx_wait_ms = 0.1;
+
+ target_main_dt = TARGET_MAIN_DT;
+ actual_main_dt = TARGET_MAIN_DT;
+ average_main_dt = TARGET_MAIN_DT;
+ average_main_dt_k = AVERAGE_DT_K_GAIN;
+
+ target_tx_dt = TARGET_TX_DT;
+ actual_tx_dt = TARGET_TX_DT;
+ average_tx_dt = TARGET_TX_DT;
+ average_tx_dt_k = AVERAGE_DT_K_GAIN;
+
+ target_rx_dt = TARGET_RX_DT;
+ actual_rx_dt = TARGET_RX_DT;
+ average_rx_dt = TARGET_RX_DT;
+ average_rx_dt_k = AVERAGE_DT_K_GAIN;
+
+ target_imu_dt = TARGET_IMU_DT;
+ actual_imu_dt = TARGET_IMU_DT;
+ average_imu_dt = TARGET_IMU_DT;
+ average_imu_dt_k = AVERAGE_DT_K_GAIN;
estimated_altitude = 0;
estimated_rotation_q1 = 0;
@@ -89,20 +123,214 @@
target_rotation_p = 0;
target_rotation_r = 0;
- target_position_x_isRequired = false;
- target_position_y_isRequired = false;
- target_position_z_isRequired = false;
- target_rotation_y_isRequired = false;
- target_rotation_p_isRequired = false;
- target_rotation_r_isRequired = false;
+ target_position_x_isRequired = 0;
+ target_position_y_isRequired = 0;
+ target_position_z_isRequired = 0;
+ target_rotation_y_isRequired = 0;
+ target_rotation_p_isRequired = 0;
+ target_rotation_r_isRequired = 0;
+
+ ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 1;
+ }
+
+#ifndef QUAD_STATE_NO_JSON
+ /**
+ * @return number of setted members.
+ */
+ int setFromJSON(const char* JSON_frame){
+ int setted = 0;
+ picojson::value json;
+ picojson::parse(json, JSON_frame, JSON_frame + strlen(JSON_frame));
+
+ unsigned int i=0;
+
+#define JSON_SET(var) { \
+ picojson::value j = json.get(i++); \
+ if(j.is<double>()){ \
+ var = j.get<double>(); \
+ ++setted; \
+ } \
+ }
- ZERO_ALL_MOTORS_NOW__DANGER = false;
+ JSON_SET(requested_throttle_1)
+ JSON_SET(requested_throttle_2)
+ JSON_SET(requested_throttle_3)
+ JSON_SET(requested_throttle_4)
+
+ JSON_SET(actual_throttle_1)
+ JSON_SET(actual_throttle_2)
+ JSON_SET(actual_throttle_3)
+ JSON_SET(actual_throttle_4)
+
+ JSON_SET(target_main_dt)
+ JSON_SET(actual_main_dt)
+ JSON_SET(average_main_dt)
+ JSON_SET(average_main_dt_k)
+
+ JSON_SET(target_tx_dt)
+ JSON_SET(actual_tx_dt)
+ JSON_SET(average_tx_dt)
+ JSON_SET(average_tx_dt_k)
+
+ JSON_SET(target_rx_dt)
+ JSON_SET(actual_rx_dt)
+ JSON_SET(average_rx_dt)
+ JSON_SET(average_rx_dt_k)
+
+ JSON_SET(target_imu_dt)
+ JSON_SET(actual_imu_dt)
+ JSON_SET(average_imu_dt)
+ JSON_SET(average_imu_dt_k)
+
+ JSON_SET(estimated_altitude)
+ JSON_SET(estimated_rotation_q1)
+ JSON_SET(estimated_rotation_q2)
+ JSON_SET(estimated_rotation_q3)
+ JSON_SET(estimated_rotation_q4)
+
+ JSON_SET(estimated_position_x)
+ JSON_SET(estimated_position_y)
+ JSON_SET(estimated_position_z)
+ JSON_SET(estimated_rotation_y)
+ JSON_SET(estimated_rotation_p)
+ JSON_SET(estimated_rotation_r)
+
+ JSON_SET(target_position_x)
+ JSON_SET(target_position_y)
+ JSON_SET(target_position_z)
+ JSON_SET(target_rotation_y)
+ JSON_SET(target_rotation_p)
+ JSON_SET(target_rotation_r)
+
+ JSON_SET(target_position_x_isRequired)
+ JSON_SET(target_position_y_isRequired)
+ JSON_SET(target_position_z_isRequired)
+ JSON_SET(target_rotation_y_isRequired)
+ JSON_SET(target_rotation_p_isRequired)
+ JSON_SET(target_rotation_r_isRequired)
+
+ JSON_SET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER)
+
+#undef JSON_SET
+
+ return setted;
}
+
+ std::string getJSON() const{
+
+ picojson::array array_vector;
+
+#define JSON_GET(var) array_vector.push_back(picojson::value(var));
+
+
+ JSON_GET(requested_throttle_1)
+ JSON_GET(requested_throttle_2)
+ JSON_GET(requested_throttle_3)
+ JSON_GET(requested_throttle_4)
+
+ JSON_GET(actual_throttle_1)
+ JSON_GET(actual_throttle_2)
+ JSON_GET(actual_throttle_3)
+ JSON_GET(actual_throttle_4)
+
+ JSON_GET(target_main_dt)
+ JSON_GET(actual_main_dt)
+ JSON_GET(average_main_dt)
+ JSON_GET(average_main_dt_k)
+
+ JSON_GET(target_tx_dt)
+ JSON_GET(actual_tx_dt)
+ JSON_GET(average_tx_dt)
+ JSON_GET(average_tx_dt_k)
+
+ JSON_GET(target_rx_dt)
+ JSON_GET(actual_rx_dt)
+ JSON_GET(average_rx_dt)
+ JSON_GET(average_rx_dt_k)
+
+ JSON_GET(target_imu_dt)
+ JSON_GET(actual_imu_dt)
+ JSON_GET(average_imu_dt)
+ JSON_GET(average_imu_dt_k)
+
+ JSON_GET(estimated_altitude)
+ JSON_GET(estimated_rotation_q1)
+ JSON_GET(estimated_rotation_q2)
+ JSON_GET(estimated_rotation_q3)
+ JSON_GET(estimated_rotation_q4)
+
+ JSON_GET(estimated_position_x)
+ JSON_GET(estimated_position_y)
+ JSON_GET(estimated_position_z)
+ JSON_GET(estimated_rotation_y)
+ JSON_GET(estimated_rotation_p)
+ JSON_GET(estimated_rotation_r)
+
+ JSON_GET(target_position_x)
+ JSON_GET(target_position_y)
+ JSON_GET(target_position_z)
+ JSON_GET(target_rotation_y)
+ JSON_GET(target_rotation_p)
+ JSON_GET(target_rotation_r)
+
+ JSON_GET(target_position_x_isRequired)
+ JSON_GET(target_position_y_isRequired)
+ JSON_GET(target_position_z_isRequired)
+ JSON_GET(target_rotation_y_isRequired)
+ JSON_GET(target_rotation_p_isRequired)
+ JSON_GET(target_rotation_r_isRequired)
+
+ JSON_GET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER)
+
+#undef JSON_GET
+
+ return picojson::value(array_vector).serialize();
+ }
+#endif
+
};
-struct QuadCommandHeader; //to be used with TCP (or UDP for control signals)
-struct QuadCommandHeader {
- uint8_t xor_name;
- uint8_t xor_name_repeat;
- uint32_t payload_length;
-};
+
+///Calc dt in seconds.
+#define QUAD_STATE_READ_ACTUAL_DT(state, what, timer) { \
+ state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us(); \
+}
+
+///Sleep until the setted DT is reached.
+#define QUAD_STATE_WAIT_DT_TARGET(actual, target) { \
+ if(target-actual > 0.01*target && target-actual > 0.0001){ \
+ Thread::wait((uint32_t) (1000.0*target-1000.0*actual)); \
+ } else \
+ Thread::yield(); \
+}
+
+///Calc actual dt and update average
+#define QUAD_STATE_UPDATE_DT(state, what, timer) { \
+ state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us(); \
+ timer.reset(); \
+ state.average_ ## what ## _dt *= 0.9999999999999999 - state.average_ ## what ## _dt_k; \
+ state.average_ ## what ## _dt += state.actual_ ## what ## _dt * state.average_ ## what ## _dt_k; \
+}
+
+
+/*
+ //Calc dt in seconds
+ #define DT_READ() mainQuadState.actual_throttle_dt = (float) (1.0/1000000.0) * dt_t.read_us()
+ DT_READ();
+ if(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt > MAIN_CYCLE_TARGET_DT/10){
+ //if the dt is very small
+ //sleep until the setted DT is reached.
+ Thread::wait(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt);
+ DT_READ();
+ }
+ dt_timer.reset();
+
+ //Calc average dt
+ mainQuadState.average_throttle_dt -= mainQuadState.average_throttle_dt * mainQuadState.average_throttle_dt_k;
+ mainQuadState.average_throttle_dt += mainQuadState.actual_throttle_dt * mainQuadState.average_throttle_dt_k;
+*/
+
+
+
+
+#endif //QUAD_STATE__H
\ No newline at end of file