diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 2 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 5 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.h | 2 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 4 |
5 files changed, 10 insertions, 8 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d876f1a39..59f4b10be 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -56,7 +56,7 @@ ECL_PitchController::ECL_PitchController() : { } -float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, +float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ @@ -108,7 +108,8 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc float pitch_error = pitch_setpoint - pitch; /* rate setpoint from current error and time constant */ - _rate_setpoint = pitch_error / _tc; + float theta_dot_setpoint = pitch_error / _tc; + _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian /* add turn offset */ _rate_setpoint += turn_offset; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 1e6cec6a1..41aa48677 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -56,7 +56,7 @@ class __EXPORT ECL_PitchController public: ECL_PitchController(); - float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, + float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index b9a73fc02..c42e1856a 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -58,7 +58,7 @@ ECL_RollController::ECL_RollController() : } -float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, +float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ @@ -79,7 +79,8 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra } float roll_error = roll_setpoint - roll; - _rate_setpoint = roll_error / _tc; + float phi_dot_setpoint = roll_error / _tc; + _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian /* limit the rate */ if (_max_rate > 0.01f) { diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 0d4ea9333..e19812ea8 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -56,7 +56,7 @@ class __EXPORT ECL_RollController public: ECL_RollController(); - float control(float roll_setpoint, float roll, float roll_rate, + float control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate, float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index eb67874db..c8a59356c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -635,11 +635,11 @@ FixedwingAttitudeControl::task_main() } } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, _att.pitch, _att.yawspeed, airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; - float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, + float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, _att.yawspeed, airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; |