HAL_ChibiOS: support saving persistent parameters to bootloader sector

when we flash the bootloader we can save key peristent parameters to
the end of the bootloader sector, allowing them to persistent across
changes of firmware type

This allows for factory temperature calibration

libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp

@@ -221,6 +221,10 @@ static void main_loop()
 
     g_callbacks->setup();
 
+#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
+    utilInstance.apply_persistent_params();
+#endif
+
 #ifdef IOMCU_FW
     stm32_watchdog_init();
 #elif !defined(HAL_BOOTLOADER_BUILD)

libraries/AP_HAL_ChibiOS/Util.cpp

@@ -27,6 +27,11 @@
 #include <AP_Common/ExpandingString.h>
 #include "sdcard.h"
 #include "shared_dma.h"
+#include <AP_Common/ExpandingString.h>
+
+#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
+#include <AP_InertialSensor/AP_InertialSensor.h>
+#endif
 
 #if HAL_WITH_IO_MCU
 #include <AP_BoardConfig/AP_BoardConfig.h>
@@ -209,8 +214,32 @@ Util::FlashBootloader Util::flash_bootloader()
     // make sure size is multiple of 32
     fw_size = (fw_size + 31U) & ~31U;
 
+    bool uptodate = true;
     const uint32_t addr = hal.flash->getpageaddr(0);
-    if (!memcmp(fw, (const void*)addr, fw_size)) {
+
+    if (memcmp(fw, (const void*)addr, fw_size) != 0) {
+        uptodate = false;
+    }
+
+#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
+    // see if we should store persistent parameters along with the
+    // bootloader. We only do this on boards using a single sector for
+    // the bootloader. The persistent parameters are stored as text at
+    // the end of the sector
+    const int32_t space_available = hal.flash->getpagesize(0) - int32_t(fw_size);
+    ExpandingString persistent_params {}, old_persistent_params {};
+    if (get_persistent_params(persistent_params) &&
+        space_available >= persistent_params.get_length() &&
+        (!load_persistent_params(old_persistent_params) ||
+         strcmp(persistent_params.get_string(),
+                old_persistent_params.get_string()) != 0)) {
+        // persistent parameters have changed, we will update
+        // bootloader to allow storage of the params
+        uptodate = false;
+    }
+#endif
+
+    if (uptodate) {
         Debug("Bootloader up-to-date\n");
         AP_ROMFS::free(fw);
         return FlashBootloader::NO_CHANGE;
@@ -249,6 +278,12 @@ Util::FlashBootloader Util::flash_bootloader()
             continue;
         }
         Debug("Flash OK\n");
+#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
+        if (persistent_params.get_length()) {
+            const uint32_t ofs = hal.flash->getpagesize(0) - persistent_params.get_length();
+            hal.flash->write(addr+ofs, persistent_params.get_string(), persistent_params.get_length());
+        }
+#endif
         hal.flash->keep_unlocked(false);
         AP_ROMFS::free(fw);
         return FlashBootloader::OK;
@@ -342,3 +377,80 @@ void Util::dma_info(ExpandingString &str)
     ChibiOS::Shared_DMA::dma_info(str);
 }
 #endif
+
+#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
+
+static const char *persistent_header = "{{PERSISTENT_START_V1}}\n";
+
+/*
+  create a set of persistent parameters in string form
+ */
+bool Util::get_persistent_params(ExpandingString &str) const
+{
+    str.printf("%s", persistent_header);
+#if HAL_INS_TEMPERATURE_CAL_ENABLE
+    const auto *ins = AP_InertialSensor::get_singleton();
+    if (ins) {
+        ins->get_persistent_params(str);
+    }
+#endif
+    if (str.has_failed_allocation() || str.get_length() <= strlen(persistent_header)) {
+        // no data
+        return false;
+    }
+    // ensure that the length is a multiple of 32 to meet flash alignment requirements
+    while (!str.has_failed_allocation() && str.get_length() % 32 != 0) {
+        str.append("\n", 1);
+    }
+    return !str.has_failed_allocation();
+}
+
+/*
+  load a set of persistent parameters in string form from the bootloader sector
+ */
+bool Util::load_persistent_params(ExpandingString &str) const
+{
+    const uint32_t addr = hal.flash->getpageaddr(0);
+    const uint32_t size = hal.flash->getpagesize(0);
+    const char *s = (const char *)memmem((void*)addr, size,
+                                         persistent_header,
+                                         strlen(persistent_header));
+    if (s) {
+        str.append(s, (addr+size) - uint32_t(s));
+        return !str.has_failed_allocation();
+    }
+    return false;
+}
+
+/*
+  apply persistent parameters from the bootloader sector to AP_Param
+ */
+void Util::apply_persistent_params(void) const
+{
+    ExpandingString str {};
+    if (!load_persistent_params(str)) {
+        return;
+    }
+    char *s = str.get_writeable_string();
+    char *saveptr;
+    s += strlen(persistent_header);
+    uint32_t count = 0;
+    for (char *p = strtok_r(s, "\n", &saveptr);
+         p; p = strtok_r(nullptr, "\n", &saveptr)) {
+        char *eq = strchr(p, int('='));
+        if (eq) {
+            *eq = 0;
+            if (AP_Param::set_default_by_name(p, atof(eq+1))) {
+                count++;
+            }
+        }
+    }
+    if (count) {
+        AP_Param::invalidate_count();
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Loaded %u persistent parameters",
+                      unsigned(count));
+    }
+}
+
+#endif // HAL_ENABLE_SAVE_PERSISTENT_PARAMS
+

libraries/AP_HAL_ChibiOS/Util.h

@@ -21,6 +21,15 @@
 #include "AP_HAL_ChibiOS.h"
 #include <ch.h>
 
+class ExpandingString;
+
+#ifndef HAL_ENABLE_SAVE_PERSISTENT_PARAMS
+// on F7 and H7 we will try to save key persistent parameters at the
+// end of the bootloader sector. This enables temperature calibration
+// data to be saved persistently in the factory
+#define HAL_ENABLE_SAVE_PERSISTENT_PARAMS !defined(HAL_BOOTLOADER_BUILD) && (defined(STM32F7) || defined(STM32H7))
+#endif
+
 class ChibiOS::Util : public AP_HAL::Util {
 public:
     static Util *from(AP_HAL::Util *util) {
@@ -66,7 +75,12 @@ public:
     // request information on dma contention
     void dma_info(ExpandingString &str) override;
 #endif
-    
+
+#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
+    // apply persistent parameters to current parameters
+    void apply_persistent_params(void) const;
+#endif
+
 private:
 #ifdef HAL_PWM_ALARM
     struct ToneAlarmPwmGroup {
@@ -98,4 +112,10 @@ private:
     // stm32F4 and F7 have 20 total RTC backup registers. We use the first one for boot type
     // flags, so 19 available for persistent data
     static_assert(sizeof(persistent_data) <= 19*4, "watchdog persistent data too large");
+
+#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS
+    // save/load key persistent parameters in bootloader sector
+    bool get_persistent_params(ExpandingString &str) const;
+    bool load_persistent_params(ExpandingString &str) const;
+#endif
 };