kill task in case of exception in armed state

libraries/AP_HAL_F4Light/0_Done.md

@@ -156,6 +156,7 @@
 * renamed board *_MP32V1F4 to *_Revolution to simplify things
 * fixed bug in RC_Input that cause permanent Failsafe
 * added motor clipping reporting and baro compensation by GPS from https://github.com/DuraCopter/ardupilot
+* in case of any HardFault or Panic() in armed state, kill a current task and resume (or reboot a FC) instead of hang
 * ...
 * a lot of minor enhancements
 

libraries/AP_HAL_F4Light/Scheduler.cpp

@@ -751,6 +751,8 @@ task_t* Scheduler::get_empty_task(){
     return NULL;
 }
 
+#endif
+
 void Scheduler::stop_task(void *h){
     if(h) {
         task_t *tp = (task_t *)h ;
@@ -759,7 +761,6 @@ void Scheduler::stop_task(void *h){
         interrupts();
     }
 }
-#endif
 
 
 // task's executor, which calls user's function having semaphore
@@ -931,7 +932,33 @@ static uint16_t next_log_ptr(uint16_t sched_log_ptr){
 }
 #endif
 
+// exception occures in armed state - try to kill current task, or reboot if this is main task
+void Scheduler::_try_kill_task_or_reboot(uint8_t n){
+    task_t  *me = s_running; // current task
+    uint8_t tmp = task_n;
+
+    if(tmp==0 || me->id == 0) { // no tasks yet or in main task
+        board_set_rtc_register(FORCE_APP_RTC_SIGNATURE, RTC_SIGNATURE_REG); // force bootloader to not wait
+        _reboot(false);
+    }
+    stop_task(me); // exclude task from planning
+    task_n = 0;    // printf() can call yield while we now between live and death
+
+    printf("\nTaks %d killed by exception %d!\n",me->id, n);
+
+    task_n = tmp;
+    next_task = get_next_task();
+}
 
+void Scheduler::_go_next_task() {
+    plan_context_switch();    
+
+    while(1);
+}
+
+void Scheduler::_stop_multitask(){
+    task_n = 0;
+}
 
 
 // this function called only from SVC Level ISRs so there is no need to be reentrant
@@ -1421,4 +1448,6 @@ void * hal_register_task(voidFuncPtr task, uint32_t stack) {
 }
 
 bool hal_is_armed() { return hal.util->get_soft_armed();  }
-
+void hal_try_kill_task_or_reboot(uint8_t n) { Scheduler::_try_kill_task_or_reboot(n); }
+void hal_go_next_task() { Scheduler::_go_next_task(); }
+void hal_stop_multitask() { Scheduler::_stop_multitask(); }

libraries/AP_HAL_F4Light/Scheduler.h

@@ -104,6 +104,9 @@ extern "C" {
 
     void switchContext();
     void __do_context_switch();
+    void hal_try_kill_task_or_reboot(uint8_t n);
+    void hal_go_next_task();
+    void hal_stop_multitask();
 
     extern task_t *s_running; // running task 
     extern task_t *next_task; // task to run next
@@ -338,7 +341,7 @@ public:
         task->active=true;
         _forced_task = task; // force it, to not spent time for  search by priority
         context_switch_isr();
-  } 
+    } 
 #endif
 //]  
 
@@ -391,6 +394,10 @@ public:
     }
 
     static void SVC_Handler(uint32_t * svc_args); // many functions called via SVC for hardware serialization
+    
+    static void _try_kill_task_or_reboot(uint8_t n); // exception occures in armed state - try to kill current task
+    static void _go_next_task();
+    static void _stop_multitask();
 
     static volatile bool need_switch_task;   // should be public for access from C code
 //}

libraries/AP_HAL_F4Light/boards/f4light_Airbot/system_stm32f4xx.c

@@ -335,7 +335,7 @@ void SystemCoreClockUpdate(void)
   SystemCoreClock >>= tmp;
 }
 
-extern void __error(uint32_t num, uint32_t pc, uint32_t lr);
+extern void __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag);
 
 /**
   * @brief  Configures the System clock source, PLL Multiplier and Divider factors, 
@@ -468,7 +468,7 @@ void SetSysClock(uint8_t oc)
   else
   { /* If HSE fails to start-up, the application will have wrong clock
          configuration. User can add here some code to deal with this error */
-        __error(12,0,0);
+        __error(12,0,0,0);
   }
 
 /******************************************************************************/

libraries/AP_HAL_F4Light/boards/f4light_AirbotV2/system_stm32f4xx.c

@@ -333,7 +333,7 @@ void SystemCoreClockUpdate(void)
   SystemCoreClock >>= tmp;
 }
 
