Skip to content

Commit

Permalink
AP_HAL_ChibiOS: add support for bidir DShot support in RCOutput
Browse files Browse the repository at this point in the history 
add support for sampling GPIO pins using timer
don't restart pwm group when not doing bi-dir
fix hwdef generation preproc for TIM DMA
decode telemetry at the start of the dshot cycle
calculate dshot pulse separation correctly and ensure we output rapidly enough
calculate dshot min periods and timeouts correctly
refactor dshot_send() into dshot_send_groups()
use bi-dir dshot channel mask
selectively enable bi-dir RC Channels
process bi-dir mask correctly when allocating DMA channels
allow UP and CH DMA channel sharing
optionally enable bidir vars in hwdef.

enable bi-dir dshot in KakuteF7Mini
enable bi-dir dshot in OmnibusF4Pro
enable bi-dir dshot in OmnibusNanoV6
enable bi-dir dshot in MatekF405
enable bi-dir dshot in fmuv5
enable bi-dir dshot in fmuv3
enable bi-dir dshot in OmnibusF7V2
enable bi-dir dshot in OmnibusNanoV6
enable bi-dir dshot in CubeOrange
enable bi-dir dshot in Pixracer
enable bi-dir dshot in mRoPixracerPro

Co-authored-by: bugobliterator <[email protected]>
  • Loading branch information
@andyp1per @bugobliterator @tridge
2 people authored and tridge committed Dec 30, 2020
1 parent b5688c0 commit 401e5c2
Show file tree
Hide file tree
Showing 18 changed files with 3,289 additions and 99 deletions.
210 changes: 156 additions & 54 deletions libraries/AP_HAL_ChibiOS/RCOutput.cpp
View file

Large diffs are not rendered by default.

148 changes: 134 additions & 14 deletions libraries/AP_HAL_ChibiOS/RCOutput.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
#define DISABLE_DSHOT
#endif

#define RCOU_DSHOT_TIMING_DEBUG 0

class ChibiOS::RCOutput : public AP_HAL::RCOutput {
public:
void init() override;
Expand All @@ -48,6 +50,13 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
max_pwm = _esc_pwm_max;
return true;
}
// surface dshot telemetry for use by the harmonic notch and status information
#ifdef HAL_WITH_BIDIR_DSHOT
uint16_t get_erpm(uint8_t chan) const override { return _bdshot.erpm[chan]; }
float get_erpm_error_rate(uint8_t chan) const override {
return 100.0f * float(_bdshot.erpm_errors[chan]) / (1 + _bdshot.erpm_errors[chan] + _bdshot.erpm_clean_frames[chan]);
}
#endif
void set_output_mode(uint16_t mask, const enum output_mode mode) override;
bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const override;

Expand Down Expand Up @@ -132,6 +141,14 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
*/
void set_telem_request_mask(uint16_t mask) override { telem_request_mask = (mask >> chan_offset); }

#ifdef HAL_WITH_BIDIR_DSHOT
/*
enable bi-directional telemetry request for a mask of channels. This is used
with DShot to get telemetry feedback
*/
void set_bidir_dshot_mask(uint16_t mask) override;
#endif

/*
get safety switch state, used by Util.cpp
*/
Expand Down Expand Up @@ -174,6 +191,27 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
void serial_led_send(const uint16_t chan) override;

private:
enum class DshotState {
IDLE = 0,
SEND_START = 1,
SEND_COMPLETE = 2,
RECV_START = 3,
RECV_COMPLETE = 4
};

/*
DShot handling
*/
// the pre-bit is needed with TIM5, or we can get some corrupt frames
static const uint8_t dshot_pre = 1;
static const uint8_t dshot_post = 2;
static const uint16_t dshot_bit_length = 16 + dshot_pre + dshot_post;
static const uint16_t DSHOT_BUFFER_LENGTH = dshot_bit_length * 4 * sizeof(uint32_t);
static const uint16_t MIN_GCR_BIT_LEN = 7;
static const uint16_t MAX_GCR_BIT_LEN = 22;
static const uint16_t GCR_TELEMETRY_BIT_LEN = MAX_GCR_BIT_LEN;
static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(uint32_t);

struct pwm_group {
// only advanced timers can do high clocks needed for more than 400Hz
bool advanced_timer;
Expand All @@ -183,6 +221,13 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
bool have_up_dma; // can we do DMAR outputs for DShot?
uint8_t dma_up_stream_id;
uint8_t dma_up_channel;
#ifdef HAL_WITH_BIDIR_DSHOT
struct {
bool have_dma;
uint8_t stream_id;
uint8_t channel;
} dma_ch[4];
#endif
uint8_t alt_functions[4];
ioline_t pal_lines[4];

Expand All @@ -200,6 +245,8 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
uint32_t rc_frequency;
bool in_serial_dma;
uint64_t last_dmar_send_us;
uint32_t dshot_pulse_time_us;
uint32_t dshot_pulse_send_time_us;
virtual_timer_t dma_timeout;
uint8_t serial_nleds;
uint8_t clock_mask;
Expand All @@ -217,6 +264,58 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
// thread waiting for byte to be written
thread_t *waiter;
} serial;

// support for bi-directional dshot
volatile DshotState dshot_state;

