From f3f3056dba2c2f1db73c8f696fa8ae8e58db94ed Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 3 Apr 2021 23:00:38 +0100 Subject: [PATCH] AP_HAL_ChibiOS: add support for dshot commands to rcout add support for dshot beepcodes through tonealarm add support for dshot reversal and command queue add support for dshot commands to all channels correctly manage channel enablement in PWM groups Correctly send dshot commands when using bi-dir dshot allow reversible settings to be changed ChibiOS: allow more than one type of ESC for dshot commands Only execute reverse/reversible commands on BLHeli add support for checking active ESCS mark ESCs active when bdshot telemetry is returned allow dshot alarm to be disabled allow priroitized dshot commands --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 47 +++--- libraries/AP_HAL_ChibiOS/RCOutput.h | 93 +++++++++++- libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp | 27 ++-- libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp | 143 ++++++++++++++++++ libraries/AP_HAL_ChibiOS/Util.cpp | 34 ++++- libraries/AP_HAL_ChibiOS/Util.h | 5 +- .../AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat | 1 - 7 files changed, 305 insertions(+), 45 deletions(-) create mode 100644 libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index e0771330be..f3b9c4d785 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -40,12 +40,6 @@ extern AP_IOMCU iomcu; #define RCOU_SERIAL_TIMING_DEBUG 0 -#if RCOU_DSHOT_TIMING_DEBUG -#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0) -#else -#define TOGGLE_PIN_DEBUG(pin) do {} while (0) -#endif - #define TELEM_IC_SAMPLE 16 struct RCOutput::pwm_group RCOutput::pwm_group_list[] = { HAL_PWM_GROUPS }; @@ -58,9 +52,6 @@ static const eventmask_t EVT_PWM_START = EVENT_MASK(12); static const eventmask_t EVT_PWM_SYNTHETIC_SEND = EVENT_MASK(13); static const eventmask_t EVT_PWM_SEND_NEXT = EVENT_MASK(14); -// marker for a disabled channel -#define CHAN_DISABLED 255 - // #pragma GCC optimize("Og") /* @@ -104,7 +95,6 @@ void RCOutput::init() #endif chMtxObjectInit(&trigger_mutex); chVTObjectInit(&_dshot_rate_timer); - // setup default output rate of 50Hz set_freq(0xFFFF ^ ((1U<ch_mask |= 1U << chan; } } @@ -520,9 +511,18 @@ void RCOutput::disable_ch(uint8_t chan) if (grp) { pwmDisableChannel(grp->pwm_drv, i); en_mask &= ~(1U<<(chan - chan_offset)); + grp->ch_mask &= ~(1U << chan); } } +bool RCOutput::prepare_for_arming() +{ + // force all the ESCs to be active, in the future consider returning false + // if ESCs are not active that we require + _active_escs_mask = (en_mask << chan_offset); + return true; +} + void RCOutput::write(uint8_t chan, uint16_t period_us) { if (chan >= max_channels) { @@ -587,7 +587,7 @@ void RCOutput::push_local(void) } for (uint8_t j = 0; j < 4; j++) { uint8_t chan = group.chan[j]; - if (chan == CHAN_DISABLED) { + if (!group.is_chan_enabled(j)) { continue; } if (outmask & (1UL<get_soft_armed() + && is_dshot_protocol(group.current_mode) + && group_escs_active(group) // only send when someone is listening + && (dshot_command_is_active(group) + || (_dshot_command_queue.pop(_dshot_current_command) && dshot_command_is_active(group)))) { + dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan); + _dshot_current_command.cycle--; + // actually do a dshot send + } else if (group.can_send_dshot_pulse()) { dshot_send(group, time_out_us); } } +#endif //#ifndef DISABLE_DSHOT } void RCOutput::dshot_send_next_group(void* p) @@ -1333,6 +1345,7 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us) uint32_t now = AP_HAL::millis(); if (bdshot_decode_dshot_telemetry(group, group.bdshot.prev_telem_chan)) { _bdshot.erpm_clean_frames[chan]++; + _active_escs_mask |= (1<>chan_offset)) { + if (chan_mask & (_reversible_mask>>chan_offset)) { // this is a DShot-3D output, map so that 1500 PWM is zero throttle reversed if (value < 1000) { value = 2000 - value; diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 999f43ee7e..7440cb91f3 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -32,6 +32,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput { public: + // disabled channel marker + const static uint8_t CHAN_DISABLED = 255; + void init() override; void set_freq(uint32_t chmask, uint16_t freq_hz) override; uint16_t get_freq(uint8_t ch) override; @@ -138,14 +141,14 @@ public: /* enable telemetry request for a mask of channels. This is used - with DShot to get telemetry feedback + with Dshot to get telemetry feedback */ 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 + with Dshot to get telemetry feedback */ void set_bidir_dshot_mask(uint16_t mask) override; #endif @@ -155,6 +158,15 @@ public: */ void set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) override; +#ifndef DISABLE_DSHOT + /* + Set/get the dshot esc_type + */ + void set_dshot_esc_type(DshotEscType dshot_esc_type) override { _dshot_esc_type = dshot_esc_type; } + + DshotEscType get_dshot_esc_type() const override { return _dshot_esc_type; } +#endif + /* get safety switch state, used by Util.cpp */ @@ -170,14 +182,37 @@ public: */ void set_safety_mask(uint16_t mask) { safety_mask = mask; } +#ifndef DISABLE_DSHOT /* - * mark the channels in chanmask as reversible. This is needed for some ESC types (such as DShot) + * mark the channels in chanmask as reversible. This is needed for some ESC types (such as Dshot) * so that output scaling can be performed correctly. The chanmask passed is added (ORed) into * any existing mask. */ - void set_reversible_mask(uint16_t chanmask) override { - reversible_mask |= chanmask; - } + void set_reversible_mask(uint16_t chanmask) override; + + /* + * mark the channels in chanmask as reversed. The chanmask passed is added (ORed) into + * any existing mask. + */ + void set_reversed_mask(uint16_t chanmask) override; + + /* + mark escs as active for the purpose of sending dshot commands + */ + void set_active_escs_mask(uint16_t chanmask) override { _active_escs_mask |= chanmask; } + + /* + Send a dshot command, if command timout is 0 then 10 commands are sent + chan is the servo channel to send the command to + */ + void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) override; + +#endif + + /* + If not already done flush any dshot commands still pending + */ + bool prepare_for_arming() override; /* setup serial LED output for a given channel number, with @@ -342,6 +377,11 @@ private: bool can_send_dshot_pulse() const { return is_dshot_protocol(current_mode) && AP_HAL::micros() - last_dmar_send_us > (dshot_pulse_time_us + 50); } + + // return whether the group channel is both enabled in the group and for output + bool is_chan_enabled(uint8_t c) const { + return chan[c] != CHAN_DISABLED && (ch_mask & (1U << chan[c])); + } }; /* timer thread for use by dshot events @@ -409,6 +449,7 @@ private: // 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; @@ -431,12 +472,36 @@ private: // virtual timer for post-push() pulses virtual_timer_t _dshot_rate_timer; +#ifndef DISABLE_DSHOT + // dshot commands + // RingBuffer to store outgoing request. + struct DshotCommandPacket { + uint8_t command; + uint32_t cycle; + uint8_t chan; + }; + + ObjectBuffer _dshot_command_queue{8}; + DshotCommandPacket _dshot_current_command; + + DshotEscType _dshot_esc_type; + + bool dshot_command_is_active(const pwm_group& group) const { + return (_dshot_current_command.chan == RCOutput::ALL_CHANNELS || (group.ch_mask & (1UL << _dshot_current_command.chan))) + && _dshot_current_command.cycle > 0; + } +#endif uint16_t safe_pwm[max_channels]; // pwm to use when safety is on bool corked; // mask of channels that are running in high speed uint16_t fast_channel_mask; uint16_t io_fast_channel_mask; - uint16_t reversible_mask; + // mask of channels that are 3D capable + uint16_t _reversible_mask; + // mask of channels that should be reversed at startup + uint16_t _reversed_mask; + // mask of active ESCs + uint16_t _active_escs_mask; // min time to trigger next pulse to prevent overlap uint64_t min_pulse_trigger_us; @@ -457,6 +522,11 @@ private: bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; } + // are all the ESCs returning data + bool group_escs_active(const pwm_group& group) const { + return group.ch_mask > 0 && (group.ch_mask & _active_escs_mask) == group.ch_mask; + } + // find a channel group given a channel number struct pwm_group *find_chan(uint8_t chan, uint8_t &group_idx); @@ -501,6 +571,7 @@ private: void dshot_send_groups(uint32_t time_out_us); void dshot_send(pwm_group &group, uint32_t time_out_us); + void dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan); static void dshot_update_tick(void* p); static void dshot_send_next_group(void* p); // release locks on the groups that are pending in reverse order @@ -523,7 +594,7 @@ private: 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); + 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); @@ -553,4 +624,10 @@ private: }; +#if RCOU_DSHOT_TIMING_DEBUG +#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0) +#else +#define TOGGLE_PIN_DEBUG(pin) do {} while (0) +#endif + #endif // HAL_USE_PWM diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index 49a5ac9109..51c92ff46c 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp @@ -29,17 +29,12 @@ extern const AP_HAL::HAL& hal; #if RCOU_DSHOT_TIMING_DEBUG #define DEBUG_CHANNEL 1 #define TOGGLE_PIN_CH_DEBUG(pin, channel) do { if (channel == DEBUG_CHANNEL) palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0) -#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0) #else #define TOGGLE_PIN_CH_DEBUG(pin, channel) do {} while (0) -#define TOGGLE_PIN_DEBUG(pin) do {} while (0) #endif #define TELEM_IC_SAMPLE 16 -// marker for a disabled channel -#define CHAN_DISABLED 255 - /* * enable bi-directional telemetry request for a mask of channels. This is used * with DShot to get telemetry feedback @@ -68,7 +63,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) bool set_curr_chan = false; for (uint8_t i = 0; i < 4; i++) { - if (group.chan[i] == CHAN_DISABLED || + if (!group.is_chan_enabled(i) || !group.dma_ch[i].have_dma || !(_bdshot.mask & (1 << group.chan[i]))) { continue; } @@ -102,7 +97,7 @@ 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++) { - if (group.chan[i] == CHAN_DISABLED || !(_bdshot.mask & (1 << group.chan[i]))) { + if (!group.is_chan_enabled(i) || !(_bdshot.mask & (1 << group.chan[i]))) { continue; } uint8_t curr_chan = i; @@ -403,7 +398,7 @@ void RCOutput::bdshot_finish_dshot_gcr_transaction(void *p) */ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan) { - if (group.chan[chan] == CHAN_DISABLED) { + if (!group.is_chan_enabled(chan)) { return true; } @@ -450,7 +445,7 @@ uint8_t RCOutput::bdshot_find_next_ic_channel(const pwm_group& group) uint8_t chan = group.bdshot.curr_telem_chan; for (uint8_t i = 1; i < 4; i++) { const uint8_t next_chan = (chan + i) % 4; - if (group.chan[next_chan] != CHAN_DISABLED && + if (group.is_chan_enabled(next_chan) && group.bdshot.ic_dma_handle[next_chan] != nullptr) { return next_chan; } @@ -486,11 +481,15 @@ void RCOutput::dma_up_irq_callback(void *p, uint32_t flags) // sending is done, in 30us the ESC will send telemetry bdshot_receive_pulses_DMAR(group); } else { - // non-bidir case - // this prevents us ever having two dshot pulses too close together - // dshot mandates a minimum pulse separation of 40us, WS2812 mandates 50us so we - // pick the higher value - chVTSetI(&group->dma_timeout, chTimeUS2I(50), dma_unlock, p); + // non-bidir case, this prevents us ever having two dshot pulses too close together + if (is_dshot_protocol(group->current_mode)) { + // since we could be sending a dshot command, wait the full telemetry pulse width + // dshot mandates a minimum pulse separation of 40us + chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_send_time_us + 30U + 40U), dma_unlock, p); + } else { + // WS2812 mandates a minimum pulse separation of 50us + chVTSetI(&group->dma_timeout, chTimeUS2I(50U), dma_unlock, p); + } } chSysUnlockFromISR(); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp new file mode 100644 index 0000000000..c502f7ad22 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp @@ -0,0 +1,143 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "RCOutput.h" +#include +#include "hwdef/common/stm32_util.h" +#include +#include + +#if HAL_USE_PWM == TRUE +#ifndef DISABLE_DSHOT + +using namespace ChibiOS; + +extern const AP_HAL::HAL& hal; + +void RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t chan) +{ + if (!group.can_send_dshot_pulse()) { + return; + } + + if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) { + // doing serial output or DMAR input, don't send DShot pulses + return; + } + + TOGGLE_PIN_DEBUG(81); + // first make sure we have the DMA channel before anything else + + osalDbgAssert(!group.dma_handle->is_locked(), "DMA handle is already locked"); + group.dma_handle->lock(); + + // only the timer thread releases the locks + group.dshot_waiter = rcout_thread_ctx; + bool bdshot_telem = false; +#ifdef HAL_WITH_BIDIR_DSHOT + // no need to get the input capture lock + group.bdshot.enabled = false; + if ((_bdshot.mask & group.ch_mask) == group.ch_mask) { + 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); + } + pwmStart(group.pwm_drv, &group.pwm_cfg); + group.pwm_started = true; + } +#endif + + memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH); + + // keep the other ESCs armed rather than sending nothing + const uint16_t zero_packet = create_dshot_packet(0, false, bdshot_telem); + 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))) { + fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet, group.bit_width_mul); + } else if (group.is_chan_enabled(i)) { + fill_DMA_buffer_dshot(group.dma_buffer + i, 4, zero_packet, group.bit_width_mul); + } + } + + chEvtGetAndClearEvents(group.dshot_event_mask); + // start sending the pulses out + send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH); + TOGGLE_PIN_DEBUG(81); +} + +// Send a dshot command, if command timout is 0 then 10 commands are sent +// chan is the servo channel to send the command to +void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority) +{ + // don't accept any LED or BEEP commands while initializing + if ((_dshot_period_us == 0 || _dshot_calibrating) && !priority) { + return; + } + + DshotCommandPacket pkt; + pkt.command = command; + pkt.chan = chan; + if (_dshot_period_us == 0 || _dshot_calibrating || command_timeout_ms == 0) { + pkt.cycle = MAX(10, repeat_count); + } else { + pkt.cycle = MAX(command_timeout_ms * 1000 / _dshot_period_us, repeat_count); + } + + // prioritize anything that is not an LED or BEEP command + if (!_dshot_command_queue.push(pkt) && priority) { + _dshot_command_queue.push_force(pkt); + } +} + +// Set the dshot outputs that should be reversed (as opposed to 3D) +// The chanmask passed is added (ORed) into any existing mask. +void RCOutput::set_reversed_mask(uint16_t chanmask) { + _reversed_mask |= chanmask; + + for (uint8_t i=0; iget_dshot_esc_type() != RCOutput::DSHOT_ESC_BLHELI)) { + return; + } + + if (is_zero(frequency)) { // silence + hal.rcout->send_dshot_command(RCOutput::DSHOT_RESET, RCOutput::ALL_CHANNELS, duration_ms); + } else if (frequency < 1047) { // C + hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP1, RCOutput::ALL_CHANNELS, duration_ms); + } else if (frequency < 1175) { // D + hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP2, RCOutput::ALL_CHANNELS, duration_ms); + } else if (frequency < 1319) { // E + hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP3, RCOutput::ALL_CHANNELS, duration_ms); + } else if (frequency < 1397) { // F + hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP4, RCOutput::ALL_CHANNELS, duration_ms); + } else { // G+ + hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP5, RCOutput::ALL_CHANNELS, duration_ms); + } #endif // HAL_PWM_ALARM +} +#endif /* set HW RTC in UTC microseconds diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 51729d96ab..8d403d7abb 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -59,9 +59,10 @@ public: bool get_system_id(char buf[40]) override; bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override; -#ifdef HAL_PWM_ALARM - bool toneAlarm_init() override; +#if defined(HAL_PWM_ALARM) || HAL_DSHOT_ALARM + bool toneAlarm_init(uint8_t types) override; void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override; + static uint8_t _toneAlarm_types; #endif // return true if the reason for the reboot was a watchdog reset diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat index 42f49640f7..66c279077f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat @@ -215,4 +215,3 @@ define HAL_OSD_TYPE_DEFAULT 1 ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin define BOARD_PWM_COUNT_DEFAULT 13 -