From 7621c3c0eb875aa337a6a9e0b588bf8fab7fc8bd Mon Sep 17 00:00:00 2001
From: = <fiorenzani@avinc.com>
Date: Tue, 17 Sep 2024 15:52:38 -0700
Subject: [PATCH] introducing changes from
 https://github.com/ArduPilot/ardupilot/pull/25334/files

---
 libraries/AP_DDS/AP_DDS_Client.cpp | 26 ++++++++++++++++++++++++++
 libraries/AP_DDS/AP_DDS_Client.h   |  3 +++
 2 files changed, 29 insertions(+)

diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
index 77f052de6204d6..c2505cbde11bcc 100644
--- a/libraries/AP_DDS/AP_DDS_Client.cpp
+++ b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -13,6 +13,7 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Arming/AP_Arming.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <RC_Channel/RC_Channel.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
 
 #include "ardupilot_msgs/srv/ArmMotors.h"
@@ -557,6 +558,31 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received sensor_msgs/Joy: %f, %f, %f, %f",
                           msg_prefix, rx_joy_topic.axes[0], rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]);
             // TODO implement joystick RC control to AP
+            const uint32_t tnow = AP_HAL::millis();
+            for (uint8_t i=0; i<rx_joy_topic.axes_size; i++) {
+                const uint16_t override_data = static_cast<uint16_t>(rx_joy_topic.axes[i]);
+                const RC_Channel *c = rc().channel(i);
+                radio_min_val=c->get_radio_min();
+                radio_max_val=c->get_radio_max();
+                // Per MAVLink spec a value of UINT16_MAX means to ignore this field.
+                if (override_data != UINT16_MAX) {
+                    const uint16_t mapped_data=linear_interpolate(radio_min_val,radio_max_val,override_data,-1.0,1.0);
+                    RC_Channels::set_override(i, mapped_data, tnow);
+                }
+            }
+            for (uint8_t i=8; i<rx_joy_topic.axes_size; i++) {
+                const uint16_t override_data = static_cast<uint16_t>(rx_joy_topic.axes[i]);
+
+                // Per MAVLink spec a value of zero or UINT16_MAX means to
+                // ignore this field.
+                if (override_data != 0 && override_data != UINT16_MAX) {
+                    // per the mavlink spec, a value of UINT16_MAX-1 means
+                    // return the field to RC radio values:
+                    const uint16_t value = override_data == (UINT16_MAX-1) ? 0 : linear_interpolate(radio_min_val,radio_max_val,override_data,-1.0,1.0);
+                    RC_Channels::set_override(i, value, tnow);
+                    }
+            }
+
         } else {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received sensor_msgs/Joy. Axes size must be >= 4", msg_prefix);
         }
diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h
index daddfff96877f4..b912cd6b11c4b7 100644
--- a/libraries/AP_DDS/AP_DDS_Client.h
+++ b/libraries/AP_DDS/AP_DDS_Client.h
@@ -138,6 +138,9 @@ class AP_DDS_Client
     // The last ms timestamp AP_DDS wrote a gps global origin message
     uint64_t last_gps_global_origin_time_ms;
 
+    int16_t  radio_min_val;
+    int16_t  radio_max_val;    
+
     // functions for serial transport
     bool ddsSerialInit();
     static bool serial_transport_open(uxrCustomTransport* args);