diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index 74c47e1a2c90..bb79cd54f871 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -6,6 +6,7 @@ uint8 TECS_MODE_LAND = 3 uint8 TECS_MODE_LAND_THROTTLELIM = 4 uint8 TECS_MODE_BAD_DESCENT = 5 uint8 TECS_MODE_CLIMBOUT = 6 +uint8 TECS_MODE_ECO = 7 float32 altitude_sp @@ -42,3 +43,9 @@ float32 throttle_sp float32 pitch_sp_rad uint8 mode + +uint8 flight_phase + +uint8 TECS_FLIGHT_PHASE_LEVEL = 0 # In level flight (no altitude setpoint change) +uint8 TECS_FLIGHT_PHASE_CLIMB = 1 # Climb is triggered (altitude setpoint is increasing) +uint8 TECS_FLIGHT_PHASE_DESCEND = 2 # Descend is triggered (altitude setpoint is descreasing) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index e33250e22639..a02cf8ff43ba 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -50,6 +50,12 @@ static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a fil * @author Paul Riseborough */ + +TECS::TECS() +{ + _tecs_status_pub.advertise(); +} + /* * This function implements a complementary filter to estimate the climb rate when * inertial nav data is not available. It also calculates a true airspeed derivative @@ -227,7 +233,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 @@ -452,6 +458,38 @@ void TECS::_updateTrajectoryGenerationConstraints() _velocity_control_traj_generator.setMaxVelDown(_max_climb_rate); // different convention for FW than for MC } +void TECS::_updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint) +{ + const bool input_is_height_rate = PX4_ISFINITE(height_rate_setpoint); + + // update flight phase state (are we flying level, climbing or descending) + if (input_is_height_rate) { + + if (PX4_ISFINITE(_velocity_control_traj_generator.getCurrentPosition())) { + // we have a valid altitude setpoint which means that we are flying level + _flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL; + + } else if (_velocity_control_traj_generator.getCurrentVelocity() > FLT_EPSILON) { + _flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB; + + } else { + _flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND; + } + + } else { + // stay in flight phase level if only small altitude changes are demanded (<10m) + if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() > 10.0f) { + _flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB; + + } else if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() < -10.0f) { + _flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND; + + } else { + _flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL; + } + } +} + void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate, float target_sinkrate, float altitude_amsl) { @@ -554,7 +592,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) + float hgt_rate_sp, bool eco_mode_enabled) { // Calculate the time since last update (seconds) uint64_t now = hrt_absolute_time(); @@ -591,10 +629,16 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set // Detect an uncommanded descent caused by an unachievable airspeed demand _detect_uncommanded_descent(); + _eco_mode_enabled = eco_mode_enabled; + // Calculate the demanded true airspeed _update_speed_setpoint(); - _calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climbrate, target_sinkrate, baro_altitude); + _updateFlightPhase(hgt_setpoint, hgt_rate_sp); + + 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(); @@ -618,15 +662,24 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set } else if (_climbout_mode_active) { _tecs_mode = ECL_TECS_MODE_CLIMBOUT; + } else if (_eco_mode_enabled) { + _tecs_mode = ECL_TECS_MODE_ECO; + } else { // This is the default operation mode _tecs_mode = ECL_TECS_MODE_NORMAL; } + tecs_status_publish(now); } void TECS::_update_speed_height_weights() { + if (_eco_mode_enabled) { + _pitch_speed_weight = _pitch_speed_weight_eco; + _height_error_gain = _height_error_gain_eco; + } + // Calculate the weight applied to control of specific kinetic energy error _SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f); @@ -642,3 +695,67 @@ 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; + + case TECS::ECL_TECS_MODE_ECO: + tecs_status.mode = tecs_status_s::TECS_MODE_ECO; + break; + + default: + tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL; + } + + 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.flight_phase = _flight_phase; + 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 f95f48342388..cf3dc725b779 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -49,10 +49,14 @@ #include #include +#include +#include +#include + class TECS { public: - TECS() = default; + TECS(); ~TECS() = default; // no copy, assignment, move, move assignment @@ -90,11 +94,8 @@ 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 get_throttle_setpoint() { return _last_throttle_setpoint; } - float get_pitch_setpoint() { return _last_pitch_setpoint; } - float get_speed_weight() { return _pitch_speed_weight; } + float pitch_limit_min, float pitch_limit_max, float hgt_rate_sp = NAN, + bool eco_mode_enabled = false); void reset_state() { _states_initialized = false; } @@ -102,7 +103,8 @@ class TECS ECL_TECS_MODE_NORMAL = 0, ECL_TECS_MODE_UNDERSPEED, ECL_TECS_MODE_BAD_DESCENT, - ECL_TECS_MODE_CLIMBOUT + ECL_TECS_MODE_CLIMBOUT, + ECL_TECS_MODE_ECO }; void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; } @@ -140,51 +142,31 @@ 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; } - // TECS status - uint64_t timestamp() { return _pitch_update_timestamp; } - ECL_TECS_MODE tecs_mode() { return _tecs_mode; } - - 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; } + // 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; } - 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; } + // getters + float get_throttle_setpoint() { return _last_throttle_setpoint; } + float get_pitch_setpoint() { return _last_pitch_setpoint; } - 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; } + int get_flight_phase() { return _flight_phase; } + void tecs_status_publish(const hrt_abstime &now); /** * Handle the altitude reset @@ -237,6 +219,10 @@ class TECS float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s) float _SEB_rate_ff{1.0f}; + 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) float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m) @@ -295,6 +281,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) @@ -307,6 +296,10 @@ class TECS bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled bool _states_initialized{false}; ///< true when TECS states have been iniitalized bool _in_air{false}; ///< true when the vehicle is flying + bool _eco_mode_enabled{false}; + + // flight phase + int _flight_phase{tecs_status_s::TECS_FLIGHT_PHASE_LEVEL}; /** * Update the airspeed internal state using a second order complementary filter @@ -351,6 +344,8 @@ class TECS void _updateTrajectoryGenerationConstraints(); + void _updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint); + void _calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate, float target_sinkrate, float altitude_amsl); @@ -376,4 +371,6 @@ class TECS ManualVelocitySmoothingZ _velocity_control_traj_generator; // generates height rate and altitude setpoint trajectory when height rate is commanded + uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; + }; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 0a452b3aaf63..53c94ced5d0e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -70,8 +70,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : // limit to 50 Hz _local_pos_sub.set_interval_ms(20); - _tecs_status_pub.advertise(); - /* fetch initial parameter values */ parameters_update(); } @@ -131,6 +129,12 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get()); _tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get()); + _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 */ @@ -354,24 +358,64 @@ FixedwingPositionControl::get_demanded_airspeed() return altctrl_airspeed; } + float -FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed) +FixedwingPositionControl::get_cruise_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, + const Vector2f &ground_speed, float dt, const FW_SPEED_MODE spd_mode) { + float airspeed_setpoint = _param_fw_airspd_trim.get(); + + 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) + if (_airspeed_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { + wind_s wind_estimate; + + if (_wind_sub.update(&wind_estimate)) { + const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east); + + if (PX4_ISFINITE(wind.length())) { + airspeed_setpoint += _param_fw_wind_arsp_sc.get() * wind.length(); + } + } + } + + } else if (spd_mode == FW_SPEED_MODE::FW_SPEED_MODE_DASH) { + airspeed_setpoint = _param_fw_airspd_max.get(); + } + + // Adapt cruise airspeed setpoint if given from other source (e.g. position setpoint) + if (PX4_ISFINITE(pos_sp_cru_airspeed) && pos_sp_cru_airspeed > 0.1f) { + airspeed_setpoint = constrain(pos_sp_cru_airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_max.get()); + } + + // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained + if (!_l1_control.circle_mode()) { + /* + * This error value ensures that a plane (as long as its throttle capability is + * not exceeded) travels towards a waypoint (and is not pushed more and more away + * by wind). Not countering this would lead to a fly-away. Only non-zero in presence + * of sufficient wind. "minimum ground speed undershoot". + */ + const float ground_speed_body = _body_velocity(0); + + if (ground_speed_body < _param_fw_gnd_spd_min.get()) { + airspeed_setpoint += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f); + } + } + + // Constrain cruise airspeed to be in valid and safe range + /* * Calculate accelerated stall airspeed factor from commanded bank angle and use it to increase minimum airspeed. * - * We don't know the stall speed of the aircraft, but assuming user defined - * minimum airspeed (FW_AIRSPD_MIN) is slightly larger than stall speed - * this is close enough. - * - * increase lift vector to balance additional weight in bank - * cos(bank angle) = W/L = 1/n - * n is the load factor - * - * lift is proportional to airspeed^2 so the increase in stall speed is - * Vsacc = Vs * sqrt(n) + * Increase lift vector to balance additional weight in bank + * cos(bank angle) = W/L = 1/n, n is the load factor * + * lift is proportional to airspeed^2 so the increase in stall speed is Vsacc = Vs * sqrt(n) */ + float adjusted_min_airspeed = _param_fw_airspd_min.get(); if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) { @@ -380,88 +424,98 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const _param_fw_airspd_min.get(), _param_fw_airspd_max.get()); } - // groundspeed undershoot - if (!_l1_control.circle_mode()) { - /* - * This error value ensures that a plane (as long as its throttle capability is - * not exceeded) travels towards a waypoint (and is not pushed more and more away - * by wind). Not countering this would lead to a fly-away. - */ - const float ground_speed_body = _body_velocity(0); + // constrain airspeed setpoint changes with slew rate of 1 m/s/s + const float airspeed_setpoint_slew_rate = 1.f; - if (ground_speed_body < _param_fw_gnd_spd_min.get()) { - airspeed_demand += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f); - } + // initialize to current airspeed setpoint + if (!PX4_ISFINITE(_last_airspeed_setpoint)) { + _last_airspeed_setpoint = airspeed_setpoint; } - // add minimum ground speed undershoot (only non-zero in presence of sufficient wind) - // sanity check: limit to range - return constrain(airspeed_demand, adjusted_min_airspeed, _param_fw_airspd_max.get()); + if (dt > FLT_EPSILON) { + airspeed_setpoint = constrain(airspeed_setpoint, _last_airspeed_setpoint - airspeed_setpoint_slew_rate * dt, + _last_airspeed_setpoint + airspeed_setpoint_slew_rate * dt); + _last_airspeed_setpoint = airspeed_setpoint; + } + + return constrain(airspeed_setpoint, adjusted_min_airspeed, _param_fw_airspd_max.get()); } -void -FixedwingPositionControl::tecs_status_publish() +FixedwingPositionControl::FW_SPEED_MODE +FixedwingPositionControl::getSpeedMode() { - tecs_status_s t{}; + FW_SPEED_MODE_COMMANDED new_mode = static_cast(_param_fw_spd_mode_set.get()); - switch (_tecs.tecs_mode()) { - case TECS::ECL_TECS_MODE_NORMAL: - t.mode = tecs_status_s::TECS_MODE_NORMAL; - break; + const float altitude_amsl_min = max(_tecs.get_hgt_setpoint() - _param_fw_spdm_alt_er_u.get(), + _local_pos.ref_alt + _param_fw_alt_spdm_min.get()); + const float altitude_amsl_max = _tecs.get_hgt_setpoint() + _param_fw_spdm_alt_er_o.get(); - case TECS::ECL_TECS_MODE_UNDERSPEED: - t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; - break; + const bool altitude_conditions_met = _current_altitude <= altitude_amsl_max + && _current_altitude >= altitude_amsl_min; - case TECS::ECL_TECS_MODE_BAD_DESCENT: - t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; - break; + if (!altitude_conditions_met) { + // reset timer + _time_conditions_not_met = _local_pos.timestamp; + } + + // add a 10s timeout after conditions where not met + const bool conditions_for_eco_dash_met = hrt_elapsed_time(&_time_conditions_not_met) > 10_s; - case TECS::ECL_TECS_MODE_CLIMBOUT: - t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; + 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 break; - } - t.altitude_sp = _tecs.hgt_setpoint(); - t.altitude_filtered = _tecs.vert_pos_state(); + 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; - t.true_airspeed_sp = _tecs.TAS_setpoint_adj(); - t.true_airspeed_filtered = _tecs.tas_state(); + } else { + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + } - t.height_rate_setpoint = _tecs.hgt_rate_setpoint(); - t.height_rate = _tecs.vert_vel_state(); + break; - 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(); + case ECO_FULL: + if (conditions_for_eco_dash_met) { + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO; - t.total_energy_error = _tecs.STE_error(); - t.total_energy_rate_error = _tecs.STE_rate_error(); + } else { + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + } - t.energy_distribution_error = _tecs.SEB_error(); - t.energy_distribution_rate_error = _tecs.SEB_rate_error(); + break; - 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(); + 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; - 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(); + } else { + 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; - t.throttle_integ = _tecs.throttle_integ_state(); - t.pitch_integ = _tecs.pitch_integ_state(); + } else { + speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; + } - t.throttle_sp = _tecs.get_throttle_setpoint(); - t.pitch_sp_rad = _tecs.get_pitch_setpoint(); + break; + } - t.timestamp = hrt_absolute_time(); + return speed_mode_current; +} - _tecs_status_pub.publish(t); +void +FixedwingPositionControl::resetEcoDashAllowed() +{ + _time_conditions_not_met = _local_pos.timestamp; } void @@ -612,7 +666,6 @@ FixedwingPositionControl::getManualHeightRateSetpoint() const float climb_rate_target = _param_climbrate_target.get(); height_rate_setpoint = pitch * climb_rate_target; - } return height_rate_setpoint; @@ -734,6 +787,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c /* get circle mode */ const bool was_circle_mode = _l1_control.circle_mode(); + 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()); _tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get()); @@ -784,15 +839,15 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c break; case position_setpoint_s::SETPOINT_TYPE_POSITION: - control_auto_position(now, 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, 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: - control_auto_landing(now, curr_pos, ground_speed, pos_sp_prev, current_sp); + control_auto_landing(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp); break; case position_setpoint_s::SETPOINT_TYPE_TAKEOFF: @@ -907,7 +962,7 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now) _param_fw_thr_cruise.get(), false, _param_fw_p_lim_min.get(), - tecs_status_s::TECS_MODE_NORMAL, + false, descend_rate); _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle @@ -982,8 +1037,9 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons } void -FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +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 FW_SPEED_MODE spd_mode) { const float acc_rad = _l1_control.switch_distance(500.0f); Vector2d curr_wp{0, 0}; @@ -1006,15 +1062,6 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve prev_wp(1) = pos_sp_curr.lon; } - - float mission_airspeed = _param_fw_airspd_trim.get(); - - if (PX4_ISFINITE(pos_sp_curr.cruising_speed) && - pos_sp_curr.cruising_speed > 0.1f) { - - mission_airspeed = pos_sp_curr.cruising_speed; - } - float tecs_fw_thr_min; float tecs_fw_thr_max; float tecs_fw_mission_throttle; @@ -1080,20 +1127,21 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(now, position_sp_alt, - calculate_target_airspeed(mission_airspeed, ground_speed), + 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 Vector2d &curr_pos, +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}; @@ -1114,13 +1162,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect prev_wp(1) = pos_sp_curr.lon; } - float mission_airspeed = _param_fw_airspd_trim.get(); - - if (PX4_ISFINITE(pos_sp_curr.cruising_speed) && - pos_sp_curr.cruising_speed > 0.1f) { - - mission_airspeed = pos_sp_curr.cruising_speed; - } + float mission_airspeed = pos_sp_curr.cruising_speed; float tecs_fw_thr_min; float tecs_fw_thr_max; @@ -1191,14 +1233,15 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect } tecs_update_pitch_throttle(now, alt_sp, - calculate_target_airspeed(mission_airspeed, ground_speed), + 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 @@ -1261,15 +1304,15 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get()); tecs_update_pitch_throttle(now, pos_sp_curr.alt, - calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed), + get_cruise_airspeed_setpoint(now, _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed, + 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(), _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here? _param_fw_thr_cruise.get(), _runway_takeoff.climbout(), - radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())), - tecs_status_s::TECS_MODE_TAKEOFF); + radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get()))); // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint()); @@ -1340,15 +1383,14 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo takeoff_throttle, _param_fw_thr_cruise.get(), true, - radians(_takeoff_pitch_min.get()), - tecs_status_s::TECS_MODE_TAKEOFF); + radians(_takeoff_pitch_min.get())); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f)); } else { tecs_update_pitch_throttle(now, pos_sp_curr.alt, - calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed), + 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(), @@ -1373,7 +1415,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } void -FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos, +FixedwingPositionControl::control_auto_landing(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) { /* current waypoint (the one currently heading for) */ @@ -1566,7 +1608,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec 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, - calculate_target_airspeed(airspeed_land, ground_speed), + 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, @@ -1574,7 +1616,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec throttle_land, false, _land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()), - _land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); + true); if (!_land_noreturn_vertical) { // just started with the flaring phase @@ -1636,7 +1678,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); tecs_update_pitch_throttle(now, altitude_desired, - calculate_target_airspeed(airspeed_approach, ground_speed), + 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(), @@ -1691,7 +1733,7 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const _param_fw_thr_cruise.get(), false, pitch_limit_min, - tecs_status_s::TECS_MODE_NORMAL, + false, height_rate_sp); _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); @@ -1750,7 +1792,7 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const _param_fw_thr_cruise.get(), false, pitch_limit_min, - tecs_status_s::TECS_MODE_NORMAL, + false, height_rate_sp); /* heading control */ @@ -1988,21 +2030,25 @@ FixedwingPositionControl::Run() case FW_POSCTRL_MODE_AUTO_ALTITUDE: { control_auto_fixed_bank_alt_hold(_local_pos.timestamp); + resetEcoDashAllowed(); break; } case FW_POSCTRL_MODE_AUTO_CLIMBRATE: { control_auto_descend(_local_pos.timestamp); + resetEcoDashAllowed(); break; } case FW_POSCTRL_MODE_MANUAL_POSITION: { control_manual_position(_local_pos.timestamp, curr_pos, ground_speed); + resetEcoDashAllowed(); break; } case FW_POSCTRL_MODE_MANUAL_ALTITUDE: { control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed); + resetEcoDashAllowed(); break; } @@ -2013,6 +2059,8 @@ FixedwingPositionControl::Run() reset_takeoff_state(); } + resetEcoDashAllowed(); + _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get()); break; @@ -2123,7 +2171,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, - uint8_t mode, 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; @@ -2187,8 +2236,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo } /* No underspeed protection in landing mode */ - _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND - || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); + _tecs.set_detect_underspeed_enabled(!disable_underspeed_detection); /* tell TECS to update its state, but let it know when it cannot actually control the plane */ bool in_air_alt_control = (!_landed && @@ -2225,9 +2273,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()), - _param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp); - - tecs_status_publish(); + 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 b960443f5826..3ffeb02519d8 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 @@ -89,6 +88,7 @@ #include #include #include +#include #include #include @@ -152,11 +152,11 @@ class FixedwingPositionControl final : public ModuleBase _attitude_sp_pub; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication - uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication - uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication + uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication ///< TECS status publication uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data @@ -267,6 +267,24 @@ class FixedwingPositionControl final : public ModuleBase) _param_nav_loiter_rad, - (ParamFloat) _takeoff_pitch_min + (ParamFloat) _takeoff_pitch_min, - ) + //Speed mode params + (ParamFloat) _param_fw_wind_arsp_sc, + (ParamInt) _param_fw_spd_mode_set, + (ParamFloat) _param_fw_alt_spdm_min, + (ParamFloat) _param_fw_spdm_alt_er_u, + (ParamFloat) _param_fw_spdm_alt_er_o, + (ParamFloat) _param_fw_t_alt_tc_eco, + (ParamFloat) _param_fw_t_spdweight_eco, + (ParamFloat) _param_fw_t_clmb_r_sp_eco + ) }; #endif // FIXEDWINGPOSITIONCONTROL_HPP_ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 156006326aa0..9a22f6e2f2a6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -854,3 +854,126 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30); * @group Mission */ PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f); + +/** + * Wind-based airspeed scaling factor in Eco + * + * Multiplying this factor with the current absolute wind estimate gives the airspeed offset + * added to the setpoint (which is FW_AIRSPD_MIN) in Eco mode (see FW_SPD_MODE_SET). This helps to make the + * system more robust against disturbances in high wind. + * + * setpoint = FW_AIRSPD_MIN + FW_LND_AIRSPD_SC * wind + * + * @unit norm + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.0f); + +/** + * FW Speed mode setting + * + * Setting + * + * @min 0 + * @max 4 + * @value 0 Normal + * @value 1 Eco cruise + * @value 2 Eco full + * @value 3 Dash cruise + * @value 4 Dash full + * @group FW TECS + */ +PARAM_DEFINE_INT32(FW_SPD_MODE_SET, 0); + + +/** + * Max altitude undershoot for Eco/Dash + * + * Eco/Dash mode is disabled if the current altitude overshoots the setpoint by this value. + * + * @min 1.0 + * @max 100.0 + * @decimal 1 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_U, 10.f); + +/** + * Max altitude overshoot for Eco/Dash + * + * Eco/Dash mode is disabled if the current altitude undershoots the setpoint by this value. + * + * @min 1.0 + * @max 100.0 + * @decimal 1 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_O, 20.f); + +/** + * Minimum height above home for Eco/Dash + * + * Eco/Dash mode can be enabled if above this relative altitude to home. + * + * @min -200.0 + * @max 200.0 + * @decimal 1 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_SPDM_ALT_MIN, 50.f); + +/** + * Speed <--> Altitude priority in Eco mode + * + * Overrides FW_T_SPDWEIGHT in Eco mode. + * + * Set it close to 2 for looser altitude control. + * That helps increasing power efficiency by having a smoother throttle setpoint, plus + * it enables the vehicle to exploit favorable local uplift conditions to gain some altitude. + * + * @min 0.0 + * @max 2.0 + * @decimal 1 + * @increment 1.0 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT_E, 1.8f); + +/** + * Eco Mode: Altitude error time constant. + * + * Overrides FW_T_ALT_TC in Eco mode. + * + * Increase it for looser altitude control. + * That helps increasing power efficiency by having a smoother throttle setpoint, plus + * it enables the vehicle to exploit favorable local uplift conditions to gain some altitude. + * + * @min 2.0 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_ALT_TC_E, 10.0f); + +/** + * Eco target climbrate. + * + * Overrides FW_T_CLMB_R_SP in Eco mode. + * + * The rate at which the vehicle will climb in autonomous modes to achieve altitude setpoints in Eco mode. + * + * @unit m/s + * @min 0.5 + * @max 15 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP_E, 2.0f);