diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 9dd1bd0893..b8611d9ff2 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -75,6 +75,7 @@ FixedwingAttitudeControl::parameters_update() _proportional_gain = matrix::Vector3f(1.0f / _param_fw_r_tc.get(), 1.0f / _param_fw_p_tc.get(), 0.0f); + _yaw_w = _param_fw_yaw_weight.get(); _wheel_ctrl.set_k_p(_param_fw_wr_p.get()); _wheel_ctrl.set_k_i(_param_fw_wr_i.get()); @@ -85,7 +86,7 @@ FixedwingAttitudeControl::parameters_update() } void -FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body, const float roll) +FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) { if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) { @@ -103,7 +104,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body, const float pitch_body = constrain(pitch_body, -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); - float yaw_sp = yaw_body + sinf(roll) * 0.0f; + float yaw_sp = yaw_body; const Quatf q(Eulerf(roll_body, pitch_body, yaw_sp)); q.copyTo(_att_sp.q_d); @@ -254,7 +255,7 @@ void FixedwingAttitudeControl::Run() const matrix::Eulerf euler_angles(_R); - vehicle_manual_poll(euler_angles.psi(), euler_angles.phi()); + vehicle_manual_poll(euler_angles.psi()); vehicle_attitude_setpoint_poll(); @@ -297,14 +298,13 @@ void FixedwingAttitudeControl::Run() if (q_sp.isAllFinite()) { Quatf q_current(att.q); - Quatf q_err = (q_current.inversed() * q_sp).canonical(); Vector3f e = 2.f * q_err.imag(); Vector3f body_rates_setpoint; body_rates_setpoint(0) = _proportional_gain(0) * e(0); - body_rates_setpoint(1) = _proportional_gain(1) * e(1); - body_rates_setpoint(2) = 0.f; // yaw handled later + body_rates_setpoint(1) = _proportional_gain(1) * e(1) - 6.f * _yaw_w * e(2) * tanf(euler_angles.phi()); + body_rates_setpoint(2) = 0.f; // yaw handled through turn coordination and manual yaw input // Turn coordination const float V = math::max(get_airspeed_constrained(), 0.1f); @@ -312,7 +312,6 @@ void FixedwingAttitudeControl::Run() body_rates_setpoint(2) = r_tc_ff; - body_rates_setpoint(0) = math::constrain(body_rates_setpoint(0), -radians(_param_fw_r_rmax.get()), radians(_param_fw_r_rmax.get())); body_rates_setpoint(1) = math::constrain(body_rates_setpoint(1), -radians(_param_fw_p_rmax_neg.get()), radians(_param_fw_p_rmax_pos.get())); body_rates_setpoint(2) = math::constrain(body_rates_setpoint(2), -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get())); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 6884f7cdd6..e0c4e10eac 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -157,15 +157,17 @@ private: (ParamFloat) _param_fw_wr_imax, (ParamFloat) _param_fw_wr_p, + (ParamFloat) _param_fw_yaw_weight, (ParamFloat) _param_fw_y_rmax, (ParamFloat) _param_man_yr_max ) matrix::Vector3f _proportional_gain; + float _yaw_w{1.0f}; WheelController _wheel_ctrl; void parameters_update(); - void vehicle_manual_poll(const float yaw_body, const float roll); + void vehicle_manual_poll(const float yaw_body); void vehicle_attitude_setpoint_poll(); void vehicle_land_detected_poll(); float get_airspeed_constrained(); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 554eac360c..bf018a3776 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -197,6 +197,21 @@ PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f); */ PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); +/** + * Yaw weight + * + * Scales yaw-error correction through the pitch-rate setpoint in non-linear attitude control. + * + * For yaw control tuning use FW_Y_RMAX and rate controller tuning. This ratio has no impact on yaw gain. + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.1 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_YAW_WEIGHT, 1.0f); + /** * Pitch setpoint offset (pitch at level flight) * diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index 30d91d2b25..e6cbedfea2 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -299,7 +299,7 @@ void FwLateralLongitudinalControl::Run() float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f; float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f; - float yaw_body = _yaw + sinf(roll_body) * 0.45f; + float yaw_body = _yaw; const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f; @@ -610,7 +610,6 @@ void FwLateralLongitudinalControl::updateAttitude() { const Eulerf euler_angles(R); _long_control_state.pitch_rad = euler_angles.theta(); _yaw = euler_angles.psi(); - _roll = euler_angles.phi(); // load factor due to banking const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp index 7ded9002ac..b2205bd5a1 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -192,7 +192,6 @@ private: hrt_abstime _time_wind_last_received{0}; SlewRate _roll_slew_rate; float _yaw{0.f}; - float _roll{0.f}; struct lateral_control_state { matrix::Vector2f ground_speed; matrix::Vector2f wind_speed;