From 723a7bb6478bfa2956583eda416273d612334c53 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 20 Apr 2019 19:49:46 +1000
Subject: [PATCH] HAL_ChibiOS: save/restore home position in backup registers

---
 libraries/AP_HAL_ChibiOS/Util.cpp             | 16 ++++++
 libraries/AP_HAL_ChibiOS/Util.h               |  6 +++
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.c  | 35 +++----------
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.h  | 10 ++--
 .../AP_HAL_ChibiOS/hwdef/common/watchdog.c    | 52 ++++++++++++++-----
 .../AP_HAL_ChibiOS/hwdef/common/watchdog.h    | 10 ++++
 6 files changed, 84 insertions(+), 45 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp
index 44bcf8289c..5e320707e0 100644
--- a/libraries/AP_HAL_ChibiOS/Util.cpp
+++ b/libraries/AP_HAL_ChibiOS/Util.cpp
@@ -296,3 +296,19 @@ void Util::set_soft_armed(const bool b)
     AP_HAL::Util::set_soft_armed(b);
     stm32_set_backup_armed(b);
 }
+
+// backup home state for restore on watchdog reset
+void Util::set_backup_home_state(int32_t lat, int32_t lon, int32_t alt_cm) const
+{
+    stm32_set_backup_home(lat, lon, alt_cm);
+}
+
+// backup home state for restore on watchdog reset
+bool Util::get_backup_home_state(int32_t &lat, int32_t &lon, int32_t &alt_cm) const
+{
+    if (was_watchdog_reset()) {
+        stm32_get_backup_home(&lat, &lon, &alt_cm);
+        return true;
+    }
+    return false;
+}
diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h
index de63cd71d4..ae5e19f0b1 100644
--- a/libraries/AP_HAL_ChibiOS/Util.h
+++ b/libraries/AP_HAL_ChibiOS/Util.h
@@ -69,6 +69,12 @@ public:
     // return true if vehicle was armed and this was a watchdog reset
     bool was_watchdog_armed() const override;
 
+    // backup home state for restore on watchdog reset
+    void set_backup_home_state(int32_t lat, int32_t lon, int32_t alt_cm) const override;
+
+    // backup home state for restore on watchdog reset
+    bool get_backup_home_state(int32_t &lat, int32_t &lon, int32_t &alt_cm) const override;
+    
 private:
 #ifdef HAL_PWM_ALARM
     struct ToneAlarmPwmGroup {
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
index a91d389f6e..c7345d1b35 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
@@ -219,14 +219,14 @@ uint32_t get_fattime()
     return fattime;
 }
 
-// get RTC backup register 0
-static uint32_t get_rtc_backup0(void)
+// get RTC backup register
+uint32_t get_rtc_backup(uint8_t n)
 {
-	return RTC->BKP0R;
+    return ((__IO uint32_t *)&RTC->BKP0R)[n];
 }
 
 // set RTC backup register 0
