From ddb5d88dc60227f6aec0cd653f51b12401906d2f Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Thu, 22 Feb 2024 22:23:41 -0300
Subject: [PATCH] AP_Motors6DOF: use its own rc_write, which allows trimming
 motor outputs

---
 libraries/AP_Motors/AP_Motors6DOF.cpp   | 12 ++++++++++++
 libraries/AP_Motors/AP_Motors6DOF.h     |  2 ++
 libraries/AP_Motors/AP_Motors_Class.cpp |  2 +-
 3 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Motors/AP_Motors6DOF.cpp b/libraries/AP_Motors/AP_Motors6DOF.cpp
index 239544f9685bd6..1cadb4cc7615f7 100644
--- a/libraries/AP_Motors/AP_Motors6DOF.cpp
+++ b/libraries/AP_Motors/AP_Motors6DOF.cpp
@@ -20,6 +20,7 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_HAL/AP_HAL.h>
 #include "AP_Motors6DOF.h"
+#include <SRV_Channel/SRV_Channel.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -237,6 +238,17 @@ int16_t AP_Motors6DOF::calc_thrust_to_pwm(float thrust_in) const
     return 1500 + thrust_in * (thrust_in > 0 ? range_up : range_down);
 }
 
+void AP_Motors6DOF::rc_write(uint8_t chan, uint16_t pwm)
+{
+    SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan);
+    if ((1U<<chan) & _motor_pwm_range_mask) {
+        // note that PWM_MIN/MAX has been forced to 1000/2000
+        SRV_Channels::set_output_scaled(function, pwm - 1000);
+    } else {
+        SRV_Channels::set_output_pwm_trimmed(function, pwm);
+    }
+}
+
 void AP_Motors6DOF::output_to_motors()
 {
     int8_t i;
diff --git a/libraries/AP_Motors/AP_Motors6DOF.h b/libraries/AP_Motors/AP_Motors6DOF.h
index 90d7c0e00f90a6..278d65417cf44e 100644
--- a/libraries/AP_Motors/AP_Motors6DOF.h
+++ b/libraries/AP_Motors/AP_Motors6DOF.h
@@ -49,6 +49,8 @@ class AP_Motors6DOF : public AP_MotorsMatrix {
 
     bool set_reversed(int motor_number, bool reversed);
 
+    void rc_write(uint8_t chan, uint16_t pwm) override;
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo        var_info[];
 
diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp
index 9fbdf5ea4dae90..6990bbbb355657 100644
--- a/libraries/AP_Motors/AP_Motors_Class.cpp
+++ b/libraries/AP_Motors/AP_Motors_Class.cpp
@@ -109,7 +109,7 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
         // note that PWM_MIN/MAX has been forced to 1000/2000
         SRV_Channels::set_output_scaled(function, pwm - 1000);
     } else {
-        SRV_Channels::set_output_pwm(function, pwm);
+        SRV_Channels::set_output_pwm_trimmed(function, pwm);
     }
 }