AP_HAL_ChibiOS: ensure pwm driver is reset properly for dshot command…

…s on iomcu
ArduPilot · Oct 8, 2023 · ac08abf · ac08abf
1 parent ebfd5d1
commit ac08abf
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 16 deletions.
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
@@ -244,29 +244,24 @@ void RCOutput::bdshot_reset_pwm(pwm_group& group, uint8_t telem_channel)
     // at the point this is called we will have done input capture on two CC channels
     // we need to switch those channels back to output and the default settings
     // all other channels will not have been modified
-    switch (group.bdshot.telem_tim_ch[telem_channel]) {
-    case 0: // CC1
-    case 1: // CC2
+    uint8_t timer_channel = telem_channel<4 ? group.bdshot.telem_tim_ch[telem_channel] : RCOutput::ALL_CHANNELS;
+    if (timer_channel == 0 || timer_channel == 1 || timer_channel == RCOutput::ALL_CHANNELS) { // CC1 & CC2
         MODIFY_REG(TIMx->CCER, TIM_CCER_CC2E | TIM_CCER_CC1E, 0);   // disable CC so that it can be modified
         MODIFY_REG(TIMx->CCMR1, (TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC),
             STM32_TIM_CCMR1_OC1M(6) | STM32_TIM_CCMR1_OC1PE);
         MODIFY_REG(TIMx->CCMR1, (TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC),
             STM32_TIM_CCMR1_OC2M(6) | STM32_TIM_CCMR1_OC2PE);
         MODIFY_REG(TIMx->CCER, (TIM_CCER_CC1P | TIM_CCER_CC2P),
             (TIM_CCER_CC1P | TIM_CCER_CC2P | TIM_CCER_CC1E | TIM_CCER_CC2E));
-        break;
-    case 2: // CC3
-    case 3: // CC4
+    }
+    if (timer_channel == 2 || timer_channel == 3 || timer_channel == RCOutput::ALL_CHANNELS) { // CC3 & CC4
         MODIFY_REG(TIMx->CCER, TIM_CCER_CC3E | TIM_CCER_CC4E, 0);   // disable CC so that it can be modified
         MODIFY_REG(TIMx->CCMR2, (TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC),
             STM32_TIM_CCMR2_OC3M(6) | STM32_TIM_CCMR2_OC3PE);
         MODIFY_REG(TIMx->CCMR2, (TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC),
             STM32_TIM_CCMR2_OC4M(6) | STM32_TIM_CCMR2_OC4PE);
         MODIFY_REG(TIMx->CCER, (TIM_CCER_CC3P | TIM_CCER_CC4P),
             (TIM_CCER_CC3P | TIM_CCER_CC4P | TIM_CCER_CC3E | TIM_CCER_CC4E));
-        break;
-    default:
-        break;
     }
 
     uint32_t psc = (group.pwm_drv->clock / group.pwm_drv->config->frequency) - 1;
@@ -743,7 +738,7 @@ uint32_t RCOutput::bdshot_get_output_rate_hz(const enum output_mode mode)
     }
 }
 
-#define INVALID_ERPM 0xffffU
+#define INVALID_ERPM 0xfffU
 
 // decode a telemetry packet from a GCR encoded stride buffer, take from betaflight decodeTelemetryPacket
 // see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 for a description of the protocol
@@ -804,8 +799,8 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
     }
 
     // eRPM = m << e (see https://github.com/bird-sanctuary/extended-dshot-telemetry)
-    uint8_t expo = ((encodederpm & 0xfffffe00U) >> 9U) & 0xffU;
-    uint16_t value = (encodederpm & 0x000001ffU);
+    uint8_t expo = ((encodederpm & 0xfffffe00U) >> 9U) & 0xffU; // 3bits
+    uint16_t value = (encodederpm & 0x000001ffU);               // 9bits
     uint8_t normalized_chan = chan;
 #if HAL_WITH_IO_MCU
     if (AP_BoardConfig::io_dshot()) {

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
@@ -62,12 +62,13 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
     group.bdshot.enabled = false;
     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) {
-            pwmStop(group.pwm_drv);
+            bdshot_reset_pwm(group, RCOutput::ALL_CHANNELS);
+        }
+        else {
+            pwmStart(group.pwm_drv, &group.pwm_cfg);
+            group.pwm_started = true;
         }
-        pwmStart(group.pwm_drv, &group.pwm_cfg);
-        group.pwm_started = true;
     }
 #endif