AP_HAL_ChibiOS: stash lr_thd in watchdog hardfault handler

libraries/AP_HAL/Util.h

@@ -72,6 +72,7 @@ public:
         uint8_t fault_thd_prio;
         uint32_t fault_addr;
         uint32_t fault_icsr;
+        uint32_t fault_lr;
     };
     struct PersistentData persistent_data;
 

libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp

@@ -226,7 +226,7 @@ static void main_loop()
     if (hal.util->was_watchdog_reset()) {
         AP::internalerror().error(AP_InternalError::error_t::watchdog_reset);
         const AP_HAL::Util::PersistentData &pd = last_persistent_data;
-        AP::logger().WriteCritical("WDOG", "TimeUS,Task,IErr,IErrCnt,MavMsg,MavCmd,SemLine,FL,FT,FA,FP,ICSR", "QbIIHHHHHIBI",
+        AP::logger().WriteCritical("WDOG", "TimeUS,Tsk,IE,IEC,MvMsg,MvCmd,SmLn,FL,FT,FA,FP,ICSR,LR", "QbIIHHHHHIBII",
                                    AP_HAL::micros64(),
                                    pd.scheduler_task,
                                    pd.internal_errors,
@@ -238,7 +238,8 @@ static void main_loop()
                                    pd.fault_type,
                                    pd.fault_addr,
                                    pd.fault_thd_prio,
-                                   pd.fault_icsr);
+                                   pd.fault_icsr,
+                                   pd.fault_lr);
     }
 #endif // HAL_NO_LOGGING
 #endif // IOMCU_FW

libraries/AP_HAL_ChibiOS/system.cpp

@@ -56,7 +56,7 @@ void NMI_Handler(void) { while (1); }
 /*
   save watchdog data for a hard fault
  */
-static void save_fault_watchdog(uint16_t line, FaultType fault_type, uint32_t fault_addr)
+    static void save_fault_watchdog(uint16_t line, FaultType fault_type, uint32_t fault_addr, uint32_t lr)
 {
 #ifndef HAL_BOOTLOADER_BUILD
     bool using_watchdog = AP_BoardConfig::watchdog_enabled();
@@ -67,6 +67,7 @@ static void save_fault_watchdog(uint16_t line, FaultType fault_type, uint32_t fa
         pd.fault_addr = fault_addr;
         pd.fault_thd_prio = chThdGetPriorityX();
         pd.fault_icsr = SCB->ICSR;
+        pd.fault_lr = lr;
         stm32_watchdog_save((uint32_t *)&hal.util->persistent_data, (sizeof(hal.util->persistent_data)+3)/4);
     }
 #endif
@@ -98,7 +99,7 @@ void HardFault_Handler(void) {
     (void)isFaultOnStacking;
     (void)isFaultAddressValid;
 
-    save_fault_watchdog(__LINE__, faultType, faultAddress);
+    save_fault_watchdog(__LINE__, faultType, faultAddress, (uint32_t)ctx.lr_thd);
 
 #ifdef HAL_GPIO_PIN_FAULT
     while (true) {
@@ -151,7 +152,7 @@ void UsageFault_Handler(void) {
     (void)isUnalignedAccessFault;
     (void)isDivideByZeroFault;
 
-    save_fault_watchdog(__LINE__, faultType, faultAddress);
+    save_fault_watchdog(__LINE__, faultType, faultAddress, (uint32_t)ctx.lr_thd);
 
     //Cause debugger to stop. Ignored if no debugger is attached
     while(1) {}
@@ -183,7 +184,7 @@ void MemManage_Handler(void) {
     (void)isExceptionStackingFault;
     (void)isFaultAddressValid;
 
-    save_fault_watchdog(__LINE__, faultType, faultAddress);
+    save_fault_watchdog(__LINE__, faultType, faultAddress, (uint32_t)ctx.lr_thd);
 
     while(1) {}
 }