-extern void __error(uint32_t num, uint32_t pc, uint32_t lr);
+extern void __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag);
 
 /**
   * @brief  Configures the System clock source, PLL Multiplier and Divider factors, 
@@ -466,7 +466,7 @@ void SetSysClock(uint8_t oc)
   else
   { /* If HSE fails to start-up, the application will have wrong clock
          configuration. User can add here some code to deal with this error */
-        __error(12,0,0);
+        __error(12,0,0,0);
   }
 
 /******************************************************************************/

libraries/AP_HAL_F4Light/boards/f4light_MatekF405_CTR/system_stm32f4xx.c

@@ -466,7 +466,7 @@ void SetSysClock(uint8_t oc)
   else
   { /* If HSE fails to start-up, the application will have wrong clock
          configuration. User can add here some code to deal with this error */
-        __error(12,0,0);
+        __error(12,0,0,0);
   }
 
 /******************************************************************************/

libraries/AP_HAL_F4Light/boards/f4light_MiniF4_OSD/system_stm32f4xx.c

@@ -333,7 +333,7 @@ void SystemCoreClockUpdate(void)
   SystemCoreClock >>= tmp;
 }
 
-extern void __error(uint32_t num, uint32_t pc, uint32_t lr);
+extern void __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag);
 
 /**
   * @brief  Configures the System clock source, PLL Multiplier and Divider factors, 
@@ -466,7 +466,7 @@ void SetSysClock(uint8_t oc)
   else
   { /* If HSE fails to start-up, the application will have wrong clock
          configuration. User can add here some code to deal with this error */
-        __error(12,0,0);
+        __error(12,0,0,0);
   }
 
 /******************************************************************************/

libraries/AP_HAL_F4Light/boards/f4light_Revolution/system_stm32f4xx.c

@@ -333,7 +333,7 @@ void SystemCoreClockUpdate(void)
   SystemCoreClock >>= tmp;
 }
 
-extern void __error(uint32_t num, uint32_t pc, uint32_t lr);
+extern void __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag);
 
 /**
   * @brief  Configures the System clock source, PLL Multiplier and Divider factors, 
@@ -466,7 +466,7 @@ void SetSysClock(uint8_t oc)
   else
   { /* If HSE fails to start-up, the application will have wrong clock
          configuration. User can add here some code to deal with this error */
-        __error(12,0,0);
+        __error(12,0,0,0);
   }
 
 /******************************************************************************/

libraries/AP_HAL_F4Light/boards/f4light_Revolution_EE128/system_stm32f4xx.c

@@ -333,7 +333,7 @@ void SystemCoreClockUpdate(void)
   SystemCoreClock >>= tmp;
 }
 
-extern void __error(uint32_t num, uint32_t pc, uint32_t lr);
+extern void __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag);
 
 /**
   * @brief  Configures the System clock source, PLL Multiplier and Divider factors, 
@@ -466,7 +466,7 @@ void SetSysClock(uint8_t oc)
   else
   { /* If HSE fails to start-up, the application will have wrong clock
          configuration. User can add here some code to deal with this error */
-        __error(12,0,0);
+        __error(12,0,0,0);
   }
 
 /******************************************************************************/

libraries/AP_HAL_F4Light/boards/f4light_Revolution_SD/system_stm32f4xx.c

@@ -333,7 +333,7 @@ void SystemCoreClockUpdate(void)
   SystemCoreClock >>= tmp;
 }
 
-extern void __error(uint32_t num, uint32_t pc, uint32_t lr);
+extern void __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag);
 
 /**
   * @brief  Configures the System clock source, PLL Multiplier and Divider factors, 
@@ -466,7 +466,7 @@ void SetSysClock(uint8_t oc)
   else
   { /* If HSE fails to start-up, the application will have wrong clock
          configuration. User can add here some code to deal with this error */
-        __error(12,0,0);
+        __error(12,0,0,0);
   }
 
 /******************************************************************************/

libraries/AP_HAL_F4Light/boards/f4light_cl_racing/system_stm32f4xx.c

@@ -333,7 +333,7 @@ void SystemCoreClockUpdate(void)
   SystemCoreClock >>= tmp;
 }
 
-extern void __error(uint32_t num, uint32_t pc, uint32_t lr);
+extern void __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag);
 
 /**
   * @brief  Configures the System clock source, PLL Multiplier and Divider factors, 
@@ -466,7 +466,7 @@ void SetSysClock(uint8_t oc)
   else
   { /* If HSE fails to start-up, the application will have wrong clock
          configuration. User can add here some code to deal with this error */
-        __error(12,0,0);
+        __error(12,0,0, 0);
   }
 
 /******************************************************************************/

