Create AP_RCProtocol_SFML

libraries/AP_HAL_SITL/RCInput.cpp

@@ -8,14 +8,6 @@
 #include <SITL/SITL.h>
 #include <AP_RCProtocol/AP_RCProtocol.h>
 
-#ifdef SFML_JOYSTICK
-  #ifdef HAVE_SFML_GRAPHICS_HPP
-    #include <SFML/Window/Joystick.hpp>
-  #elif HAVE_SFML_GRAPHIC_H
-    #include <SFML/Window/Joystick.h>
-  #endif
-#endif // SFML_JOYSTICK
-
 using namespace HALSITL;
 
 extern const AP_HAL::HAL& hal;
@@ -50,22 +42,7 @@ uint16_t RCInput::read(uint8_t ch)
     if (ch >= num_channels()) {
         return 0;
     }
-#ifdef SFML_JOYSTICK
-    SITL::SIM *_sitl = AP::sitl();
-    if (_sitl) {
-        const sf::Joystick::Axis axis = sf::Joystick::Axis(_sitl->sfml_joystick_axis[ch].get());
-        const unsigned int stickID = _sitl->sfml_joystick_id;
-        if (sf::Joystick::hasAxis(stickID, axis)) {
-            return (constrain_float(sf::Joystick::getAxisPosition(stickID, axis) + 100, 0, 200) * 5) + 1000;
-        } else {
-            return 0;
-        }
-    } else {
-      return 0;
-    }
-#else
     return _sitlState->pwm_input[ch];
-#endif
 }
 
 uint8_t RCInput::read(uint16_t* periods, uint8_t len)
@@ -86,11 +63,7 @@ uint8_t RCInput::num_channels()
     }
     SITL::SIM *_sitl = AP::sitl();
     if (_sitl) {
-#ifdef SFML_JOYSTICK
-        return (sf::Joystick::isConnected(_sitl->sfml_joystick_id.get())) ? ARRAY_SIZE(_sitl->sfml_joystick_axis) : 0;
-#else
         return MIN(_sitl->rc_chancount.get(), SITL_RC_INPUT_CHANNELS);
-#endif // SFML_JOYSTICK
     }
     return SITL_RC_INPUT_CHANNELS;
 }

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -35,6 +35,7 @@
 #include "AP_RCProtocol_DroneCAN.h"
 #include "AP_RCProtocol_GHST.h"
 #include "AP_RCProtocol_MAVLinkRadio.h"
+#include "AP_RCProtocol_Joystick_SFML.h"
 #include <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 
@@ -92,6 +93,9 @@ void AP_RCProtocol::init()
 #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
     backend[AP_RCProtocol::MAVLINK_RADIO] = new AP_RCProtocol_MAVLinkRadio(*this);
 #endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+    backend[AP_RCProtocol::JOYSTICK_SFML] = new AP_RCProtocol_Joystick_SFML(*this);
+#endif
 }
 
 AP_RCProtocol::~AP_RCProtocol()
@@ -437,6 +441,9 @@ bool AP_RCProtocol::new_input()
 #endif
 #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
         AP_RCProtocol::MAVLINK_RADIO,
+#endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+        AP_RCProtocol::JOYSTICK_SFML,
 #endif
     };
     for (const auto protocol : pollable) {
@@ -573,6 +580,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
 #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
     case MAVLINK_RADIO:
         return "MAVRadio";
+#endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+    case JOYSTICK_SFML:
+        return "SFML";
 #endif
     case NONE:
         break;

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -80,6 +80,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
         MAVLINK_RADIO = 15,
+#endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+        JOYSTICK_SFML = 16,
 #endif
         NONE    //last enum always is None
     };
@@ -167,6 +170,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
         case MAVLINK_RADIO:
+#endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+        case JOYSTICK_SFML:
 #endif
         case NONE:
             return false;

libraries/AP_RCProtocol/AP_RCProtocol_Joystick_SFML.cpp

@@ -0,0 +1,48 @@
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+
+#include "AP_RCProtocol_Joystick_SFML.h"
+
+#include <SITL/SITL.h>
+
+#ifdef HAVE_SFML_GRAPHICS_HPP
+#include <SFML/Window/Joystick.hpp>
+#elif HAVE_SFML_GRAPHIC_H
+#include <SFML/Window/Joystick.h>
+#endif
+
+void AP_RCProtocol_Joystick_SFML::update()
+{
+    auto *_sitl = AP::sitl();
+    if (_sitl == nullptr) {
+        return;
+    }
+
+    sf::Joystick::update();
+
+    const unsigned int stick_id = _sitl->sfml_joystick_id;
+    if (!sf::Joystick::isConnected(stick_id)) {
+        return;
+    }
+
+    uint16_t pwm_values[ARRAY_SIZE(_sitl->sfml_joystick_axis)]{};
+    for (uint8_t ch=0; ch<ARRAY_SIZE(_sitl->sfml_joystick_axis); ch++) {
+        const sf::Joystick::Axis axis = sf::Joystick::Axis(_sitl->sfml_joystick_axis[ch].get());
+        if (!sf::Joystick::hasAxis(stick_id, axis)) {
+            continue;
+        }
+
+        // pos is a value between -100 and 100:
+        const auto pos = sf::Joystick::getAxisPosition(stick_id, axis);
+
+        // convert to a "pwm" value between 1000 and 2000:
+        const uint16_t pwm = (constrain_float(pos + 100, 0, 200) * 5) + 1000;
+        pwm_values[ch] = pwm;
+    }
+
+    // never in failsafe:
+    add_input(ARRAY_SIZE(pwm_values), pwm_values, false);
+}
+
+#endif  // AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_Joystick_SFML.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+
+#include "AP_RCProtocol_Backend.h"
+
+class AP_RCProtocol_Joystick_SFML : public AP_RCProtocol_Backend {
+public:
+
+    AP_RCProtocol_Joystick_SFML(AP_RCProtocol &_frontend) :
+        AP_RCProtocol_Backend(_frontend) {
+    }
+
+    void update() override;
+
+private:
+
+    uint32_t last_receive_ms;
+};
+
+
+#endif  // AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_config.h

@@ -35,6 +35,10 @@
 #define AP_RCPROTOCOL_IBUS_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+#define AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED defined(SFML_JOYSTICK)
+#endif
+
 #ifndef AP_RCPROTOCOL_PPMSUM_ENABLED
 #define AP_RCPROTOCOL_PPMSUM_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED
 #endif