From f82c72265382a27affb0ee018dc431db64bb46d2 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Thu, 3 Feb 2022 10:16:39 +0100 Subject: [PATCH] Publish correct orbit status with goto (#19102) For fixedwings, the orbit status was publishing zero radius when a goto waypoint was being passed. This commit corrects this by passing the default loiter radius as the guidance logic was using --- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index f29a4361f0..cde39b63d9 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -2605,7 +2605,15 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_ { orbit_status_s orbit_status{}; orbit_status.timestamp = hrt_absolute_time(); - orbit_status.radius = static_cast(pos_sp.loiter_direction) * pos_sp.loiter_radius; + float loiter_radius = pos_sp.loiter_radius; + uint8_t loiter_direction = pos_sp.loiter_direction; + + if (fabsf(loiter_radius) < FLT_EPSILON) { + loiter_radius = _param_nav_loiter_rad.get(); + loiter_direction = (loiter_radius > 0) ? 1 : -1; + } + + orbit_status.radius = static_cast(loiter_direction) * loiter_radius; orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL orbit_status.x = static_cast(pos_sp.lat); orbit_status.y = static_cast(pos_sp.lon);