Matteo Terruzzi
Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
Diff: state.h
- Revision:
- 4:46f3c3dd5977
- Parent:
- 3:5e7476bca25f
- Child:
- 5:33abcc31b0aa
--- a/state.h Sun Oct 27 01:05:35 2013 +0000
+++ b/state.h Sat May 03 13:22:30 2014 +0000
@@ -11,128 +11,195 @@
//this struct must be the same for all the programs communicating with the Quad.
struct QuadState;
struct QuadState {
- double requested_throttle_1; //negative if not requested
- double requested_throttle_2;
- double requested_throttle_3;
- double requested_throttle_4;
+
+ 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
-
-
+
+
void reset (){
-
- requested_throttle_1 = -1;
- requested_throttle_2 = -1;
- requested_throttle_3 = -1;
- requested_throttle_4 = -1;
+
+ 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;
-
- ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 1;
+
+ timestamp = 0;
+
+ ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0;
}
-
+
#ifndef QUAD_STATE_NO_JSON
/**
* @return number of setted members.
@@ -141,7 +208,7 @@
int setted = 0;
picojson::value json;
picojson::parse(json, JSON_frame, JSON_frame + strlen(JSON_frame));
-
+
unsigned int i=0;
#define JSON_SET(var) { \
@@ -151,139 +218,205 @@
++setted; \
} \
}
-
- JSON_SET(requested_throttle_1)
- JSON_SET(requested_throttle_2)
- JSON_SET(requested_throttle_3)
- JSON_SET(requested_throttle_4)
+
+ 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 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(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
@@ -324,7 +457,7 @@
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;