From 09a0d8d0c0ff060f1ea9d85d6923bf70c1b15f8f Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 13 Sep 2020 00:34:38 +0530 Subject: [PATCH] HAL_SITL: add support for AP_Periph SITL build --- libraries/AP_HAL_SITL/CANSocketIface.cpp | 4 ++ libraries/AP_HAL_SITL/GPIO.cpp | 3 + libraries/AP_HAL_SITL/GPIO.h | 2 + libraries/AP_HAL_SITL/HAL_SITL_Class.cpp | 15 ++++- libraries/AP_HAL_SITL/I2CDevice.cpp | 3 + libraries/AP_HAL_SITL/I2CDevice.h | 3 + libraries/AP_HAL_SITL/RCInput.cpp | 2 +- libraries/AP_HAL_SITL/RCInput.h | 2 +- libraries/AP_HAL_SITL/RCOutput.cpp | 7 +- libraries/AP_HAL_SITL/RCOutput.h | 2 +- libraries/AP_HAL_SITL/SITL_Periph_State.cpp | 50 ++++++++++++++ libraries/AP_HAL_SITL/SITL_Periph_State.h | 75 +++++++++++++++++++++ libraries/AP_HAL_SITL/SITL_State.cpp | 2 +- libraries/AP_HAL_SITL/SITL_State.h | 4 ++ libraries/AP_HAL_SITL/SITL_cmdline.cpp | 2 +- libraries/AP_HAL_SITL/Scheduler.cpp | 8 +++ libraries/AP_HAL_SITL/UARTDriver.cpp | 11 ++- libraries/AP_HAL_SITL/Util.cpp | 2 + libraries/AP_HAL_SITL/Util.h | 2 + libraries/AP_HAL_SITL/sitl_airspeed.cpp | 2 +- libraries/AP_HAL_SITL/sitl_gps.cpp | 16 ++++- libraries/AP_HAL_SITL/sitl_rangefinder.cpp | 2 +- 22 files changed, 206 insertions(+), 13 deletions(-) create mode 100644 libraries/AP_HAL_SITL/SITL_Periph_State.cpp create mode 100644 libraries/AP_HAL_SITL/SITL_Periph_State.h diff --git a/libraries/AP_HAL_SITL/CANSocketIface.cpp b/libraries/AP_HAL_SITL/CANSocketIface.cpp index 94cbb95ddf..5dea3e94d6 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.cpp +++ b/libraries/AP_HAL_SITL/CANSocketIface.cpp @@ -43,7 +43,11 @@ extern const AP_HAL::HAL& hal; using namespace HALSITL; +#if HAL_MAX_CAN_PROTOCOL_DRIVERS #define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANLinuxIface", fmt, ##args); } while (0) +#else +#define Debug(fmt, args...) +#endif CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES]; diff --git a/libraries/AP_HAL_SITL/GPIO.cpp b/libraries/AP_HAL_SITL/GPIO.cpp index ec195e7c08..27c9b06c0a 100644 --- a/libraries/AP_HAL_SITL/GPIO.cpp +++ b/libraries/AP_HAL_SITL/GPIO.cpp @@ -1,6 +1,8 @@ #include "GPIO.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) + using namespace HALSITL; extern const AP_HAL::HAL& hal; @@ -108,3 +110,4 @@ void DigitalSource::toggle() { return hal.gpio->write(_pin, !hal.gpio->read(_pin)); } +#endif diff --git a/libraries/AP_HAL_SITL/GPIO.h b/libraries/AP_HAL_SITL/GPIO.h index 38af0571e6..d165215630 100644 --- a/libraries/AP_HAL_SITL/GPIO.h +++ b/libraries/AP_HAL_SITL/GPIO.h @@ -1,6 +1,7 @@ #pragma once #include "AP_HAL_SITL.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) class HALSITL::GPIO : public AP_HAL::GPIO { public: @@ -34,3 +35,4 @@ public: private: uint8_t _pin; }; +#endif \ No newline at end of file diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index f0b7d91db4..47dd2509c4 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -34,10 +34,16 @@ using namespace HALSITL; static Storage sitlStorage; static SITL_State sitlState; static Scheduler sitlScheduler(&sitlState); +#if !defined(HAL_BUILD_AP_PERIPH) static RCInput sitlRCInput(&sitlState); static RCOutput sitlRCOutput(&sitlState); -static AnalogIn sitlAnalogIn(&sitlState); static GPIO sitlGPIO(&sitlState); +#else +static Empty::RCInput sitlRCInput; +static Empty::RCOutput sitlRCOutput; +static Empty::GPIO sitlGPIO; +#endif +static AnalogIn sitlAnalogIn(&sitlState); static DSP dspDriver; @@ -55,8 +61,11 @@ static UARTDriver sitlUart5Driver(5, &sitlState); static UARTDriver sitlUart6Driver(6, &sitlState); static UARTDriver sitlUart7Driver(7, &sitlState); +#if defined(HAL_BUILD_AP_PERIPH) +static Empty::I2CDeviceManager i2c_mgr_instance; +#else static I2CDeviceManager i2c_mgr_instance; - +#endif static Util utilInstance(&sitlState); @@ -201,6 +210,7 @@ void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const callbacks->setup(); scheduler->system_initialized(); +#ifndef HAL_NO_LOGGING 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", @@ -213,6 +223,7 @@ void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const pd.last_mavlink_cmd, pd.semaphore_line); } +#endif bool using_watchdog = AP_BoardConfig::watchdog_enabled(); if (using_watchdog) { diff --git a/libraries/AP_HAL_SITL/I2CDevice.cpp b/libraries/AP_HAL_SITL/I2CDevice.cpp index becb8a580f..2dde8a5b83 100644 --- a/libraries/AP_HAL_SITL/I2CDevice.cpp +++ b/libraries/AP_HAL_SITL/I2CDevice.cpp @@ -17,6 +17,8 @@ #include "I2CDevice.h" #include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) + #include extern const AP_HAL::HAL& hal; @@ -212,3 +214,4 @@ bool I2CDevice::adjust_periodic_callback(Device::PeriodicHandle h, uint32_t peri return false; } +#endif //#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) diff --git a/libraries/AP_HAL_SITL/I2CDevice.h b/libraries/AP_HAL_SITL/I2CDevice.h index 9d8d5c31f6..034967e5d3 100644 --- a/libraries/AP_HAL_SITL/I2CDevice.h +++ b/libraries/AP_HAL_SITL/I2CDevice.h @@ -19,6 +19,8 @@ #include #include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) + #include #include @@ -125,3 +127,4 @@ protected: #define NUM_SITL_I2C_BUSES 4 static I2CBus buses[]; }; +#endif //#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) diff --git a/libraries/AP_HAL_SITL/RCInput.cpp b/libraries/AP_HAL_SITL/RCInput.cpp index 951703e22a..f1f4d11c92 100644 --- a/libraries/AP_HAL_SITL/RCInput.cpp +++ b/libraries/AP_HAL_SITL/RCInput.cpp @@ -1,5 +1,5 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) #include "RCInput.h" #include diff --git a/libraries/AP_HAL_SITL/RCInput.h b/libraries/AP_HAL_SITL/RCInput.h index b327c87d4e..e991a40132 100644 --- a/libraries/AP_HAL_SITL/RCInput.h +++ b/libraries/AP_HAL_SITL/RCInput.h @@ -2,7 +2,7 @@ #pragma once #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) #define SITL_RC_INPUT_CHANNELS 16 #include "AP_HAL_SITL.h" diff --git a/libraries/AP_HAL_SITL/RCOutput.cpp b/libraries/AP_HAL_SITL/RCOutput.cpp index 4871fdb293..6429340400 100644 --- a/libraries/AP_HAL_SITL/RCOutput.cpp +++ b/libraries/AP_HAL_SITL/RCOutput.cpp @@ -1,4 +1,7 @@ #include + +#if !defined(HAL_BUILD_AP_PERIPH) + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include "RCOutput.h" @@ -130,7 +133,7 @@ void RCOutput::serial_led_send(const uint16_t chan) } } -#endif +#endif //CONFIG_HAL_BOARD == HAL_BOARD_SITL void RCOutput::force_safety_off(void) { @@ -149,3 +152,5 @@ bool RCOutput::force_safety_on(void) } return sitl->force_safety_on(); } + +#endif //!defined(HAL_BUILD_AP_PERIPH) diff --git a/libraries/AP_HAL_SITL/RCOutput.h b/libraries/AP_HAL_SITL/RCOutput.h index 0576f2625f..462f9886d1 100644 --- a/libraries/AP_HAL_SITL/RCOutput.h +++ b/libraries/AP_HAL_SITL/RCOutput.h @@ -1,7 +1,7 @@ #pragma once #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) #include "AP_HAL_SITL.h" class HALSITL::RCOutput : public AP_HAL::RCOutput { diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp new file mode 100644 index 0000000000..62ab0d6485 --- /dev/null +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -0,0 +1,50 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && defined(HAL_BUILD_AP_PERIPH) + +#include "AP_HAL_SITL.h" +#include "AP_HAL_SITL_Namespace.h" +#include "HAL_SITL_Class.h" +#include "UARTDriver.h" +#include "Scheduler.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +using namespace HALSITL; + +void SITL_State::init(int argc, char * const argv[]) { + +} + +void SITL_State::wait_clock(uint64_t wait_time_usec) { + while (AP_HAL::native_micros64() < wait_time_usec) { + usleep(1000); + } +} + + +int SITL_State::gps_pipe(uint8_t index) { + return 0; +} + +int SITL_State::sim_fd(const char *name, const char *arg) { + return 0; +} + +int SITL_State::sim_fd_write(const char *name) { + return 0; +} + + +#endif //CONFIG_HAL_BOARD == HAL_BOARD_SITL && defined(HAL_BUILD_AP_PERIPH) diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.h b/libraries/AP_HAL_SITL/SITL_Periph_State.h new file mode 100644 index 0000000000..2f4c37ef19 --- /dev/null +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.h @@ -0,0 +1,75 @@ +#pragma once + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && defined(HAL_BUILD_AP_PERIPH) + +#include "AP_HAL_SITL.h" +#include "AP_HAL_SITL_Namespace.h" +#include "HAL_SITL_Class.h" +#include "RCInput.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +class HAL_SITL; + +class HALSITL::SITL_State { + friend class HALSITL::Scheduler; + friend class HALSITL::Util; + friend class HALSITL::GPIO; +public: + void init(int argc, char * const argv[]); + int gps_pipe(uint8_t index); + + // create a file descriptor attached to a virtual device; type of + // device is given by name parameter + int sim_fd(const char *name, const char *arg); + // returns a write file descriptor for a created virtual device + int sim_fd_write(const char *name); + + bool use_rtscts(void) const { + return _use_rtscts; + } + + uint16_t base_port(void) const { + return _base_port; + } + + // simulated airspeed, sonar and battery monitor + uint16_t sonar_pin_value; // pin 0 + uint16_t airspeed_pin_value; // pin 1 + uint16_t airspeed_2_pin_value; // pin 2 + uint16_t voltage_pin_value; // pin 13 + uint16_t current_pin_value; // pin 12 + uint16_t voltage2_pin_value; // pin 15 + uint16_t current2_pin_value; // pin 14 + // paths for UART devices + const char *_uart_path[7] { + "tcp:5860", + "fifo:/tmp/ap_gps0", + "tcp:5861", + "tcp:5862", + "fifo:/tmp/ap_gps0", + "tcp:5863", + "tcp:5864", + }; +private: + + void wait_clock(uint64_t wait_time_usec); + bool _use_rtscts; + uint16_t _base_port; + + const char *defaults_path = HAL_PARAM_DEFAULTS_PATH; +}; + +#endif diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 2a500db007..1e105278ce 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -1,6 +1,6 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) #include "AP_HAL_SITL.h" #include "AP_HAL_SITL_Namespace.h" diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index d4b0bcc556..f317160ae0 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -3,6 +3,9 @@ #include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if defined(HAL_BUILD_AP_PERIPH) +#include "SITL_Periph_State.h" +#else #include "AP_HAL_SITL.h" #include "AP_HAL_SITL_Namespace.h" @@ -295,4 +298,5 @@ private: const char *_home_str; }; +#endif // defined(HAL_BUILD_AP_PERIPH) #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 5622e24d27..d709bd4f2e 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -1,6 +1,6 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) #include "AP_HAL_SITL.h" #include "AP_HAL_SITL_Namespace.h" diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 992c4183bd..60cdfa411f 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -153,11 +153,15 @@ void Scheduler::system_initialized() { // i386 with gcc doesn't work with FE_INVALID exceptions |= FE_INVALID; #endif +#if !defined(HAL_BUILD_AP_PERIPH) if (_sitlState->_sitl == nullptr || _sitlState->_sitl->float_exception) { feenableexcept(exceptions); } else { feclearexcept(exceptions); } +#else + feclearexcept(exceptions); +#endif _initialized = true; } @@ -241,14 +245,18 @@ void Scheduler::_run_io_procs() hal.uartH->_timer_tick(); hal.storage->_timer_tick(); +#ifndef HAL_BUILD_AP_PERIPH // in lieu of a thread-per-bus: ((HALSITL::I2CDeviceManager*)(hal.i2c_mgr))->_timer_tick(); +#endif #if SITL_STACK_CHECKING_ENABLED check_thread_stacks(); #endif +#ifndef HAL_BUILD_AP_PERIPH AP::RC().update(); +#endif } /* diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index b373cc1329..0196e23a81 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -103,6 +103,14 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) _uart_path = strdup(args1); _uart_baudrate = baudrate; _uart_start_connection(); + } else if (strcmp(devtype, "fifo") == 0) { + ::printf("Reading FIFO file @ %s\n", args1); + _fd = ::open(args1, O_RDONLY | O_NONBLOCK); + if (_fd >= 0) { + _connected = true; + } else { + ::printf("Failed Reading FIFO file @ %s\n", args1); + } } else if (strcmp(devtype, "sim") == 0) { if (!_connected) { ::printf("SIM connection %s:%s on port %u\n", args1, args2, _portNumber); @@ -640,6 +648,7 @@ void UARTDriver::_timer_tick(void) } ssize_t nwritten; uint32_t max_bytes = 10000; +#if !defined(HAL_BUILD_AP_PERIPH) SITL::SITL *_sitl = AP::sitl(); if (_sitl && _sitl->telem_baudlimit_enable) { // limit byte rate to configured baudrate @@ -651,7 +660,7 @@ void UARTDriver::_timer_tick(void) } last_tick_us = now; } - +#endif if (_packetise) { uint16_t n = _writebuffer.available(); n = MIN(n, max_bytes); diff --git a/libraries/AP_HAL_SITL/Util.cpp b/libraries/AP_HAL_SITL/Util.cpp index b984cb04ab..d9d428151e 100644 --- a/libraries/AP_HAL_SITL/Util.cpp +++ b/libraries/AP_HAL_SITL/Util.cpp @@ -131,6 +131,7 @@ void *HALSITL::Util::heap_realloc(void *heap_ptr, void *ptr, size_t new_size) #endif // ENABLE_HEAP +#if !defined(HAL_BUILD_AP_PERIPH) enum AP_HAL::Util::safety_state HALSITL::Util::safety_switch_state(void) { const SITL::SITL *sitl = AP::sitl(); @@ -146,3 +147,4 @@ void HALSITL::Util::set_cmdline_parameters() AP_Param::set_default_by_name(param.name, param.value); } } +#endif diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h index ed06fe249f..809c131626 100644 --- a/libraries/AP_HAL_SITL/Util.h +++ b/libraries/AP_HAL_SITL/Util.h @@ -55,7 +55,9 @@ public: // return true if the reason for the reboot was a watchdog reset bool was_watchdog_reset() const override { return getenv("SITL_WATCHDOG_RESET") != nullptr; } +#if !defined(HAL_BUILD_AP_PERIPH) enum safety_state safety_switch_state(void) override; +#endif bool trap() const override { #if defined(__CYGWIN__) || defined(__CYGWIN64__) diff --git a/libraries/AP_HAL_SITL/sitl_airspeed.cpp b/libraries/AP_HAL_SITL/sitl_airspeed.cpp index 41ddc1809f..b979541df6 100644 --- a/libraries/AP_HAL_SITL/sitl_airspeed.cpp +++ b/libraries/AP_HAL_SITL/sitl_airspeed.cpp @@ -7,7 +7,7 @@ */ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) #include "AP_HAL_SITL.h" #include "AP_HAL_SITL_Namespace.h" diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 8e137b80d9..742ec56dad 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -7,7 +7,7 @@ */ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) #include "AP_HAL_SITL.h" #include "AP_HAL_SITL_Namespace.h" @@ -27,6 +27,7 @@ #include #include #include +#include #pragma GCC diagnostic ignored "-Wunused-result" @@ -36,7 +37,7 @@ extern const AP_HAL::HAL& hal; // state of GPS emulation static struct gps_state { /* pipe emulating UBLOX GPS serial stream */ - int gps_fd, client_fd; + int gps_fd, client_fd, ext_fifo_fd; uint32_t last_update; // milliseconds uint8_t next_index; @@ -65,6 +66,7 @@ ssize_t SITL_State::gps_read(int fd, void *buf, size_t count) /* setup GPS input pipe */ +const char * gps_fifo[2] = {"/tmp/ap_gps0", "/tmp/ap_gps1"}; int SITL_State::gps_pipe(uint8_t idx) { int fd[2]; @@ -72,6 +74,10 @@ int SITL_State::gps_pipe(uint8_t idx) return gps_state[idx].client_fd; } pipe(fd); + if (mkfifo(gps_fifo[idx], 0666) < 0) { + printf("MKFIFO failed with %s\n", strerror(errno)); + } + gps_state[idx].gps_fd = fd[1]; gps_state[idx].client_fd = fd[0]; gps_state[idx].last_update = AP_HAL::millis(); @@ -90,6 +96,12 @@ void SITL_State::_gps_write(const uint8_t *p, uint16_t size, uint8_t instance) if (instance == 1 && _sitl->gps_disable[instance]) { return; } + // also write to external fifo + int fd = open(gps_fifo[instance], O_WRONLY | O_NONBLOCK); + if (fd >= 0) { + write(fd, p, size); + close(fd); + } while (size--) { if (_sitl->gps_byteloss[instance] > 0.0f) { float r = ((((unsigned)random()) % 1000000)) / 1.0e4; diff --git a/libraries/AP_HAL_SITL/sitl_rangefinder.cpp b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp index da84122e4f..48cb1a9e45 100644 --- a/libraries/AP_HAL_SITL/sitl_rangefinder.cpp +++ b/libraries/AP_HAL_SITL/sitl_rangefinder.cpp @@ -6,7 +6,7 @@ */ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) #include "AP_HAL_SITL.h" #include "AP_HAL_SITL_Namespace.h"