From 30c8c2371667a46747961128f4e2a05fdc2ee567 Mon Sep 17 00:00:00 2001 From: QiTao Weng <47102225+qtweng@users.noreply.github.com> Date: Wed, 2 Jul 2025 14:10:28 -0500 Subject: [PATCH] Correct all ADSB heading related field to be in positive range (#25128) --- src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp index 546bbd2bf7..d09ce7280d 100644 --- a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp @@ -614,7 +614,7 @@ void SagetechMXS::handle_svr(sg_svr_t svr) } if (svr.validity.surfHeading) { - t.heading = matrix::wrap_pi((float)svr.surface.heading * (M_PI_F / 180.0f)); + t.heading = matrix::wrap_2pi((float)svr.surface.heading * (M_PI_F / 180.0f)); t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; } } @@ -622,7 +622,7 @@ void SagetechMXS::handle_svr(sg_svr_t svr) if (svr.type == svrAirborne) { if (svr.validity.airSpeed) { t.hor_velocity = (svr.airborne.speed * SAGETECH_SCALE_KNOTS_TO_M_PER_SEC); //Convert from knots to meters/second - t.heading = matrix::wrap_pi((float)svr.airborne.heading * (M_PI_F / 180.0f)); + t.heading = matrix::wrap_2pi((float)svr.airborne.heading * (M_PI_F / 180.0f)); t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; }