Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 10:38:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

kill task in case of exception in armed state

Browse Source
This commit is contained in:
night-ghost 2018-04-12 13:40:39 +05:00 committed by Andrew Tridgell
parent c85bae8d88
commit 051c8e9fe2
17 changed files with 124 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_HAL_F4Light/0_Done.md
View File

@ -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

33
libraries/AP_HAL_F4Light/Scheduler.cpp
View File

@ -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(); }

9
libraries/AP_HAL_F4Light/Scheduler.h
View File

@ -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
//}

4
libraries/AP_HAL_F4Light/boards/f4light_Airbot/system_stm32f4xx.c
View File

@ -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);
}
/******************************************************************************/

4
libraries/AP_HAL_F4Light/boards/f4light_AirbotV2/system_stm32f4xx.c
View File

@ -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);
}
/******************************************************************************/

2
libraries/AP_HAL_F4Light/boards/f4light_MatekF405_CTR/system_stm32f4xx.c
View File

@ -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);
}
/******************************************************************************/

4
libraries/AP_HAL_F4Light/boards/f4light_MiniF4_OSD/system_stm32f4xx.c
View File

@ -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);
}
/******************************************************************************/

4
libraries/AP_HAL_F4Light/boards/f4light_Revolution/system_stm32f4xx.c
View File

@ -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);
}
/******************************************************************************/

4
libraries/AP_HAL_F4Light/boards/f4light_Revolution_EE128/system_stm32f4xx.c
View File

@ -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);
}
/******************************************************************************/

4
libraries/AP_HAL_F4Light/boards/f4light_Revolution_SD/system_stm32f4xx.c
View File

@ -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);
}
/******************************************************************************/

4
libraries/AP_HAL_F4Light/boards/f4light_cl_racing/system_stm32f4xx.c
View File

@ -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);
}
/******************************************************************************/

2
libraries/AP_HAL_F4Light/hardware/hal/syscalls.c
View File

@ -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)

28
libraries/AP_HAL_F4Light/hardware/hal/systick.c
View File

@ -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

2
libraries/AP_HAL_F4Light/hardware/hal/systick.h
View File

@ -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);

6
libraries/AP_HAL_F4Light/system.cpp
View File

@ -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();
}

4
libraries/AP_HAL_F4Light/wirish/cxxabi-compat.cpp
View File

@ -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);
}

82
libraries/AP_HAL_F4Light/wirish/exc.S
View File

@ -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, [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]
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 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
mov r1, #0
ldr r2, SYSTICK_CSR @ Disable systick handler
str r1, [r2]
mov r3, #0
ldr r2, MPU_CTRL @ disable MPU
str r1, [r2]
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
str r3, [r2]
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
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
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