From 20a1e5f77cb451f4e43d2dd617770fcec84141a0 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 25 Nov 2021 11:39:54 +0100 Subject: [PATCH] FW Position Control: simplify underspeed disabling logic for tecs Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 21 ++++++++----------- .../FixedwingPositionControl.hpp | 4 ++-- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index efe3383267..f5f2d74b05 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -915,7 +915,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 @@ -1347,8 +1347,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _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()); @@ -1419,8 +1418,7 @@ 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)); @@ -1653,7 +1651,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 @@ -1770,7 +1768,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()); @@ -1829,7 +1827,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 */ @@ -2217,7 +2215,7 @@ 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) { const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f); _last_tecs_update = now; @@ -2281,8 +2279,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 && diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 6befe703f7..508f3fa3e0 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -363,7 +363,7 @@ 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, - uint8_t mode = tecs_status_s::TECS_MODE_NORMAL, float hgt_rate_sp = NAN); + bool disable_underspeed_detection = false, float hgt_rate_sp = NAN); DEFINE_PARAMETERS(