AP_HAL_ChibiOS: allow dshot rate to be set so as to both regularize the output as well as

allow faster rates synchronized to the loop rate
synchronize 1Khz and use Betaflight definition for prescaler
adjust dshot bitrates
don't allocate IC DMA if already allocated
cancel DMA pulses correctly
try really hard to align pulses with push() by making each dshot pulse event driven

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -98,6 +98,7 @@ void RCOutput::init()
     }
 #endif
     chMtxObjectInit(&trigger_mutex);
+    chVTObjectInit(&_dshot_rate_timer);
 
     // setup default output rate of 50Hz
     set_freq(0xFFFF ^ ((1U<<chan_offset)-1), 50);
@@ -122,6 +123,7 @@ void RCOutput::init()
 void RCOutput::rcout_thread()
 {
     uint32_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout
+    uint32_t last_cycle_run_us = 0;
 
     rcout_thread_ctx = chThdGetSelfX();
 
@@ -130,33 +132,62 @@ void RCOutput::rcout_thread()
         hal.scheduler->delay_microseconds(1000);
     }
 
+    // dshot is quite sensitive to timing, it's important to output pulses as
+    // regularly as possible at the correct bitrate
     while (true) {
-        const uint32_t period_us = AP_HAL::micros() - last_thread_run_us;
-        if (period_us < 1000) {
-            chEvtWaitOneTimeout(EVT_PWM_SEND, chTimeUS2I(1000 - period_us));
-        }
+
+        chEvtWaitOne(EVT_PWM_SEND);
+
         // start the clock
         last_thread_run_us = AP_HAL::micros();
 
-        // main thread requested a new dshot send
+        // this is when the cycle is supposed to start
+        if (_dshot_cycle == 0) {
+            last_cycle_run_us = AP_HAL::micros();
+            // register a timer for the next tick if push() will not be providing it
+            if (_dshot_rate != 1) {
+                chVTSet(&_dshot_rate_timer, chTimeUS2I(_dshot_period_us), dshot_update_tick, this);
+            }
+        }
+
+        // if DMA sharing is in effect there can be quite a delay between the request to begin the cycle and
+        // actually sending out data - thus we need to work out how much time we have left to collect the locks
+        uint32_t time_out_us = (_dshot_cycle + 1) * _dshot_period_us + last_cycle_run_us;
+        if (!_dshot_rate) {
+            time_out_us = last_thread_run_us + _dshot_period_us;
+        }
+
+        // main thread requested a new dshot send or we timed out - if we are not running
+        // as a multiple of loop rate then ignore EVT_PWM_SEND events to preserve periodicity
         if (!serial_group) {
-            // do a blocking send now, to guarantee DShot sends at
-            // above 1000 Hz. This makes the protocol more reliable on
-            // long cables, and also keeps some ESCs happy that don't
-            // like low rates
-            dshot_send_groups();
+            dshot_send_groups(time_out_us);
+
             // now unlock everything
-            dshot_collect_dma_locks(last_thread_run_us);
+            dshot_collect_dma_locks(time_out_us);
+
+            _dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;
         }
 
         // process any pending RC output requests
-        timer_tick(last_thread_run_us);
+        timer_tick(time_out_us);
     }
 }
 
+void RCOutput::dshot_update_tick(void* p)
+{
+    chSysLockFromISR();
+    RCOutput* rcout = (RCOutput*)p;
+
+    if (rcout->_dshot_cycle < rcout->_dshot_rate - 1) {
+        chVTSetI(&rcout->_dshot_rate_timer, chTimeUS2I(rcout->_dshot_period_us), dshot_update_tick, p);
+    }
+    chEvtSignalI(rcout->rcout_thread_ctx, EVT_PWM_SEND);
+    chSysUnlockFromISR();
+}
+
 #ifndef HAL_NO_SHARED_DMA
 // release locks on the groups that are pending in reverse order
