mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-28 02:33:58 -04:00
AP_HAL: move methods for stack overflow and memguard to AP_HAL/Util
also add reboot on memory errors for iomcu
This commit is contained in:
parent
6b668d0113
commit
679fa817e6
@ -417,3 +417,7 @@
|
||||
#endif
|
||||
|
||||
#define HAL_GPIO_LED_OFF (!HAL_GPIO_LED_ON)
|
||||
|
||||
#ifndef HAL_REBOOT_ON_MEMORY_ERRORS
|
||||
#define HAL_REBOOT_ON_MEMORY_ERRORS defined(IOMCU_FW)
|
||||
#endif
|
||||
|
@ -80,3 +80,42 @@ void AP_HAL::Util::set_soft_armed(const bool b)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
#include <stdio.h>
|
||||
// stack overflow hook for low level RTOS code, C binding
|
||||
void AP_stack_overflow(const char *thread_name)
|
||||
{
|
||||
#if HAL_REBOOT_ON_MEMORY_ERRORS
|
||||
(void)thread_name;
|
||||
hal.scheduler->reboot(false);
|
||||
#else
|
||||
static bool done_stack_overflow;
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::stack_overflow);
|
||||
if (!done_stack_overflow) {
|
||||
// we don't want to record the thread name more than once, as
|
||||
// first overflow can trigger a 2nd
|
||||
strncpy_noterm(hal.util->persistent_data.thread_name4, thread_name, 4);
|
||||
done_stack_overflow = true;
|
||||
}
|
||||
hal.util->persistent_data.fault_type = 42; // magic value
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
AP_HAL::panic("stack overflow %s", thread_name);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// hook for memory guard errors with --enable-memory-guard
|
||||
void AP_memory_guard_error(uint32_t size)
|
||||
{
|
||||
#if HAL_REBOOT_ON_MEMORY_ERRORS
|
||||
(void)size;
|
||||
hal.scheduler->reboot(false);
|
||||
#else
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::mem_guard);
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
::printf("memory guard error size=%u\n", unsigned(size));
|
||||
AP_HAL::panic("memory guard size=%u", unsigned(size));
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
@ -219,3 +219,8 @@ protected:
|
||||
bool soft_armed = false;
|
||||
uint32_t last_armed_change_ms;
|
||||
};
|
||||
|
||||
extern "C" {
|
||||
void AP_stack_overflow(const char *thread_name);
|
||||
void AP_memory_guard_error(uint32_t size);
|
||||
}
|
||||
|
@ -1,4 +1,9 @@
|
||||
#include "system.h"
|
||||
#include "AP_HAL.h"
|
||||
#include <AP_InternalError/AP_InternalError.h>
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
uint16_t WEAK AP_HAL::millis16()
|
||||
{
|
||||
|
Loading…
Reference in New Issue
Block a user