forked from Archive/PX4-Autopilot
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:
parent
a01ca427b0
commit
39732abf1f
|
@ -96,15 +96,21 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
|
||||||
|
|
||||||
/* input conditioning */
|
/* input conditioning */
|
||||||
float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
|
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
|
/* Calculate desired y-axis angular rate needed to compensate for roll angle.
|
||||||
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
|
For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
|
||||||
tanf(roll) * sinf(roll)) * _roll_ff;
|
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) {
|
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 */
|
/* Calculate the error */
|
||||||
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
|
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 +
|
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
|
||||||
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
|
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
|
||||||
|
|
||||||
/* Transform estimation to body angular rates (jacobian) */
|
_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;
|
||||||
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;
|
|
||||||
|
|
||||||
if (!lock_integrator && _k_i > 0.0f) {
|
if (!lock_integrator && _k_i > 0.0f) {
|
||||||
|
|
||||||
|
|
|
@ -109,11 +109,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
|
||||||
/* Transform setpoint to body angular rates (jacobian) */
|
/* Transform setpoint to body angular rates (jacobian) */
|
||||||
_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
|
_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 */
|
/* 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) {
|
if (!lock_integrator && _k_i > 0.0f) {
|
||||||
|
|
||||||
|
|
|
@ -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)));
|
_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 */
|
/* 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) {
|
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue