mirror of https://github.com/ArduPilot/ardupilot
317 lines
8.3 KiB
C++
317 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);
|
|
#if HAL_WITH_DSP
|
|
static DSP dspDriver;
|
|
#endif
|
|
|
|
|
|
// use the Empty HAL for hardware we don't emulate
|
|
static Empty::OpticalFlow emptyOpticalFlow;
|
|
static Empty::Flash emptyFlash;
|
|
|
|
static UARTDriver sitlSerial0Driver(0, &sitlState);
|
|
static UARTDriver sitlSerial1Driver(1, &sitlState);
|
|
static UARTDriver sitlSerial2Driver(2, &sitlState);
|
|
static UARTDriver sitlSerial3Driver(3, &sitlState);
|
|
static UARTDriver sitlSerial4Driver(4, &sitlState);
|
|
static UARTDriver sitlSerial5Driver(5, &sitlState);
|
|
static UARTDriver sitlSerial6Driver(6, &sitlState);
|
|
static UARTDriver sitlSerial7Driver(7, &sitlState);
|
|
static UARTDriver sitlSerial8Driver(8, &sitlState);
|
|
static UARTDriver sitlSerial9Driver(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(
|
|
&sitlSerial0Driver,
|
|
&sitlSerial1Driver,
|
|
&sitlSerial2Driver,
|
|
&sitlSerial3Driver,
|
|
&sitlSerial4Driver,
|
|
&sitlSerial5Driver,
|
|
&sitlSerial6Driver,
|
|
&sitlSerial7Driver,
|
|
&sitlSerial8Driver,
|
|
&sitlSerial9Driver,
|
|
&i2c_mgr_instance,
|
|
&spi_mgr_instance, /* spi */
|
|
&wspi_mgr_instance,
|
|
&sitlAnalogIn, /* analogin */
|
|
&sitlStorage, /* storage */
|
|
&sitlSerial0Driver, /* console */
|
|
&sitlGPIO, /* gpio */
|
|
&sitlRCInput, /* rcinput */
|
|
&sitlRCOutput, /* rcoutput */
|
|
&sitlScheduler, /* scheduler */
|
|
&utilInstance, /* util */
|
|
&emptyOpticalFlow, /* onboard optical flow */
|
|
&emptyFlash, /* flash driver */
|
|
#if HAL_WITH_DSP
|
|
&dspDriver, /* dsp driver */
|
|
#endif
|
|
#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
|