From 25dd08a1fdc68183f1f300f1f70d829ff9a7a134 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 11 Feb 2021 11:49:08 +1100
Subject: [PATCH] AP_InertialSensor: added force_save_calibration()

---
 libraries/AP_InertialSensor/AP_InertialSensor.cpp | 14 ++++++++++++++
 libraries/AP_InertialSensor/AP_InertialSensor.h   |  3 +++
 2 files changed, 17 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 4651dcd9d3..a21fcbd17f 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -2382,6 +2382,20 @@ void AP_InertialSensor::handle_external(const AP_ExternalAHRS::ins_data_message_
 }
 #endif // HAL_EXTERNAL_AHRS_ENABLED
 
+// force save of current calibration as valid
+void AP_InertialSensor::force_save_calibration(void)
+{
+    for (uint8_t i=0; i<_accel_count; i++) {
+        if (_accel_id[i] != 0) {
+            _accel_id[i].save();
+            // we also save the scale as the default of 1.0 may be
+            // over a stored value of 0.0
+            _accel_scale[i].save();
+            _accel_id_ok[i] = true;
+        }
+    }
+}
+
 namespace AP {
 
 AP_InertialSensor &ins()
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 2be2ac6f25..bd6efbf11d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -441,6 +441,9 @@ public:
     void get_persistent_params(ExpandingString &str) const;
 #endif
 
+    // force save of current calibration as valid
+    void force_save_calibration(void);
+
 private:
     // load backend drivers
     bool _add_backend(AP_InertialSensor_Backend *backend);