-void RCOutput::dshot_collect_dma_locks(uint32_t last_run_us)
+void RCOutput::dshot_collect_dma_locks(uint32_t time_out_us)
 {
     if (NUM_GROUPS == 0) {
         return;
@@ -165,11 +196,17 @@ void RCOutput::dshot_collect_dma_locks(uint32_t last_run_us)
         pwm_group &group = pwm_group_list[i];
         if (group.dma_handle != nullptr && group.dma_handle->is_locked()) {
             // calculate how long we have left
-            const uint32_t deltat = AP_HAL::micros() - last_run_us;
+            uint32_t now = AP_HAL::micros();
             // if we have time left wait for the event
             eventmask_t mask = 0;
-            if (deltat < 1000) {
-                mask = chEvtWaitOneTimeout(group.dshot_event_mask, chTimeUS2I(1000 - deltat));
+            const uint32_t pulse_elapsed_us = now - group.last_dmar_send_us;
+            if (now < time_out_us) {
+                mask = chEvtWaitOneTimeout(group.dshot_event_mask,
+                    chTimeUS2I(MAX(time_out_us - now, group.dshot_pulse_send_time_us - pulse_elapsed_us)));
+            } else if (pulse_elapsed_us < group.dshot_pulse_send_time_us) {
+                // better to let the burst write in progress complete rather than cancelling mid way through
+                mask = chEvtWaitOneTimeout(group.dshot_event_mask,
+                    chTimeUS2I(group.dshot_pulse_send_time_us - pulse_elapsed_us));
             }
             // no time left cancel and restart
             if (!mask) {
@@ -345,6 +382,34 @@ void RCOutput::set_default_rate(uint16_t freq_hz)
     }
 }
 
+/*
+  Set the dshot rate as a multiple of the loop rate
+ */
+void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
+{
+    // for low loop rates simply output at 1Khz on a timer
+    if (loop_rate_hz <= 100) {
+        _dshot_period_us = 1000UL;
+        _dshot_rate = 0;
+        return;
+    }
+
+    uint16_t drate = dshot_rate * loop_rate_hz;
+    _dshot_rate = dshot_rate;
+    // never allow rates below 500hz
+    while (drate < 500) {
+        _dshot_rate++;
+        drate = _dshot_rate * loop_rate_hz;
+    }
+    // prevent stupidly high rates, ideally should also prevent high rates
+    // with slower dshot variants
+    if (drate > 4000) {
+        _dshot_rate = 4000 / loop_rate_hz;
+        drate = _dshot_rate * loop_rate_hz;
+    }
+    _dshot_period_us = 1000000UL / drate;
+}
+
 /*
   find pwm_group and index in group given a channel number
  */
@@ -631,7 +696,7 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
         group.dma_buffer_len = buffer_length;
     }
     // reset the pulse time inside the lock
-    group.dshot_pulse_time_us = pulse_time_us;
+    group.dshot_pulse_time_us = group.dshot_pulse_send_time_us = pulse_time_us;
 
 #ifdef HAL_WITH_BIDIR_DSHOT
     // configure input capture DMA if required
@@ -648,42 +713,30 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
         group.pwm_started = false;
     }
 
-    // adjust frequency to give an allowed value given the
-    // clock. There is probably a better way to do this
-    const uint32_t original_bitrate = bitrate;
-    uint32_t freq = 0;
-    uint32_t target_frequency = bitrate * bit_width;
-    while (true) {
-        uint32_t clock_hz = group.pwm_drv->clock;
-        target_frequency = bitrate * bit_width;
-        uint32_t prescaler = clock_hz / target_frequency;
-        freq = clock_hz / prescaler;
-        // hal.console->printf("CLOCK=%u BW=%u FREQ=%u PRE=%u BR=%u\n", clock_hz, bit_width, freq/bit_width, prescaler, bitrate);
-        if (prescaler > 0x8000) {
-            group.dma_handle->unlock();
-            return false;
-        }
-        if (!choose_high) {
-            break;
-        }
-        // we want to choose a frequency that gives at least the
-        // target, erring on the high side not low side
-        uint32_t actual_bitrate = freq / bit_width;
-        if (actual_bitrate >= original_bitrate || bitrate < 10000) {
-            break;
-        }
-        bitrate += 10000;
-        if (bitrate >= 2 * original_bitrate) {
-            break;
-        }
+    // original prescaler calculation from betaflight. bi-dir dshot is incredibly sensitive to the bitrate
+    const uint32_t target_frequency = bitrate * bit_width;
+    uint32_t prescaler = uint32_t(lrintf((float) group.pwm_drv->clock / (bitrate * bit_width) + 0.01f) - 1);
+    uint32_t freq = group.pwm_drv->clock / prescaler;
+    if (freq > target_frequency && !choose_high) {
+        prescaler++;
+    } else if (freq < target_frequency && choose_high) {
+        prescaler--;
+    }
+    if (prescaler > 0x8000) {
+        group.dma_handle->unlock();
+        return false;
     }
 
+    freq = group.pwm_drv->clock / prescaler;
     group.pwm_cfg.frequency = freq;
     group.pwm_cfg.period = bit_width;
     group.pwm_cfg.dier = TIM_DIER_UDE;
     group.pwm_cfg.cr2 = 0;
     group.bit_width_mul = (freq + (target_frequency/2)) / target_frequency;
 
+    //hal.console->printf("CLOCK=%u BW=%u FREQ=%u BR=%u MUL=%u PRE=%u\n", unsigned(group.pwm_drv->clock), unsigned(bit_width), unsigned(group.pwm_cfg.frequency),
+    //    unsigned(bitrate), unsigned(group.bit_width_mul), unsigned(prescaler));
+
     for (uint8_t j=0; j<4; j++) {
         pwmmode_t mode = group.pwm_cfg.channels[j].mode;
         if (mode != PWM_OUTPUT_DISABLED) {
@@ -775,7 +828,7 @@ void RCOutput::set_group_mode(pwm_group &group)
 
         // configure timer driver for DMAR at requested rate
         if (!setup_group_DMA(group, rate, bit_period, active_high,
-            MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), false, pulse_send_time_us)) {
+            MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), group.current_mode != MODE_PWM_DSHOT150, pulse_send_time_us)) {
             group.current_mode = MODE_PWM_NORMAL;
             break;
         }
@@ -1013,7 +1066,7 @@ void RCOutput::trigger_groups(void)
   periodic timer. This is used for oneshot and dshot modes, plus for
   safety switch update. Runs every 1000us.
  */
-void RCOutput::timer_tick(uint32_t last_run_us)
+void RCOutput::timer_tick(uint32_t time_out_us)
 {
     safety_update();
 
@@ -1022,20 +1075,21 @@ void RCOutput::timer_tick(uint32_t last_run_us)
     }
 
     // if we have enough time left send out LED data
-    if (serial_led_pending && AP_HAL::micros() - last_run_us < 500) {
+    if (serial_led_pending && (time_out_us > (AP_HAL::micros() + (_dshot_period_us >> 1)))) {
         serial_led_pending = false;
         for (auto &group : pwm_group_list) {
             serial_led_pending |= !serial_led_send(group);
         }
 
         // release locks on the groups that are pending in reverse order
-        dshot_collect_dma_locks(last_run_us);
+        dshot_collect_dma_locks(time_out_us);
     }
 
     if (min_pulse_trigger_us == 0) {
         return;
     }
-
+#if !APM_BUILD_TYPE(APM_BUILD_ArduCopter)
+    // this exists simply to cater for non-multirotors whose loop rate might be 50Hz
     uint32_t now = AP_HAL::micros();
 
     if (now > min_pulse_trigger_us &&
@@ -1043,10 +1097,11 @@ void RCOutput::timer_tick(uint32_t last_run_us)
         // trigger at a minimum of 250Hz
         trigger_groups();
     }
+#endif
 }
 
 // send dshot for all groups that support it
-void RCOutput::dshot_send_groups()
+void RCOutput::dshot_send_groups(uint32_t time_out_us)
 {
     if (serial_group) {
         return;
@@ -1055,7 +1110,7 @@ void RCOutput::dshot_send_groups()
     // actually do a dshot send
     for (auto &group : pwm_group_list) {
         if (group.can_send_dshot_pulse()) {
-            dshot_send(group);
+            dshot_send(group, time_out_us);
             // delay sending the next group by the same amount as the IRQ dead time
             // to avoid irq collisions
             if (group.bdshot.enabled) {
@@ -1154,7 +1209,7 @@ void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t
   This call be called in blocking mode from the timer, in which case it waits for the DMA lock.
   In normal operation it doesn't wait for the DMA lock.
  */
-void RCOutput::dshot_send(pwm_group &group)
+void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
 {
 #ifndef DISABLE_DSHOT
     if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
@@ -1165,6 +1220,14 @@ void RCOutput::dshot_send(pwm_group &group)
     // first make sure we have the DMA channel before anything else
     osalDbgAssert(!group.dma_handle->is_locked(), "DMA handle is already locked");
     group.dma_handle->lock();
+
+    // if we are sharing UP channels then it might have taken a long time to get here,
+    // if there's not enough time to actually send a pulse then cancel
+    if (AP_HAL::micros() + group.dshot_pulse_time_us > time_out_us) {
+        group.dma_handle->unlock();
+        return;
+    }
+
     // only the timer thread releases the locks
     group.dshot_waiter = rcout_thread_ctx;
 #ifdef HAL_WITH_BIDIR_DSHOT
@@ -1406,8 +1469,16 @@ void RCOutput::dma_cancel(pwm_group& group)
         dmaStreamDisable(group.bdshot.ic_dma[group.bdshot.curr_telem_chan]);
     }
 #endif
+    // normally the CCR registers are reset by the final 0 in the DMA buffer
+    // since we are cancelling early they need to be reset to avoid infinite pulses
+    for (uint8_t i = 0; i < 4; i++) {
+        if (group.chan[i] != CHAN_DISABLED) {
+            group.pwm_drv->tim->CCR[i] = 0;
+        }
+    }
     chVTResetI(&group.dma_timeout);
     chEvtGetAndClearEventsI(group.dshot_event_mask);
+
     group.dshot_state = DshotState::IDLE;
     chSysUnlock();
 }

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -150,6 +150,11 @@ public:
     void set_bidir_dshot_mask(uint16_t mask) override;
 #endif
 
+    /*
+      Set the dshot rate as a multiple of the loop rate
+     */
+    void set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) override;
+
     /*
       get safety switch state, used by Util.cpp
     */
@@ -414,6 +419,15 @@ private:
 #endif
     } _bdshot;
 
+    // dshot period
+    uint32_t _dshot_period_us;
+    // dshot rate as a multiple of loop rate or 0 for 1Khz
+    uint8_t _dshot_rate;
+    // dshot periods since the last push()
+    uint8_t _dshot_cycle;
+    // virtual timer for post-push() pulses
+    virtual_timer_t _dshot_rate_timer;
+
     uint16_t safe_pwm[max_channels]; // pwm to use when safety is on
     bool corked;
     // mask of channels that are running in high speed
@@ -482,8 +496,9 @@ private:
     uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem);
     void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
 
-    void dshot_send_groups();
-    void dshot_send(pwm_group &group);
+    void dshot_send_groups(uint32_t time_out_us);
+    void dshot_send(pwm_group &group, uint32_t time_out_us);
+    static void dshot_update_tick(void* p);
     // release locks on the groups that are pending in reverse order
     void dshot_collect_dma_locks(uint32_t last_run_us);
     static void dma_up_irq_callback(void *p, uint32_t flags);

libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp

@@ -63,6 +63,11 @@ void RCOutput::set_bidir_dshot_mask(uint16_t mask)
 
 bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
 {
+    // check if already allocated
+    if (group.has_ic_dma()) {
+        return true;
+    }
+
     bool set_curr_chan = false;
 
     for (uint8_t i = 0; i < 4; i++) {