diff --git a/src/lib/rate_control/rate_control.cpp b/src/lib/rate_control/rate_control.cpp index 77b776ba11..277009c92d 100644 --- a/src/lib/rate_control/rate_control.cpp +++ b/src/lib/rate_control/rate_control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,7 +40,7 @@ using namespace matrix; -void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +void RateControl::setPidGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { _gain_p = P; _gain_i = I; diff --git a/src/lib/rate_control/rate_control.hpp b/src/lib/rate_control/rate_control.hpp index 1db18ee092..f43493c55d 100644 --- a/src/lib/rate_control/rate_control.hpp +++ b/src/lib/rate_control/rate_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,12 +51,12 @@ public: ~RateControl() = default; /** - * Set the rate control gains + * Set the rate control PID gains * @param P 3D vector of proportional gains for body x,y,z axis * @param I 3D vector of integral gains * @param D 3D vector of derivative gains */ - void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + void setPidGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); /** * Set the mximum absolute value of the integrator for all axes @@ -94,6 +94,18 @@ public: */ void resetIntegral() { _rate_int.zero(); } + /** + * Set the integral term to 0 for specific axes + * @param axis roll 0 / pitch 1 / yaw 2 + * @see _rate_int + */ + void resetIntegral(size_t axis) + { + if (axis < 3) { + _rate_int(axis) = 0.f; + } + } + /** * Get status message of controller for logging/debugging * @param rate_ctrl_status status message to fill with internal states diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 5368ca563d..7948e6eb11 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -77,7 +77,7 @@ FixedwingRateControl::parameters_update() const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get()); const Vector3f rate_d = Vector3f(_param_fw_rr_d.get(), _param_fw_pr_d.get(), _param_fw_yr_d.get()); - _rate_control.setGains(rate_p, rate_i, rate_d); + _rate_control.setPidGains(rate_p, rate_i, rate_d); _rate_control.setIntegratorLimit( Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get())); @@ -335,27 +335,36 @@ void FixedwingRateControl::Run() body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll); } - /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + // Run attitude RATE controllers which need the desired attitudes from above, add trim. const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, _landed); - float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0); - float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward; - _vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; - - float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1); - float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward; - _vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; - + const float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0); + const float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1); const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2); - float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; - _vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw; + const float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward; + const float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward; + + // Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw + float yaw_u = 0.f; + + if (_vcontrol_mode.flag_control_attitude_enabled || _param_fw_acro_yaw_en.get()) { + yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward; + + } else { + yaw_u = _manual_control_setpoint.yaw * _param_fw_man_y_sc.get(); + _rate_control.resetIntegral(2); + } if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) { _rate_control.resetIntegral(); } + _vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll; + _vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch; + _vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw; + /* throttle passed through if it is finite */ _vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f; diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index 8a9a436274..37c70a800a 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -148,6 +148,7 @@ private: (ParamFloat) _param_fw_acro_x_max, (ParamFloat) _param_fw_acro_y_max, (ParamFloat) _param_fw_acro_z_max, + (ParamInt) _param_fw_acro_yaw_en, (ParamFloat) _param_fw_airspd_max, (ParamFloat) _param_fw_airspd_min, diff --git a/src/modules/fw_rate_control/fw_rate_control_params.c b/src/modules/fw_rate_control/fw_rate_control_params.c index 2e0f6e274c..6b3e6f4c12 100644 --- a/src/modules/fw_rate_control/fw_rate_control_params.c +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -537,3 +537,16 @@ PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f); * @group FW Attitude Control */ PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0); + +/** + * Enable yaw rate controller in Acro + * + * If this parameter is set to 1, the yaw rate controller is enabled in Fixed-wing Acro mode. + * Otherwise the pilot commands directly the yaw actuator. + * It is disabled by default because an active yaw rate controller will fight against the + * natural turn coordination of the plane. + * + * @boolean + * @group FW Rate Control + */ +PARAM_DEFINE_INT32(FW_ACRO_YAW_EN, 0); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index d580dcf8ae..e8a371d1e2 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -80,7 +80,7 @@ MulticopterRateControl::parameters_updated() // to the ideal (K * [1 + 1/sTi + sTd]) form const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); - _rate_control.setGains( + _rate_control.setPidGains( rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())), rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get())));