From 008a14ddf5a063c538de6a95d06fc562fbe39222 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 11 Nov 2021 19:41:18 +0100 Subject: [PATCH] FW Position Control: remove global variable _current_speed_mode and instead pass it as argument Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 6 +- src/lib/tecs/TECS.hpp | 10 ++- .../FixedwingPositionControl.cpp | 77 ++++++++++--------- .../FixedwingPositionControl.hpp | 20 ++--- 4 files changed, 65 insertions(+), 48 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index a6a353dfb332..05940d9cf6f4 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -586,7 +586,7 @@ void TECS::_update_STE_rate_lim() void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max, - float target_climbrate, float target_sinkrate, float hgt_rate_sp, bool eco_mode_enabled) + float hgt_rate_sp, bool eco_mode_enabled) { // Calculate the time since last update (seconds) uint64_t now = hrt_absolute_time(); @@ -630,7 +630,9 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set _updateFlightPhase(hgt_setpoint, hgt_rate_sp); - _calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climbrate, target_sinkrate, baro_altitude); + const float target_climb_rate = _tecs_mode == ECL_TECS_MODE_ECO ? _target_climb_rate_eco : _target_climb_rate; + + _calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climb_rate, _target_sink_rate, baro_altitude); // Calculate the specific energy values required by the control loop _update_energy_estimates(); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 5a851aa2fe48..fc882ca952b5 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -94,7 +94,7 @@ class TECS void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, 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 pitch_limit_min, float pitch_limit_max, float hgt_rate_sp = NAN, bool eco_mode_enabled = false); void reset_state() { _states_initialized = false; } @@ -142,9 +142,13 @@ class TECS void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; } + void set_target_climb_rate(float target_climb_rate) { _target_climb_rate = target_climb_rate; } + void set_target_sink_rate(float target_sink_rate) { _target_sink_rate = target_sink_rate; } + // eco mode settings void set_speed_weight_eco(float weight_eco) { _pitch_speed_weight_eco = weight_eco; } void set_height_error_time_constant_eco(float time_const_eco) { _height_error_gain_eco = 1.0f / math::max(time_const_eco, 0.1f); } + void set_target_climb_rate_eco(float target_climb_rate_eco) { _target_climb_rate_eco = target_climb_rate_eco; } // getters float get_throttle_setpoint() { return _last_throttle_setpoint; } @@ -219,6 +223,7 @@ class TECS float _height_error_gain_eco{0.2f}; ///< in eco mode: height error inverse time constant [1/s] float _pitch_speed_weight_eco{1.0f}; ///< in eco mode: speed control weighting used by pitch demand calculation + float _target_climb_rate_eco{2.f}; // complimentary filter states float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec) @@ -278,6 +283,9 @@ class TECS float _SPE_weighting{1.0f}; float _SKE_weighting{1.0f}; + float _target_climb_rate{3.f}; + float _target_sink_rate{2.f}; + // time steps (non-fixed) float _dt{DT_DEFAULT}; ///< Time since last update of main TECS loop (sec) static constexpr float DT_DEFAULT = 0.02f; ///< default value for _dt (sec) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 2905887d2d78..bdfcb54ffe94 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -132,6 +132,9 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_weight_eco(_param_fw_t_spdweight_eco.get()); _tecs.set_height_error_time_constant_eco(_param_fw_t_alt_tc_eco.get()); + _tecs.set_target_climb_rate(_param_climbrate_target.get()); + _tecs.set_target_sink_rate(_param_sinkrate_target.get()); + _tecs.set_target_climb_rate_eco(_param_fw_t_clmb_r_sp_eco.get()); // Landing slope /* check if negative value for 2/3 of flare altitude is set for throttle cut */ @@ -358,11 +361,11 @@ FixedwingPositionControl::get_demanded_airspeed() float FixedwingPositionControl::get_cruise_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, - const Vector2f &ground_speed, float dt) + const Vector2f &ground_speed, float dt, const FW_SPEED_MODE spd_mode) { float airspeed_setpoint = _param_fw_airspd_trim.get(); - if (_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO) { + if (spd_mode == FW_SPEED_MODE::FW_SPEED_MODE_ECO) { airspeed_setpoint = _param_fw_airspd_min.get(); // Adapt cruise airspeed setpoint based on wind estimate (disable in airspeed-less mode) @@ -378,7 +381,7 @@ FixedwingPositionControl::get_cruise_airspeed_setpoint(const hrt_abstime &now, c } } - } else if (_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_DASH) { + } else if (spd_mode == FW_SPEED_MODE::FW_SPEED_MODE_DASH) { airspeed_setpoint = _param_fw_airspd_max.get(); } @@ -438,56 +441,60 @@ FixedwingPositionControl::get_cruise_airspeed_setpoint(const hrt_abstime &now, c return constrain(airspeed_setpoint, adjusted_min_airspeed, _param_fw_airspd_max.get()); } -void -FixedwingPositionControl::updateSpeedMode() +FixedwingPositionControl::FW_SPEED_MODE +FixedwingPositionControl::getSpeedMode() { FW_SPEED_MODE_COMMANDED new_mode = static_cast(_param_fw_spd_mode_set.get()); + FW_SPEED_MODE speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; //default; + switch (new_mode) { case NORMAL: - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; //default + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; //default break; case ECO_CRUISE: if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) { - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO; + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO; } else { - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; } break; case ECO_FULL: if (_conditions_for_eco_dash_met) { - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO; + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO; } else { - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; } break; case DASH_CRUISE: if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) { - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH; + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH; } else { - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; } break; case DASH_FULL: if (_conditions_for_eco_dash_met) { - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH; + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH; } else { - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; } break; } + + return speed_mode_current; } void @@ -515,7 +522,6 @@ FixedwingPositionControl::resetAutoSpeedAdaptions() { _conditions_for_eco_dash_met = false; _time_conditions_not_met = _local_pos.timestamp; - _speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; } void @@ -663,8 +669,7 @@ FixedwingPositionControl::getManualHeightRateSetpoint() } else if (_manual_control_setpoint_altitude < - deadBand) { /* pitching up */ float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor; - const float climb_rate_target = _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO ? - _param_fw_t_clmb_r_sp_eco.get() : _param_climbrate_target.get(); + const float climb_rate_target = _param_climbrate_target.get(); height_rate_setpoint = pitch * climb_rate_target; } @@ -789,7 +794,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c const bool was_circle_mode = _l1_control.circle_mode(); check_eco_dash_allowed(); - updateSpeedMode(); + FW_SPEED_MODE spd_mode = getSpeedMode(); /* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */ _tecs.set_speed_weight(_param_fw_t_spdweight.get()); @@ -839,11 +844,11 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c break; case position_setpoint_s::SETPOINT_TYPE_POSITION: - control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp); + control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, spd_mode); break; case position_setpoint_s::SETPOINT_TYPE_LOITER: - control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); + control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next, spd_mode); break; case position_setpoint_s::SETPOINT_TYPE_LAND: @@ -1038,7 +1043,8 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons void FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, + const FW_SPEED_MODE spd_mode) { const float acc_rad = _l1_control.switch_distance(500.0f); Vector2d curr_wp{0, 0}; @@ -1126,20 +1132,21 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(now, position_sp_alt, - get_cruise_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt), + get_cruise_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt, spd_mode), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), tecs_fw_thr_min, tecs_fw_thr_max, tecs_fw_mission_throttle, false, - radians(_param_fw_p_lim_min.get())); + radians(_param_fw_p_lim_min.get()), + spd_mode == FW_SPEED_MODE_ECO); } void FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, - const position_setpoint_s &pos_sp_next) + const position_setpoint_s &pos_sp_next, const FW_SPEED_MODE spd_mode) { Vector2d curr_wp{0, 0}; Vector2d prev_wp{0, 0}; @@ -1231,14 +1238,15 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa } tecs_update_pitch_throttle(now, alt_sp, - get_cruise_airspeed_setpoint(now, mission_airspeed, ground_speed, dt), + get_cruise_airspeed_setpoint(now, mission_airspeed, ground_speed, dt, spd_mode), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), tecs_fw_thr_min, tecs_fw_thr_max, tecs_fw_mission_throttle, false, - radians(_param_fw_p_lim_min.get())); + radians(_param_fw_p_lim_min.get()), + spd_mode == FW_SPEED_MODE_ECO); } void @@ -1302,7 +1310,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo tecs_update_pitch_throttle(now, pos_sp_curr.alt, get_cruise_airspeed_setpoint(now, _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed, - dt), + dt, FW_SPEED_MODE::FW_SPEED_MODE_NORMAL), radians(_param_fw_p_lim_min.get()), radians(takeoff_pitch_max_deg), _param_fw_thr_min.get(), @@ -1387,7 +1395,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } else { tecs_update_pitch_throttle(now, pos_sp_curr.alt, - get_cruise_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed, dt), + get_cruise_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed, dt, FW_SPEED_MODE::FW_SPEED_MODE_NORMAL), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), @@ -1605,7 +1613,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f; tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel, - get_cruise_airspeed_setpoint(now, airspeed_land, ground_speed, dt), + get_cruise_airspeed_setpoint(now, airspeed_land, ground_speed, dt, FW_SPEED_MODE::FW_SPEED_MODE_NORMAL), radians(_param_fw_lnd_fl_pmin.get()), radians(_param_fw_lnd_fl_pmax.get()), 0.0f, @@ -1675,7 +1683,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); tecs_update_pitch_throttle(now, altitude_desired, - get_cruise_airspeed_setpoint(now, airspeed_approach, ground_speed, dt), + get_cruise_airspeed_setpoint(now, airspeed_approach, ground_speed, dt, FW_SPEED_MODE::FW_SPEED_MODE_NORMAL), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), @@ -2168,7 +2176,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, bool climbout_mode, float climbout_pitch_min_rad, - bool disable_underspeed_detection, float hgt_rate_sp) + bool disable_underspeed_detection, float hgt_rate_sp, + bool eco_mode_enabled) { const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f); _last_tecs_update = now; @@ -2261,9 +2270,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo } } - const float climb_rate_target = _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO ? - _param_fw_t_clmb_r_sp_eco.get() : _param_climbrate_target.get(); - _tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()), _current_altitude, alt_sp, airspeed_sp, _airspeed, _eas2tas, @@ -2272,8 +2278,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo throttle_min, throttle_max, throttle_cruise, pitch_min_rad - radians(_param_fw_psp_off.get()), pitch_max_rad - radians(_param_fw_psp_off.get()), - climb_rate_target, _param_sinkrate_target.get(), hgt_rate_sp, - _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO); + hgt_rate_sp, eco_mode_enabled); } 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 45afb5242fd0..742f997eb6e5 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -272,7 +272,7 @@ class FixedwingPositionControl final : public ModuleBase