struct {
uint16_t erpm[4];
volatile bool enabled;
#ifdef HAL_WITH_BIDIR_DSHOT
const stm32_dma_stream_t *ic_dma[4];
uint16_t dma_tx_size; // save tx value from last read
Shared_DMA *ic_dma_handle[4];
uint8_t telem_tim_ch[4];
uint8_t curr_telem_chan;
uint8_t prev_telem_chan;
uint16_t telempsc;
uint32_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN];
#if RCOU_DSHOT_TIMING_DEBUG
uint16_t telem_rate[4];
uint16_t telem_err_rate[4];
uint64_t last_print; // debug
#endif
#endif
} bdshot;

#ifdef HAL_WITH_BIDIR_DSHOT
// do we have an input capture dma channel
bool has_ic_dma() const {
return bdshot.ic_dma_handle[bdshot.curr_telem_chan] != nullptr;
}

bool has_shared_ic_up_dma() const {
return bdshot.ic_dma_handle[bdshot.curr_telem_chan] == dma_handle;
}

// is input capture currently enabled
bool ic_dma_enabled() const {
return bdshot.enabled && has_ic_dma() && bdshot.ic_dma[bdshot.curr_telem_chan] != nullptr;
}

bool has_ic() const {
return has_ic_dma() || has_shared_ic_up_dma();
}

// do we have any kind of input capture
bool ic_enabled() const {
return bdshot.enabled && has_ic();
}
#endif
// are we safe to send another pulse?
bool can_send_dshot_pulse() const {
return is_dshot_protocol(current_mode) && AP_HAL::micros64() - last_dmar_send_us > (dshot_pulse_time_us + 50);
}
};

/*
Expand Down Expand Up @@ -259,6 +358,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
tprio_t serial_priority;

static pwm_group pwm_group_list[];
static const uint8_t NUM_GROUPS;
uint16_t _esc_pwm_min;
uint16_t _esc_pwm_max;

Expand All @@ -279,6 +379,17 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
// these values are for the local channels. Non-local channels are handled by IOMCU
uint32_t en_mask;
uint16_t period[max_channels];
// handling of bi-directional dshot
struct {
uint16_t mask;
uint16_t erpm[max_channels];
#ifdef HAL_WITH_BIDIR_DSHOT
uint16_t erpm_errors[max_channels];
uint16_t erpm_clean_frames[max_channels];
uint32_t erpm_last_stats_ms[max_channels];
#endif
} _bdshot;

uint16_t safe_pwm[max_channels]; // pwm to use when safety is on
bool corked;
// mask of channels that are running in high speed
Expand All @@ -301,6 +412,8 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
// iomcu output mode (pwm, oneshot or oneshot125)
enum output_mode iomcu_mode = MODE_PWM_NORMAL;

bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; }

// find a channel group given a channel number
struct pwm_group *find_chan(uint8_t chan, uint8_t &group_idx);

Expand All @@ -326,16 +439,6 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {
// update safety switch and LED
void safety_update(void);

/*
DShot handling
*/
// the pre-bit is needed with TIM5, or we can get some corrupt frames
const uint8_t dshot_pre = 1;
const uint8_t dshot_post = 2;
const uint16_t dshot_bit_length = 16 + dshot_pre + dshot_post;
const uint16_t dshot_buffer_length = dshot_bit_length*4*sizeof(uint32_t);
static const uint16_t dshot_min_gap_us = 100;
uint32_t dshot_pulse_time_us;
uint16_t telem_request_mask;

/*
Expand All @@ -347,17 +450,34 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput {

void dma_allocate(Shared_DMA *ctx);
void dma_deallocate(Shared_DMA *ctx);
uint16_t create_dshot_packet(const uint16_t value, bool telem_request);
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(bool blocking);
void dshot_send(pwm_group &group, bool blocking);
static void dma_irq_callback(void *p, uint32_t flags);
static void dma_up_irq_callback(void *p, uint32_t flags);
static void dma_unlock(void *p);
bool mode_requires_dma(enum output_mode mode) const;
bool setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, const uint16_t buffer_length, bool choose_high);
void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);
void set_group_mode(pwm_group &group);
bool is_dshot_protocol(const enum output_mode mode) const;
uint32_t protocol_bitrate(const enum output_mode mode) const;
static bool is_dshot_protocol(const enum output_mode mode);
static uint32_t protocol_bitrate(const enum output_mode mode);

/*
Support for bi-direction dshot
*/
void bdshot_ic_dma_allocate(Shared_DMA *ctx);
void bdshot_ic_dma_deallocate(Shared_DMA *ctx);
static uint32_t bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count);
static bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan);
static uint8_t bdshot_find_next_ic_channel(const pwm_group& group);
static void bdshot_dma_ic_irq_callback(void *p, uint32_t flags);
static void bdshot_finish_dshot_gcr_transaction(void *p);
bool bdshot_setup_group_ic_DMA(pwm_group &group);
static void bdshot_receive_pulses_DMAR(pwm_group* group);
static void bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch);
static uint32_t bdshot_get_output_rate_hz(const enum output_mode mode);

/*
setup neopixel (WS2812B) output data for a given output channel
Expand Down
Loading

0 comments on commit 401e5c2

Please sign in to comment.