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

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:
Silvan Fuhrer 2023-07-13 11:18:54 +02:00 committed by Thomas Stastny
parent a6fcf7b48c
commit 8741a9784d
4 changed files with 3 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/TecsStatus.msg
View File

@ -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

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

@ -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;

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

@ -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)

11
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -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();