From 8caf7d5811a26ab7371d169726a9fd7d3b512121 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 22 Apr 2020 08:57:07 +0900
Subject: [PATCH] AP_AHRS: add set_alt_measurement_noise

These calls the EKF2 and EKF3's set_baro_alt_noise
---
 libraries/AP_AHRS/AP_AHRS.h          |  5 ++++-
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 11 +++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  3 +++
 3 files changed, 18 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 54f7bf892d..f44612c014 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -561,7 +561,10 @@ public:
 
     // return current vibration vector for primary IMU
     Vector3f get_vibration(void) const;
-    
+
+    // set and save the alt noise parameter value
+    virtual void set_alt_measurement_noise(float noise) {};
+
     // allow threads to lock against AHRS update
     HAL_Semaphore &get_semaphore(void) {
         return _rsem;
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 05b9b3b1aa..68ce7d1817 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -2290,4 +2290,15 @@ bool AP_AHRS_NavEKF::is_ext_nav_used_for_yaw(void) const
     return false;
 }
 
+// set and save the alt noise parameter value
+void AP_AHRS_NavEKF::set_alt_measurement_noise(float noise)
+{
+#if HAL_NAVEKF2_AVAILABLE
+    EKF2.set_baro_alt_noise(noise);
+#endif
+#if HAL_NAVEKF3_AVAILABLE
+    EKF3.set_baro_alt_noise(noise);
+#endif
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index df348c6ecf..a02a7ce9de 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -298,6 +298,9 @@ public:
     // check whether external navigation is providing yaw.  Allows compass pre-arm checks to be bypassed
     bool is_ext_nav_used_for_yaw(void) const override;
 
+    // set and save the ALT_M_NSE parameter value
+    void set_alt_measurement_noise(float noise) override;
+
     // these are only out here so vehicles can reference them for parameters
 #if HAL_NAVEKF2_AVAILABLE
     NavEKF2 EKF2;