Skip to content

Commit

Permalink
FW Position Control: remove global variable _current_speed_mode and i…
Browse files Browse the repository at this point in the history 
…nstead pass it as argument

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
  • Loading branch information
@sfuhrer
sfuhrer committed Nov 11, 2021
1 parent 341cc50 commit 008a14d
Show file tree
Hide file tree
Showing 4 changed files with 65 additions and 48 deletions.
6 changes: 4 additions & 2 deletions src/lib/tecs/TECS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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();
Expand Down
10 changes: 9 additions & 1 deletion src/lib/tecs/TECS.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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; }
Expand Down Expand Up @@ -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; }
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down
77 changes: 41 additions & 36 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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)
Expand All @@ -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();
}

Expand Down Expand Up @@ -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<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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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};
Expand Down Expand Up @@ -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};
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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(),
Expand Down Expand Up @@ -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(),
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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(),
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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,
Expand All @@ -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)
Expand Down
20 changes: 11 additions & 9 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,15 +272,15 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
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,
ECO_CRUISE,
ECO_FULL,
DASH_CRUISE,
DASH_FULL
} _speed_mode_setting{NORMAL};
};

bool _conditions_for_eco_dash_met{false};
hrt_abstime _time_conditions_not_met{0};
Expand Down Expand Up @@ -345,11 +345,12 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
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,
Expand All @@ -366,7 +367,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro

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();
Expand All @@ -376,7 +377,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
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();

Expand All @@ -387,7 +388,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
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(

Expand Down

0 comments on commit 008a14d

Please sign in to comment.