aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: allow dshot to be used even if bdshot was specified.

Browse Source 
correct zero handling in bdshot decoding
This commit is contained in:
Andy Piper 2023-11-16 16:27:16 +00:00 committed by Andrew Tridgell
parent 43b6fc0dba
commit 2415c2998b
4 changed files with 28 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -16,6 +16,7 @@
* Bi-directional dshot based on Betaflight, code by Andy Piper and Siddharth Bharat Purohit
*
* There really is no dshot reference. For information try these resources:
* https://brushlesswhoop.com/dshot-and-bidirectional-dshot/
* https://blck.mn/2016/11/dshot-the-new-kid-on-the-block/
* https://www.swallenhardware.io/battlebots/2019/4/20/a-developers-guide-to-dshot-escs
*/
@ -981,10 +982,11 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
#ifdef HAL_WITH_BIDIR_DSHOT
// configure input capture DMA if required
if (is_bidir_dshot_enabled()) {
if (is_bidir_dshot_enabled(group)) {
if (!bdshot_setup_group_ic_DMA(group)) {
group.dma_handle->unlock();
return false;
_bdshot.mask &= ~group.ch_mask; // only use dshot on this group
_bdshot.disabled_mask |= group.ch_mask;
active_high = true;
}
}
#endif
@ -1115,7 +1117,7 @@ void RCOutput::set_group_mode(pwm_group &group)
case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: {
#if HAL_DSHOT_ENABLED
const uint32_t rate = protocol_bitrate(group.current_mode);
bool active_high = is_bidir_dshot_enabled() ? false : true;
bool active_high = is_bidir_dshot_enabled(group) ? false : true;
bool at_least_freq = false;
// calculate min time between pulses
const uint32_t pulse_send_time_us = 1000000UL * dshot_bit_length / rate;
@ -1131,7 +1133,7 @@ void RCOutput::set_group_mode(pwm_group &group)
group.current_mode = MODE_PWM_NORMAL;
break;
}
if (is_bidir_dshot_enabled()) {
if (is_bidir_dshot_enabled(group)) {
group.dshot_pulse_send_time_us = pulse_send_time_us;
// to all intents and purposes the pulse time of send and receive are the same
group.dshot_pulse_time_us = pulse_send_time_us + pulse_send_time_us + 30;

5
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -314,6 +314,8 @@ private:
// input capture is expecting TELEM_IC_SAMPLE (16) ticks per transition (22) so the maximum
// value of the counter in CCR registers is 16*22 == 352, so must be 16-bit
static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(dmar_uint_t);
static const uint16_t INVALID_ERPM = 0xffffU;
static const uint16_t ZERO_ERPM = 0x0fffU;
struct pwm_group {
// only advanced timers can do high clocks needed for more than 400Hz
@ -551,6 +553,7 @@ private:
// handling of bi-directional dshot
struct {
uint32_t mask;
uint32_t disabled_mask; // record of channels that were tried, but failed
uint16_t erpm[max_channels];
volatile uint32_t update_mask;
#ifdef HAL_WITH_BIDIR_DSHOT
@ -619,7 +622,7 @@ private:
volatile bool _initialised;
bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; }
bool is_bidir_dshot_enabled(const pwm_group& group) const { return (_bdshot.mask & group.ch_mask) != 0; }
static bool is_dshot_send_allowed(DshotState state) {
return state == DshotState::IDLE || state == DshotState::RECV_COMPLETE || state == DshotState::RECV_FAILED;

26
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View File

@ -58,11 +58,12 @@ void RCOutput::set_bidir_dshot_mask(uint32_t mask)
iomcu.set_bidir_dshot_mask(mask & iomcu_mask);
}
#endif
_bdshot.mask = (mask >> chan_offset);
const uint32_t local_mask = (mask >> chan_offset) & ~_bdshot.disabled_mask;
_bdshot.mask = local_mask;
// we now need to reconfigure the DMA channels since they are affected by the value of the mask
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
if (((group.ch_mask << chan_offset) & mask) == 0) {
if ((group.ch_mask & local_mask) == 0) {
// this group is not affected
continue;
}
@ -101,7 +102,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_deallocate, void, Shared_DMA *));
}
if (!group.bdshot.ic_dma_handle[i]) {
return false;
goto ic_dma_fail;
}
}
}
@ -149,11 +150,11 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
if (!group.dma_ch[curr_chan].have_dma) {
// We can't find a DMA channel to use so
// return error
return false;
goto ic_dma_fail;
}
if (group.bdshot.ic_dma_handle[i]) {
INTERNAL_ERROR(AP_InternalError::error_t::dma_fail);
return false;
goto ic_dma_fail;
}
// share up channel if required
if (group.dma_ch[curr_chan].stream_id == group.dma_up_stream_id) {
@ -165,7 +166,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_deallocate, void, Shared_DMA *));
}
if (!group.bdshot.ic_dma_handle[i]) {
return false;
goto ic_dma_fail;
}
group.bdshot.telem_tim_ch[i] = curr_chan;
group.dma_ch[i] = group.dma_ch[curr_chan];
@ -181,6 +182,15 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
}
return true;
ic_dma_fail:
for (uint8_t i = 0; i < 4; i++) {
if (group.bdshot.ic_dma_handle[i] != group.dma_handle) {
delete group.bdshot.ic_dma_handle[i];
}
group.bdshot.ic_dma_handle[i] = nullptr;
}
return false;
}
/*
@ -676,8 +686,6 @@ uint32_t RCOutput::bdshot_get_output_rate_hz(const enum output_mode mode)
}
}
#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
uint32_t RCOutput::bdshot_decode_telemetry_packet(dmar_uint_t* buffer, uint32_t count)
@ -801,7 +809,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
erpm = (1000000U * 60U / 100U + erpm / 2U) / erpm;
if (encodederpm == 0x0fff) { // the special 0 encoding
if (encodederpm == ZERO_ERPM) { // the special 0 encoding
erpm = 0;
}

2
libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp
View File

@ -353,8 +353,6 @@ void RCOutput::bdshot_config_icu_dshot_f1(stm32_tim_t* TIMx, uint8_t chan, uint8
}
}
#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
uint32_t RCOutput::bdshot_decode_telemetry_packet_f1(dmar_uint_t* buffer, uint32_t count, bool reversed)