libraries/AP_HAL_F4Light/hardware/hal/syscalls.c

@@ -53,7 +53,7 @@ int _kill(int pid, int sig)
 
 void _exit(int status)
 {
-    __error(13, status, 0);
+    __error(13, status, 0, 0);
 }
 
 int _getpid(void)

libraries/AP_HAL_F4Light/hardware/hal/systick.c

@@ -128,11 +128,16 @@ void __attribute__((noreturn)) error_throb(uint32_t num){
 
 
 
+extern void __go_next_task();
+extern void hal_try_kill_task_or_reboot(uint8_t num);
+extern void hal_stop_multitask();
+extern void hal_go_next_task();
+#define NVIC_CCR (*(volatile uint32_t *)0xE000ED14)
 
 // new common exception code
 // TODO: we have task switching so if fault occures in task we can just remove task from queue
 //
-void __attribute__((noreturn)) __error(uint32_t num, uint32_t pc, uint32_t lr)
+void __attribute__((noreturn)) __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag)
 {
 
 #ifdef DEBUG_BUILD
@@ -148,7 +153,7 @@ void __attribute__((noreturn)) __error(uint32_t num, uint32_t pc, uint32_t lr)
         "", // 8 
         "", // 9 
         "", // 10 
-        "PureVirtual function call", // 11
+        "Pure Virtual function call", // 11
         "failed to setup clock", // 12
         "exit from main()", // 13
         "", // 14
@@ -156,14 +161,23 @@ void __attribute__((noreturn)) __error(uint32_t num, uint32_t pc, uint32_t lr)
 #endif
 
 
-        /* Turn off peripheral interrupts */
+    /* Turn off peripheral interrupts */
     __disable_irq();
 
-    timer_disable_all(); // turn off all PWM
+
+    extern bool hal_is_armed();
+    if((flag & 0x4) && hal_is_armed()){ // not in interrupt and is armed - try to save aircraft
+        hal_try_kill_task_or_reboot(num);
+        // returned - task killed, resume normal operations
+        NVIC_CCR &= ~1; // reset flag NONEBASETHRDENA in NVIC CCR
+        __enable_irq();
+        hal_go_next_task();
+    }
 
     if(is_bare_metal())  // bare metal build without bootloader should reboot to DFU after any fault
         board_set_rtc_register(DFU_RTC_SIGNATURE, RTC_SIGNATURE_REG);
 
+    timer_disable_all(); // turn off all PWM
 
     /* Turn the USB interrupt back on so "the reboot to bootloader" keeps on functioning */
     NVIC_EnableIRQ(OTG_HS_EP1_OUT_IRQn);
@@ -172,10 +186,11 @@ void __attribute__((noreturn)) __error(uint32_t num, uint32_t pc, uint32_t lr)
     NVIC_EnableIRQ(OTG_HS_IRQn);
     NVIC_EnableIRQ(OTG_FS_IRQn);
 
-    __enable_irq();
 
     if(boardEmergencyHandler) boardEmergencyHandler(); // call emergency handler
 
+    hal_stop_multitask();
+
 #if 0
  #ifdef ERROR_USART
     usart_putstr(ERROR_USART, "\r\n!!! Exception: ");
@@ -201,7 +216,7 @@ void __attribute__((noreturn)) __error(uint32_t num, uint32_t pc, uint32_t lr)
     error_throb(num);
 }
 
-
+#if 0 
 uint32_t systick_micros(void)
 {
     volatile uint32_t fms, lms;
@@ -224,3 +239,4 @@ uint32_t systick_micros(void)
     return res;
 #undef US_PER_MS
 }
+#endif

libraries/AP_HAL_F4Light/hardware/hal/systick.h

@@ -77,7 +77,7 @@ void MemManage_Handler(void);
 void BusFault_Handler(void);
 void UsageFault_Handler(void);
 */
-void __attribute__((noreturn)) __error(uint32_t pc, uint32_t num, uint32_t lr);
+void __attribute__((noreturn)) __error(uint32_t pc, uint32_t num, uint32_t lr, uint32_t flag);
 void __attribute__((noreturn)) error_throb(uint32_t num);
 
 

libraries/AP_HAL_F4Light/system.cpp

@@ -33,6 +33,10 @@ void panic(const char *errormsg, ...)
 
     if(boardEmergencyHandler) boardEmergencyHandler(); // call emergency handler before
 
+    timer_disable_all(); // turn off all PWM
+
+    F4Light::Scheduler::_stop_multitask();
+    
     va_start(ap, errormsg);
     hal.console->vprintf(errormsg, ap);
     va_end(ap);
@@ -55,7 +59,7 @@ void cond_yield(){
         F4Light::Scheduler::yield(0);
     }
     
