fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 2021-11-25 11:39:54 +01:00 committed by JaeyoungLim
parent bbb04ab4b8
commit 20a1e5f77c
2 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
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
* 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 &&

4
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
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
* 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(