-static void set_rtc_backup0(uint32_t v)
+void set_rtc_backup(uint8_t n, uint32_t v)
 {
     if ((RCC->BDCR & RCC_BDCR_RTCEN) == 0) {
         RCC->BDCR |= STM32_RTCSEL;
@@ -237,40 +237,19 @@ static void set_rtc_backup0(uint32_t v)
 #else
     PWR->CR1 |= PWR_CR1_DBP;
 #endif
-    RTC->BKP0R = v;
+    ((__IO uint32_t *)&RTC->BKP0R)[n] = v;
 }
 
 // see if RTC registers is setup for a fast reboot
 enum rtc_boot_magic check_fast_reboot(void)
 {
-    return (enum rtc_boot_magic)get_rtc_backup0();
+    return (enum rtc_boot_magic)get_rtc_backup(0);
 }
 
 // set RTC register for a fast reboot
 void set_fast_reboot(enum rtc_boot_magic v)
 {
-    set_rtc_backup0(v);
-}
-
-// get RTC backup register 1
-uint32_t get_rtc_backup1(void)
-{
-    return RTC->BKP1R;
-}
-
-// set RTC backup register 1
-void set_rtc_backup1(uint32_t v)
-{
-    if ((RCC->BDCR & RCC_BDCR_RTCEN) == 0) {
-        RCC->BDCR |= STM32_RTCSEL;
-        RCC->BDCR |= RCC_BDCR_RTCEN;
-    }
-#ifdef PWR_CR_DBP
-    PWR->CR |= PWR_CR_DBP;
-#else
-    PWR->CR1 |= PWR_CR1_DBP;
-#endif
-    RTC->BKP1R = v;
+    set_rtc_backup(0, v);
 }
 
 /*
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
index 5a2e6119e9..50282eca14 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
@@ -79,12 +79,12 @@ void malloc_init(void);
 iomode_t palReadLineMode(ioline_t line);
 #endif
 
-// get RTC backup register 1
-uint32_t get_rtc_backup1(void);
+// get RTC backup register
+uint32_t get_rtc_backup(uint8_t n);
+
+// set RTC backup register
+void set_rtc_backup(uint8_t n, uint32_t v);
 
-// set RTC backup register 1
-void set_rtc_backup1(uint32_t v);
-    
 #ifdef __cplusplus
 }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c
index 8a9b26e77a..7acd386d60 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c
@@ -41,6 +41,13 @@
 #error "Unsupported IWDG MCU config"
 #endif
 
+#define WDG_SAFETY_BIT  0x01
+#define WDG_ARMED_BIT   0x02
+
+#define BKP_IDX_FLAGS   0x01
+#define BKP_IDX_HOME    0x02
+
+
 typedef struct
 {
   __IO uint32_t KR;   /*!< IWDG Key register,       Address offset: 0x00 */
@@ -54,7 +61,7 @@ typedef struct
 
 static bool was_watchdog_reset;
 static bool watchdog_enabled;
-static uint32_t boot_backup1_state;
+static uint32_t boot_backup_state[5];
 
 /*
   setup the watchdog
@@ -87,7 +94,9 @@ void stm32_watchdog_save_reason(void)
 {
     if (WDG_RESET_STATUS & WDG_RESET_IS_IWDG) {
         was_watchdog_reset = true;
-        boot_backup1_state = get_rtc_backup1();
+        for (uint8_t i=0; i<sizeof(boot_backup_state)/sizeof(boot_backup_state[0]); i++) {
+            boot_backup_state[i] = get_rtc_backup(i);
+        }
     }
 }
 
@@ -97,7 +106,9 @@ void stm32_watchdog_save_reason(void)
 void stm32_watchdog_clear_reason(void)
 {
     WDG_RESET_STATUS = WDG_RESET_CLEAR;
-    set_rtc_backup1(0);
+    for (uint8_t i=1; i<sizeof(boot_backup_state)/sizeof(boot_backup_state[0]); i++) {
+        set_rtc_backup(i, 0);
+    }
 }
 
 /*
@@ -108,9 +119,6 @@ bool stm32_was_watchdog_reset(void)
     return was_watchdog_reset;
 }
 
-#define WDG_SAFETY_BIT  0x01
-#define WDG_ARMED_BIT   0x02
-
 /*
   set the safety state in backup register
 
@@ -119,10 +127,10 @@ bool stm32_was_watchdog_reset(void)
  */
 void stm32_set_backup_safety_state(bool safety_on)
 {
-    uint32_t v = get_rtc_backup1();
+    uint32_t v = get_rtc_backup(BKP_IDX_FLAGS);
     uint32_t v2 = safety_on?(v | WDG_SAFETY_BIT):(v & ~WDG_SAFETY_BIT);
     if (v != v2) {
-        set_rtc_backup1(v2);
+        set_rtc_backup(BKP_IDX_FLAGS, v2);
     }
 }
 
@@ -131,7 +139,7 @@ void stm32_set_backup_safety_state(bool safety_on)
 */
 bool stm32_get_boot_backup_safety_state(void)
 {
-    return (boot_backup1_state & WDG_SAFETY_BIT) != 0;
+    return (boot_backup_state[BKP_IDX_FLAGS] & WDG_SAFETY_BIT) != 0;
 }
 
 /*
@@ -142,10 +150,10 @@ bool stm32_get_boot_backup_safety_state(void)
  */
 void stm32_set_backup_armed(bool armed)
 {
-    uint32_t v = get_rtc_backup1();
+    uint32_t v = get_rtc_backup(BKP_IDX_FLAGS);
     uint32_t v2 = armed?(v | WDG_ARMED_BIT): (v & ~WDG_ARMED_BIT);
     if (v != v2) {
-        set_rtc_backup1(v2);
+        set_rtc_backup(BKP_IDX_FLAGS, v2);
     }
 }
 
@@ -154,5 +162,25 @@ void stm32_set_backup_armed(bool armed)
 */
 bool stm32_get_boot_backup_armed(void)
 {
-    return (boot_backup1_state & WDG_ARMED_BIT) != 0;
+    return (boot_backup_state[BKP_IDX_FLAGS] & WDG_ARMED_BIT) != 0;
+}
+
+/*
+  set home state in backup
+ */
+void stm32_set_backup_home(int32_t lat, int32_t lon, int32_t alt_cm)
+{
+    set_rtc_backup(BKP_IDX_HOME, (uint32_t)lat);
+    set_rtc_backup(BKP_IDX_HOME+1, (uint32_t)lon);
+    set_rtc_backup(BKP_IDX_HOME+2, (uint32_t)alt_cm);
+}
+
+/*
+  get home state from backup
+ */
+void stm32_get_backup_home(int32_t *lat, int32_t *lon, int32_t *alt_cm)
+{
+    *lat = (int32_t)boot_backup_state[BKP_IDX_HOME];
+    *lon = (int32_t)boot_backup_state[BKP_IDX_HOME+1];
+    *alt_cm = (int32_t)boot_backup_state[BKP_IDX_HOME+2];
 }
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h
index 390d9e1684..8b11ffb677 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h
@@ -48,6 +48,16 @@ void stm32_set_backup_armed(bool armed);
   get the armed state in backup register from initial boot
 */
 bool stm32_get_boot_backup_armed(void);
+
+/*
+  set home state in backup
+ */
+void stm32_set_backup_home(int32_t lat, int32_t lon, int32_t alt_cm);
+
+/*
+  get home state from backup
+ */
+void stm32_get_backup_home(int32_t *lat, int32_t *lon, int32_t *alt_cm);
     
 #ifdef __cplusplus
 }