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

FW Position Control: simplify underspeed disabling logic for tecs

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-01-05 17:06:41 +01:00
committed by JaeyoungLim
parent bbb04ab4b8
commit 20a1e5f77c
2 changed files with 11 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+9 -12
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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(), _param_fw_thr_cruise.get(),
false, false,
_param_fw_p_lim_min.get(), _param_fw_p_lim_min.get(),
tecs_status_s::TECS_MODE_NORMAL, false,
descend_rate); descend_rate);
_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle _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_max.get(), // XXX should we also set runway_takeoff_throttle here?
_param_fw_thr_cruise.get(), _param_fw_thr_cruise.get(),
_runway_takeoff.climbout(), _runway_takeoff.climbout(),
radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())), radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())));
tecs_status_s::TECS_MODE_TAKEOFF);
// assign values // assign values
_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint()); _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, takeoff_throttle,
_param_fw_thr_cruise.get(), _param_fw_thr_cruise.get(),
true, true,
radians(_takeoff_pitch_min.get()), radians(_takeoff_pitch_min.get()));
tecs_status_s::TECS_MODE_TAKEOFF);
/* limit roll motion to ensure enough lift */ /* limit roll motion to ensure enough lift */
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f)); _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, throttle_land,
false, false,
_land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()), _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) { if (!_land_noreturn_vertical) {
// just started with the flaring phase // just started with the flaring phase
@@ -1770,7 +1768,7 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
_param_fw_thr_cruise.get(), _param_fw_thr_cruise.get(),
false, false,
pitch_limit_min, pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL, false,
height_rate_sp); height_rate_sp);
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); _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(), _param_fw_thr_cruise.get(),
false, false,
pitch_limit_min, pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL, false,
height_rate_sp); height_rate_sp);
/* heading control */ /* 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 pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise, float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad, 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); const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f);
_last_tecs_update = now; _last_tecs_update = now;
@@ -2281,8 +2279,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
} }
/* No underspeed protection in landing mode */ /* No underspeed protection in landing mode */
_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND _tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
|| mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
/* tell TECS to update its state, but let it know when it cannot actually control the plane */ /* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_landed && bool in_air_alt_control = (!_landed &&
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+2 -2
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -363,7 +363,7 @@ private:
float pitch_min_rad, float pitch_max_rad, float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise, float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad, 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( DEFINE_PARAMETERS(