From 8d2d80be1616cc442d3b93eb07a2632e4bf738b5 Mon Sep 17 00:00:00 2001 From: Daniel Cook Date: Fri, 16 Feb 2024 12:59:24 +1100 Subject: [PATCH] AP_ExternalAHRS: Added filter updates for advanced navigation devices When running EAHRS update it will now check for flight mode and gnss enable and request the appropriate changes in the device filter. --- .../AP_ExternalAHRS_AdvancedNavigation.cpp | 13 +++++++++++++ .../AP_ExternalAHRS_AdvancedNavigation.h | 3 +++ 2 files changed, 16 insertions(+) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp index 30b5f1aeb91263..ef50dacc3f6170 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp @@ -236,6 +236,19 @@ bool AP_ExternalAHRS_AdvancedNavigation::request_data(void) } } + // check for updates to environment that require INS filter updates + if (_fly_forward != in_fly_forward() || _gnss_disable != gnss_is_disabled()){ + _fly_forward = in_fly_forward(); + _gnss_disable = gnss_is_disabled(); + + // Select AdNav vehicle for current flight mode. + vehicle_type_e vehicle_type = vehicle_type_3d_aircraft; + if (_fly_forward == true) { + vehicle_type = vehicle_type_fixed_wing_plane; + } + + set_filter_options(!_gnss_disable, vehicle_type); + } return true; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h index 9146f865229137..7647b55b31cea0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h @@ -478,6 +478,9 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend uint32_t _last_raw_gnss_pkt_ms; uint32_t _device_id; uint32_t _hardware_rev; + + bool _gnss_disable; + bool _fly_forward; void update_thread(); bool get_packets(void);