fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 14:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Position Control: remove global variable _current_speed_mode and instead pass it as argument

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-11-11 19:41:18 +01:00
parent 810c69d6ab
commit 4fab9c8345
4 changed files with 65 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+4 -2
View File
@@ -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();
src/lib/tecs/TECS.hpp
+9 -1
View File
@@ -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)
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+41 -36
View File
@@ -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<FW_SPEED_MODE_COMMANDED>(_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)
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+11 -9
View File
@@ -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(