dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Changed ecl_roll/pitch/yaw_controller.cpp to no longer transform ctl_data.angle_rate to angular acceleration as the angular rates are already angular accelerations as calculated by ekf_att_pos_estimator

This commit is contained in:
Eddy Scott 2015-08-11 11:14:35 -04:00 committed by Lorenz Meier
parent a01ca427b0
commit 39732abf1f
3 changed files with 14 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -96,15 +96,21 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
/* input conditioning */
float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
/* calculate the offset in the rate resulting from rolling */
//xxx needs explanation and conversion to body angular rates or should be removed
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
/* Calculate desired y-axis angular rate needed to compensate for roll angle.
For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
Availible on google books 8/11/2015:
https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(roll) * sinf(roll)));
if (inverted) {
turn_offset = -turn_offset;
body_fixed_turn_offset = -body_fixed_turn_offset;
}
/*Finally transform the body_fixed_turn offset to a change pitch angular rate */
float turn_offset = cosf(roll)*body_fixed_turn_offset-sinf(roll)*ctl_data.yaw_rate;
/* Calculate the error */
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
@ -160,11 +166,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
/* Transform estimation to body angular rates (jacobian) */
float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate;
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;
if (!lock_integrator && _k_i > 0.0f) {

5
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -109,11 +109,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
/* Transform estimation to body angular rates (jacobian) */
float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
_rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f) {

6
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -198,12 +198,8 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
}
/* Transform estimation to body angular rates (jacobian) */
float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
_rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {