Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: fix ESCs constantly arming on rover with dshot commands 

make sure debug will compile
take into account active channels when configuring bdshot
add channel mask debug output
correct set bdshot telemetry position at startup
make sure all channels in a bdshot group are pulled high to prevent spurious pulses
This commit is contained in:
Andy Piper 2022-03-25 08:09:20 +00:00 committed by Randy Mackay
parent 07837b22cf
commit 15ec9d5ab4
3 changed files with 50 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -197,6 +197,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
}
}
@ -1328,8 +1338,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;
@ -1388,18 +1399,19 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
#endif
_bdshot.erpm[chan] = erpm;
#endif
if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
// safety is on, don't output anything
continue;
}
uint16_t pwm = period[chan];
if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
// safety is on, overwride pwm
pwm = 0;
if (pwm == 0) {
// no pwm, don't output anything
continue;
}
const uint16_t chan_mask = (1U<<chan);
if (pwm == 0) {
// no output
continue;
}
pwm = constrain_int16(pwm, 1000, 2000);
uint16_t value = MIN(2 * (pwm - 1000), 1999);

29
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View File

@ -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;

15
libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
View File

@ -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,9 +71,13 @@ 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);
}
}
@ -78,7 +85,9 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
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;
}