From e76734fa5e3b403c418b0c47d766c70b5d4a26eb Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 16 Jul 2021 17:15:47 +0200 Subject: [PATCH] TECS: directly publish tecs_status from within TECS instance Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 59 +++++++++++++++- src/lib/tecs/TECS.hpp | 64 +++++------------- .../FixedwingPositionControl.cpp | 67 ------------------- .../FixedwingPositionControl.hpp | 2 - 4 files changed, 76 insertions(+), 116 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index e33250e226..c2823d2bb0 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -227,7 +227,7 @@ void TECS::_update_energy_estimates() // Calculate the specific energy balance demand which specifies how the available total // energy should be allocated to speed (kinetic energy) and height (potential energy) // Calculate the specific energy balance error - _SEB_error = SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting); + _SEB_error = get_SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting); // Calculate specific energy rate demands in units of (m**2/sec**3) _SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change @@ -623,6 +623,7 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set _tecs_mode = ECL_TECS_MODE_NORMAL; } + tecs_status_publish(now); } void TECS::_update_speed_height_weights() @@ -642,3 +643,59 @@ void TECS::_update_speed_height_weights() _SPE_weighting = constrain(2.0f - _SKE_weighting, 0.f, 1.f); _SKE_weighting = constrain(_SKE_weighting, 0.f, 1.f); } + +void +TECS::tecs_status_publish(const hrt_abstime &now) +{ + tecs_status_s tecs_status{}; + + switch (_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; + + case TECS::ECL_TECS_MODE_BAD_DESCENT: + tecs_status.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; + break; + + case TECS::ECL_TECS_MODE_CLIMBOUT: + tecs_status.mode = tecs_status_s::TECS_MODE_CLIMBOUT; + break; + } + + tecs_status.altitude_sp = _hgt_setpoint; + tecs_status.altitude_filtered = _vert_pos_state; + tecs_status.energy_distribution_error = _SEB_error; + tecs_status.energy_distribution_rate_error = _SEB_rate_error; + tecs_status.equivalent_airspeed_sp = _EAS_setpoint; + tecs_status.height_rate_setpoint = _hgt_rate_setpoint; + tecs_status.height_rate = _vert_vel_state; + tecs_status.pitch_integ = _pitch_integ_state; + tecs_status.pitch_sp_rad = get_pitch_setpoint(); + tecs_status.throttle_integ = _throttle_integ_state; + tecs_status.throttle_sp = get_throttle_setpoint(); + tecs_status.total_energy_balance = get_SEB(); + tecs_status.total_energy_balance_sp = get_SEB_setpoint(); + tecs_status.total_energy_balance_rate = get_SEB_rate(); + tecs_status.total_energy_balance_rate_sp = get_SEB_rate_setpoint(); + tecs_status.total_energy = get_STE(); + tecs_status.total_energy_error = _STE_error; + tecs_status.total_energy_sp = get_STE_setpoint(); + tecs_status.total_energy_rate = get_STE_rate(); + tecs_status.total_energy_rate_error = _STE_rate_error; + tecs_status.total_energy_rate_sp = get_STE_rate_setpoint(); + tecs_status.true_airspeed_sp = _TAS_setpoint_adj; + tecs_status.true_airspeed_filtered = _tas_state; + tecs_status.true_airspeed_derivative = _tas_rate_filtered; + tecs_status.true_airspeed_derivative_raw = _tas_rate_raw; + tecs_status.true_airspeed_derivative_sp = _TAS_rate_setpoint; + tecs_status.true_airspeed_innovation = _tas_innov; + + tecs_status.timestamp = now; + + _tecs_status_pub.publish(tecs_status); +} diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index f95f483423..2fe5871afd 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -49,6 +49,10 @@ #include #include +#include +#include +#include + class TECS { public: @@ -92,10 +96,6 @@ public: float throttle_min, float throttle_setpoint_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN); - float get_throttle_setpoint() { return _last_throttle_setpoint; } - float get_pitch_setpoint() { return _last_pitch_setpoint; } - float get_speed_weight() { return _pitch_speed_weight; } - void reset_state() { _states_initialized = false; } enum ECL_TECS_MODE { @@ -141,50 +141,22 @@ public: void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; } - // TECS status - uint64_t timestamp() { return _pitch_update_timestamp; } - ECL_TECS_MODE tecs_mode() { return _tecs_mode; } + // getters + float get_throttle_setpoint() { return _last_throttle_setpoint; } + float get_pitch_setpoint() { return _last_pitch_setpoint; } - float hgt_setpoint() { return _hgt_setpoint; } - float vert_pos_state() { return _vert_pos_state; } - - float TAS_setpoint_adj() { return _TAS_setpoint_adj; } - float tas_state() { return _tas_state; } - float getTASInnovation() { return _tas_innov; } - - float hgt_rate_setpoint() { return _hgt_rate_setpoint; } - float vert_vel_state() { return _vert_vel_state; } - - float get_EAS_setpoint() { return _EAS_setpoint; }; - float TAS_rate_setpoint() { return _TAS_rate_setpoint; } - float speed_derivative() { return _tas_rate_filtered; } - float speed_derivative_raw() { return _tas_rate_raw; } - - float STE_error() { return _STE_error; } - float STE_rate_error() { return _STE_rate_error; } - - float SEB_error() { return _SEB_error; } - float SEB_rate_error() { return _SEB_rate_error; } - - float throttle_integ_state() { return _throttle_integ_state; } - float pitch_integ_state() { return _pitch_integ_state; } - - float STE() { return _SPE_estimate + _SKE_estimate; } - - float STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; } - - float STE_rate() { return _SPE_rate + _SKE_rate; } - - float STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; } - - float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; } - - float SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; } - - float SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; } - - float SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; } + float get_hgt_setpoint() { return _hgt_setpoint; } + float get_hgt_rate_setpoint() { return _hgt_rate_setpoint; } + float get_STE() { return _SPE_estimate + _SKE_estimate; } + float get_STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; } + float get_STE_rate() { return _SPE_rate + _SKE_rate; } + float get_STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; } + float get_SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; } + float get_SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; } + float get_SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; } + float get_SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; } + void tecs_status_publish(const hrt_abstime &now); /** * Handle the altitude reset diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 0a452b3aaf..21b602f6ed 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -399,70 +399,6 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const return constrain(airspeed_demand, adjusted_min_airspeed, _param_fw_airspd_max.get()); } -void -FixedwingPositionControl::tecs_status_publish() -{ - tecs_status_s t{}; - - switch (_tecs.tecs_mode()) { - case TECS::ECL_TECS_MODE_NORMAL: - t.mode = tecs_status_s::TECS_MODE_NORMAL; - break; - - case TECS::ECL_TECS_MODE_UNDERSPEED: - t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; - break; - - case TECS::ECL_TECS_MODE_BAD_DESCENT: - t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; - break; - - case TECS::ECL_TECS_MODE_CLIMBOUT: - t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; - break; - } - - t.altitude_sp = _tecs.hgt_setpoint(); - t.altitude_filtered = _tecs.vert_pos_state(); - - t.true_airspeed_sp = _tecs.TAS_setpoint_adj(); - t.true_airspeed_filtered = _tecs.tas_state(); - - t.height_rate_setpoint = _tecs.hgt_rate_setpoint(); - t.height_rate = _tecs.vert_vel_state(); - - t.equivalent_airspeed_sp = _tecs.get_EAS_setpoint(); - t.true_airspeed_derivative_sp = _tecs.TAS_rate_setpoint(); - t.true_airspeed_derivative = _tecs.speed_derivative(); - t.true_airspeed_derivative_raw = _tecs.speed_derivative_raw(); - t.true_airspeed_innovation = _tecs.getTASInnovation(); - - t.total_energy_error = _tecs.STE_error(); - t.total_energy_rate_error = _tecs.STE_rate_error(); - - t.energy_distribution_error = _tecs.SEB_error(); - t.energy_distribution_rate_error = _tecs.SEB_rate_error(); - - t.total_energy = _tecs.STE(); - t.total_energy_rate = _tecs.STE_rate(); - t.total_energy_balance = _tecs.SEB(); - t.total_energy_balance_rate = _tecs.SEB_rate(); - - t.total_energy_sp = _tecs.STE_setpoint(); - t.total_energy_rate_sp = _tecs.STE_rate_setpoint(); - t.total_energy_balance_sp = _tecs.SEB_setpoint(); - t.total_energy_balance_rate_sp = _tecs.SEB_rate_setpoint(); - - t.throttle_integ = _tecs.throttle_integ_state(); - t.pitch_integ = _tecs.pitch_integ_state(); - - t.throttle_sp = _tecs.get_throttle_setpoint(); - t.pitch_sp_rad = _tecs.get_pitch_setpoint(); - - t.timestamp = hrt_absolute_time(); - - _tecs_status_pub.publish(t); -} void FixedwingPositionControl::status_publish() @@ -612,7 +548,6 @@ FixedwingPositionControl::getManualHeightRateSetpoint() const float climb_rate_target = _param_climbrate_target.get(); height_rate_setpoint = pitch * climb_rate_target; - } return height_rate_setpoint; @@ -2226,8 +2161,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo pitch_min_rad - radians(_param_fw_psp_off.get()), pitch_max_rad - radians(_param_fw_psp_off.get()), _param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp); - - tecs_status_publish(); } void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index b960443f58..ddca722668 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -76,7 +76,6 @@ #include #include #include -#include #include #include #include @@ -284,7 +283,6 @@ private: void status_publish(); void landing_status_publish(); - void tecs_status_publish(); void abort_landing(bool abort);