AP_Motors6DOF: use its own rc_write, which allows trimming motor outputs

Williangalvani · Feb 23, 2024 · 96804d2 · 96804d2
1 parent b19f8ed
commit 96804d2
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 0 deletions.
diff --git 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,18 @@ 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)
+{
+    // almost exact copy of AP_Motors::rc_write, execpt for the _trimmed part
+    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
@@ -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[];