forked from Archive/PX4-Autopilot
TECS: remove TECS_MODE enum and instead add descriptive boolean flag to tecs_status
New flag: underspeed_mode_enabled. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
parent
a6fcf7b48c
commit
8741a9784d
|
@ -26,7 +26,4 @@ float32 throttle_sp # Current throttle setpoint [-]
|
|||
float32 pitch_sp_rad # Current pitch setpoint [rad]
|
||||
float32 throttle_trim # estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions
|
||||
|
||||
# TECS mode
|
||||
uint8 mode
|
||||
uint8 TECS_MODE_NORMAL = 0
|
||||
uint8 TECS_MODE_UNDERSPEED = 1
|
||||
bool underspeed_mode_enabled # System has detected a low airspeed and takes measures to avoid stalling the plane
|
||||
|
|
|
@ -649,7 +649,6 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
|
|||
|
||||
_control.initialize(control_setpoint, control_input, _control_param, _control_flag);
|
||||
|
||||
_debug_status.tecs_mode = _tecs_mode;
|
||||
_debug_status.control = _control.getDebugOutput();
|
||||
_debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed;
|
||||
_debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate;
|
||||
|
@ -721,17 +720,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
|
|||
// Update time stamps
|
||||
_update_timestamp = now;
|
||||
|
||||
|
||||
// Set TECS mode for next frame
|
||||
if (_control.getRatioUndersped() > FLT_EPSILON) {
|
||||
_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
|
||||
|
||||
} else {
|
||||
// This is the default operation mode
|
||||
_tecs_mode = ECL_TECS_MODE_NORMAL;
|
||||
}
|
||||
|
||||
_debug_status.tecs_mode = _tecs_mode;
|
||||
_debug_status.control = _control.getDebugOutput();
|
||||
_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
|
||||
_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
|
||||
|
|
|
@ -539,11 +539,6 @@ private:
|
|||
class TECS
|
||||
{
|
||||
public:
|
||||
enum ECL_TECS_MODE {
|
||||
ECL_TECS_MODE_NORMAL = 0,
|
||||
ECL_TECS_MODE_UNDERSPEED
|
||||
};
|
||||
|
||||
struct DebugOutput {
|
||||
TECSControl::DebugOutput control;
|
||||
float true_airspeed_filtered;
|
||||
|
@ -551,7 +546,6 @@ public:
|
|||
float altitude_reference;
|
||||
float height_rate_reference;
|
||||
float height_rate_direct;
|
||||
enum ECL_TECS_MODE tecs_mode;
|
||||
};
|
||||
public:
|
||||
TECS() = default;
|
||||
|
@ -652,15 +646,13 @@ public:
|
|||
float get_throttle_setpoint() {return _control.getThrottleSetpoint();}
|
||||
|
||||
uint64_t timestamp() { return _update_timestamp; }
|
||||
ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
|
||||
bool underspeed_detected() { return _control.getRatioUndersped() > 0.f; }
|
||||
|
||||
private:
|
||||
TECSControl _control; ///< Control submodule.
|
||||
TECSAirspeedFilter _airspeed_filter; ///< Airspeed filter submodule.
|
||||
TECSAltitudeReferenceModel _altitude_reference_model; ///< Setpoint reference model submodule.
|
||||
|
||||
enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode.
|
||||
|
||||
hrt_abstime _update_timestamp{0}; ///< last timestamp of the update function call.
|
||||
|
||||
float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec)
|
||||
|
|
|
@ -485,16 +485,6 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
|
|||
|
||||
const TECS::DebugOutput &debug_output{_tecs.getStatus()};
|
||||
|
||||
switch (_tecs.tecs_mode()) {
|
||||
case TECS::ECL_TECS_MODE_NORMAL:
|
||||
tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
|
||||
break;
|
||||
|
||||
case TECS::ECL_TECS_MODE_UNDERSPEED:
|
||||
tecs_status.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
|
||||
break;
|
||||
}
|
||||
|
||||
tecs_status.altitude_sp = alt_sp;
|
||||
tecs_status.altitude_reference = debug_output.altitude_reference;
|
||||
tecs_status.height_rate_reference = debug_output.height_rate_reference;
|
||||
|
@ -516,6 +506,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
|
|||
tecs_status.throttle_sp = _tecs.get_throttle_setpoint();
|
||||
tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint();
|
||||
tecs_status.throttle_trim = throttle_trim;
|
||||
tecs_status.underspeed_mode_enabled = _tecs.underspeed_detected();
|
||||
|
||||
tecs_status.timestamp = hrt_absolute_time();
|
||||
|
||||
|
|
Loading…
Reference in New Issue