Fix ESCs constantly arming on rover with dshot commands

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -201,6 +201,16 @@ void RCOutput::rcout_thread()
 
         // process any pending RC output requests
         timer_tick(time_out_us);
+#if RCOU_DSHOT_TIMING_DEBUG
+        static bool output_masks = true;
+        if (AP_HAL::millis() > 5000 && output_masks) {
+            output_masks = false;
+            hal.console->printf("bdmask 0x%x, en_mask 0x%lx, 3dmask 0x%x:\n", _bdshot.mask, en_mask, _reversible_mask);
+            for (auto &group : pwm_group_list) {
+                hal.console->printf("  timer %u: ch_mask 0x%x, en_mask 0x%x\n", group.timer_id, group.ch_mask, group.en_mask);
+            }
+        }
+#endif
     }
 }
 
@@ -1338,8 +1348,9 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
     // assume that we won't be able to get the input capture lock
     group.bdshot.enabled = false;
 
+    uint16_t active_channels = group.ch_mask & group.en_mask;
     // now grab the input capture lock if we are able, we can only enable bi-dir on a group basis
-    if (((_bdshot.mask & group.ch_mask) == group.ch_mask) && group.has_ic()) {
+    if (((_bdshot.mask & active_channels) == active_channels) && group.has_ic()) {
         if (group.has_shared_ic_up_dma()) {
             // no locking required
             group.bdshot.enabled = true;
@@ -1398,19 +1409,20 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
 #endif
             _bdshot.erpm[chan] = erpm;
 #endif
-            uint16_t pwm = period[chan];
-
             if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
-                // safety is on, overwride pwm
-                pwm = 0;
+                // safety is on, don't output anything
+                continue;
             }
 
-            const uint16_t chan_mask = (1U<<chan);
+            uint16_t pwm = period[chan];
+
             if (pwm == 0) {
-                // no output
+                // no pwm, don't output anything
                 continue;
             }
 
+            const uint16_t chan_mask = (1U<<chan);
+
             pwm = constrain_int16(pwm, 1000, 2000);
             uint16_t value = MIN(2 * (pwm - 1000), 1999);
 

libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp

@@ -62,18 +62,12 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
         return true;
     }
 
-    bool set_curr_chan = false;
-
+    // allocate input capture DMA handles
     for (uint8_t i = 0; i < 4; i++) {
         if (!group.is_chan_enabled(i) ||
             !group.dma_ch[i].have_dma || !(_bdshot.mask & (1 << group.chan[i]))) {
             continue;
         }
-        // make sure we don't start on a disabled channel
-        if (!set_curr_chan) {
-            group.bdshot.curr_telem_chan = i;
-            set_curr_chan = true;
-        }
         pwmmode_t mode = group.pwm_cfg.channels[i].mode;
         if (mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW ||
             mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH) {
@@ -99,6 +93,15 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
     // We might need to do sharing of timers for telemetry feedback
     // due to lack of available DMA channels
     for (uint8_t i = 0; i < 4; i++) {
+        // we must pull all the allocated channels high to prevent them going low
+        // when the pwm peripheral is stopped
+        if (group.chan[i] != CHAN_DISABLED && _bdshot.mask & group.ch_mask) {
+            // bi-directional dshot requires less than MID2 speed and PUSHPULL in order to avoid noise on the line
+            // when switching from output to input
+            palSetLineMode(group.pal_lines[i], PAL_MODE_ALTERNATE(group.alt_functions[i])
+                | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_PUPDR_PULLUP | PAL_STM32_OSPEED_MID1);
+        }
+
         if (!group.is_chan_enabled(i) || !(_bdshot.mask & (1 << group.chan[i]))) {
             continue;
         }
@@ -137,10 +140,14 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
             group.bdshot.telem_tim_ch[i] = curr_chan;
             group.dma_ch[i] = group.dma_ch[curr_chan];
         }
-        // bi-directional dshot requires less than MID2 speed and PUSHPULL in order to avoid noise on the line
-        // when switching from output to input
-        palSetLineMode(group.pal_lines[i], PAL_MODE_ALTERNATE(group.alt_functions[i])
-            | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_PUPDR_PULLUP | PAL_STM32_OSPEED_MID1);
+    }
+
+    // now allocate the starting channel
+    for (uint8_t i = 0; i < 4; i++) {
+        if (group.chan[i] != CHAN_DISABLED && group.bdshot.ic_dma_handle[i] != nullptr) {
+            group.bdshot.curr_telem_chan = i;
+            break;
+        }
     }
 
     return true;

libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp

@@ -38,7 +38,9 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
         return false;
     }
 
+#ifdef HAL_GPIO_LINE_GPIO81
     TOGGLE_PIN_DEBUG(81);
+#endif
     // first make sure we have the DMA channel before anything else
 
     osalDbgAssert(!group.dma_handle->is_locked(), "DMA handle is already locked");
@@ -48,9 +50,10 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
     group.dshot_waiter = rcout_thread_ctx;
     bool bdshot_telem = false;
 #ifdef HAL_WITH_BIDIR_DSHOT
+    uint16_t active_channels = group.ch_mask & group.en_mask;
     // no need to get the input capture lock
     group.bdshot.enabled = false;
-    if ((_bdshot.mask & group.ch_mask) == group.ch_mask) {
+    if ((_bdshot.mask & active_channels) == active_channels) {
         bdshot_telem = true;
         // it's not clear why this is required, but without it we get no output
         if (group.pwm_started) {
@@ -68,17 +71,23 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
     const uint16_t packet = create_dshot_packet(command, true, bdshot_telem);
 
     for (uint8_t i = 0; i < 4; i++) {
-        if (group.chan[i] == chan || (chan == RCOutput::ALL_CHANNELS && group.is_chan_enabled(i))) {
+        if (!group.is_chan_enabled(i)) {
+            continue;
+        }
+
+        if (group.chan[i] == chan || chan == RCOutput::ALL_CHANNELS) {
             fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet, group.bit_width_mul);
-        } else if (group.is_chan_enabled(i)) {
+        } else {
             fill_DMA_buffer_dshot(group.dma_buffer + i, 4, zero_packet, group.bit_width_mul);
         }
     }
 
     chEvtGetAndClearEvents(group.dshot_event_mask);
     // start sending the pulses out
     send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH);
+#ifdef HAL_GPIO_LINE_GPIO81
     TOGGLE_PIN_DEBUG(81);
+#endif
 
     return true;
 }

libraries/AP_HAL_SITL/RCOutput.cpp

@@ -35,21 +35,22 @@ void RCOutput::enable_ch(uint8_t ch)
     if (!(_enable_mask & (1U << ch))) {
         Debug("enable_ch(%u)\n", ch);
     }
-    _enable_mask |= 1U << ch;
+    _enable_mask |= (1U << ch);
 }
 
 void RCOutput::disable_ch(uint8_t ch)
 {
     if (_enable_mask & (1U << ch)) {
         Debug("disable_ch(%u)\n", ch);
     }
-    _enable_mask &= ~1U << ch;
+    _enable_mask &= ~(1U << ch);
 }
 
 void RCOutput::write(uint8_t ch, uint16_t period_us)
 {
     _sitlState->output_ready = true;
-    if (ch < SITL_NUM_CHANNELS && (_enable_mask & (1U<<ch))) {
+    // FIXME: something in sitl is expecting to be able to read and write disabled channels
+    if (ch < SITL_NUM_CHANNELS /*&& (_enable_mask & (1U<<ch))*/) {
         if (_corked) {
             _pending[ch] = period_us;
         } else {
@@ -60,7 +61,8 @@ void RCOutput::write(uint8_t ch, uint16_t period_us)
 
 uint16_t RCOutput::read(uint8_t ch)
 {
-    if (ch < SITL_NUM_CHANNELS) {
+    // FIXME: something in sitl is expecting to be able to read and write disabled channels
+    if (ch < SITL_NUM_CHANNELS /*&& (_enable_mask & (1U<<ch))*/) {
         return _sitlState->pwm_output[ch];
     }
     return 0;

libraries/SRV_Channel/SRV_Channel.h

@@ -183,7 +183,7 @@ class SRV_Channel {
 
     // check if a function is valid for indexing into functions
     static bool valid_function(Aux_servo_function_t fn) {
-        return fn >= 0 && fn < k_nr_aux_servo_functions;
+        return fn > k_none && fn < k_nr_aux_servo_functions;
     }
     bool valid_function(void) const {
         return valid_function(function);

libraries/SRV_Channel/SRV_Channel_aux.cpp

@@ -208,6 +208,8 @@ void SRV_Channels::enable_aux_servos()
         // see if it is a valid function
         if (c.valid_function()) {
             hal.rcout->enable_ch(c.ch_num);
+        } else {
+            hal.rcout->disable_ch(c.ch_num);
         }
 
         // output some servo functions before we fiddle with the