From 75ad1a7d31a4baf6623cab0714cb72765b26e057 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 31 Dec 2020 09:42:44 +1100
Subject: [PATCH] AP_AHRS: send a text msg when active EKF changes

---
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 10 ++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  2 ++
 2 files changed, 12 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index f3ef490ec4..b98473f0ce 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -29,6 +29,7 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_Notify/AP_Notify.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <GCS_MAVLink/GCS.h>
 
 #if AP_AHRS_NAVEKF_AVAILABLE
 
@@ -67,6 +68,9 @@ void AP_AHRS_NavEKF::init()
         EKF2.set_enable(true);
     }
 #endif
+
+    last_active_ekf_type = (EKFType)_ekf_type.get();
+
     // init parent class
     AP_AHRS_DCM::init();
 }
@@ -173,6 +177,12 @@ void AP_AHRS_NavEKF::update(bool skip_ins_update)
     // update NMEA output
     update_nmea_out();
 #endif
+
+    EKFType active = active_EKF_type();
+    if (active != last_active_ekf_type) {
+        last_active_ekf_type = active;
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AHRS: EKF%u active", unsigned(active));
+    }
 }
 
 void AP_AHRS_NavEKF::update_DCM(bool skip_ins_update)
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index cee713a53e..a818096450 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -389,6 +389,8 @@ private:
     TriState takeoffExpectedState = TriState::UNKNOWN;
     TriState terrainHgtStableState = TriState::UNKNOWN;
 
+    EKFType last_active_ekf_type;
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     SITL::SITL *_sitl;
     uint32_t _last_body_odm_update_ms;