From 638778f1a6aa7bbdfb9b02b0a9071c1ef3c8504a Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 5 Mar 2024 16:50:57 +1100
Subject: [PATCH] AP_RCProtocol: move support for SFML joysticks down into
 AP_RCProtocol

---
 libraries/AP_RCProtocol/AP_RCProtocol.cpp     | 11 +++++
 libraries/AP_RCProtocol/AP_RCProtocol.h       |  6 +++
 .../AP_RCProtocol_Joystick_SFML.cpp           | 48 +++++++++++++++++++
 .../AP_RCProtocol_Joystick_SFML.h             | 24 ++++++++++
 .../AP_RCProtocol/AP_RCProtocol_config.h      |  4 ++
 5 files changed, 93 insertions(+)
 create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_Joystick_SFML.cpp
 create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_Joystick_SFML.h

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
index 7e53bda5e79793..d65a792b021a91 100644
--- 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
index aae2a3b1ecc3ff..b4107994bba076 100644
--- 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
new file mode 100644
index 00000000000000..b0e23b0e00bc3c
--- /dev/null
+++ 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
new file mode 100644
index 00000000000000..cb10f680654e08
--- /dev/null
+++ 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
index 7136a381e34ef9..675d4af668b3cc 100644
--- 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