Skip to content

Commit

Permalink
AP_HAL_ChibiOS: bdshot for f103 iofirmware
Browse files Browse the repository at this point in the history 
add support to tell if shared DMA channel is actually shared
avoid starting and stopping the timer peripheral with bdshot
ensure that rcout DMA allocation and deallocation happens entirely within the lock
increase rcout thread working area for bdshot
fix mode mask that is sent to the iomcu
ensure iomcu rcout thread gets timeouts for callbacks
control bdshot input and output line levels on f103
use input capture channel pairs to read rising and falling edges of telemetry on f103
reset channel pairs together on iomcu
generalize the bdshot input path to support suitable buffer sizes for iomcu
generalize DMAR reading of CCR registers to read two at a time on iomcu
enable bi-directional dshot channels on PWM1-4 on iomcu
add methods to directly access erpm values from rcout
update erpm mask and esc telemetry correctly for firmware supporting dshot
add support for propagating bdmask to iomcu
dshot commands to all channels need to be aware of iomcu
ensure esc type is propagated to iomcu
cope with iomcu channel numbering when using EDT
ensure pwm driver is reset properly for dshot commands on iomcu
correctly reset pwm for dshot commands
correctly mask off bdshot bits going to iomcu
don't reset GPIO modes on disabled lines
don't reset pwm_started when sharing DMA channels
set thread name on iomcu rcout and reduce stack size on iomcu
ensure that bdshot pulses with no response are handled correctly
correctly setup DMA for input capture on f103
deal with out of order captured bytes when decoding bdshot telemetry
ensure DMA sharing on f103 does not pull lines low
only disable the timer peripheral when switching DMA channels on iomcu
add support for waiting for _UP to finish before proceeding with dshot
re-order iomcu dshot channels to let TIM4_UP go first
ensure that a cascading event will always come when expected on rcout
allow timeouts when using cascading dshot
always rotate telemetry channel after trying to capture input
cater for both in order and out-of-order bdshot telemetry packets
cope with reversed packets when decoding bdshot telemetry
ensure UP DMA channel is fully free on iomcu before starting next dshot cycle
  • Loading branch information
@andyp1per
andyp1per committed Oct 17, 2023
1 parent abe436c commit 9081465
Show file tree
Hide file tree
Showing 9 changed files with 583 additions and 158 deletions.
231 changes: 127 additions & 104 deletions libraries/AP_HAL_ChibiOS/RCOutput.cpp
View file

Large diffs are not rendered by default.

35 changes: 31 additions & 4 deletions libraries/AP_HAL_ChibiOS/RCOutput.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,9 +26,16 @@
#if HAL_USE_PWM == TRUE

#if defined(IOMCU_FW)
#ifdef HAL_WITH_BIDIR_DSHOT
typedef uint16_t dmar_uint_t; // save memory to allow dshot on IOMCU
typedef int16_t dmar_int_t;
#else
typedef uint8_t dmar_uint_t; // save memory to allow dshot on IOMCU
typedef int8_t dmar_int_t;
#endif
#else
typedef uint32_t dmar_uint_t;
typedef int32_t dmar_int_t;
#endif

#define RCOU_DSHOT_TIMING_DEBUG 0
Expand Down Expand Up @@ -59,6 +66,11 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
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]);
}
/*
allow all erpm values to be read and for new updates to be detected - primarily for IOMCU
*/
bool new_erpm() override { return _bdshot.update_mask != 0; }
uint32_t read_erpm(uint16_t* erpm, uint8_t len) override;
#endif
void set_output_mode(uint32_t mask, const enum output_mode mode) override;
enum output_mode get_output_mode(uint32_t& mask) override;
Expand Down Expand Up @@ -277,7 +289,8 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
SEND_START = 1,
SEND_COMPLETE = 2,
RECV_START = 3,
RECV_COMPLETE = 4
RECV_COMPLETE = 4,
RECV_FAILED = 5
};

struct PACKED SerialLed {
Expand All @@ -297,7 +310,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
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);
// 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);

struct pwm_group {
// only advanced timers can do high clocks needed for more than 400Hz
Expand All @@ -315,6 +330,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
uint8_t stream_id;
uint8_t channel;
} dma_ch[4];
bool shared_up_dma; // do we need to wait for TIMx_UP DMA to be finished after use
#endif
uint8_t alt_functions[4];
ioline_t pal_lines[4];
Expand Down Expand Up @@ -380,8 +396,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
uint8_t telem_tim_ch[4];
uint8_t curr_telem_chan;
uint8_t prev_telem_chan;
Shared_DMA *curr_ic_dma_handle; // a shortcut to avoid logic errors involving the wrong lock
uint16_t telempsc;
uint32_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN];
dmar_uint_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN];
#if RCOU_DSHOT_TIMING_DEBUG
uint16_t telem_rate[4];
uint16_t telem_err_rate[4];
Expand Down Expand Up @@ -525,6 +542,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
struct {
uint32_t mask;
uint16_t erpm[max_channels];
volatile uint32_t update_mask;
#ifdef HAL_WITH_BIDIR_DSHOT
uint16_t erpm_errors[max_channels];
uint16_t erpm_clean_frames[max_channels];
Expand Down Expand Up @@ -593,6 +611,10 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput

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

static bool is_dshot_send_allowed(DshotState state) {
return state == DshotState::IDLE || state == DshotState::RECV_COMPLETE || state == DshotState::RECV_FAILED;
}

// are all the ESCs returning data
bool group_escs_active(const pwm_group& group) const {
return group.en_mask > 0 && (group.en_mask & _active_escs_mask) == group.en_mask;
Expand Down Expand Up @@ -644,12 +666,15 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem);
void fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);

// event to allow dshot cascading
static const eventmask_t DSHOT_CASCADE = EVENT_MASK(16);
void dshot_send_groups(uint64_t time_out_us);
void dshot_send(pwm_group &group, uint64_t time_out_us);
bool dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan);
static void dshot_update_tick(virtual_timer_t*, void* p);
static void dshot_send_next_group(void* p);
// release locks on the groups that are pending in reverse order
sysinterval_t calc_ticks_remaining(pwm_group &group, uint64_t time_out_us, uint64_t cycle_length_us);
void dshot_collect_dma_locks(uint64_t last_run_us, bool led_thread = false);
static void dma_up_irq_callback(void *p, uint32_t flags);
static void dma_unlock(virtual_timer_t*, void *p);
Expand All @@ -668,14 +693,16 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
*/
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 uint32_t bdshot_decode_telemetry_packet(dmar_uint_t* buffer, uint32_t count, bool reversed);
bool bdshot_decode_telemetry_from_erpm(uint16_t erpm, uint8_t chan);
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(virtual_timer_t* vt, void *p);
bool bdshot_setup_group_ic_DMA(pwm_group &group);
void bdshot_prepare_for_next_pulse(pwm_group& group);
static void bdshot_receive_pulses_DMAR(pwm_group* group);
static void bdshot_reset_pwm(pwm_group& group, uint8_t telem_channel);
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);

Expand Down
Loading

0 comments on commit 9081465

Please sign in to comment.