From 0607982b234f4ddc0f8037829fcb051d8cb34b98 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Fri, 21 Jan 2022 09:13:03 +0100 Subject: [PATCH] Publish orbit status also when the waypoint is a loiter waypoint (#19048) * Publish orbit status also when the waypoint is a loiter waypoint * Remove redundant orbit status publishing --- src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 68d24fad2a..92e24042c1 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -872,6 +872,11 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c _position_sp_type = position_sp_type; + if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER + || current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + publishOrbitStatus(current_sp); + } + switch (position_sp_type) { case position_setpoint_s::SETPOINT_TYPE_IDLE: _att_sp.thrust_body[0] = 0.0f; @@ -889,8 +894,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c case position_setpoint_s::SETPOINT_TYPE_LOITER: control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); - publishOrbitStatus(current_sp); - break; case position_setpoint_s::SETPOINT_TYPE_LAND: