From 8ed1b02301e6a80d8c04132c9e504f9c4f82728a 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 0a34615a52..2a70b9fc50 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp
+++ b/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;
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h
index 9aa9ee0959..633dfd802d 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.h
@@ -80,6 +80,9 @@ public:
 #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 @@ public:
 #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;
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 0000000000..b0e23b0e00
--- /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 0000000000..cb10f68065
--- /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 e886c8635f..168f289f11 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h
+++ b/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