mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
3c82ac6043
Returns a mutable reference to the same HAL for certain purposes where the HAL needs to be mutated to avoid UB problems with casting away const and to make the fact that mutation is happening obvious.
313 lines
8.3 KiB
C++
313 lines
8.3 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
#include <assert.h>
|
|
#include <errno.h>
|
|
#include <signal.h>
|
|
#include <stdio.h>
|
|
|
|
#include "AP_HAL_SITL.h"
|
|
#include "AP_HAL_SITL_Namespace.h"
|
|
#include "HAL_SITL_Class.h"
|
|
#include "Scheduler.h"
|
|
#include "AnalogIn.h"
|
|
#include "UARTDriver.h"
|
|
#include "I2CDevice.h"
|
|
#include "Storage.h"
|
|
#include "RCInput.h"
|
|
#include "RCOutput.h"
|
|
#include "GPIO.h"
|
|
#include "SITL_State.h"
|
|
#include "Util.h"
|
|
#include "DSP.h"
|
|
#include "CANSocketIface.h"
|
|
#include "SPIDevice.h"
|
|
|
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
|
#include <AP_HAL_Empty/AP_HAL_Empty.h>
|
|
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
|
|
#include <AP_InternalError/AP_InternalError.h>
|
|
#include <AP_Logger/AP_Logger.h>
|
|
#include <AP_RCProtocol/AP_RCProtocol_config.h>
|
|
|
|
using namespace HALSITL;
|
|
|
|
HAL_SITL& hal_sitl = (HAL_SITL&)AP_HAL::get_HAL_mutable();
|
|
|
|
static Storage sitlStorage;
|
|
static SITL_State sitlState;
|
|
static Scheduler sitlScheduler(&sitlState);
|
|
#if AP_RCPROTOCOL_ENABLED
|
|
static RCInput sitlRCInput(&sitlState);
|
|
#else
|
|
static Empty::RCInput sitlRCInput;
|
|
#endif
|
|
static RCOutput sitlRCOutput(&sitlState);
|
|
static GPIO sitlGPIO(&sitlState);
|
|
static AnalogIn sitlAnalogIn(&sitlState);
|
|
static DSP dspDriver;
|
|
|
|
|
|
// use the Empty HAL for hardware we don't emulate
|
|
static Empty::OpticalFlow emptyOpticalFlow;
|
|
static Empty::Flash emptyFlash;
|
|
|
|
static UARTDriver sitlUart0Driver(0, &sitlState);
|
|
static UARTDriver sitlUart1Driver(1, &sitlState);
|
|
static UARTDriver sitlUart2Driver(2, &sitlState);
|
|
static UARTDriver sitlUart3Driver(3, &sitlState);
|
|
static UARTDriver sitlUart4Driver(4, &sitlState);
|
|
static UARTDriver sitlUart5Driver(5, &sitlState);
|
|
static UARTDriver sitlUart6Driver(6, &sitlState);
|
|
static UARTDriver sitlUart7Driver(7, &sitlState);
|
|
static UARTDriver sitlUart8Driver(8, &sitlState);
|
|
static UARTDriver sitlUart9Driver(9, &sitlState);
|
|
|
|
static I2CDeviceManager i2c_mgr_instance;
|
|
|
|
#if defined(HAL_BUILD_AP_PERIPH)
|
|
static Empty::SPIDeviceManager spi_mgr_instance;
|
|
#else
|
|
static SPIDeviceManager spi_mgr_instance;
|
|
#endif
|
|
static Util utilInstance(&sitlState);
|
|
|
|
#if HAL_NUM_CAN_IFACES
|
|
static HALSITL::CANIface* canDrivers[HAL_NUM_CAN_IFACES];
|
|
#endif
|
|
|
|
static Empty::WSPIDeviceManager wspi_mgr_instance;
|
|
|
|
HAL_SITL::HAL_SITL() :
|
|
AP_HAL::HAL(
|
|
&sitlUart0Driver, /* uartA */
|
|
&sitlUart1Driver, /* uartB */
|
|
&sitlUart2Driver, /* uartC */
|
|
&sitlUart3Driver, /* uartD */
|
|
&sitlUart4Driver, /* uartE */
|
|
&sitlUart5Driver, /* uartF */
|
|
&sitlUart6Driver, /* uartG */
|
|
&sitlUart7Driver, /* uartH */
|
|
&sitlUart8Driver, /* uartI */
|
|
&sitlUart9Driver, /* uartJ */
|
|
&i2c_mgr_instance,
|
|
&spi_mgr_instance, /* spi */
|
|
&wspi_mgr_instance,
|
|
&sitlAnalogIn, /* analogin */
|
|
&sitlStorage, /* storage */
|
|
&sitlUart0Driver, /* console */
|
|
&sitlGPIO, /* gpio */
|
|
&sitlRCInput, /* rcinput */
|
|
&sitlRCOutput, /* rcoutput */
|
|
&sitlScheduler, /* scheduler */
|
|
&utilInstance, /* util */
|
|
&emptyOpticalFlow, /* onboard optical flow */
|
|
&emptyFlash, /* flash driver */
|
|
&dspDriver, /* dsp driver */
|
|
#if HAL_NUM_CAN_IFACES
|
|
(AP_HAL::CANIface**)canDrivers
|
|
#else
|
|
nullptr
|
|
#endif
|
|
), /* CAN */
|
|
_sitl_state(&sitlState)
|
|
{}
|
|
|
|
static char *new_argv[100];
|
|
|
|
/*
|
|
save watchdog data
|
|
*/
|
|
static bool watchdog_save(const uint32_t *data, uint32_t nwords)
|
|
{
|
|
int fd = ::open("persistent.dat", O_WRONLY|O_CREAT|O_TRUNC, 0644);
|
|
bool ret = false;
|
|
if (fd != -1) {
|
|
if (::write(fd, data, nwords*4) == (ssize_t)(nwords*4)) {
|
|
ret = true;
|
|
}
|
|
::close(fd);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
load watchdog data
|
|
*/
|
|
static bool watchdog_load(uint32_t *data, uint32_t nwords)
|
|
{
|
|
int fd = ::open("persistent.dat", O_RDONLY, 0644);
|
|
bool ret = false;
|
|
if (fd != -1) {
|
|
ret = (::read(fd, data, nwords*4) == (ssize_t)(nwords*4));
|
|
::close(fd);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
implement watchdoh reset via SIGALRM
|
|
*/
|
|
static void sig_alrm(int signum)
|
|
{
|
|
static char env[] = "SITL_WATCHDOG_RESET=1";
|
|
putenv(env);
|
|
printf("GOT SIGALRM\n");
|
|
execv(new_argv[0], new_argv);
|
|
}
|
|
|
|
void HAL_SITL::exit_signal_handler(int signum)
|
|
{
|
|
HALSITL::Scheduler::_should_exit = true;
|
|
}
|
|
|
|
void HAL_SITL::setup_signal_handlers() const
|
|
{
|
|
struct sigaction sa = { };
|
|
|
|
sa.sa_flags = SA_NOCLDSTOP;
|
|
sa.sa_handler = HAL_SITL::exit_signal_handler;
|
|
sigaction(SIGTERM, &sa, NULL);
|
|
#if defined(HAL_COVERAGE_BUILD) && HAL_COVERAGE_BUILD == 1
|
|
sigaction(SIGINT, &sa, NULL);
|
|
sigaction(SIGHUP, &sa, NULL);
|
|
sigaction(SIGQUIT, &sa, NULL);
|
|
#endif
|
|
|
|
}
|
|
|
|
/*
|
|
fill 8k of stack with NaN. This allows us to find uses of
|
|
uninitialised memory without valgrind
|
|
*/
|
|
static void fill_stack_nan(void)
|
|
{
|
|
float stk[2048];
|
|
fill_nanf(stk, ARRAY_SIZE(stk));
|
|
}
|
|
|
|
uint8_t HAL_SITL::get_instance() const
|
|
{
|
|
return _sitl_state->get_instance();
|
|
}
|
|
|
|
#if defined(HAL_BUILD_AP_PERIPH)
|
|
bool HAL_SITL::run_in_maintenance_mode() const
|
|
{
|
|
return _sitl_state->run_in_maintenance_mode();
|
|
}
|
|
#endif
|
|
|
|
void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const
|
|
{
|
|
assert(callbacks);
|
|
|
|
utilInstance.init(argc, argv);
|
|
_sitl_state->init(argc, argv);
|
|
|
|
scheduler->init();
|
|
serial(0)->begin(115200);
|
|
|
|
rcin->init();
|
|
rcout->init();
|
|
|
|
// spi->init();
|
|
analogin->init();
|
|
|
|
if (getenv("SITL_WATCHDOG_RESET")) {
|
|
INTERNAL_ERROR(AP_InternalError::error_t::watchdog_reset);
|
|
if (watchdog_load((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4)) {
|
|
serial(0)->printf("Loaded watchdog data");
|
|
utilInstance.last_persistent_data = utilInstance.persistent_data;
|
|
}
|
|
}
|
|
|
|
// form a new argv, removing problem parameters. This is used for reboot
|
|
uint8_t new_argv_offset = 0;
|
|
for (uint8_t i=0; i<ARRAY_SIZE(new_argv) && i<argc; i++) {
|
|
if (!strcmp(argv[i], "-w")) {
|
|
// don't wipe params on reboot
|
|
continue;
|
|
}
|
|
new_argv[new_argv_offset++] = argv[i];
|
|
}
|
|
|
|
fill_stack_nan();
|
|
|
|
callbacks->setup();
|
|
scheduler->set_system_initialized();
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
if (getenv("SITL_WATCHDOG_RESET")) {
|
|
const AP_HAL::Util::PersistentData &pd = util->persistent_data;
|
|
AP::logger().WriteCritical("WDOG", "TimeUS,Task,IErr,IErrCnt,IErrLn,MavMsg,MavCmd,SemLine", "QbIHHHHH",
|
|
AP_HAL::micros64(),
|
|
pd.scheduler_task,
|
|
pd.internal_errors,
|
|
pd.internal_error_count,
|
|
pd.internal_error_last_line,
|
|
pd.last_mavlink_msgid,
|
|
pd.last_mavlink_cmd,
|
|
pd.semaphore_line);
|
|
}
|
|
#endif
|
|
|
|
bool using_watchdog = AP_BoardConfig::watchdog_enabled();
|
|
if (using_watchdog) {
|
|
signal(SIGALRM, sig_alrm);
|
|
alarm(2);
|
|
}
|
|
setup_signal_handlers();
|
|
|
|
uint32_t last_watchdog_save = AP_HAL::millis();
|
|
uint8_t fill_count = 0;
|
|
|
|
while (true) {
|
|
if (HALSITL::Scheduler::_should_exit) {
|
|
::fprintf(stderr, "Exitting\n");
|
|
exit(0);
|
|
}
|
|
if (fill_count++ % 10 == 0) {
|
|
// only fill every 10 loops. This still gives us a lot of
|
|
// protection, but saves a lot of CPU
|
|
fill_count = 1u;
|
|
fill_stack_nan();
|
|
}
|
|
callbacks->loop();
|
|
HALSITL::Scheduler::_run_io_procs();
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
if (now - last_watchdog_save >= 100 && using_watchdog) {
|
|
// save persistent data every 100ms
|
|
last_watchdog_save = now;
|
|
watchdog_save((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4);
|
|
}
|
|
|
|
if (using_watchdog) {
|
|
// note that this only works for a speedup of 1
|
|
alarm(2);
|
|
}
|
|
}
|
|
|
|
actually_reboot();
|
|
}
|
|
|
|
void HAL_SITL::actually_reboot()
|
|
{
|
|
execv(new_argv[0], new_argv);
|
|
AP_HAL::panic("PANIC: REBOOT FAILED: %s", strerror(errno));
|
|
}
|
|
|
|
static HAL_SITL hal_sitl_inst;
|
|
|
|
const AP_HAL::HAL& AP_HAL::get_HAL() {
|
|
return hal_sitl_inst;
|
|
}
|
|
|
|
AP_HAL::HAL& AP_HAL::get_HAL_mutable() {
|
|
return hal_sitl_inst;
|
|
}
|
|
|
|
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
|