AP_HAL_ChibiOS: dshot commands for reverse/3D should be sent to IOMCU

normalize servo/FMU channels for dshot commands and 3D mask
ArduPilot · Aug 21, 2024 · 273e6ab · 273e6ab
1 parent a427c8f
commit 273e6ab
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 7 deletions.
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -1650,7 +1650,9 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
                 bdshot_decode_telemetry_from_erpm(group.bdshot.erpm[i], chan);
             }
 #endif
-            if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
+            const uint32_t servo_chan_mask = 1U<<(chan+chan_offset);
+
+            if (safety_on && !(safety_mask & servo_chan_mask)) {
                 // safety is on, don't output anything
                 continue;
             }
@@ -1662,12 +1664,10 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
                 continue;
             }
 
-            const uint32_t chan_mask = (1U<<chan);
-
             pwm = constrain_int16(pwm, 1000, 2000);
             uint16_t value = MIN(2 * (pwm - 1000), 1999);
 
-            if ((chan_mask & _reversible_mask) != 0) {
+            if ((servo_chan_mask & _reversible_mask) != 0) {
                 // this is a DShot-3D output, map so that 1500 PWM is zero throttle reversed
                 if (value < 1000) {
                     value = 1999 - value;
@@ -1690,6 +1690,7 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
             }
 
             // according to sskaug requesting telemetry while trying to arm may interfere with the good frame calc
+            const uint32_t chan_mask = (1U<<chan);
             bool request_telemetry = telem_request_mask & chan_mask;
             uint16_t packet = create_dshot_packet(value, request_telemetry,
 #ifdef HAL_WITH_BIDIR_DSHOT

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
@@ -33,6 +33,8 @@ using namespace ChibiOS;
 
 extern const AP_HAL::HAL& hal;
 
+// send a dshot command to group
+// chan is the FMU channel to send the command to
 bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t chan)
 {
     if (!group.can_send_dshot_pulse()) {
@@ -112,10 +114,11 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman
     DshotCommandPacket pkt;
     pkt.command = command;
     if (chan != ALL_CHANNELS) {
-        pkt.chan = chan - chan_offset;
+        pkt.chan = chan - chan_offset;  // normalize to FMU channel
     } else {
         pkt.chan = ALL_CHANNELS;
     }
+
     if (command_timeout_ms == 0) {
         pkt.cycle = MAX(10, repeat_count);
     } else {
@@ -156,8 +159,9 @@ void RCOutput::update_channel_masks() {
         return;
     }
 
-#if HAL_PWM_COUNT > 0
-    for (uint8_t i=chan_offset; i<HAL_PWM_COUNT+chan_offset; i++) {
+    // The masks use servo channel numbering
+#if NUM_SERVO_CHANNELS > 0
+    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
         switch (_dshot_esc_type) {
             case DSHOT_ESC_BLHELI:
             case DSHOT_ESC_BLHELI_S: