dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

TECS:use tas_setpoint instead of measured tas for specific kinetic energy calculation

This commit is contained in:
Konrad 2024-02-13 14:57:08 +01:00
parent 80677cfdfd
commit bec3bedaae
2 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/lib/tecs/TECS.cpp
View File

@ -231,6 +231,8 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param
AltitudePitchControl control_setpoint; AltitudePitchControl control_setpoint;
control_setpoint.tas_setpoint = setpoint.tas_setpoint;
control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag); control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param); control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
@ -274,6 +276,7 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &
AltitudePitchControl control_setpoint; AltitudePitchControl control_setpoint;
control_setpoint.tas_setpoint = setpoint.tas_setpoint;
control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag); control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
if (PX4_ISFINITE(setpoint.altitude_rate_setpoint_direct)) { if (PX4_ISFINITE(setpoint.altitude_rate_setpoint_direct)) {
@ -350,7 +353,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt
// Calculate specific energy rate demands in units of (m**2/sec**3) // Calculate specific energy rate demands in units of (m**2/sec**3)
specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint * specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint *
CONSTANTS_ONE_G; // potential energy rate of change CONSTANTS_ONE_G; // potential energy rate of change
specific_energy_rates.ske_rate.setpoint = input.tas * specific_energy_rates.ske_rate.setpoint = control_setpoint.tas_setpoint *
control_setpoint.tas_rate_setpoint; // kinetic energy rate of change control_setpoint.tas_rate_setpoint; // kinetic energy rate of change
// Calculate specific energy rates in units of (m**2/sec**3) // Calculate specific energy rates in units of (m**2/sec**3)

1
src/lib/tecs/TECS.hpp
View File

@ -366,6 +366,7 @@ private:
struct AltitudePitchControl { struct AltitudePitchControl {
float altitude_rate_setpoint; ///< Controlled altitude rate setpoint [m/s]. float altitude_rate_setpoint; ///< Controlled altitude rate setpoint [m/s].
float tas_rate_setpoint; ///< Controlled true airspeed rate setpoint [m/s²]. float tas_rate_setpoint; ///< Controlled true airspeed rate setpoint [m/s²].
float tas_setpoint; ///< Controller true airspeed setpoint [m/s]
}; };
/** /**