HAL_ChibiOS: added ability to restore safety state on watchdog reset

# Conflicts:
#	libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -20,6 +20,7 @@
 #include <AP_HAL/utility/RingBuffer.h>
 #include "GPIO.h"
 #include "hwdef/common/stm32_util.h"
+#include "hwdef/common/watchdog.h"
 
 #if HAL_USE_PWM == TRUE
 
@@ -1330,9 +1331,14 @@ AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void)
 {
 #if HAL_WITH_IO_MCU
     if (AP_BoardConfig::io_enabled()) {
-        return iomcu.get_safety_switch_state();
+        safety_state = iomcu.get_safety_switch_state();
     }
 #endif
+    if (safety_state == AP_HAL::Util::SAFETY_ARMED) {
+        stm32_set_backup_safety_state(false);
+    } else if (safety_state == AP_HAL::Util::SAFETY_DISARMED) {
+        stm32_set_backup_safety_state(true);
+    }
     return safety_state;
 }
 

libraries/AP_HAL_ChibiOS/Util.h

@@ -63,6 +63,11 @@ public:
 
     // return true if the reason for the reboot was a watchdog reset
     bool was_watchdog_reset() const override { return stm32_was_watchdog_reset(); }
+
+    // return true if safety was off and this was a watchdog reset
+    bool was_watchdog_safety_off() const override {
+        return stm32_was_watchdog_reset() && stm32_get_boot_backup_safety_state() == false;
+    }
     
 private:
 #ifdef HAL_PWM_ALARM

libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c

@@ -252,6 +252,29 @@ 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;
+}
+
+#endif //NO_FASTBOOT
+
 /*
   enable peripheral power if needed This is done late to prevent
   problems with CTS causing SiK radios to stay in the bootloader. A

libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h

@@ -79,6 +79,12 @@ void malloc_init(void);
 iomode_t palReadLineMode(ioline_t line);
 #endif
 
+// get RTC backup register 1
+uint32_t get_rtc_backup1(void);
+
+// set RTC backup register 1
+void set_rtc_backup1(uint32_t v);
+    
 #ifdef __cplusplus
 }
 #endif

libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c

@@ -4,6 +4,7 @@
 
 #include "hal.h"
 #include "watchdog.h"
+#include "stm32_util.h"
 
 #ifndef IWDG_BASE
 #if defined(STM32H7)
@@ -53,6 +54,7 @@ typedef struct
 
 static bool was_watchdog_reset;
 static bool watchdog_enabled;
+static bool boot_safety_state;
 
 /*
   setup the watchdog
@@ -85,6 +87,7 @@ void stm32_watchdog_save_reason(void)
 {
     if (WDG_RESET_STATUS & WDG_RESET_IS_IWDG) {
         was_watchdog_reset = true;
+        boot_safety_state = stm32_get_backup_safety_state();
     }
 }
 
@@ -104,3 +107,35 @@ bool stm32_was_watchdog_reset(void)
     return was_watchdog_reset;
 }
 
+/*
+  set the safety state in backup register
+
+  This is stored so that the safety state can be restored after a
+  watchdog reset
+ */
+void stm32_set_backup_safety_state(bool safety_on)
+{
+    uint32_t v = get_rtc_backup1();
+    uint32_t v2 = safety_on?(v|1):(v&~1);
+    if (v != v2) {
+        set_rtc_backup1(v2);
+    }
+}
+
+/*
+  get the safety state in backup register
+  return true if safety is marked as safety on
+*/
+bool stm32_get_backup_safety_state(void)
+{
+    uint32_t v = get_rtc_backup1();
+    return (v&1) != 0;
+}
+
+/*
+  get the safety state in backup register from initial boot
+*/
+bool stm32_get_boot_backup_safety_state(void)
+{
+    return boot_safety_state;
+}

libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h

@@ -28,6 +28,22 @@ void stm32_watchdog_save_reason(void);
   clear reset reason code
  */
 void stm32_watchdog_clear_reason(void);
+
+/*
+  set the safety state in backup register
+ */
+void stm32_set_backup_safety_state(bool safety_on);
+
+/*
+  get the safety state in backup register
+  return true if safety is marked as safety on
+ */
+bool stm32_get_backup_safety_state(void);
+    
+/*
+  get the safety state in backup register from initial boot
+*/
+bool stm32_get_boot_backup_safety_state(void);
     
 #ifdef __cplusplus
 }