diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index a6a353dfb3..05940d9cf6 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 1a74671290..3b80e82ea7 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -94,7 +94,7 @@ public: 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 @@ public: 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; } @@ -217,6 +221,7 @@ private: 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) @@ -276,6 +281,9 @@ private: 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 b883bbf3d2..a9c6e1a98e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -134,6 +134,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 */ @@ -360,11 +363,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) @@ -380,7 +383,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(); } @@ -440,56 +443,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 @@ -517,7 +524,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 @@ -665,8 +671,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; } @@ -791,7 +796,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()); @@ -843,11 +848,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: @@ -1042,7 +1047,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}; @@ -1130,20 +1136,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}; @@ -1235,14 +1242,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 @@ -1306,7 +1314,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(), @@ -1391,7 +1399,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(), @@ -1609,7 +1617,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, @@ -1679,7 +1687,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(), @@ -2172,7 +2180,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; @@ -2265,9 +2274,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, @@ -2276,8 +2282,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 45afb5242f..742f997eb6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -272,7 +272,7 @@ private: FW_SPEED_MODE_NORMAL, FW_SPEED_MODE_ECO, FW_SPEED_MODE_DASH, - } _speed_mode_current{FW_SPEED_MODE_NORMAL}; + }; enum FW_SPEED_MODE_COMMANDED { NORMAL, @@ -280,7 +280,7 @@ private: ECO_FULL, DASH_CRUISE, DASH_FULL - } _speed_mode_setting{NORMAL}; + }; bool _conditions_for_eco_dash_met{false}; hrt_abstime _time_conditions_not_met{0}; @@ -345,11 +345,12 @@ private: void control_auto_descend(const hrt_abstime &now); void 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); void 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 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 FW_SPEED_MODE spd_mode); void control_auto_takeoff(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, @@ -366,7 +367,7 @@ private: float get_demanded_airspeed(); float 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); void reset_takeoff_state(bool force = false); void reset_landing_state(); @@ -376,7 +377,7 @@ private: void publishOrbitStatus(const position_setpoint_s pos_sp); void check_eco_dash_allowed(); - void updateSpeedMode(); + FW_SPEED_MODE getSpeedMode(); void update_wind_mode(); void resetAutoSpeedAdaptions(); @@ -387,7 +388,8 @@ private: 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 = false, float hgt_rate_sp = NAN); + bool disable_underspeed_detection = false, float hgt_rate_sp = NAN, + bool enable_eco_mode = false); DEFINE_PARAMETERS(