HAL_ChibiOS: added save/restore of attitude in backup registers

libraries/AP_HAL_ChibiOS/Util.cpp

@@ -361,3 +361,19 @@ bool Util::get_backup_home_state(int32_t &lat, int32_t &lon, int32_t &alt_cm) co
     }
     return false;
 }
+
+// backup atttude for restore on watchdog reset
+void Util::set_backup_attitude(int32_t roll_cd, int32_t pitch_cd, int32_t yaw_cd) const
+{
+    stm32_set_attitude(roll_cd, pitch_cd, yaw_cd);
+}
+
+// get watchdog reset attitude
+bool Util::get_backup_attitude(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_cd) const
+{
+    if (was_watchdog_reset()) {
+        stm32_get_attitude(&roll_cd, &pitch_cd, &yaw_cd);
+        return true;
+    }
+    return false;
+}

libraries/AP_HAL_ChibiOS/Util.h

@@ -79,6 +79,12 @@ public:
 
     // 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;
+
+    // backup atttude for restore on watchdog reset
+    void set_backup_attitude(int32_t roll_cd, int32_t pitch_cd, int32_t yaw_cd) const override;
+
+    // get watchdog reset attitude
+    bool get_backup_attitude(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_cd) const override;
     
 private:
 #ifdef HAL_PWM_ALARM

libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c

@@ -46,7 +46,7 @@
 
 #define BKP_IDX_FLAGS   0x01
 #define BKP_IDX_HOME    0x02
-
+#define BKP_IDX_RPY     0x05
 
 typedef struct
 {
@@ -61,7 +61,7 @@ typedef struct
 
 static bool was_watchdog_reset;
 static bool watchdog_enabled;
-static uint32_t boot_backup_state[5];
+static uint32_t boot_backup_state[8];
 
 /*
   setup the watchdog
@@ -186,3 +186,23 @@ void stm32_get_backup_home(int32_t *lat, int32_t *lon, int32_t *alt_cm)
     *lon = (int32_t)boot_backup_state[BKP_IDX_HOME+1];
     *alt_cm = (int32_t)boot_backup_state[BKP_IDX_HOME+2];
 }
+
+/*
+  set attitude in backup
+ */
+void stm32_set_attitude(int32_t roll_cd, int32_t pitch_cd, int32_t yaw_cd)
+{
+    set_rtc_backup(BKP_IDX_RPY, (uint32_t)roll_cd);
+    set_rtc_backup(BKP_IDX_RPY+1, (uint32_t)pitch_cd);
+    set_rtc_backup(BKP_IDX_RPY+2, (uint32_t)yaw_cd);
+}
+
+/*
+  get attitude from backup
+ */
+void stm32_get_attitude(int32_t *roll_cd, int32_t *pitch_cd, int32_t *yaw_cd)
+{
+    *roll_cd = (int32_t)boot_backup_state[BKP_IDX_RPY];
+    *pitch_cd = (int32_t)boot_backup_state[BKP_IDX_RPY+1];
+    *yaw_cd = (int32_t)boot_backup_state[BKP_IDX_RPY+2];
+}

libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h

@@ -58,6 +58,16 @@ 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);
+
+/*
+  set attitude in backup
+ */
+void stm32_set_attitude(int32_t roll_cd, int32_t pitch_cd, int32_t yaw_cd);
+
+/*
+  get attitude from backup
+ */
+void stm32_get_attitude(int32_t *roll_cd, int32_t *pitch_cd, int32_t *yaw_cd);
     
 #ifdef __cplusplus
 }