-    last_yield=F4Light::Scheduler::_micros();
+    last_yield = F4Light::Scheduler::_micros();
 }
 
 

libraries/AP_HAL_F4Light/wirish/cxxabi-compat.cpp

@@ -3,7 +3,7 @@
 
 extern "C" void __cxa_pure_virtual(void) {
 
-    extern void __error(uint32_t num, uint32_t pc, uint32_t lr);
+    extern void __error(uint32_t num, uint32_t pc, uint32_t lr, uint32_t flag);
 
-    __error(11, 0, 0);
+    __error(11, 0, 0, 0);
 }

libraries/AP_HAL_F4Light/wirish/exc.S

@@ -88,63 +88,57 @@ FLASH_IRQHandler:
 
 .thumb_func
 __default_exc:
+    cpsid i                     @ Disable global interrupts
+
+    ldr r1, BFAR                @ for debug, to see what happens
+    ldr r1, [r1]
+    ldr r1, CFSR
+    ldr r1, [r1]
+    ldr r1, HFSR
+    ldr r1, [r1]
+    ldr r1, DFSR
+    ldr r1, [r1]
+    ldr r1, AFSR
+    ldr r1, [r1]
+    ldr r1, SHCSR
+    ldr r1, [r1]
+    ldr r1, MMFAR
+    ldr r1, [r1]
+
+    ldr r2, NVIC_CCR            @ Enable returning to thread mode even if there are
+    mov r1 ,#1                  @ pending exceptions. See flag NONEBASETHRDENA - http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0337e/Cihcbadd.html
+    str r1, [r2]
+
     tst lr, #4
     ite eq
     mrseq r1, msp
     mrsne r1, psp 
  
     ldr r3, [r1, #24]           @ PC of exception - if access to wrong address
-    ldr r1, [r1, #20]           @ LR of exception - if call to wrong address
-
-    ldr r2, BFAR                @ for debug, to see what happens
-    ldr r2, [r2]
-    ldr r2, CFSR
-    ldr r2, [r2]
-    ldr r2, HFSR
-    ldr r2, [r2]
-    ldr r2, DFSR
-    ldr r2, [r2]
-    ldr r2, AFSR
-    ldr r2, [r2]
-    ldr r2, SHCSR
-    ldr r2, [r2]
-    ldr r2, MMFAR
-    ldr r2, [r2]
-
-    mov r12, r1
-    ldr r2, NVIC_CCR            @ Enable returning to thread mode even if there are
-    mov r1 ,#1                  @ pending exceptions. See flag NONEBASETHRDENA - http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0337e/Cihcbadd.html
-    str r1, [r2]
-    cpsid i                     @ Disable global interrupts
+    ldr r2, [r1, #20]           @ LR of exception - if call to wrong address
 
-    mov r1, #0
-    ldr r2, SYSTICK_CSR         @ Disable systick handler
-    str r1, [r2]
+    str r0, [r1]                @ exception number - to R0
+    str r3, [r1, #4]            @ PC of exception  - to R1
+    str r2, [r1, #8]            @ LR of exception  - to R2
+    str lr, [r1, #12]           @ exception code   - to R3
+
+    mov r3, #0
     ldr r2, MPU_CTRL            @ disable MPU
-    str r1, [r2]
+    str r3, [r2]
+
+    ldr r2, CPSR_MASK           @ Set CPSR to default
+    str r2, [r1,#28]
+
+    ldr r2, TARGET_PC           @ Set target pc
+    str r2, [r1,#24]
+
+    DSB
+    ISB
     
-    ldr r1, CPSR_MASK           @ Set default CPSR
-    mov r2, r12
-    push {r1}                   @                        SP+4
-    ldr r1, TARGET_PC           @ Set target pc
-    push {r1}                   @                        SP+8
-    push {r1}                   @ target LR - the same   SP+12
-    ldr r1, [sp, #12]           @ R0 of interrupted program 
-    push {r1}                   @  will be in R12        SP+16
-    ldr r1, [sp, #28]           @ R3 of interrupted program (12+16)
-    push {r1}                   @  will be in R3
-    push {r2}                   @ R2 - LR
-    push {r3}                   @ R1 - PC of exception
-    push {r0}                   @ R0 - exception code
-
-    tst     lr, #4
-    ite eq
-    ldreq r1, EXC_RETURN        @ Return to thread mode with PSP
-    ldrne r1, EXC_RETURN        @ Return to thread mode with MSP
-    mov lr, r1
     bx lr                       @ Exception exit
 
 
+
 .thumb_func
 __do_context_switch: @ we already in interrupt so all interrupts with higher priority will use MSP - so dont need to disable interrupts