ardupilot/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp

118 lines
3.2 KiB
C++
Raw Normal View History

#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2015-10-19 11:26:18 -03:00
#include <assert.h>
#include <errno.h>
2015-10-19 11:26:18 -03:00
#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 "Storage.h"
#include "RCInput.h"
2012-12-17 23:56:21 -04:00
#include "RCOutput.h"
#include "GPIO.h"
#include "SITL_State.h"
#include "Util.h"
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
using namespace HALSITL;
static Storage sitlStorage;
static SITL_State sitlState;
static Scheduler sitlScheduler(&sitlState);
static RCInput sitlRCInput(&sitlState);
static RCOutput sitlRCOutput(&sitlState);
static AnalogIn sitlAnalogIn(&sitlState);
static GPIO sitlGPIO(&sitlState);
// use the Empty HAL for hardware we don't emulate
2015-12-01 15:57:35 -04:00
static Empty::I2CDeviceManager i2c_mgr_instance;
static Empty::SPIDeviceManager emptySPI;
static Empty::OpticalFlow emptyOpticalFlow;
2019-03-25 21:14:30 -03:00
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);
2016-04-19 00:48:18 -03:00
static UARTDriver sitlUart5Driver(5, &sitlState);
2018-06-27 08:34:24 -03:00
static UARTDriver sitlUart6Driver(6, &sitlState);
static Util utilInstance(&sitlState);
HAL_SITL::HAL_SITL() :
AP_HAL::HAL(
2017-01-09 08:23:23 -04:00
&sitlUart0Driver, /* uartA */
&sitlUart1Driver, /* uartB */
&sitlUart2Driver, /* uartC */
&sitlUart3Driver, /* uartD */
&sitlUart4Driver, /* uartE */
&sitlUart5Driver, /* uartF */
2018-06-27 08:34:24 -03:00
&sitlUart6Driver, /* uartG */
2015-12-01 15:57:35 -04:00
&i2c_mgr_instance,
2017-01-09 08:23:23 -04:00
&emptySPI, /* spi */
&sitlAnalogIn, /* analogin */
&sitlStorage, /* storage */
2017-01-09 08:23:23 -04:00
&sitlUart0Driver, /* console */
&sitlGPIO, /* gpio */
&sitlRCInput, /* rcinput */
&sitlRCOutput, /* rcoutput */
&sitlScheduler, /* scheduler */
&utilInstance, /* util */
2017-04-02 11:55:19 -03:00
&emptyOpticalFlow, /* onboard optical flow */
2019-03-25 21:14:30 -03:00
&emptyFlash, /* flash driver */
2017-04-02 11:55:19 -03:00
nullptr), /* CAN */
_sitl_state(&sitlState)
{}
static char *new_argv[100];
2015-10-19 12:59:47 -03:00
void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const
{
2015-10-19 12:59:47 -03:00
assert(callbacks);
_sitl_state->init(argc, argv);
scheduler->init();
uartA->begin(115200);
rcin->init();
rcout->init();
2017-01-09 08:23:23 -04:00
// spi->init();
analogin->init();
2015-10-19 11:26:18 -03:00
callbacks->setup();
scheduler->system_initialized();
2018-05-28 13:20:30 -03:00
while (!HALSITL::Scheduler::_should_reboot) {
2015-10-19 11:26:18 -03:00
callbacks->loop();
HALSITL::Scheduler::_run_io_procs();
2015-10-19 11:26:18 -03:00
}
// form a new argv, removing problem parameters
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];
}
execv(new_argv[0], new_argv);
AP_HAL::panic("PANIC: REBOOT FAILED: %s", strerror(errno));
2015-10-19 11:26:18 -03:00
}
const AP_HAL::HAL& AP_HAL::get_HAL() {
static const HAL_SITL hal;
return hal;
}
2017-01-09 08:23:23 -04:00
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL