fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 22:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FWPositionController: remove factor of 2 for switching to LOITER if altitude is not reached

Browse Source 
Instead check if system has previously switched into LOITER to acheive the current
WP of type POSITION, and in that case stay in LOITER until altitude is reached.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-01-26 17:11:34 +01:00
committed by Roman Bapst
parent e862fde084
commit 48f2b42e12
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+6 -3
View File
@@ -1025,11 +1025,14 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
}
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
// POSITION: achieve position setpoint altitude via loiter
// close to waypoint, but altitude error greater than twice acceptance
// Achieve position setpoint altitude via loiter when laterally close to WP.
// Detect if system has switchted into a Loiter before (check _position_sp_type), and in that
// case remove the dist_xy check (not switch out of Loiter until altitude is reached).
if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
&& (dist_z > _param_nav_fw_alt_rad.get())
&& (dist_xy < 2.f * math::max(acc_rad, loiter_radius_abs))) {
&& (dist_xy < math::max(acc_rad, loiter_radius_abs)
|| _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
}