From 5c823debc04f28cc0c6a9eccc4298a4f75126988 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Jan 2021 16:01:04 +1100 Subject: [PATCH] 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 --- .../AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp | 4 + libraries/AP_HAL_ChibiOS/Util.cpp | 114 +++++++++++++++++- libraries/AP_HAL_ChibiOS/Util.h | 22 +++- 3 files changed, 138 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 4a38ef0651..acfcefde71 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/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) diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index a9b713154a..90c4f35587 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -27,6 +27,11 @@ #include #include "sdcard.h" #include "shared_dma.h" +#include + +#if HAL_ENABLE_SAVE_PERSISTENT_PARAMS +#include +#endif #if HAL_WITH_IO_MCU #include @@ -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 + diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 36bc95ac57..ebf23b3b6b 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -21,6 +21,15 @@ #include "AP_HAL_ChibiOS.h" #include +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 };