From 9f1f8348f526eb1be7006720964ae7016179e387 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 9 Nov 2021 11:48:38 +0100 Subject: [PATCH] FW Position Control: centralise airspeed setpoint adaptions for auto flight in single function Adapt the airspeed setpoint by: - speed mode (ECO/NORMAL/DASH --> airspeed MIN, TRIM, MAX) - if position setpoint contains valid airspeed setpoint use that instead - increase setpoint if groundspeed gets low - increase setpoint if close to MIN and banking (compensate higher load factor) There is a slew rate limiting in place that allows max 1m/s/s rate of change of the airspeed setpoint. Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 117 ++++++++++-------- .../FixedwingPositionControl.hpp | 14 ++- 2 files changed, 74 insertions(+), 57 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index b2622ead21..33063b8888 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -354,24 +354,51 @@ 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) { + float airspeed_setpoint = _param_fw_airspd_trim.get(); + + if (_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO) { + airspeed_setpoint = _param_fw_airspd_min.get(); + + } else if (_speed_mode_current == 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,23 +407,21 @@ 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 @@ -835,15 +860,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); 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); 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: @@ -1033,7 +1058,7 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons } void -FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos, +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 float acc_rad = _l1_control.switch_distance(500.0f); @@ -1057,15 +1082,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; @@ -1131,7 +1147,7 @@ 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), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), tecs_fw_thr_min, @@ -1142,7 +1158,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve } 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) { @@ -1165,13 +1181,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; @@ -1242,7 +1252,7 @@ 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), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), tecs_fw_thr_min, @@ -1312,7 +1322,8 @@ 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), radians(_param_fw_p_lim_min.get()), radians(takeoff_pitch_max_deg), _param_fw_thr_min.get(), @@ -1397,7 +1408,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } 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), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), @@ -1422,7 +1433,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) */ @@ -1615,7 +1626,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), radians(_param_fw_lnd_fl_pmin.get()), radians(_param_fw_lnd_fl_pmax.get()), 0.0f, @@ -1685,7 +1696,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), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 24d66433c3..a836a3360a 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -296,6 +296,8 @@ private: FW_WIND_MODE _fw_wind_mode_detected_prev{FW_WIND_MODE_NORMAL}; + float _last_airspeed_setpoint{NAN}; + param_t _param_handle_airspeed_trans{PARAM_INVALID}; float _param_airspeed_trans{NAN}; @@ -353,15 +355,18 @@ private: void control_auto_fixed_bank_alt_hold(const hrt_abstime &now); void control_auto_descend(const hrt_abstime &now); - void control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, + 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); - void control_auto_loiter(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, + 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); 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, const position_setpoint_s &pos_sp_curr); - void control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, + void 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); void control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed); @@ -371,7 +376,8 @@ private: float get_tecs_thrust(); float get_demanded_airspeed(); - float calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed); + float get_cruise_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, + const Vector2f &ground_speed, float dt); void reset_takeoff_state(bool force = false); void reset_landing_state();