From c05e0f076b0f770f03458ed0fe5c168383a11696 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 7 Jul 2022 12:27:35 +0300 Subject: [PATCH] FixedWingPositionControl: do not pass trim throttle via position setpoint triplet - trim throttle is handled entirely by the position controller - navigator should use speed setpoints - added flag gliding_enabled in position setpoint to stills support gliding Signed-off-by: RomanBapst --- msg/position_setpoint.msg | 1 + .../FixedwingPositionControl.cpp | 54 ++----------------- .../FixedwingPositionControl.hpp | 3 +- src/modules/navigator/mission_block.cpp | 4 ++ 4 files changed, 11 insertions(+), 51 deletions(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index a3a1668a46..50163bae56 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -40,6 +40,7 @@ int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation float32 cruising_speed # the generally desired cruising speed (not a hard constraint) +bool gliding_enabled # commands the vehicle to glide if the capability is available (fixed wing only) float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint) bool disable_weather_vane # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index e3ce212fed..a7fddf10b3 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -955,7 +955,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), _param_fw_thr_max.get(), - _param_fw_thr_cruise.get(), false, _param_fw_p_lim_min.get()); @@ -989,7 +988,6 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), _param_fw_thr_max.get(), - _param_fw_thr_cruise.get(), false, _param_fw_p_lim_min.get(), false, @@ -1088,26 +1086,16 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co float tecs_fw_thr_min; float tecs_fw_thr_max; - float tecs_fw_mission_throttle; - float mission_throttle = _param_fw_thr_cruise.get(); - - if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) && - pos_sp_curr.cruising_throttle >= 0.0f) { - mission_throttle = pos_sp_curr.cruising_throttle; - } - - if (mission_throttle < _param_fw_thr_min.get()) { + if (pos_sp_curr.gliding_enabled) { /* enable gliding with this waypoint */ _tecs.set_speed_weight(2.0f); tecs_fw_thr_min = 0.0; tecs_fw_thr_max = 0.0; - tecs_fw_mission_throttle = 0.0; } else { tecs_fw_thr_min = _param_fw_thr_min.get(); tecs_fw_thr_max = _param_fw_thr_max.get(); - tecs_fw_mission_throttle = mission_throttle; } // waypoint is a plain navigation waypoint @@ -1187,7 +1175,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co 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())); } @@ -1198,26 +1185,16 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co { float tecs_fw_thr_min; float tecs_fw_thr_max; - float tecs_fw_mission_throttle; - float mission_throttle = _param_fw_thr_cruise.get(); - - if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) && - pos_sp_curr.cruising_throttle >= 0.0f) { - mission_throttle = pos_sp_curr.cruising_throttle; - } - - if (mission_throttle < _param_fw_thr_min.get()) { + if (pos_sp_curr.gliding_enabled) { /* enable gliding with this waypoint */ _tecs.set_speed_weight(2.0f); tecs_fw_thr_min = 0.0; tecs_fw_thr_max = 0.0; - tecs_fw_mission_throttle = 0.0; } else { tecs_fw_thr_min = _param_fw_thr_min.get(); tecs_fw_thr_max = _param_fw_thr_max.get(); - tecs_fw_mission_throttle = mission_throttle; } // waypoint is a plain navigation waypoint @@ -1254,7 +1231,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co 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()), tecs_status_s::TECS_MODE_NORMAL, @@ -1295,26 +1271,16 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons float tecs_fw_thr_min; float tecs_fw_thr_max; - float tecs_fw_mission_throttle; - float mission_throttle = _param_fw_thr_cruise.get(); - - if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) && - pos_sp_curr.cruising_throttle >= 0.0f) { - mission_throttle = pos_sp_curr.cruising_throttle; - } - - if (mission_throttle < _param_fw_thr_min.get()) { + if (pos_sp_curr.gliding_enabled) { /* enable gliding with this waypoint */ _tecs.set_speed_weight(2.0f); tecs_fw_thr_min = 0.0; tecs_fw_thr_max = 0.0; - tecs_fw_mission_throttle = 0.0; } else { tecs_fw_thr_min = _param_fw_thr_min.get(); tecs_fw_thr_max = _param_fw_thr_max.get(); - tecs_fw_mission_throttle = mission_throttle; } /* waypoint is a loiter waypoint */ @@ -1391,7 +1357,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons 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())); } @@ -1482,7 +1447,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo 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()))); @@ -1570,7 +1534,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo radians(takeoff_pitch_max_deg), _param_fw_thr_min.get(), takeoff_throttle, - _param_fw_thr_cruise.get(), true, radians(_takeoff_pitch_min.get())); @@ -1585,7 +1548,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), takeoff_throttle, - _param_fw_thr_cruise.get(), false, radians(_param_fw_p_lim_min.get())); } @@ -1805,8 +1767,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed); - const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f; - /* lateral guidance */ if (_param_fw_use_npfg.get()) { _npfg.setAirspeedNom(target_airspeed * _eas2tas); @@ -1858,7 +1818,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo radians(_param_fw_lnd_fl_pmax.get()), 0.0f, throttle_max, - throttle_land, false, _land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()), true); @@ -1962,7 +1921,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), _param_fw_thr_max.get(), - _param_fw_thr_cruise.get(), false, radians(_param_fw_p_lim_min.get())); _att_sp.pitch_body = get_tecs_pitch(); @@ -2022,7 +1980,6 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), throttle_max, - _param_fw_thr_cruise.get(), false, pitch_limit_min, false, @@ -2152,7 +2109,6 @@ FixedwingPositionControl::control_manual_position(const float control_interval, radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), throttle_max, - _param_fw_thr_cruise.get(), false, pitch_limit_min, false, @@ -2598,7 +2554,7 @@ float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float void FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad, float pitch_max_rad, - float throttle_min, float throttle_max, float throttle_cruise, + float throttle_min, float throttle_max, bool climbout_mode, float climbout_pitch_min_rad, bool disable_underspeed_detection, float hgt_rate_sp) { @@ -2682,7 +2638,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva climbout_pitch_min_rad - radians(_param_fw_psp_off.get()), throttle_min, throttle_max, - throttle_cruise, + throttle_trim_comp, pitch_min_rad - radians(_param_fw_psp_off.get()), pitch_max_rad - radians(_param_fw_psp_off.get()), _param_climbrate_target.get(), diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 61a63f08a8..c42dcbbe05 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -628,7 +628,6 @@ private: * @param pitch_max_rad Nominal pitch angle command maximum [rad] * @param throttle_min Minimum throttle command [0,1] * @param throttle_max Maximum throttle command [0,1] - * @param throttle_cruise Throttle required for level flight at cruising airspeed [0,1] * @param climbout_mode True if TECS should engage climbout mode * @param climbout_pitch_min_rad Minimum pitch angle command in climbout mode [rad] * @param disable_underspeed_detection True if underspeed detection should be disabled @@ -636,7 +635,7 @@ private: */ void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad, float pitch_max_rad, - float throttle_min, float throttle_max, float throttle_cruise, + float throttle_min, float throttle_max, bool climbout_mode, float climbout_pitch_min_rad, bool disable_underspeed_detection = false, float hgt_rate_sp = NAN); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 6e864eb7a8..1c1c8d2630 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -620,6 +620,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->cruising_speed = _navigator->get_cruising_speed(); sp->cruising_throttle = _navigator->get_cruising_throttle(); + // for fixed wing we don't use cruising_throttle directly anymore, instead we command airspeed setpoints via cruising_speed + // we still use cruising throttle here to determine if gliding is enabled + sp->gliding_enabled = (_navigator->get_cruising_throttle() < FLT_EPSILON); + switch (item.nav_cmd) { case NAV_CMD_IDLE: sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;