HAL_SITL: add support for AP_Periph SITL build

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];
 

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

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

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) {

libraries/AP_HAL_SITL/I2CDevice.cpp

@@ -17,6 +17,8 @@
 #include "I2CDevice.h"
 
 #include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
+
 #include <SITL/SITL.h>
 
 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)

libraries/AP_HAL_SITL/I2CDevice.h

@@ -19,6 +19,8 @@
 #include <inttypes.h>
 
 #include <AP_HAL/HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
+
 #include <AP_HAL/I2CDevice.h>
 #include <AP_HAL/utility/OwnPtr.h>
 
@@ -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)

libraries/AP_HAL_SITL/RCInput.cpp

@@ -1,5 +1,5 @@
 #include <AP_HAL/AP_HAL.h>
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
 
 #include "RCInput.h"
 #include <SITL/SITL.h>

libraries/AP_HAL_SITL/RCInput.h

@@ -2,7 +2,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL.h>
-#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"

libraries/AP_HAL_SITL/RCOutput.cpp

@@ -1,4 +1,7 @@
 #include <AP_HAL/AP_HAL.h>
+
+#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)

libraries/AP_HAL_SITL/RCOutput.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL.h>
-#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 {

libraries/AP_HAL_SITL/SITL_Periph_State.cpp

@@ -0,0 +1,50 @@
+#include <AP_HAL/AP_HAL.h>
+
+#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 <stdio.h>
+#include <signal.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <sys/select.h>
+
+#include <AP_Param/AP_Param.h>
+#include <SITL/SIM_JSBSim.h>
+#include <AP_HAL/utility/Socket.h>
+
+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)

libraries/AP_HAL_SITL/SITL_Periph_State.h

@@ -0,0 +1,75 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+
+#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 <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/udp.h>
+#include <arpa/inet.h>
+
+#include <AP_Baro/AP_Baro.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
+#include <AP_Compass/AP_Compass.h>
+#include <AP_Terrain/AP_Terrain.h>
+#include <AP_HAL/utility/Socket.h>
+
+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

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -1,6 +1,6 @@
 #include <AP_HAL/AP_HAL.h>
 
-#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"

libraries/AP_HAL_SITL/SITL_State.h

@@ -3,6 +3,9 @@
 #include <AP_HAL/AP_HAL.h>
 
 #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

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -1,6 +1,6 @@
 #include <AP_HAL/AP_HAL.h>
 
-#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"

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
 }
 
 /*

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);

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

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__)

libraries/AP_HAL_SITL/sitl_airspeed.cpp

@@ -7,7 +7,7 @@
  */
 
 #include <AP_HAL/AP_HAL.h>
-#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"

libraries/AP_HAL_SITL/sitl_gps.cpp

@@ -7,7 +7,7 @@
  */
 
 #include <AP_HAL/AP_HAL.h>
-#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 <sys/stat.h>
 #include <sys/types.h>
 #include <fcntl.h>
+#include <errno.h>
 
 #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;

libraries/AP_HAL_SITL/sitl_rangefinder.cpp

@@ -6,7 +6,7 @@
  */
 
 #include <AP_HAL/AP_HAL.h>
-#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"