From 6e911c59e9d78ac3c656503a886b8aad81dfb302 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 27 Feb 2024 09:22:11 +1100 Subject: [PATCH] AP_ExternalAHRS: fixed origin and latlon scale for AdNav driver --- .../AP_ExternalAHRS_AdvancedNavigation.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp index fb4a0f14bcd4ab..2d51db65501d85 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp @@ -666,12 +666,17 @@ void AP_ExternalAHRS_AdvancedNavigation::handle_packet() if (get_gnss_capability()) { state.have_location = true; + const double latlon_scale = degrees(1) * 1.0e7; state.location = Location{ - (int32_t) (degrees(_msg.packet.payload.system_state.llh[0]) * 1.0e7), - (int32_t) (degrees(_msg.packet.payload.system_state.llh[1]) * 1.0e7), + (int32_t) (_msg.packet.payload.system_state.llh[0] * latlon_scale), + (int32_t) (_msg.packet.payload.system_state.llh[1] * latlon_scale), (int32_t) (_msg.packet.payload.system_state.llh[2] *1.0e2), Location::AltFrame::ABSOLUTE }; + if (!state.have_origin) { + state.origin = state.location; + state.have_origin = true; + } state.last_location_update_us = AP_HAL::micros(); }