fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 22:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw att control: also transform rate estimate

Browse Source
This commit is contained in:
Thomas Gubler
2013-10-24 17:39:27 +02:00
parent 1c57d7de43
commit cb5e5e9143
2 changed files with 17 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+8 -3
View File
@@ -107,14 +107,19 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
turn_offset = -turn_offset;
float pitch_error = pitch_setpoint - pitch;
/* rate setpoint from current error and time constant */
/* /* Apply P controller: rate setpoint from current error and time constant */
float theta_dot_setpoint = pitch_error / _tc;
_rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
/* Transform setpoint to body angular rates */
_rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian //XXX: use desired yaw_rate?
/* add turn offset */
_rate_setpoint += turn_offset;
_rate_error = _rate_setpoint - pitch_rate;
/* Transform estimation to body angular rates */
float pitch_rate_body = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
_rate_error = _rate_setpoint - pitch_rate_body;
float ilimit_scaled = 0.0f;
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+9 -2
View File
@@ -79,8 +79,12 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
}
float roll_error = roll_setpoint - roll;
/* Apply P controller */
float phi_dot_setpoint = roll_error / _tc;
_rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian
/* Transform setpoint to body angular rates */
_rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian //XXX: use desired yaw_rate?
/* limit the rate */
if (_max_rate > 0.01f) {
@@ -88,8 +92,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
_rate_error = _rate_setpoint - roll_rate;
/* Transform estimation to body angular rates */
float roll_rate_body = roll_rate - sinf(pitch) * yaw_rate; //jacobian
/* Calculate body angular rate error */
_rate_error = _rate_setpoint - roll_rate_body; //body angular rate error
float ilimit_scaled = 0.0f;