mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-16 12:11:35 +08:00
Fix heightrate feedforard for fixdwings
This commit is contained in:
+15
-3
@@ -148,6 +148,8 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference.
|
// Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference.
|
||||||
|
_alt_rate_ref = setpoint.alt_rate;
|
||||||
|
|
||||||
if (PX4_ISFINITE(setpoint.alt_rate)) {
|
if (PX4_ISFINITE(setpoint.alt_rate)) {
|
||||||
_alt_rate_ref = setpoint.alt_rate;
|
_alt_rate_ref = setpoint.alt_rate;
|
||||||
|
|
||||||
@@ -317,9 +319,17 @@ float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const In
|
|||||||
float TECSControl::_calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param ¶m) const
|
float TECSControl::_calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param ¶m) const
|
||||||
{
|
{
|
||||||
float altitude_rate_output;
|
float altitude_rate_output;
|
||||||
altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain +
|
|
||||||
param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate + setpoint.altitude_rate_setpoint;
|
if (PX4_ISFINITE(input.altitude_rate_sp)) {
|
||||||
altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
|
// Control only altitude rate if a valid setpoint is specified
|
||||||
|
altitude_rate_output = input.altitude_rate_sp;
|
||||||
|
altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain +
|
||||||
|
param.altitude_setpoint_gain_ff * setpoint.altitude_reference.alt_rate + setpoint.altitude_rate_setpoint;
|
||||||
|
altitude_rate_output = math::constrain(altitude_rate_output, -param.max_sink_rate, param.max_climb_rate);
|
||||||
|
}
|
||||||
|
|
||||||
return altitude_rate_output;
|
return altitude_rate_output;
|
||||||
}
|
}
|
||||||
@@ -650,6 +660,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
|
|||||||
|
|
||||||
const TECSControl::Input control_input{ .altitude = altitude,
|
const TECSControl::Input control_input{ .altitude = altitude,
|
||||||
.altitude_rate = altitude_rate,
|
.altitude_rate = altitude_rate,
|
||||||
|
.altitude_rate_sp = 0.0f,
|
||||||
.tas = eas_to_tas * equivalent_airspeed,
|
.tas = eas_to_tas * equivalent_airspeed,
|
||||||
.tas_rate = 0.0f};
|
.tas_rate = 0.0f};
|
||||||
|
|
||||||
@@ -724,6 +735,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
|
|||||||
|
|
||||||
const TECSControl::Input control_input{ .altitude = altitude,
|
const TECSControl::Input control_input{ .altitude = altitude,
|
||||||
.altitude_rate = hgt_rate,
|
.altitude_rate = hgt_rate,
|
||||||
|
.altitude_rate_sp = hgt_rate_sp,
|
||||||
.tas = eas_to_tas * eas.speed,
|
.tas = eas_to_tas * eas.speed,
|
||||||
.tas_rate = eas_to_tas * eas.speed_rate};
|
.tas_rate = eas_to_tas * eas.speed_rate};
|
||||||
|
|
||||||
|
|||||||
@@ -260,6 +260,7 @@ public:
|
|||||||
struct Input {
|
struct Input {
|
||||||
float altitude; ///< Current altitude amsl of the UAS [m].
|
float altitude; ///< Current altitude amsl of the UAS [m].
|
||||||
float altitude_rate; ///< Current altitude rate of the UAS [m/s].
|
float altitude_rate; ///< Current altitude rate of the UAS [m/s].
|
||||||
|
float altitude_rate_sp; ///< Current altitude rate setpoint [m/s]
|
||||||
float tas; ///< Current true airspeed of the UAS [m/s].
|
float tas; ///< Current true airspeed of the UAS [m/s].
|
||||||
float tas_rate; ///< Current true airspeed rate of the UAS [m/s²].
|
float tas_rate; ///< Current true airspeed rate of the UAS [m/s²].
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1119,7 +1119,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
|
|||||||
tecs_fw_thr_min,
|
tecs_fw_thr_min,
|
||||||
tecs_fw_thr_max,
|
tecs_fw_thr_max,
|
||||||
_param_sinkrate_target.get(),
|
_param_sinkrate_target.get(),
|
||||||
_param_climbrate_target.get());
|
_param_climbrate_target.get(),
|
||||||
|
false,
|
||||||
|
pos_sp_curr.vz);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
|||||||
Reference in New Issue
Block a user