HAL_SITL: support 16 RC input channels in SITL

libraries/AP_HAL_SITL/RCInput.cpp

@@ -21,31 +21,41 @@ bool SITLRCInput::new_input()
     return false;
 }
 
-uint16_t SITLRCInput::read(uint8_t ch) {
+uint16_t SITLRCInput::read(uint8_t ch)
-    if (ch >= 8) {
+{
+    if (ch >= SITL_RC_INPUT_CHANNELS) {
         return 0;
     }
     return _override[ch]? _override[ch] : _sitlState->pwm_input[ch];
 }
 
-uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len) {
+uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len)
+{
+    if (len > SITL_RC_INPUT_CHANNELS) {
+        len = SITL_RC_INPUT_CHANNELS;
+    }
     for (uint8_t i=0; i<len; i++) {
         periods[i] = _override[i]? _override[i] : _sitlState->pwm_input[i];
     }
     return 8;
 }
 
-bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len) {
+bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len)
+{
     bool res = false;
+    if (len > SITL_RC_INPUT_CHANNELS) {
+        len = SITL_RC_INPUT_CHANNELS;
+    }
     for (uint8_t i = 0; i < len; i++) {
         res |= set_override(i, overrides[i]);
     }
     return res;
 }
 
-bool SITLRCInput::set_override(uint8_t channel, int16_t override) {
+bool SITLRCInput::set_override(uint8_t channel, int16_t override)
+{
     if (override < 0) return false; /* -1: no change. */
-    if (channel < 8) {
+    if (channel < SITL_RC_INPUT_CHANNELS) {
         _override[channel] = override;
         if (override != 0) {
             return true;
@@ -56,8 +66,6 @@ bool SITLRCInput::set_override(uint8_t channel, int16_t override) {
 
 void SITLRCInput::clear_overrides()
 {
-    for (uint8_t i = 0; i < 8; i++) {
+    memset(_override, 0, sizeof(_override));
-        _override[i] = 0;
-    }
 }
 #endif

libraries/AP_HAL_SITL/RCInput.h

@@ -1,9 +1,10 @@
 
-#ifndef __AP_HAL_SITL_RCINPUT_H__
+#pragma once
-#define __AP_HAL_SITL_RCINPUT_H__
 
 #include <AP_HAL/AP_HAL.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#define SITL_RC_INPUT_CHANNELS 16
+
 #include "AP_HAL_SITL.h"
 
 class HALSITL::SITLRCInput : public AP_HAL::RCInput {
@@ -11,26 +12,25 @@ public:
     SITLRCInput(SITL_State *sitlState) {
         _sitlState = sitlState;
     }
-    void init();
+    void init() override;
-    bool  new_input();
+    bool  new_input() override;
-    uint8_t num_channels() {
+    uint8_t num_channels() override {
-        return 8;
+        return SITL_RC_INPUT_CHANNELS;
     }
-    uint16_t read(uint8_t ch);
+    uint16_t read(uint8_t ch) override;
-    uint8_t read(uint16_t* periods, uint8_t len);
+    uint8_t read(uint16_t* periods, uint8_t len) override;
 
-    bool set_overrides(int16_t *overrides, uint8_t len);
+    bool set_overrides(int16_t *overrides, uint8_t len) override;
-    bool set_override(uint8_t channel, int16_t override);
+    bool set_override(uint8_t channel, int16_t override) override;
-    void clear_overrides();
+    void clear_overrides() override;
 
 private:
     SITL_State *_sitlState;
     bool _valid;
 
     /* override state */
-    uint16_t _override[8];
+    uint16_t _override[SITL_RC_INPUT_CHANNELS];
 };
 
 #endif
-#endif // __AP_HAL_SITL_RCINPUT_H__
 

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -19,6 +19,7 @@
 
 #include <AP_Param/AP_Param.h>
 #include <SITL/SIM_JSBSim.h>
+#include <AP_HAL/utility/Socket.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -114,34 +115,12 @@ void SITL_State::_sitl_setup(const char *home_str)
  */
 void SITL_State::_setup_fdm(void)
 {
-    int one=1, ret;
+    if (!_sitl_rc_in.bind("0.0.0.0", _simin_port)) {
-    struct sockaddr_in sockaddr;
+        fprintf(stderr, "SITL: socket bind failed - %s\n", strerror(errno));
-
-    memset(&sockaddr,0,sizeof(sockaddr));
-
-#ifdef HAVE_SOCK_SIN_LEN
-    sockaddr.sin_len = sizeof(sockaddr);
-#endif
-    sockaddr.sin_port = htons(_simin_port);
-    sockaddr.sin_family = AF_INET;
-
-    _sitl_fd = socket(AF_INET, SOCK_DGRAM, 0);
-    if (_sitl_fd == -1) {
-        fprintf(stderr, "SITL: socket failed - %s\n", strerror(errno));
         exit(1);
     }
-
+    _sitl_rc_in.reuseaddress();
-    /* we want to be able to re-use ports quickly */
+    _sitl_rc_in.set_blocking(false);
-    setsockopt(_sitl_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
-
-    ret = bind(_sitl_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
-    if (ret == -1) {
-        fprintf(stderr, "SITL: bind failed on port %u - %s\n",
-                (unsigned)ntohs(sockaddr.sin_port), strerror(errno));
-        exit(1);
-    }
-
-    HALSITL::SITLUARTDriver::_set_nonblocking(_sitl_fd);
 }
 #endif
 
@@ -215,15 +194,16 @@ void SITL_State::_fdm_input(void)
 {
     ssize_t size;
     struct pwm_packet {
-        uint16_t pwm[8];
+        uint16_t pwm[16];
     } pwm_pkt;
 
-    size = recv(_sitl_fd, &pwm_pkt, sizeof(pwm_pkt), MSG_DONTWAIT);
+    size = _sitl_rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0);
     switch (size) {
-    case sizeof(pwm_pkt): {
+    case 8*2:
+    case 16*2: {
         // a packet giving the receiver PWM inputs
         uint8_t i;
-        for (i=0; i<8; i++) {
+        for (i=0; i<size/2; i++) {
             // setup the pwn input for the RC channel inputs
             if (pwm_pkt.pwm[i] != 0) {
                 pwm_input[i] = pwm_pkt.pwm[i];

libraries/AP_HAL_SITL/SITL_State.h

@@ -9,6 +9,7 @@
 #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>
@@ -44,7 +45,7 @@ public:
     ssize_t gps_read(int fd, void *buf, size_t count);
     uint16_t pwm_output[SITL_NUM_CHANNELS];
     uint16_t last_pwm_output[SITL_NUM_CHANNELS];
-    uint16_t pwm_input[8];
+    uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
     bool new_rc_input;
     void loop_hook(void);
     uint16_t base_port(void) const {
@@ -148,7 +149,7 @@ private:
     OpticalFlow *_optical_flow;
     AP_Terrain *_terrain;
 
-    int _sitl_fd;
+    SocketAPM _sitl_rc_in{true};
     SITL::SITL *_sitl;
     uint16_t _rcout_port;
     uint16_t _simin_port;