AP_RCProtocol: move support for SFML joysticks down into AP_RCProtocol

ArduPilot · Mar 5, 2024 · 638778f · 638778f
1 parent 0ee21c1
commit 638778f
Show file tree

Hide file tree

Showing 5 changed files with 93 additions and 0 deletions.
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
@@ -34,6 +34,7 @@
 #include "AP_RCProtocol_FPort2.h"
 #include "AP_RCProtocol_DroneCAN.h"
 #include "AP_RCProtocol_GHST.h"
+#include "AP_RCProtocol_Joystick_SFML.h"
 #include <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 
@@ -88,6 +89,9 @@ void AP_RCProtocol::init()
 #if AP_RCPROTOCOL_GHST_ENABLED
     backend[AP_RCProtocol::GHST] = new AP_RCProtocol_GHST(*this);
 #endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+    backend[AP_RCProtocol::JOYSTICK_SFML] = new AP_RCProtocol_Joystick_SFML(*this);
+#endif
 }
 
 AP_RCProtocol::~AP_RCProtocol()
@@ -422,6 +426,9 @@ bool AP_RCProtocol::new_input()
     const rcprotocol_t pollable[] {
 #if AP_RCPROTOCOL_DRONECAN_ENABLED
         AP_RCProtocol::DRONECAN,
+#endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+        AP_RCProtocol::JOYSTICK_SFML,
 #endif
     };
     for (const auto protocol : pollable) {
@@ -554,6 +561,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
 #if AP_RCPROTOCOL_GHST_ENABLED
     case GHST:
         return "GHST";
+#endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+    case JOYSTICK_SFML:
+        return "SFML";
 #endif
     case NONE:
         break;

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h
@@ -74,6 +74,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_GHST_ENABLED
         GHST       = 14,
+#endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+        JOYSTICK_SFML = 16,
 #endif
         NONE    //last enum always is None
     };
@@ -158,6 +161,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_DRONECAN_ENABLED
         case DRONECAN:
+#endif
+#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
+        case JOYSTICK_SFML:
 #endif
         case NONE:
             return false;

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Joystick_SFML.cpp b/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
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Joystick_SFML.h b/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
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h
@@ -34,6 +34,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