Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
state.h
- Committer:
- MatteoT
- Date:
- 2014-05-24
- Revision:
- 7:cda17cffec3c
- Parent:
- 6:4ddd68260f76
File content as of revision 7:cda17cffec3c:
#ifndef QUAD_STATE__H #define QUAD_STATE__H #include <stdint.h> #ifndef QUAD_STATE_NO_JSON #endif //this struct must be the same for all the programs communicating with the Quad. struct QuadState; struct QuadState { //double[76] double reference_throttle_1; double reference_throttle_2; double reference_throttle_3; double reference_throttle_4; double actual_throttle_1; double actual_throttle_2; double actual_throttle_3; double actual_throttle_4; double pid_rotation_r_error; double pid_rotation_r_error_integrative; double pid_rotation_r_error_derivative; double pid_rotation_r_kP; double pid_rotation_r_kI; double pid_rotation_r_kD; double pid_rotation_r_out; double pid_rotation_p_error; double pid_rotation_p_error_integrative; double pid_rotation_p_error_derivative; double pid_rotation_p_kP; double pid_rotation_p_kI; double pid_rotation_p_kD; double pid_rotation_p_out; double pid_rotation_y_error; double pid_rotation_y_error_integrative; double pid_rotation_y_error_derivative; double pid_rotation_y_kP; double pid_rotation_y_kI; double pid_rotation_y_kD; double pid_rotation_y_out; double target_main_dt; double actual_main_dt; double average_main_dt; double average_main_dt_k; double target_tx_dt; double actual_tx_dt; double average_tx_dt; double average_tx_dt_k; double target_rx_dt; double actual_rx_dt; double average_rx_dt; double average_rx_dt_k; 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; double estimated_position_x; double estimated_position_y; double estimated_position_z; double estimated_rotation_y; double estimated_rotation_p; double estimated_rotation_r; double sensor_accel_x; double sensor_accel_y; double sensor_accel_z; double sensor_gyro_r; double sensor_gyro_p; double sensor_gyro_y; 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; 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 timestamp; double ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER; //1=OK; 0=EmergencyStop; error otherwise static int length () { return 76; } void reset (){ reference_throttle_1 = -1000; reference_throttle_2 = -1000; reference_throttle_3 = -1000; reference_throttle_4 = -1000; actual_throttle_1 = 0; actual_throttle_2 = 0; actual_throttle_3 = 0; actual_throttle_4 = 0; pid_rotation_r_error = 0; pid_rotation_r_error_integrative = 0; pid_rotation_r_error_derivative = 0; pid_rotation_r_kP = 0.3; pid_rotation_r_kI = 0; pid_rotation_r_kD = 1.2; pid_rotation_r_out = 0; pid_rotation_p_error = 0; pid_rotation_p_error_integrative = 0; pid_rotation_p_error_derivative = 0; pid_rotation_p_kP = 0.3; pid_rotation_p_kI = 0; pid_rotation_p_kD = 1.2; pid_rotation_p_out = 0; pid_rotation_y_error = 0; pid_rotation_y_error_integrative = 0; pid_rotation_y_error_derivative = 0; pid_rotation_y_kP = 0.3; pid_rotation_y_kI = 0; pid_rotation_y_kD = 0.1; pid_rotation_y_out = 0; 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; estimated_rotation_q2 = 0; estimated_rotation_q3 = 0; estimated_rotation_q4 = 0; estimated_position_x = 0; estimated_position_y = 0; estimated_position_z = 0; estimated_rotation_y = 0; estimated_rotation_p = 0; estimated_rotation_r = 0; sensor_accel_x = 0; sensor_accel_y = 0; sensor_accel_z = 0; sensor_gyro_r = 0; sensor_gyro_p = 0; sensor_gyro_y = 0; target_position_x = 0; target_position_y = 0; target_position_z = 0; target_rotation_y = 0; target_rotation_p = 0; target_rotation_r = 0; 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; timestamp = 0; ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; } #ifndef QUAD_STATE_NO_JSON /** Sets the QuadState by parsing a JSON string. * @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; \ } \ } */ #define JSON_SET(var) , & var 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]" JSON_SET(reference_throttle_1) JSON_SET(reference_throttle_2) JSON_SET(reference_throttle_3) JSON_SET(reference_throttle_4) JSON_SET(actual_throttle_1) JSON_SET(actual_throttle_2) JSON_SET(actual_throttle_3) JSON_SET(actual_throttle_4) JSON_SET(pid_rotation_r_error) JSON_SET(pid_rotation_r_error_integrative) JSON_SET(pid_rotation_r_error_derivative) JSON_SET(pid_rotation_r_kP) JSON_SET(pid_rotation_r_kI) JSON_SET(pid_rotation_r_kD) JSON_SET(pid_rotation_r_out) JSON_SET(pid_rotation_p_error) JSON_SET(pid_rotation_p_error_integrative) JSON_SET(pid_rotation_p_error_derivative) JSON_SET(pid_rotation_p_kP) JSON_SET(pid_rotation_p_kI) JSON_SET(pid_rotation_p_kD) JSON_SET(pid_rotation_p_out) JSON_SET(pid_rotation_y_error) JSON_SET(pid_rotation_y_error_integrative) JSON_SET(pid_rotation_y_error_derivative) JSON_SET(pid_rotation_y_kP) JSON_SET(pid_rotation_y_kI) JSON_SET(pid_rotation_y_kD) JSON_SET(pid_rotation_y_out) 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(sensor_accel_x) JSON_SET(sensor_accel_y) JSON_SET(sensor_accel_z) JSON_SET(sensor_gyro_r) JSON_SET(sensor_gyro_p) JSON_SET(sensor_gyro_y) 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(timestamp) JSON_SET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER) ); #undef JSON_SET //return 76; } /** Prints the QuadState in JSON format on the specified buffer. * @return total number of characters written. On failure, a negative number is returned. */ int getJSON(char * buffer) const{ //picojson::array array_vector; //#define JSON_GET(var) array_vector.push_back(picojson::value(var)); #define JSON_GET(var) ,var 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" JSON_GET(reference_throttle_1) JSON_GET(reference_throttle_2) JSON_GET(reference_throttle_3) JSON_GET(reference_throttle_4) JSON_GET(actual_throttle_1) JSON_GET(actual_throttle_2) JSON_GET(actual_throttle_3) JSON_GET(actual_throttle_4) JSON_GET(pid_rotation_r_error) JSON_GET(pid_rotation_r_error_integrative) JSON_GET(pid_rotation_r_error_derivative) JSON_GET(pid_rotation_r_kP) JSON_GET(pid_rotation_r_kI) JSON_GET(pid_rotation_r_kD) JSON_GET(pid_rotation_r_out) JSON_GET(pid_rotation_p_error) JSON_GET(pid_rotation_p_error_integrative) JSON_GET(pid_rotation_p_error_derivative) JSON_GET(pid_rotation_p_kP) JSON_GET(pid_rotation_p_kI) JSON_GET(pid_rotation_p_kD) JSON_GET(pid_rotation_p_out) JSON_GET(pid_rotation_y_error) JSON_GET(pid_rotation_y_error_integrative) JSON_GET(pid_rotation_y_error_derivative) JSON_GET(pid_rotation_y_kP) JSON_GET(pid_rotation_y_kI) JSON_GET(pid_rotation_y_kD) JSON_GET(pid_rotation_y_out) 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(sensor_accel_x) JSON_GET(sensor_accel_y) JSON_GET(sensor_accel_z) JSON_GET(sensor_gyro_r) JSON_GET(sensor_gyro_p) JSON_GET(sensor_gyro_y) 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(timestamp) JSON_GET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER) ); #undef JSON_GET //return picojson::value(array_vector).serialize(); } #endif }; ///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.001*target && target-actual > 0.00001){*/ \ Thread::wait((double) 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