From 285b53fe074343d202ada50d4d5fd00717cb62a5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 18 Jan 2021 12:29:02 +1100
Subject: [PATCH] AP_InertialSensor: use vector polyfit

this reduces memory usage
---
 .../AP_InertialSensor/AP_InertialSensor.h     |  4 +-
 .../AP_InertialSensor_tempcal.cpp             | 50 +++++++++----------
 2 files changed, 27 insertions(+), 27 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 4b92924bd1..19fd7acb68 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -728,7 +728,9 @@ public:
                 Vector3f sum;
                 uint32_t sum_count;
                 LowPassFilter2p<float> temp_filter;
-                PolyFit<4> pfit[3];
+                // double precision is needed for good results when we
+                // span a wide range of temperatures
+                PolyFit<4, double, Vector3f> pfit;
             } state[2];
 
             void add_sample(const Vector3f &sample, float temperature, LearnState &state);
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp
index 1ffe5835b7..53051ad4cc 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp
@@ -237,18 +237,21 @@ void AP_InertialSensor::TCal::correct_gyro(float temperature, float cal_temp, Ve
     correct_sensor(temperature, cal_temp, gyro_coeff, gyro);
 }
 
+/*
+  for SITL we don't apply the temperature limits and use mid-point as
+  reference. This makes the SITL data independent of TEMP_REFERENCE
+  and prevents an abrupt change at the endpoints
+ */
 void AP_InertialSensor::TCal::sitl_apply_accel(float temperature, Vector3f &accel) const
 {
-    Vector3f v;
-    correct_sensor(temperature, TEMP_REFERENCE, accel_coeff, v);
-    accel -= v;
+    const float tmid = 0.5*(temp_max+temp_min);
+    accel += polynomial_eval(temperature - tmid, accel_coeff);
 }
 
 void AP_InertialSensor::TCal::sitl_apply_gyro(float temperature, Vector3f &gyro) const
 {
-    Vector3f v;
-    correct_sensor(temperature, TEMP_REFERENCE, gyro_coeff, v);
-    gyro -= v;
+    const float tmid = 0.5*(temp_max+temp_min);
+    gyro += polynomial_eval(temperature - tmid, gyro_coeff);
 }
 
 AP_InertialSensor::TCal::Learn::Learn(TCal &_tcal, float _start_temp) :
@@ -312,9 +315,7 @@ void AP_InertialSensor::TCal::Learn::add_sample(const Vector3f &sample, float te
                        st.sum_count);
     
     
-    st.pfit[0].update(tdiff, st.sum.x);
-    st.pfit[1].update(tdiff, st.sum.y);
-    st.pfit[2].update(tdiff, st.sum.z);
+    st.pfit.update(tdiff, st.sum);
 
     st.sum.zero();
     st.sum_count = 0;
@@ -408,28 +409,25 @@ bool AP_InertialSensor::TCal::Learn::save_calibration(float temperature)
 {
     Vector3f coefficients[3];
 
-    for (uint8_t i=0; i<3; i++) {
-        float p[4];
-        if (!state[0].pfit[i].get_polynomial(p)) {
-            return false;
-        }
-        for (uint8_t k=0; k<3; k++) {
-            coefficients[k][i] = p[2-k] * SCALE_FACTOR;
-        }
+    Vector3f p[4];
+    if (!state[0].pfit.get_polynomial(p)) {
+        return false;
+    }
+    for (uint8_t k=0; k<3; k++) {
+        coefficients[k] = p[2-k] * SCALE_FACTOR;
     }
+
     for (uint8_t k=0; k<3; k++) {
         tcal.accel_coeff[k].set_and_save_ifchanged(coefficients[k]);
     }
 
-    for (uint8_t i=0; i<3; i++) {
-        float p[4];
-        if (!state[1].pfit[i].get_polynomial(p)) {
-            return false;
-        }
-        for (uint8_t k=0; k<3; k++) {
-            coefficients[k][i] = p[2-k] * SCALE_FACTOR;
-        }
+    if (!state[1].pfit.get_polynomial(p)) {
+        return false;
     }
+    for (uint8_t k=0; k<3; k++) {
+        coefficients[k] = p[2-k] * SCALE_FACTOR;
+    }
+
     for (uint8_t k=0; k<3; k++) {
         tcal.gyro_coeff[k].set_and_save_ifchanged(coefficients[k]);
     }
@@ -502,7 +500,7 @@ void AP_InertialSensor::get_persistent_params(ExpandingString &str) const
     if (save_options) {
         /*
           we also have to save the TCAL_OPTIONS parameter so that
-          fuuture flashing of the bootloader doesn't cause an erase
+          future flashing of the bootloader doesn't cause an erase
          */
         str.printf("INS_TCAL_OPTIONS=%u\n", unsigned(tcal_options.get()));
     }