forked from Archive/PX4-Autopilot
FW Rate Controller: allow to enable/disable yaw axis in Acro (#21566)
* RateControl: rename setGains to setPidGains to be more precise Signed-off-by: Silvan Fuhrer <silvan@auterion.com> * FW Rate controller: only allow to disable Yaw in Acro, not roll and pitch Signed-off-by: Silvan Fuhrer <silvan@auterion.com> --------- Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
parent
ee96d209d7
commit
ad769db8d6
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -148,6 +148,7 @@ private:
|
|||
(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
|
||||
(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
|
||||
(ParamFloat<px4::params::FW_ACRO_Z_MAX>) _param_fw_acro_z_max,
|
||||
(ParamInt<px4::params::FW_ACRO_YAW_EN>) _param_fw_acro_yaw_en,
|
||||
|
||||
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
|
||||
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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())));
|
||||
|
|
Loading…
Reference in New Issue