From 382772d99920142df008052a9dd8247e15682116 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Mar 2018 08:49:40 +1100 Subject: [PATCH] HAL_ChibiOS: initial support for ESC serial output --- libraries/AP_HAL_ChibiOS/GPIO.cpp | 119 +- libraries/AP_HAL_ChibiOS/GPIO.h | 10 +- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 644 +++++++-- libraries/AP_HAL_ChibiOS/RCOutput.h | 112 +- .../AP_HAL_ChibiOS/examples/DShot/DShot.cpp | 1166 +++++++++++++++-- .../hwdef/scripts/chibios_hwdef.py | 27 +- .../hwdef/skyviper-f412-rev1/hwdef.dat | 7 +- .../hwdef/skyviper-f412/hwdef.dat | 7 +- .../hwdef/skyviper-v2450/hwdef.dat | 6 +- 9 files changed, 1822 insertions(+), 276 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index 14bc41a2f7..60167c0fde 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -27,6 +27,7 @@ static struct gpio_entry { bool enabled; uint8_t pwm_num; ioline_t pal_line; + uint16_t port; } _gpio_tab[] = HAL_GPIO_PINS; #define NUM_PINS ARRAY_SIZE_SIMPLE(_gpio_tab) @@ -50,60 +51,14 @@ static struct gpio_entry *gpio_by_pin_num(uint8_t pin_num) static void ext_interrupt_cb(EXTDriver *extp, expchannel_t channel); -static AP_HAL::Proc ext_irq[22]; // ext int irq list -static EXTConfig extcfg = { - { - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL}, - {EXT_CH_MODE_DISABLED, NULL} - } -}; - -static const uint32_t irq_port_list[] = { - HAL_GPIO_INTERRUPT_PORT, //Chan 0 - HAL_GPIO_INTERRUPT_PORT, //Chan 1 - HAL_GPIO_INTERRUPT_PORT, //Chan 2 - HAL_GPIO_INTERRUPT_PORT, //Chan 3 - HAL_GPIO_INTERRUPT_PORT, //Chan 4 - HAL_GPIO_INTERRUPT_PORT, //Chan 5 - HAL_GPIO_INTERRUPT_PORT, //Chan 6 - HAL_GPIO_INTERRUPT_PORT, //Chan 7 - HAL_GPIO_INTERRUPT_PORT, //Chan 8 - HAL_GPIO_INTERRUPT_PORT, //Chan 9 - HAL_GPIO_INTERRUPT_PORT, //Chan 10 - HAL_GPIO_INTERRUPT_PORT, //Chan 11 - HAL_GPIO_INTERRUPT_PORT, //Chan 12 - HAL_GPIO_INTERRUPT_PORT, //Chan 13 - HAL_GPIO_INTERRUPT_PORT, //Chan 14 - HAL_GPIO_INTERRUPT_PORT //Chan 15 -}; +static EXTConfig extcfg; +static AP_HAL::Proc ext_irq[EXT_MAX_CHANNELS]; // ext int irq list GPIO::GPIO() {} void GPIO::init() { - extStart(&EXTD1, &extcfg); // auto-disable pins being used for PWM output based on BRD_PWM_COUNT parameter uint8_t pwm_count = AP_BoardConfig::get_pwm_count(); for (uint8_t i=0; ipal_line); + if (p && ext_irq[pad] != nullptr && ext_irq[pad] != p) { + // already used + hal.console->printf("A fail\n"); + return false; + } else if (!p && !ext_irq[pad]) { + // nothing to remove + hal.console->printf("D fail\n"); + return false; + } + uint32_t chmode = 0; switch(mode) { case HAL_GPIO_INTERRUPT_LOW: - extcfg.channels[interrupt_num].mode = EXT_CH_MODE_LOW_LEVEL; + chmode = EXT_CH_MODE_LOW_LEVEL; break; case HAL_GPIO_INTERRUPT_FALLING: - extcfg.channels[interrupt_num].mode = EXT_CH_MODE_FALLING_EDGE; + chmode = EXT_CH_MODE_FALLING_EDGE; break; case HAL_GPIO_INTERRUPT_RISING: - extcfg.channels[interrupt_num].mode = EXT_CH_MODE_RISING_EDGE; + chmode = EXT_CH_MODE_RISING_EDGE; break; case HAL_GPIO_INTERRUPT_BOTH: - extcfg.channels[interrupt_num].mode = EXT_CH_MODE_BOTH_EDGES; + chmode = EXT_CH_MODE_BOTH_EDGES; + break; + default: + if (p) { + return false; + } break; - default: return false; } - extcfg.channels[interrupt_num].mode |= EXT_CH_MODE_AUTOSTART | irq_port_list[interrupt_num]; - ext_irq[interrupt_num] = p; - extcfg.channels[interrupt_num].cb = ext_interrupt_cb; + if (_ext_started) { + extStop(&EXTD1); + _ext_started = false; + } + extcfg.channels[pad].mode = chmode; + extcfg.channels[pad].mode |= (p?EXT_CH_MODE_AUTOSTART:0) | g->port; + ext_irq[pad] = p; + extcfg.channels[pad].cb = ext_interrupt_cb; extStart(&EXTD1, &extcfg); + _ext_started = true; return true; } @@ -211,9 +196,23 @@ void DigitalSource::toggle() { _v = !_v; } -void ext_interrupt_cb(EXTDriver *extp, expchannel_t channel) { +void ext_interrupt_cb(EXTDriver *extp, expchannel_t channel) +{ if (ext_irq[channel] != nullptr) { ext_irq[channel](); } } + +/* + set the output mode for a pin. Used to restore an alternate function + after using a pin as GPIO. Private to HAL_ChibiOS +*/ +void GPIO::_set_mode(uint8_t pin, uint32_t mode) +{ + struct gpio_entry *g = gpio_by_pin_num(pin); + if (g) { + palSetLineMode(g->pal_line, mode); + } +} + #endif //HAL_BOARD_ChibiOS diff --git a/libraries/AP_HAL_ChibiOS/GPIO.h b/libraries/AP_HAL_ChibiOS/GPIO.h index 919d4fc2fe..8cdd047e92 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.h +++ b/libraries/AP_HAL_ChibiOS/GPIO.h @@ -47,8 +47,16 @@ public: bool usb_connected(void) override; void set_usb_connected() { _usb_connected = true; } + + /* + set the output mode for a pin. Used to restore an alternate function + after using a pin as GPIO. Private to HAL_ChibiOS + */ + void _set_mode(uint8_t pin, uint32_t mode); + private: - bool _usb_connected = false; + bool _usb_connected; + bool _ext_started; }; class ChibiOS::DigitalSource : public AP_HAL::DigitalSource { diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 1a05cc3f26..c85397f7b1 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -17,6 +17,8 @@ #include "RCOutput.h" #include #include +#include +#include "GPIO.h" #if HAL_USE_PWM == TRUE @@ -30,11 +32,18 @@ extern AP_IOMCU iomcu; #endif struct RCOutput::pwm_group RCOutput::pwm_group_list[] = { HAL_PWM_GROUPS }; +struct RCOutput::irq_state RCOutput::irq; #define NUM_GROUPS ARRAY_SIZE_SIMPLE(pwm_group_list) +// marker for a disabled channel #define CHAN_DISABLED 255 +// #pragma GCC optimize("Og") + +/* + initialise RC output driver + */ void RCOutput::init() { for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { @@ -50,20 +59,67 @@ void RCOutput::init() } if (group.ch_mask != 0) { pwmStart(group.pwm_drv, &group.pwm_cfg); + group.pwm_started = true; } } #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { iomcu.init(); - // with IOMCU the local channels start at 8 + // with IOMCU the local (FMU) channels start at 8 chan_offset = 8; total_channels += chan_offset; } #endif chMtxObjectInit(&trigger_mutex); + + // setup default output rate of 50Hz + set_freq(0xFFFF, 50); } +/* + setup the output frequency for a group and start pwm output + */ +void RCOutput::set_freq_group(pwm_group &group) +{ + uint16_t freq_set = group.rc_frequency; + uint32_t old_clock = group.pwm_cfg.frequency; + + if (freq_set > 400) { + // use a 8MHz clock + group.pwm_cfg.frequency = 8000000; + } else if (freq_set <= 400) { + // use a 1MHz clock + group.pwm_cfg.frequency = 1000000; + } + + // check if the frequency is possible, and keep halving + // down to 1MHz until it is OK with the hardware timer we + // are using. If we don't do this we'll hit an assert in + // the ChibiOS PWM driver on some timers + PWMDriver *pwmp = group.pwm_drv; + uint32_t psc = (pwmp->clock / pwmp->config->frequency) - 1; + while ((psc > 0xFFFF || ((psc + 1) * pwmp->config->frequency) != pwmp->clock) && + group.pwm_cfg.frequency > 1000000) { + group.pwm_cfg.frequency /= 2; + psc = (pwmp->clock / pwmp->config->frequency) - 1; + } + + if (old_clock != group.pwm_cfg.frequency || !group.pwm_started) { + // we need to stop and start to setup the new clock + if (group.pwm_started) { + pwmStop(group.pwm_drv); + } + pwmStart(group.pwm_drv, &group.pwm_cfg); + group.pwm_started = true; + } + pwmChangePeriod(group.pwm_drv, + group.pwm_cfg.frequency/freq_set); +} + +/* + set output frequency in HZ for a set of channels given by a mask + */ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) { //check if the request spans accross any of the channel groups @@ -71,15 +127,24 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { + // change frequency on IOMCU iomcu.set_freq(chmask, freq_hz); } #endif + // convert to a local (FMU) channel mask chmask >>= chan_offset; if (chmask == 0) { return; } + /* + we enable the new frequency on all groups that have one + of the requested channels. This means we may enable high + speed on some channels that aren't requested, but that + is needed in order to fly a vehicle such a a hex + multicopter properly + */ for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { // greater than 400 doesn't give enough room at higher periods for // the down pulse. This still allows for high rate with oneshot and dshot. @@ -89,44 +154,9 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) group_freq = 400; } if ((group.ch_mask & chmask) != 0) { - /* - we enable the new frequency on all groups that have one - of the requested channels. This means we may enable high - speed on some channels that aren't requested, but that - is needed in order to fly a vehicle such a a hex - multicopter properly - */ + group.rc_frequency = group_freq; + set_freq_group(group); update_mask |= group.ch_mask; - uint16_t freq_set = group_freq; - uint32_t old_clock = group.pwm_cfg.frequency; - - if (freq_set > 400 && group.pwm_cfg.frequency == 1000000) { - // need to change to an 8MHz clock - group.pwm_cfg.frequency = 8000000; - } else if (freq_set <= 400 && group.pwm_cfg.frequency == 8000000) { - // need to change to an 1MHz clock - group.pwm_cfg.frequency = 1000000; - } - - // check if the frequency is possible, and keep halving - // down to 1MHz until it is OK with the hardware timer we - // are using. If we don't do this we'll hit an assert in - // the ChibiOS PWM driver on some timers - PWMDriver *pwmp = group.pwm_drv; - uint32_t psc = (pwmp->clock / pwmp->config->frequency) - 1; - while ((psc > 0xFFFF || ((psc + 1) * pwmp->config->frequency) != pwmp->clock) && - group.pwm_cfg.frequency > 1000000) { - group.pwm_cfg.frequency /= 2; - psc = (pwmp->clock / pwmp->config->frequency) - 1; - } - - if (old_clock != group.pwm_cfg.frequency) { - // we need to stop and start to setup the new clock - pwmStop(group.pwm_drv); - pwmStart(group.pwm_drv, &group.pwm_cfg); - } - pwmChangePeriod(group.pwm_drv, - group.pwm_cfg.frequency/freq_set); } if (group_freq > 50) { fast_channel_mask |= group.ch_mask; @@ -264,6 +294,9 @@ void RCOutput::push_local(void) for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; + if (!group.pwm_started) { + continue; + } for (uint8_t j = 0; j < 4; j++) { uint8_t chan = group.chan[j]; if (chan == CHAN_DISABLED) { @@ -282,7 +315,7 @@ void RCOutput::push_local(void) } pwmEnableChannel(group.pwm_drv, j, period_us); } else if (group.current_mode < MODE_PWM_DSHOT150) { - uint32_t width = (group.pwm_cfg.frequency/1000000) * period_us; + uint32_t width = (group.pwm_cfg.frequency/1000000U) * period_us; pwmEnableChannel(group.pwm_drv, j, width); } else if (group.current_mode >= MODE_PWM_DSHOT150 && group.current_mode <= MODE_PWM_DSHOT1200) { // set period_us to time for pulse output, to enable very fast rates @@ -363,6 +396,158 @@ void RCOutput::read_last_sent(uint16_t* period_us, uint8_t len) } } +/* + does an output mode require the use of the UP DMA channel? + */ +bool RCOutput::mode_requires_dma(enum output_mode mode) const +{ + switch (mode) { + case MODE_PWM_DSHOT150: + case MODE_PWM_DSHOT300: + case MODE_PWM_DSHOT600: + case MODE_PWM_DSHOT1200: + return true; + default: + break; + } + return false; +} + +/* + setup a group for DMA output at a given bitrate. The bit_width is + the value for a pulse width in the DMA buffer for a full bit. + + This is used for both DShot and serial output + */ +bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high) +{ + if (!group.dma_buffer) { + group.dma_buffer = (uint32_t *)hal.util->malloc_type(dshot_buffer_length, AP_HAL::Util::MEM_DMA_SAFE); + if (!group.dma_buffer) { + return false; + } + } + // for dshot we setup for DMAR based output + if (!group.dma) { + group.dma = STM32_DMA_STREAM(group.dma_up_stream_id); + group.dma_handle = new Shared_DMA(group.dma_up_stream_id, SHARED_DMA_NONE, + FUNCTOR_BIND_MEMBER(&RCOutput::dma_allocate, void, Shared_DMA *), + FUNCTOR_BIND_MEMBER(&RCOutput::dma_deallocate, void, Shared_DMA *)); + if (!group.dma_handle) { + return false; + } + } + + // hold the lock during setup, to ensure there isn't a DMA operation ongoing + group.dma_handle->lock(); + + // configure timer driver for DMAR at requested rate + if (group.pwm_started) { + pwmStop(group.pwm_drv); + group.pwm_started = false; + } + + // adjust frequency to give an allowed value given the + // clock. There is probably a better way to do this + uint32_t clock_hz = group.pwm_drv->clock; + uint32_t target_frequency = bitrate * bit_width; + uint32_t prescaler = clock_hz / target_frequency; + while ((clock_hz / prescaler) * prescaler != clock_hz && prescaler <= 0x8000) { + prescaler++; + } + uint32_t freq = clock_hz / prescaler; + if (prescaler > 0x8000) { + group.dma_handle->unlock(); + return false; + } + + group.pwm_cfg.frequency = freq; + group.pwm_cfg.period = bit_width; + group.pwm_cfg.dier = TIM_DIER_UDE; + group.pwm_cfg.cr2 = 0; + group.bit_width_mul = (freq + (target_frequency/2)) / target_frequency; + + for (uint8_t j=0; j<4; j++) { + if (group.pwm_cfg.channels[j].mode != PWM_OUTPUT_DISABLED) { + group.pwm_cfg.channels[j].mode = active_high?PWM_OUTPUT_ACTIVE_HIGH:PWM_OUTPUT_ACTIVE_LOW; + } + } + + pwmStart(group.pwm_drv, &group.pwm_cfg); + group.pwm_started = true; + + for (uint8_t j=0; j<4; j++) { + if (group.chan[j] != CHAN_DISABLED) { + pwmEnableChannel(group.pwm_drv, j, 0); + } + } + group.dma_handle->unlock(); + return true; +} + + +/* + setup output mode for a group, using group.current_mode. Used to restore output + after serial operations + */ +void RCOutput::set_group_mode(pwm_group &group) +{ + if (group.pwm_started) { + pwmStop(group.pwm_drv); + group.pwm_started = false; + } + + switch (group.current_mode) { + case MODE_PWM_BRUSHED: + // force zero output initially + for (uint8_t i=0; i<4; i++) { + if (group.chan[i] == CHAN_DISABLED) { + continue; + } + uint8_t chan = chan_offset + group.chan[i]; + write(chan, 0); + } + break; + + case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: { + const uint16_t rates[(1 + MODE_PWM_DSHOT1200) - MODE_PWM_DSHOT150] = { 150, 300, 600, 1200 }; + uint32_t rate = rates[uint8_t(group.current_mode - MODE_PWM_DSHOT150)] * 1000UL; + const uint32_t bit_period = 19; + + // configure timer driver for DMAR at requested rate + if (!setup_group_DMA(group, rate, bit_period, true)) { + group.current_mode = MODE_PWM_NONE; + break; + } + + // calculate min time between pulses + dshot_pulse_time_us = 1000000UL * dshot_bit_length / rate; + break; + } + + case MODE_PWM_ONESHOT: + // for oneshot we force 1Hz output and then trigger on each loop + pwmChangePeriod(group.pwm_drv, group.pwm_cfg.frequency); + break; + + case MODE_PWM_NORMAL: + case MODE_PWM_NONE: + // nothing needed + break; + } + + if (group.current_mode != MODE_PWM_NONE && + !group.pwm_started) { + pwmStart(group.pwm_drv, &group.pwm_cfg); + group.pwm_started = true; + for (uint8_t j=0; j<4; j++) { + if (group.chan[j] != CHAN_DISABLED) { + pwmEnableChannel(group.pwm_drv, j, 0); + } + } + } +} + /* setup output mode */ @@ -374,64 +559,11 @@ void RCOutput::set_output_mode(uint16_t mask, enum output_mode mode) // this group is not affected continue; } + if (mode_requires_dma(mode) && !group.have_up_dma) { + mode = MODE_PWM_NONE; + } group.current_mode = mode; - if (mode == MODE_PWM_BRUSHED) { - // force zero output initially - for (uint8_t i=chan_offset; i<16; i++) { - if ((group.ch_mask << chan_offset) & (1U<= MODE_PWM_DSHOT150 && mode <= MODE_PWM_DSHOT1200) { - if (!group.have_up_dma) { - // fallback to ONESHOT125 - hal.console->printf("DShot unavailable on mask 0x%04x\n", group.ch_mask); - group.current_mode = MODE_PWM_NORMAL; - } else { - if (!group.dma_buffer) { - group.dma_buffer = (uint32_t *)hal.util->malloc_type(dshot_buffer_length, AP_HAL::Util::MEM_DMA_SAFE); - if (!group.dma_buffer) { - hal.console->printf("DShot setup no memory\n"); - group.current_mode = MODE_PWM_NORMAL; - continue; - } - } - // for dshot we setup for DMAR based output - if (!group.dma) { - group.dma = STM32_DMA_STREAM(group.dma_up_stream_id); - group.dma_handle = new Shared_DMA(group.dma_up_stream_id, SHARED_DMA_NONE, - FUNCTOR_BIND_MEMBER(&RCOutput::dma_allocate, void, Shared_DMA *), - FUNCTOR_BIND_MEMBER(&RCOutput::dma_deallocate, void, Shared_DMA *)); - if (!group.dma_handle) { - hal.console->printf("DShot setup no memory\n"); - group.current_mode = MODE_PWM_NORMAL; - continue; - } - } - const uint16_t rates[(1 + MODE_PWM_DSHOT1200) - MODE_PWM_DSHOT150] = { 150, 300, 600, 1200 }; - uint32_t rate = rates[uint8_t(mode - MODE_PWM_DSHOT150)] * 1000UL; - const uint32_t bit_period = 19; - // configure timer driver for DMAR at requested rate - pwmStop(group.pwm_drv); - group.pwm_cfg.frequency = rate * bit_period * dshot_clockmul; - group.pwm_cfg.period = bit_period * dshot_clockmul; - group.pwm_cfg.dier = TIM_DIER_UDE; - group.pwm_cfg.cr2 = 0; - pwmStart(group.pwm_drv, &group.pwm_cfg); - for (uint8_t j=0; j<4; j++) { - if (group.chan[j] != CHAN_DISABLED) { - pwmEnableChannel(group.pwm_drv, j, 0); - } - } - // calculate min time between pulses - dshot_pulse_time_us = 1000000UL * dshot_bit_length / rate; - } - } - if (group.current_mode == MODE_PWM_ONESHOT) { - // for oneshot we force 1Hz output and then trigger on each loop - pwmChangePeriod(group.pwm_drv, group.pwm_cfg.frequency); - } + set_group_mode(group); } #if HAL_WITH_IO_MCU if (mode == MODE_PWM_ONESHOT && @@ -520,20 +652,30 @@ void RCOutput::trigger_groups(void) // guarantee minimum pulse separation hal.scheduler->delay_microseconds(min_pulse_trigger_us - now); } + osalSysLock(); for (uint8_t i = 0; i < NUM_GROUPS; i++) { pwm_group &group = pwm_group_list[i]; + if (irq.waiter) { + // doing serial output, don't send pulses + continue; + } if (group.current_mode == MODE_PWM_ONESHOT) { if (trigger_groupmask & (1U<tim->EGR = STM32_TIM_EGR_UG; } - } else if (group.current_mode >= MODE_PWM_DSHOT150 && group.current_mode <= MODE_PWM_DSHOT1200) { - dshot_send(group, false); } } osalSysUnlock(); + for (uint8_t i = 0; i < NUM_GROUPS; i++) { + pwm_group &group = pwm_group_list[i]; + if (group.current_mode >= MODE_PWM_DSHOT150 && group.current_mode <= MODE_PWM_DSHOT1200) { + dshot_send(group, false); + } + } + /* calculate time that we are allowed to trigger next pulse to guarantee at least a 50us gap between pulses @@ -550,6 +692,7 @@ void RCOutput::trigger_groups(void) A mininum output rate helps with some oneshot ESCs */ + void RCOutput::timer_tick(void) { for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { @@ -621,10 +764,10 @@ uint16_t RCOutput::create_dshot_packet(const uint16_t value) /* fill in a DMA buffer for dshot */ -void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet) +void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul) { - const uint8_t DSHOT_MOTOR_BIT_0 = 7 * dshot_clockmul; - const uint8_t DSHOT_MOTOR_BIT_1 = 14 * dshot_clockmul; + const uint32_t DSHOT_MOTOR_BIT_0 = 7 * clockmul; + const uint32_t DSHOT_MOTOR_BIT_1 = 14 * clockmul; for (uint16_t i = 0; i < 16; i++) { buffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; packet <<= 1; @@ -638,6 +781,11 @@ void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t */ void RCOutput::dshot_send(pwm_group &group, bool blocking) { + if (irq.waiter) { + // doing serial output, don't send DShot pulses + return; + } + uint8_t groupidx = &group - pwm_group_list; if (blocking) { group.dma_handle->lock(); @@ -659,10 +807,21 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking) value += 47; } uint16_t packet = create_dshot_packet(value); - fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet); + fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet, group.bit_width_mul); } } + // start sending the pulses out + send_pulses_DMAR(group, dshot_buffer_length); +} + +/* + send a series of pulses for a group using DMAR. Pulses must have + been encoded into the group dma_buffer with interleaving for the 4 + channels in the group + */ +void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length) +{ /* The DMA approach we are using is based on the DMAR method from betaflight. We use the TIMn_UP DMA channel for the timer, and @@ -677,7 +836,7 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking) */ dmaStreamSetPeripheral(group.dma, &(group.pwm_drv->tim->DMAR)); dmaStreamSetMemory0(group.dma, group.dma_buffer); - dmaStreamSetTransactionSize(group.dma, dshot_buffer_length/sizeof(uint32_t)); + dmaStreamSetTransactionSize(group.dma, buffer_length/sizeof(uint32_t)); dmaStreamSetFIFO(group.dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL); dmaStreamSetMode(group.dma, STM32_DMA_CR_CHSEL(group.dma_up_channel) | @@ -699,7 +858,284 @@ void RCOutput::dma_irq_callback(void *p, uint32_t flags) { pwm_group *group = (pwm_group *)p; dmaStreamDisable(group->dma); - group->dma_handle->unlock(); + if (group->in_serial_dma && irq.waiter) { + // tell the waiting process we've done the DMA + chSysLockFromISR(); + chEvtSignalI(irq.waiter, serial_event_mask); + chSysUnlockFromISR(); + } else { + group->dma_handle->unlock_from_IRQ(); + } +} + +/* + setup for serial output to an ESC using the given + baudrate. Assumes 1 start bit, 1 stop bit, LSB first and 8 + databits. This is used for passthrough ESC configuration and + firmware flashing + + While serial output is active normal output to the channel group is + suspended. +*/ +bool RCOutput::serial_setup_output(uint8_t chan, uint32_t baudrate) +{ + // account for IOMCU channels + chan -= chan_offset; + pwm_group *new_serial_group = nullptr; + + // find the channel group + for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { + pwm_group &group = pwm_group_list[i]; + if (group.ch_mask & (1U<serial.bit_time_us = 1000000UL / baudrate; + + // remember the thread that set things up. This is also used to + // mark the group as doing serial output, so normal output is + // suspended + irq.waiter = chThdGetSelfX(); + + return true; +} + + +/* + fill in a DMA buffer for a serial byte, assuming 1 start bit and 1 stop bit + */ +void RCOutput::fill_DMA_buffer_byte(uint32_t *buffer, uint8_t stride, uint8_t b, uint32_t bitval) +{ + const uint32_t BIT_0 = bitval; + const uint32_t BIT_1 = 0; + + // start bit + buffer[0] = BIT_0; + + // stop bit + buffer[9*stride] = BIT_1; + + // 8 data bits + for (uint8_t i = 0; i < 8; i++) { + buffer[(1 + i) * stride] = (b & 1) ? BIT_1 : BIT_0; + b >>= 1; + } +} + + +/* + send one serial byte, blocking call, should be called with the DMA lock held +*/ +bool RCOutput::serial_write_byte(uint8_t b) +{ + chEvtGetAndClearEvents(serial_event_mask); + + fill_DMA_buffer_byte(serial_group->dma_buffer+serial_group->serial.chan, 4, b, serial_group->bit_width_mul*10); + + serial_group->in_serial_dma = true; + + // start sending the pulses out + send_pulses_DMAR(*serial_group, 10*4*sizeof(uint32_t)); + + // wait for the event + eventmask_t mask = chEvtWaitAnyTimeout(serial_event_mask, MS2ST(2)); + + serial_group->in_serial_dma = false; + + return (mask & serial_event_mask) != 0; +} + +/* + send a set of serial bytes, blocking call +*/ +bool RCOutput::serial_write_bytes(const uint8_t *bytes, uint16_t len) +{ + if (!serial_group) { + return false; + } + serial_group->dma_handle->lock(); + memset(serial_group->dma_buffer, 0, dshot_buffer_length); + while (len--) { + if (!serial_write_byte(*bytes++)) { + serial_group->dma_handle->unlock(); + return false; + } + } + serial_group->dma_handle->unlock(); + // add a small delay for last word of output to have completely + // finished + hal.scheduler->delay_microseconds(10); + return true; +} + +/* + irq handler for bit transition in serial_read_byte() + This implements a one byte soft serial reader + */ +void RCOutput::serial_bit_irq(void) +{ + uint32_t now = AP_HAL::micros(); + uint8_t bit = palReadLine(irq.line); + bool send_signal = false; + + if (irq.nbits == 0 || bit == irq.last_bit) { + // start of byte, should be low + if (bit != 0) { + irq.byteval = 0x200; + send_signal = true; + } else { + irq.nbits = 1; + irq.byte_start_us = now; + irq.bitmask = 0; + } + } else { + uint32_t dt = now - irq.byte_start_us; + uint8_t bitnum = (dt+(irq.bit_time_us/2)) / irq.bit_time_us; + + if (bitnum > 10) { + bitnum = 10; + } + if (!bit) { + // set the bits that we've processed + irq.bitmask |= ((1U<>1); + return true; +} + +/* + read a byte from a port, using serial parameters from serial_setup_output() +*/ +uint16_t RCOutput::serial_read_bytes(uint8_t *buf, uint16_t len) +{ + pwm_group &group = *serial_group; + uint8_t chan = group.chan[group.serial.chan]; + uint32_t gpio_mode = PAL_MODE_INPUT | PAL_STM32_OSPEED_LOWEST | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_PUPDR_PULLUP; + uint32_t restore_mode = PAL_MODE_ALTERNATE(group.alt_functions[group.serial.chan]) | PAL_STM32_OSPEED_MID2 | PAL_STM32_OTYPE_PUSHPULL; + uint16_t i = 0; + + tprio_t oldprio = chThdGetSelfX()->realprio; + chThdSetPriority(HIGHPRIO); + +#ifndef HAL_GPIO_LINE_GPIO50 + // GPIO lines not setup for PWM outputs in hwdef.dat + return false; +#endif + + // assume GPIO mappings for PWM outputs start at 50 + const uint8_t gpio_pin = 50 + chan; + ((GPIO *)hal.gpio)->_set_mode(gpio_pin, gpio_mode); + + chEvtGetAndClearEvents(serial_event_mask); + + irq.line = group.pal_lines[group.serial.chan]; + irq.nbits = 0; + irq.bitmask = 0; + irq.byteval = 0; + irq.bit_time_us = serial_group->serial.bit_time_us; + irq.last_bit = 0; + irq.waiter = chThdGetSelfX(); + + if (!hal.gpio->attach_interrupt(gpio_pin, serial_bit_irq, HAL_GPIO_INTERRUPT_BOTH)) { + return false; + } + + for (i=0; iattach_interrupt(gpio_pin, nullptr, 0); + irq.waiter = nullptr; + + ((GPIO *)hal.gpio)->_set_mode(gpio_pin, restore_mode); + chThdSetPriority(oldprio); + return i; +} + +/* + end serial output +*/ +void RCOutput::serial_end(void) +{ + if (serial_group) { + // restore normal output + set_group_mode(*serial_group); + set_freq_group(*serial_group); + irq.waiter = nullptr; + } + serial_group = nullptr; } #endif // HAL_USE_PWM diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 5fbd4cf22b..f4b34aa379 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -69,6 +69,46 @@ public: timer push (for oneshot min rate) */ void timer_tick(void) override; + + /* + setup for serial output to a set of ESCs, using the given + baudrate. Assumes 1 start bit, 1 stop bit, LSB first and 8 + databits. This is used for ESC configuration and firmware + flashing + */ + bool setup_serial_output(uint16_t chan_mask, ByteBuffer *buffer, uint32_t baudrate); + + /* + setup for serial output to an ESC using the given + baudrate. Assumes 1 start bit, 1 stop bit, LSB first and 8 + databits. This is used for passthrough ESC configuration and + firmware flashing + + While serial output is active normal output to this channel is + suspended. Output to some other channels (such as those in the + same channel timer group) may also be stopped, depending on the + implementation + */ + bool serial_setup_output(uint8_t chan, uint32_t baudrate) override; + + /* + write a set of bytes to an ESC, using settings from + serial_setup_output. This is a blocking call + */ + bool serial_write_bytes(const uint8_t *bytes, uint16_t len) override; + + /* + read a byte from a port, using serial parameters from serial_setup_output() + return the number of bytes read + */ + uint16_t serial_read_bytes(uint8_t *buf, uint16_t len) override; + + /* + stop serial output. This restores the previous output mode for + the channel and any other channels that were stopped by + serial_setup_output() + */ + void serial_end(void) override; private: struct pwm_group { @@ -80,14 +120,68 @@ private: bool have_up_dma; // can we do DMAR outputs for DShot? uint8_t dma_up_stream_id; uint8_t dma_up_channel; + uint8_t alt_functions[4]; + ioline_t pal_lines[4]; + + // below this line is not initialised by hwdef.h enum output_mode current_mode; + uint16_t frequency_hz; uint16_t ch_mask; const stm32_dma_stream_t *dma; Shared_DMA *dma_handle; uint32_t *dma_buffer; bool have_lock; + bool pwm_started; + uint32_t bit_width_mul; + uint32_t rc_frequency; + bool in_serial_dma; + + // serial output + struct { + // expected time per bit + uint32_t bit_time_us; + + // channel to output to within group (0 to 3) + uint8_t chan; + + // thread waiting for byte to be written + thread_t *waiter; + } serial; }; + /* + structure for IRQ handler for soft-serial input + */ + static struct irq_state { + // ioline for port being read + ioline_t line; + + // time the current byte started + uint32_t byte_start_us; + + // number of bits we have read in this byte + uint8_t nbits; + + // bitmask of bits so far (includes start and stop bits) + uint16_t bitmask; + + // value of completed byte (includes start and stop bits) + uint16_t byteval; + + // expected time per bit + uint32_t bit_time_us; + + // the bit value of the last bit received + uint8_t last_bit; + + // thread waiting for byte to be read + thread_t *waiter; + } irq; + + + // the group being used for serial output + struct pwm_group *serial_group; + static pwm_group pwm_group_list[]; uint16_t _esc_pwm_min; uint16_t _esc_pwm_max; @@ -130,20 +224,34 @@ private: // trigger group pulses void trigger_groups(void); + // setup output frequency for a group + void set_freq_group(pwm_group &group); + /* DShot handling */ const uint8_t dshot_post = 2; - const uint8_t dshot_clockmul = 2; const uint16_t dshot_bit_length = 16 + dshot_post; const uint16_t dshot_buffer_length = dshot_bit_length*4*sizeof(uint32_t); uint32_t dshot_pulse_time_us; void dma_allocate(Shared_DMA *ctx); void dma_deallocate(Shared_DMA *ctx); uint16_t create_dshot_packet(const uint16_t value); - void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet); + void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul); void dshot_send(pwm_group &group, bool blocking); static void dma_irq_callback(void *p, uint32_t flags); + 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); + void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length); + void set_group_mode(pwm_group &group); + + // serial output support + static const eventmask_t serial_event_mask = EVENT_MASK(1); + bool serial_write_byte(uint8_t b); + bool serial_read_byte(uint8_t &b); + void fill_DMA_buffer_byte(uint32_t *buffer, uint8_t stride, uint8_t b , uint32_t bitval); + static void serial_bit_irq(void); + }; #endif // HAL_USE_PWM diff --git a/libraries/AP_HAL_ChibiOS/examples/DShot/DShot.cpp b/libraries/AP_HAL_ChibiOS/examples/DShot/DShot.cpp index d478839971..eaccc72614 100644 --- a/libraries/AP_HAL_ChibiOS/examples/DShot/DShot.cpp +++ b/libraries/AP_HAL_ChibiOS/examples/DShot/DShot.cpp @@ -1,5 +1,7 @@ /* - example outputting DShot on 4 TIM1 channels on fmuv4 board + example implementing MSP and BLHeli passthrough protocol in ArduPilot + + With thanks to betaflight for a great reference implementation */ #include @@ -7,135 +9,1113 @@ #include "ch.h" #include "hal.h" #include "hwdef.h" +#include +#include +#include "msp_protocol.h" +#include "blheli_4way_protocol.h" + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); void setup(); void loop(); -const AP_HAL::HAL& hal = AP_HAL::get_HAL(); - -// use TIM1_CH1 to TIM1_CH4 -#define DMA_STREAM STM32_TIM_TIM1_UP_DMA_STREAM -#define PWMD PWMD1 -#define DMA_CH STM32_TIM_TIM1_UP_DMA_CHAN - -// choose DShot rate. Can be 150, 300, 600 or 1200 -#define DSHOT_RATE 600U -#define DSHOT_BIT_PERIOD 19U - -#define DSHOT_PWM_FREQUENCY (DSHOT_RATE * 1000U * DSHOT_BIT_PERIOD) - -#define DSHOT_BIT_LENGTH 18 // includes two reset bits - -// pulse width for 0 and 1 bits -#define DSHOT_MOTOR_BIT_0 7 -#define DSHOT_MOTOR_BIT_1 14 - -static uint32_t *buffer; -static uint16_t buffer_length = DSHOT_BIT_LENGTH*4*sizeof(uint32_t); -const stm32_dma_stream_t *dma; +//#pragma GCC optimize("Og") /* - setup pwm for all 4 channels enabled + implementation of MSP protocol based on betaflight */ -static const PWMConfig pwm_config = { - .frequency = DSHOT_PWM_FREQUENCY, - .period = DSHOT_BIT_PERIOD, - .callback = NULL, - .channels = { - {.mode = PWM_OUTPUT_ACTIVE_HIGH, .callback = NULL}, - {.mode = PWM_OUTPUT_ACTIVE_HIGH, .callback = NULL}, - {.mode = PWM_OUTPUT_ACTIVE_HIGH, .callback = NULL}, - {.mode = PWM_OUTPUT_ACTIVE_HIGH, .callback = NULL}, - }, - .cr2 = 0, - .dier = TIM_DIER_UDE, // DMA on update event for next period + +enum mspState { + MSP_IDLE=0, + MSP_HEADER_START, + MSP_HEADER_M, + MSP_HEADER_ARROW, + MSP_HEADER_SIZE, + MSP_HEADER_CMD, + MSP_COMMAND_RECEIVED +}; + +enum mspPacketType { + MSP_PACKET_COMMAND, + MSP_PACKET_REPLY +}; + +enum escProtocol { + PROTOCOL_SIMONK = 0, + PROTOCOL_BLHELI = 1, + PROTOCOL_KISS = 2, + PROTOCOL_KISSALL = 3, + PROTOCOL_CASTLE = 4, + PROTOCOL_MAX = 5, + PROTOCOL_NONE = 0xfe, + PROTOCOL_4WAY = 0xff +}; + +enum motorPwmProtocol { + PWM_TYPE_STANDARD = 0, + PWM_TYPE_ONESHOT125, + PWM_TYPE_ONESHOT42, + PWM_TYPE_MULTISHOT, + PWM_TYPE_BRUSHED, + PWM_TYPE_DSHOT150, + PWM_TYPE_DSHOT300, + PWM_TYPE_DSHOT600, + PWM_TYPE_DSHOT1200, + PWM_TYPE_PROSHOT1000, +}; + +enum MSPFeatures { + FEATURE_RX_PPM = 1 << 0, + FEATURE_INFLIGHT_ACC_CAL = 1 << 2, + FEATURE_RX_SERIAL = 1 << 3, + FEATURE_MOTOR_STOP = 1 << 4, + FEATURE_SERVO_TILT = 1 << 5, + FEATURE_SOFTSERIAL = 1 << 6, + FEATURE_GPS = 1 << 7, + FEATURE_RANGEFINDER = 1 << 9, + FEATURE_TELEMETRY = 1 << 10, + FEATURE_3D = 1 << 12, + FEATURE_RX_PARALLEL_PWM = 1 << 13, + FEATURE_RX_MSP = 1 << 14, + FEATURE_RSSI_ADC = 1 << 15, + FEATURE_LED_STRIP = 1 << 16, + FEATURE_DASHBOARD = 1 << 17, + FEATURE_OSD = 1 << 18, + FEATURE_CHANNEL_FORWARDING = 1 << 20, + FEATURE_TRANSPONDER = 1 << 21, + FEATURE_AIRMODE = 1 << 22, + FEATURE_RX_SPI = 1 << 25, + FEATURE_SOFTSPI = 1 << 26, + FEATURE_ESC_SENSOR = 1 << 27, + FEATURE_ANTI_GRAVITY = 1 << 28, + FEATURE_DYNAMIC_FILTER = 1 << 29, }; -void setup(void) { - hal.console->printf("Starting DShot test\n"); - dma = STM32_DMA_STREAM(DMA_STREAM); - buffer = (uint32_t *)hal.util->malloc_type(buffer_length, AP_HAL::Util::MEM_DMA_SAFE); +/* + state of MSP command processing + */ +static struct { + enum mspState state; + enum mspPacketType packetType; + uint8_t offset; + uint8_t dataSize; + uint8_t checksum; + uint8_t buf[192]; + uint8_t cmdMSP; + enum escProtocol escMode; + uint8_t portIndex; +} msp; - dmaStreamAllocate(dma, 10, NULL, NULL); +#define MSP_PORT_INBUF_SIZE sizeof(msp.buf) - pwmStart(&PWMD, &pwm_config); -} +enum blheliState { + BLHELI_IDLE=0, + BLHELI_HEADER_START, + BLHELI_HEADER_CMD, + BLHELI_HEADER_ADDR_LOW, + BLHELI_HEADER_ADDR_HIGH, + BLHELI_HEADER_LEN, + BLHELI_CRC1, + BLHELI_CRC2, + BLHELI_COMMAND_RECEIVED +}; /* - create a DSHOT 16 bit packet. Based on prepareDshotPacket from betaflight + state of blheli 4way protocol handling */ -static uint16_t create_dshot_packet(const uint16_t value) +static struct { + enum blheliState state; + uint8_t command; + uint16_t address; + uint16_t param_len; + uint16_t offset; + uint8_t buf[256+3+8]; + uint8_t crc1; + uint16_t crc; + uint8_t interface_mode; + uint8_t deviceInfo[4]; + uint8_t chan; + uint8_t ack; +} blheli; + + +// start of 12 byte CPU ID +#ifndef UDID_START +#define UDID_START 0x1FFF7A10 +#endif + +// fixed number of channels for now +#define NUM_ESC_CHANNELS 4 + +/* + process one byte of serial input for MSP protocol + */ +static bool msp_process_byte(uint8_t c) { - uint16_t packet = (value << 1); // no telemetry request - - // compute checksum - uint16_t csum = 0; - uint16_t csum_data = packet; - for (uint8_t i = 0; i < 3; i++) { - csum ^= csum_data; - csum_data >>= 4; + if (msp.state == MSP_IDLE) { + msp.escMode = PROTOCOL_NONE; + if (c == '$') { + msp.state = MSP_HEADER_START; + } else { + return false; + } + } else if (msp.state == MSP_HEADER_START) { + msp.state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE; + } else if (msp.state == MSP_HEADER_M) { + msp.state = MSP_IDLE; + switch (c) { + case '<': // COMMAND + msp.packetType = MSP_PACKET_COMMAND; + msp.state = MSP_HEADER_ARROW; + break; + case '>': // REPLY + msp.packetType = MSP_PACKET_REPLY; + msp.state = MSP_HEADER_ARROW; + break; + default: + break; + } + } else if (msp.state == MSP_HEADER_ARROW) { + if (c > MSP_PORT_INBUF_SIZE) { + msp.state = MSP_IDLE; + } else { + msp.dataSize = c; + msp.offset = 0; + msp.checksum = 0; + msp.checksum ^= c; + msp.state = MSP_HEADER_SIZE; + } + } else if (msp.state == MSP_HEADER_SIZE) { + msp.cmdMSP = c; + msp.checksum ^= c; + msp.state = MSP_HEADER_CMD; + } else if (msp.state == MSP_HEADER_CMD && msp.offset < msp.dataSize) { + msp.checksum ^= c; + msp.buf[msp.offset++] = c; + } else if (msp.state == MSP_HEADER_CMD && msp.offset >= msp.dataSize) { + if (msp.checksum == c) { + msp.state = MSP_COMMAND_RECEIVED; + } else { + msp.state = MSP_IDLE; + } } - csum &= 0xf; - // append checksum - packet = (packet << 4) | csum; - - return packet; + return true; } /* - fill in a DMA buffer for dshot + update CRC state for blheli protocol */ -static void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet) +static void blheli_crc_update(uint8_t c) { - for (int i = 0; i < 16; i++) { - buffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; - packet <<= 1; - } + blheli.crc = crc_xmodem_update(blheli.crc, c); } -static void test_dshot_send(void) +/* + process one byte of serial input for blheli 4way protocol + */ +static bool blheli_4way_process_byte(uint8_t c) { - uint16_t packets[4]; - for (uint8_t i=0; i<4; i++) { - packets[i] = create_dshot_packet(200 * (i+1)); - fill_DMA_buffer_dshot(buffer + i, 4, packets[i]); + if (blheli.state == BLHELI_IDLE) { + if (c == cmd_Local_Escape) { + blheli.state = BLHELI_HEADER_START; + blheli.crc = 0; + blheli_crc_update(c); + } else { + return false; + } + } else if (blheli.state == BLHELI_HEADER_START) { + blheli.command = c; + blheli_crc_update(c); + blheli.state = BLHELI_HEADER_CMD; + } else if (blheli.state == BLHELI_HEADER_CMD) { + blheli.address = c<<8; + blheli.state = BLHELI_HEADER_ADDR_HIGH; + blheli_crc_update(c); + } else if (blheli.state == BLHELI_HEADER_ADDR_HIGH) { + blheli.address |= c; + blheli.state = BLHELI_HEADER_ADDR_LOW; + blheli_crc_update(c); + } else if (blheli.state == BLHELI_HEADER_ADDR_LOW) { + blheli.state = BLHELI_HEADER_LEN; + blheli.param_len = c?c:256; + blheli.offset = 0; + blheli_crc_update(c); + } else if (blheli.state == BLHELI_HEADER_LEN) { + blheli.buf[blheli.offset++] = c; + blheli_crc_update(c); + if (blheli.offset == blheli.param_len) { + blheli.state = BLHELI_CRC1; + } + } else if (blheli.state == BLHELI_CRC1) { + blheli.crc1 = c; + blheli.state = BLHELI_CRC2; + } else if (blheli.state == BLHELI_CRC2) { + uint16_t crc = blheli.crc1<<8 | c; + if (crc == blheli.crc) { + blheli.state = BLHELI_COMMAND_RECEIVED; + } else { + blheli.state = BLHELI_IDLE; + } + } + return true; +} + +/* + send a MSP protocol reply + */ +static void msp_send_reply(uint8_t cmd, const uint8_t *buf, uint8_t len) +{ + uint8_t *b = &msp.buf[0]; + *b++ = '$'; + *b++ = 'M'; + *b++ = '>'; + *b++ = len; + *b++ = cmd; + memcpy(b, buf, len); + b += len; + uint8_t c = 0; + for (uint8_t i=0; iwrite(&msp.buf[0], len+6); +} + +static void putU16(uint8_t *b, uint16_t v) +{ + b[0] = v; + b[1] = v >> 8; +} + +static uint16_t getU16(const uint8_t *b) +{ + return b[0] | (b[1]<<8); +} + +static void putU32(uint8_t *b, uint32_t v) +{ + b[0] = v; + b[1] = v >> 8; + b[2] = v >> 16; + b[3] = v >> 24; +} + +static void putU16_BE(uint8_t *b, uint16_t v) +{ + b[0] = v >> 8; + b[1] = v; +} + +/* + process a MSP command from GCS + */ +static void msp_process_command(void) +{ + hal.console->printf("MSP cmd %u len=%u\n", msp.cmdMSP, msp.dataSize); + switch (msp.cmdMSP) { + case MSP_API_VERSION: { + uint8_t buf[3] = { MSP_PROTOCOL_VERSION, API_VERSION_MAJOR, API_VERSION_MINOR }; + msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); + break; + } + + case MSP_FC_VARIANT: + msp_send_reply(msp.cmdMSP, (const uint8_t *)ARDUPILOT_IDENTIFIER, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); + break; + + case MSP_FC_VERSION: { + uint8_t version[3] = { 3, 3, 0 }; + msp_send_reply(msp.cmdMSP, version, sizeof(version)); + break; + } + case MSP_BOARD_INFO: { + // send a generic 'ArduPilot ChibiOS' board type + uint8_t buf[7] = { 'A', 'R', 'C', 'H', 0, 0, 0 }; + msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); + break; + } + + case MSP_BUILD_INFO: { + // build date, build time, git version + uint8_t buf[26] { + 0x4d, 0x61, 0x72, 0x20, 0x31, 0x36, 0x20, 0x32, 0x30, + 0x31, 0x38, 0x30, 0x38, 0x3A, 0x34, 0x32, 0x3a, 0x32, 0x39, + 0x62, 0x30, 0x66, 0x66, 0x39, 0x32, 0x38}; + msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); + break; + } + + case MSP_REBOOT: + hal.console->printf("MSP: rebooting\n"); + hal.scheduler->reboot(false); + break; + + case MSP_UID: + // MCU identifer + msp_send_reply(msp.cmdMSP, (const uint8_t *)UDID_START, 12); + break; + + case MSP_ADVANCED_CONFIG: { + uint8_t buf[10]; + buf[0] = 1; // gyro sync denom + buf[1] = 4; // pid process denom + buf[2] = 0; // use unsynced pwm + buf[3] = (uint8_t)PWM_TYPE_DSHOT150; // motor PWM protocol + putU16(&buf[4], 480); // motor PWM Rate + putU16(&buf[6], 450); // idle offset value + buf[8] = 0; // use 32kHz + buf[9] = 0; // motor PWM inversion + msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); + break; + } + + case MSP_FEATURE_CONFIG: { + uint8_t buf[4]; + putU32(buf, 0); // from MSPFeatures enum + msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); + break; + } + + case MSP_STATUS: { + uint8_t buf[21]; + putU16(&buf[0], 2500); // loop time usec + putU16(&buf[2], 0); // i2c error count + putU16(&buf[4], 0x27); // available sensors + putU32(&buf[6], 0); // flight modes + buf[10] = 0; // pid profile index + putU16(&buf[11], 5); // system load percent + putU16(&buf[13], 0); // gyro cycle time + buf[15] = 0; // flight mode flags length + buf[16] = 18; // arming disable flags count + putU32(&buf[17], 0); // arming disable flags + msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); + break; + } + + case MSP_MOTOR_3D_CONFIG: { + uint8_t buf[6]; + putU16(&buf[0], 1406); // 3D deadband low + putU16(&buf[2], 1514); // 3D deadband high + putU16(&buf[4], 1460); // 3D neutral + msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); + break; + } + + case MSP_MOTOR_CONFIG: { + uint8_t buf[6]; + putU16(&buf[0], 1070); // min throttle + putU16(&buf[2], 2000); // max throttle + putU16(&buf[4], 1000); // min command + msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); + break; + } + + case MSP_MOTOR: { + // get the output going to each motor + uint8_t buf[16]; + for (uint8_t i = 0; i < 8; i++) { + putU16(&buf[2*i], hal.rcout->read(i)); + } + msp_send_reply(msp.cmdMSP, buf, sizeof(buf)); + break; + } + + case MSP_SET_MOTOR: { + // set the output to each motor + uint8_t nmotors = msp.dataSize / 2; + hal.console->printf("MSP_SET_MOTOR %u\n", nmotors); + hal.rcout->cork(); + for (uint8_t i = 0; i < nmotors; i++) { + uint16_t v = getU16(&msp.buf[i*2]); + hal.console->printf("MSP_SET_MOTOR %u %u\n", i, v); + hal.rcout->write(i, v); + } + hal.rcout->push(); + break; } - dmaStreamSetPeripheral(dma, &(PWMD.tim->DMAR)); - dmaStreamSetMemory0(dma, buffer); - dmaStreamSetTransactionSize(dma, buffer_length/sizeof(uint32_t)); - dmaStreamSetFIFO(dma, STM32_DMA_FCR_DMDIS | STM32_DMA_FCR_FTH_FULL); - dmaStreamSetMode(dma, - STM32_DMA_CR_CHSEL(DMA_CH) | - STM32_DMA_CR_DIR_M2P | STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD | - STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3)); - - for (uint8_t i=0; i<4; i++) { - pwmEnableChannel(&PWMD, i, 0); + case MSP_SET_4WAY_IF: { + if (msp.dataSize == 0) { + msp.escMode = PROTOCOL_4WAY; + } else if (msp.dataSize == 2) { + msp.escMode = (enum escProtocol)msp.buf[0]; + msp.portIndex = msp.buf[1]; + } + hal.console->printf("escMode=%u portIndex=%u\n", msp.escMode, msp.portIndex); + uint8_t n = NUM_ESC_CHANNELS; + switch (msp.escMode) { + case PROTOCOL_4WAY: + break; + default: + n = 0; + hal.rcout->serial_end(); + break; + } + msp_send_reply(msp.cmdMSP, &n, 1); + break; + } + default: + hal.console->printf("Unknown MSP command %u\n", msp.cmdMSP); + break; } - - // setup for 4 burst strided transfers. 0x0D is the register - // address offset of the CCR registers in the timer peripheral - PWMD.tim->DCR = 0x0D | STM32_TIM_DCR_DBL(3); - - dmaStreamEnable(dma); } -static void test_dshot_cleanup(void) + +/* + send a blheli 4way protocol reply + */ +static void blheli_send_reply(const uint8_t *buf, uint16_t len) { - dmaStreamDisable(dma); + uint8_t *b = &blheli.buf[0]; + *b++ = cmd_Remote_Escape; + *b++ = blheli.command; + putU16_BE(b, blheli.address); b += 2; + *b++ = len==256?0:len; + memcpy(b, buf, len); + b += len; + *b++ = blheli.ack; + putU16_BE(b, crc_xmodem(&blheli.buf[0], len+6)); + hal.uartC->write(&blheli.buf[0], len+8); + hal.console->printf("OutB(%u) 0x%02x ack=0x%02x\n", len+8, (unsigned)blheli.command, blheli.ack); +} + +/* + CRC used when talking to ESCs + */ +static uint16_t BL_CRC(const uint8_t *buf, uint16_t len) +{ + uint16_t crc = 0; + while (len--) { + uint8_t xb = *buf++; + for (uint8_t i = 0; i < 8; i++) { + if (((xb & 0x01) ^ (crc & 0x0001)) !=0 ) { + crc = crc >> 1; + crc = crc ^ 0xA001; + } else { + crc = crc >> 1; + } + xb = xb >> 1; + } + } + return crc; +} + +static bool isMcuConnected(void) +{ + return blheli.deviceInfo[0] > 0; +} + +static void setDisconnected(void) +{ + blheli.deviceInfo[0] = 0; + blheli.deviceInfo[1] = 0; +} + +/* + send a set of bytes to an RC output channel + */ +static bool BL_SendBuf(const uint8_t *buf, uint16_t len) +{ + bool send_crc = isMcuConnected(); + if (!hal.rcout->serial_setup_output(blheli.chan, 19200)) { + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + memcpy(blheli.buf, buf, len); + uint16_t crc = BL_CRC(buf, len); + blheli.buf[len] = crc; + blheli.buf[len+1] = crc>>8; + if (!hal.rcout->serial_write_bytes(blheli.buf, len+(send_crc?2:0))) { + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + return true; +} + +static bool BL_ReadBuf(uint8_t *buf, uint16_t len) +{ + bool check_crc = isMcuConnected() && len > 0; + uint16_t req_bytes = len+(check_crc?3:1); + uint16_t n = hal.rcout->serial_read_bytes(blheli.buf, req_bytes); + hal.console->printf("BL_ReadBuf %u -> %u\n", len, n); + if (req_bytes != n) { + hal.console->printf("short read\n"); + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + if (check_crc) { + uint16_t crc = BL_CRC(blheli.buf, len); + if ((crc & 0xff) != blheli.buf[len] || + (crc >> 8) != blheli.buf[len+1]) { + hal.console->printf("bad CRC\n"); + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + if (blheli.buf[len+2] != brSUCCESS) { + hal.console->printf("bad ACK\n"); + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + } else { + if (blheli.buf[len] != brSUCCESS) { + hal.console->printf("bad ACK1\n"); + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + } + if (len > 0) { + memcpy(buf, blheli.buf, len); + } + return true; +} + +static uint8_t BL_GetACK(uint16_t timeout_ms=2) +{ + uint8_t ack; + uint32_t start_ms = AP_HAL::millis(); + while (AP_HAL::millis() - start_ms < timeout_ms) { + if (hal.rcout->serial_read_bytes(&ack, 1) == 1) { + return ack; + } + } + // return brNONE, meaning no ACK received in the timeout + return brNONE; +} + +static bool BL_SendCMDSetAddress() +{ + // skip if adr == 0xFFFF + if (blheli.address == 0xFFFF) { + return true; + } + hal.console->printf("BL_SendCMDSetAddress 0x%04x\n", blheli.address); + uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, uint8_t(blheli.address>>8), uint8_t(blheli.address)}; + BL_SendBuf(sCMD, 4); + return BL_GetACK() == brSUCCESS; +} + +static bool BL_ReadA(uint8_t cmd, uint8_t *buf, uint16_t n) +{ + if (BL_SendCMDSetAddress()) { + uint8_t sCMD[] = {cmd, uint8_t(n==256?0:n)}; + if (!BL_SendBuf(sCMD, 2)) { + return false; + } + return BL_ReadBuf(buf, n); + } + return false; +} + +/* + connect to a blheli ESC + */ +static bool BL_ConnectEx(void) +{ + hal.console->printf("BL_ConnectEx start\n"); + setDisconnected(); + const uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 21); + + uint8_t BootInfo[8]; + if (!BL_ReadBuf(BootInfo, 8)) { + return false; + } + + // reply must start with 471 + if (strncmp((const char *)BootInfo, "471", 3) != 0) { + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + + // extract device information + blheli.deviceInfo[2] = BootInfo[3]; + blheli.deviceInfo[1] = BootInfo[4]; + blheli.deviceInfo[0] = BootInfo[5]; + + blheli.interface_mode = 0; + + uint16_t *devword = (uint16_t *)blheli.deviceInfo; + switch (*devword) { + case 0x9307: + case 0x930A: + case 0x930F: + case 0x940B: + blheli.interface_mode = imATM_BLB; + hal.console->printf("Interface type imATM_BLB\n"); + break; + case 0xF310: + case 0xF330: + case 0xF410: + case 0xF390: + case 0xF850: + case 0xE8B1: + case 0xE8B2: + blheli.interface_mode = imSIL_BLB; + hal.console->printf("Interface type imSIL_BLB\n"); + break; + case 0x1F06: + case 0x3306: + case 0x3406: + case 0x3506: + blheli.interface_mode = imARM_BLB; + hal.console->printf("Interface type imARM_BLB\n"); + break; + default: + blheli.ack = ACK_D_GENERAL_ERROR; + hal.console->printf("Unknown interface type 0x%04x\n", *devword); + break; + } + blheli.deviceInfo[3] = blheli.interface_mode; + return true; +} + +static bool BL_SendCMDKeepAlive(void) +{ + uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0}; + if (!BL_SendBuf(sCMD, 2)) { + return false; + } + if (BL_GetACK() != brERRORCOMMAND) { + return false; + } + return true; +} + +static bool BL_PageErase(void) +{ + if (BL_SendCMDSetAddress()) { + uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; + BL_SendBuf(sCMD, 2); + return BL_GetACK(1000) == brSUCCESS; + } + return false; +} + +static void BL_SendCMDRunRestartBootloader(void) +{ + uint8_t sCMD[] = {RestartBootloader, 0}; + blheli.deviceInfo[0] = 1; + BL_SendBuf(sCMD, 2); +} + +static uint8_t BL_SendCMDSetBuffer(const uint8_t *buf, uint16_t nbytes) +{ + uint8_t sCMD[] = {CMD_SET_BUFFER, 0, uint8_t(nbytes>>8), uint8_t(nbytes&0xff)}; + if (!BL_SendBuf(sCMD, 4)) { + return false; + } + uint8_t ack; + if ((ack = BL_GetACK()) != brNONE) { + hal.console->printf("BL_SendCMDSetBuffer ack failed 0x%02x\n", ack); + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + if (!BL_SendBuf(buf, nbytes)) { + hal.console->printf("BL_SendCMDSetBuffer send failed\n"); + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + return (BL_GetACK(40) == brSUCCESS); +} + +static bool BL_WriteA(uint8_t cmd, const uint8_t *buf, uint16_t nbytes, uint32_t timeout) +{ + if (BL_SendCMDSetAddress()) { + if (!BL_SendCMDSetBuffer(buf, nbytes)) { + blheli.ack = ACK_D_GENERAL_ERROR; + return false; + } + uint8_t sCMD[] = {cmd, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK(timeout) == brSUCCESS); + } + blheli.ack = ACK_D_GENERAL_ERROR; + return false; +} + +static uint8_t BL_WriteFlash(const uint8_t *buf, uint16_t n) +{ + return BL_WriteA(CMD_PROG_FLASH, buf, n, 250); +} + +static bool BL_VerifyFlash(const uint8_t *buf, uint16_t n) +{ + if (BL_SendCMDSetAddress()) { + if (!BL_SendCMDSetBuffer(buf, n)) { + return false; + } + uint8_t sCMD[] = {CMD_VERIFY_FLASH_ARM, 0x01}; + BL_SendBuf(sCMD, 2); + uint8_t ack = BL_GetACK(40); + switch (ack) { + case brSUCCESS: + blheli.ack = ACK_OK; + break; + case brERRORVERIFY: + blheli.ack = ACK_I_VERIFY_ERROR; + break; + default: + blheli.ack = ACK_D_GENERAL_ERROR; + break; + } + return true; + } + return false; +} + +/* + process a blheli 4way command from GCS + */ +static void blheli_process_command(void) +{ + hal.console->printf("BLHeli cmd 0x%02x len=%u\n", blheli.command, blheli.param_len); + blheli.ack = ACK_OK; + switch (blheli.command) { + case cmd_InterfaceTestAlive: { + hal.console->printf("cmd_InterfaceTestAlive\n"); + BL_SendCMDKeepAlive(); + if (blheli.ack != ACK_OK) { + setDisconnected(); + } + uint8_t b = 0; + blheli_send_reply(&b, 1); + break; + } + case cmd_ProtocolGetVersion: { + hal.console->printf("cmd_ProtocolGetVersion\n"); + uint8_t buf[1]; + buf[0] = SERIAL_4WAY_PROTOCOL_VER; + blheli_send_reply(buf, sizeof(buf)); + break; + } + case cmd_InterfaceGetName: { + hal.console->printf("cmd_InterfaceGetName\n"); + uint8_t buf[5] = { 4, 'A', 'R', 'D', 'U' }; + blheli_send_reply(buf, sizeof(buf)); + break; + } + case cmd_InterfaceGetVersion: { + hal.console->printf("cmd_InterfaceGetVersion\n"); + uint8_t buf[2] = { SERIAL_4WAY_VERSION_HI, SERIAL_4WAY_VERSION_LO }; + blheli_send_reply(buf, sizeof(buf)); + break; + } + case cmd_InterfaceExit: { + hal.console->printf("cmd_InterfaceExit\n"); + msp.escMode = PROTOCOL_NONE; + uint8_t b = 0; + blheli_send_reply(&b, 1); + hal.rcout->serial_end(); + break; + } + case cmd_DeviceReset: { + hal.console->printf("cmd_DeviceReset(%u)\n", unsigned(blheli.buf[0])); + blheli.chan = blheli.buf[0]; + switch (blheli.interface_mode) { + case imSIL_BLB: + case imATM_BLB: + case imARM_BLB: + BL_SendCMDRunRestartBootloader(); + break; + case imSK: + break; + } + blheli_send_reply(&blheli.chan, 1); + setDisconnected(); + break; + } + + case cmd_DeviceInitFlash: { + hal.console->printf("cmd_DeviceInitFlash(%u)\n", unsigned(blheli.buf[0])); + blheli.chan = blheli.buf[0]; + for (uint8_t tries=0; tries<5; tries++) { + blheli.ack = ACK_OK; + setDisconnected(); + if (BL_ConnectEx()) { + break; + } + hal.scheduler->delay(5); + } + uint8_t buf[4] = {blheli.deviceInfo[0], + blheli.deviceInfo[1], + blheli.deviceInfo[2], + blheli.deviceInfo[3]}; // device ID + blheli_send_reply(buf, sizeof(buf)); + break; + } + + case cmd_InterfaceSetMode: { + hal.console->printf("cmd_InterfaceSetMode(%u)\n", unsigned(blheli.buf[0])); + blheli.interface_mode = blheli.buf[0]; + blheli_send_reply(&blheli.interface_mode, 1); + break; + } + + case cmd_DeviceRead: { + uint16_t nbytes = blheli.buf[0]?blheli.buf[0]:256; + hal.console->printf("cmd_DeviceRead(%u) n=%u\n", blheli.chan, nbytes); + uint8_t buf[nbytes]; + if (!BL_ReadA(CMD_READ_FLASH_SIL, buf, nbytes)) { + nbytes = 1; + } + blheli_send_reply(buf, nbytes); + break; + } + + case cmd_DevicePageErase: { + uint8_t page = blheli.buf[0]; + hal.console->printf("cmd_DevicePageErase(%u) im=%u\n", page, blheli.interface_mode); + switch (blheli.interface_mode) { + case imSIL_BLB: + case imARM_BLB: { + if (blheli.interface_mode == imARM_BLB) { + // Address =Page * 1024 + blheli.address = page << 10; + } else { + // Address =Page * 512 + blheli.address = page << 9; + } + hal.console->printf("ARM PageErase 0x%04x\n", blheli.address); + BL_PageErase(); + blheli.address = 0; + blheli_send_reply(&page, 1); + break; + } + default: + blheli.ack = ACK_I_INVALID_CMD; + blheli_send_reply(&page, 1); + break; + } + break; + } + + case cmd_DeviceWrite: { + uint16_t nbytes = blheli.param_len; + hal.console->printf("cmd_DeviceWrite n=%u im=%u\n", nbytes, blheli.interface_mode); + uint8_t buf[nbytes]; + memcpy(buf, blheli.buf, nbytes); + switch (blheli.interface_mode) { + case imSIL_BLB: + case imATM_BLB: + case imARM_BLB: { + BL_WriteFlash(buf, nbytes); + break; + } + case imSK: { + hal.console->printf("Unsupported flash mode imSK\n"); + break; + } + } + uint8_t b=0; + blheli_send_reply(&b, 1); + break; + } + + case cmd_DeviceVerify: { + uint16_t nbytes = blheli.param_len; + hal.console->printf("cmd_DeviceWrite n=%u im=%u\n", nbytes, blheli.interface_mode); + switch (blheli.interface_mode) { + case imARM_BLB: { + uint8_t buf[nbytes]; + memcpy(buf, blheli.buf, nbytes); + BL_VerifyFlash(buf, nbytes); + break; + } + default: + blheli.ack = ACK_I_INVALID_CMD; + break; + } + uint8_t b=0; + blheli_send_reply(&b, 1); + break; + } + + case cmd_DeviceEraseAll: + case cmd_DeviceC2CK_LOW: + case cmd_DeviceReadEEprom: + case cmd_DeviceWriteEEprom: + default: + // ack=unknown command + blheli.ack = ACK_I_INVALID_CMD; + hal.console->printf("Unknown BLHeli protocol 0x%02x\n", blheli.command); + uint8_t b = 0; + blheli_send_reply(&b, 1); + break; + } +} + +static void test_dshot_out(void) +{ + hal.rcout->cork(); + for (uint8_t i=0; i<6; i++) { + hal.rcout->enable_ch(i); + hal.rcout->write(i, 1100+i*100); + } + hal.rcout->push(); +} + + +static void MSP_protocol_update(void) +{ + uint32_t n = hal.uartC->available(); + if (n > 0) { + for (uint32_t i=0; iread(); + if (b < 0) { + break; + } + if (msp.escMode == PROTOCOL_4WAY && blheli.state == BLHELI_IDLE && b == '$') { + hal.console->printf("Change to MSP mode\n"); + msp.escMode = PROTOCOL_NONE; + hal.rcout->serial_end(); + } + if (msp.escMode != PROTOCOL_4WAY && msp.state == MSP_IDLE && b == '/') { + hal.console->printf("Change to BLHeli mode\n"); + msp.escMode = PROTOCOL_4WAY; + } + if (msp.escMode == PROTOCOL_4WAY) { + blheli_4way_process_byte(b); + } else { + msp_process_byte(b); + } + } + } + if (msp.escMode == PROTOCOL_4WAY) { + if (blheli.state == BLHELI_COMMAND_RECEIVED) { + blheli_process_command(); + blheli.state = BLHELI_IDLE; + msp.state = MSP_IDLE; + } + } else if (msp.state == MSP_COMMAND_RECEIVED) { + if (msp.packetType == MSP_PACKET_COMMAND) { + msp_process_command(); + } + msp.state = MSP_IDLE; + blheli.state = BLHELI_IDLE; + } + + static uint32_t last_tick_ms; + uint32_t now = AP_HAL::millis(); + if (now - last_tick_ms > 1000) { + hal.console->printf("tick\n"); + last_tick_ms = now; + while (hal.console->available() > 0) { + hal.console->read(); + } +#if 0 + BL_ConnectEx(); + hal.scheduler->delay(5); + blheli.chan = 0; + blheli.address = 0x7c00; + BL_ReadA(CMD_READ_FLASH_SIL, blheli.buf, 256); +#endif + } +} + + +static void test_output_modes(void) +{ + static uint8_t mode; + + hal.console->printf("Test mode %u\n", mode); + + switch (mode) { + case 0: + // test normal + hal.rcout->set_output_mode(0xF, AP_HAL::RCOutput::MODE_PWM_NORMAL); + hal.rcout->set_freq(0xF, 250); + break; + + case 1: + // test oneshot + hal.rcout->set_output_mode(0xF, AP_HAL::RCOutput::MODE_PWM_ONESHOT); + hal.rcout->set_freq(0xF, 400); + break; + + case 2: + // test brushed + hal.rcout->set_output_mode(0xF, AP_HAL::RCOutput::MODE_PWM_BRUSHED); + hal.rcout->set_freq(0xF, 16000); + break; + + case 3: + // test dshot150 + hal.rcout->set_output_mode(0xF, AP_HAL::RCOutput::MODE_PWM_DSHOT150); + break; + + case 4: + // test dshot300 + hal.rcout->set_output_mode(0xF, AP_HAL::RCOutput::MODE_PWM_DSHOT300); + break; + + case 5: + // test dshot600 + hal.rcout->set_output_mode(0xF, AP_HAL::RCOutput::MODE_PWM_DSHOT600); + break; + + case 6: + // test dshot1200 + hal.rcout->set_output_mode(0xF, AP_HAL::RCOutput::MODE_PWM_DSHOT1200); + break; + + case 7: + // test serial + hal.rcout->serial_setup_output(0, 19200); + hal.scheduler->delay(10); + hal.rcout->serial_write_bytes((const uint8_t *)"Hello World", 12); + hal.scheduler->delay(10); + hal.rcout->serial_end(); + hal.scheduler->delay(10); + break; + + case 8: + hal.rcout->set_output_mode(0xF, AP_HAL::RCOutput::MODE_PWM_NONE); + break; + + default: + break; + } + + hal.rcout->cork(); + hal.rcout->enable_ch(0); + hal.rcout->enable_ch(4); + hal.rcout->enable_ch(5); + hal.rcout->write(0, 1100 + mode*100); + hal.rcout->write(4, 1000 + mode*100); + hal.rcout->write(5, 1000 + mode*100); + hal.rcout->push(); + + mode = (mode+1) % 9; +} + +void setup(void) { + hal.console->begin(115200); + hal.scheduler->delay(1000); + hal.console->printf("Starting\n"); + hal.uartC->begin(115200, 256, 256); + hal.rcout->init(); + hal.rcout->set_esc_scaling(1000, 2000); + hal.rcout->cork(); + for (uint8_t i=0; ienable_ch(i); + hal.rcout->write(i, 1000); + } + hal.rcout->push(); } void loop(void) { - hal.console->printf("tick\n"); - test_dshot_send(); - hal.scheduler->delay(1); - test_dshot_cleanup(); - hal.scheduler->delay(1000); + hal.scheduler->delay(200); + //test_dshot_out(); + //MSP_protocol_update(); + test_output_modes(); + // allow for firmware upload without power cycling for rapid + // development if (hal.console->available() > 10) { + hal.console->printf("rebooting\n"); + hal.scheduler->delay(1000); hal.scheduler->reboot(false); } } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 34efd35fe6..c2c2bc47bc 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -219,7 +219,8 @@ class generic_pin(object): if (self.type.startswith('USART') or self.type.startswith('UART')) and ( (self.label.endswith('_TX') or - self.label.endswith('_RX'))): + self.label.endswith('_RX') or + self.label.endswith('_CTS'))): # default RX/TX lines to pullup, to prevent spurious bytes # on disconnected ports. CTS is the exception, which is pulldown if self.label.endswith("CTS"): @@ -667,6 +668,8 @@ def write_PWM_config(f): 'PWM_OUTPUT_DISABLED', 'PWM_OUTPUT_DISABLED', 'PWM_OUTPUT_DISABLED', 'PWM_OUTPUT_DISABLED' ] + alt_functions = [ 0, 0, 0, 0 ] + pal_lines = [ '0', '0', '0', '0' ] for p in pwm_out: if p.type != t: continue @@ -679,6 +682,8 @@ def write_PWM_config(f): pwm = p.extra_value('PWM', type=int) chan_list[chan - 1] = pwm - 1 chan_mode[chan - 1] = 'PWM_OUTPUT_ACTIVE_HIGH' + alt_functions[chan - 1] = p.af + pal_lines[chan - 1] = 'PAL_LINE(GPIO%s, %uU)' % (p.port, p.pin) groups.append('HAL_PWM_GROUP%u' % group) if n in [1, 8]: # only the advanced timers do 8MHz clocks @@ -706,10 +711,16 @@ def write_PWM_config(f): {%s, NULL}, \\ {%s, NULL} \\ }, 0, 0}, &PWMD%u, \\ - HAL_PWM%u_DMA_CONFIG }\n''' % - (group, advanced_timer, chan_list[0], chan_list[1], - chan_list[2], chan_list[3], pwm_clock, period, chan_mode[0], - chan_mode[1], chan_mode[2], chan_mode[3], n, n)) + HAL_PWM%u_DMA_CONFIG, \\ + { %u, %u, %u, %u }, \\ + { %s, %s, %s, %s }}\n''' % + (group, advanced_timer, + chan_list[0], chan_list[1], chan_list[2], chan_list[3], + pwm_clock, period, + chan_mode[0], chan_mode[1], chan_mode[2], chan_mode[3], + n, n, + alt_functions[0], alt_functions[1], alt_functions[2], alt_functions[3], + pal_lines[0], pal_lines[1], pal_lines[2], pal_lines[3])) f.write('#define HAL_PWM_GROUPS %s\n\n' % ','.join(groups)) @@ -756,10 +767,12 @@ def write_GPIO_config(f): pin = p.pin gpios.append((gpio, pwm, port, pin, p)) gpios = sorted(gpios) + for (gpio, pwm, port, pin, p) in gpios: + f.write('#define HAL_GPIO_LINE_GPIO%u PAL_LINE(GPIO%s, %2uU)\n' % (gpio, port, pin)) f.write('#define HAL_GPIO_PINS { \\\n') for (gpio, pwm, port, pin, p) in gpios: - f.write('{ %3u, true, %2u, PAL_LINE(GPIO%s, %2uU) }, /* %s */ \\\n' % - (gpio, pwm, port, pin, p)) + f.write('{ %3u, true, %2u, PAL_LINE(GPIO%s, %2uU), EXT_MODE_GPIO%s }, /* %s */ \\\n' % + (gpio, pwm, port, pin, port, p)) # and write #defines for use by config code f.write('}\n\n') f.write('// full pin define list\n') diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412-rev1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412-rev1/hwdef.dat index aa67575e2a..14e3560d35 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412-rev1/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412-rev1/hwdef.dat @@ -82,7 +82,7 @@ PB6 LEDR OUTPUT HIGH GPIO(1) PB5 TIM3_CH2 TIM3 PWM(2) # marked CN15 front-right -PD2 RADIO_IRQ INPUT +PD2 RADIO_IRQ INPUT GPIO(100) # USART3 is sonix PC11 USART3_RX USART3 @@ -117,9 +117,8 @@ define HAL_CHIBIOS_ARCH_F412 1 define HAL_INS_DEFAULT HAL_INS_ICM20789_SPI define HAL_INS_DEFAULT_ROTATION ROTATION_NONE -# radio IRQ is on PD2 -define HAL_GPIO_RADIO_IRQ 2 -define HAL_GPIO_INTERRUPT_PORT EXT_MODE_GPIOD +# radio IRQ is on GPIO(100) +define HAL_GPIO_RADIO_IRQ 100 define HAL_RCINPUT_WITH_AP_RADIO 1 define STORAGE_FLASH_PAGE 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/hwdef.dat index 4e38dc5469..6d0a9bb870 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/hwdef.dat @@ -61,7 +61,7 @@ PA6 SPI1_MISO SPI1 PA7 SPI1_MOSI SPI1 PC4 RADIO_CE OUTPUT PC5 RADIO_PA_CTL OUTPUT -PB0 RADIO_IRQ INPUT +PB0 RADIO_IRQ INPUT GPIO(100) PB1 BATT_MON ADC1 SCALE(11) PB2 BOOT1 INPUT PB10 I2C2_SCL I2C2 @@ -109,9 +109,8 @@ define HAL_INS_DEFAULT_ROTATION ROTATION_NONE define HAL_INS_MPU60x0_I2C_BUS 1 define HAL_INS_MPU60x0_I2C_ADDR 0x68 -# radio IRQ is on PB0 -define HAL_GPIO_RADIO_IRQ 0 -define HAL_GPIO_INTERRUPT_PORT EXT_MODE_GPIOB +# radio IRQ is on GPIO(100) +define HAL_GPIO_RADIO_IRQ 100 define HAL_RCINPUT_WITH_AP_RADIO 1 define STORAGE_FLASH_PAGE 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat index fcfa7777e4..173cd2680c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat @@ -33,7 +33,11 @@ UART_ORDER OTG1 UART4 USART2 define HAL_RCINPUT_WITH_AP_RADIO 1 define HAL_GPIO_RADIO_RESET 1 // PB0 GPIO from FMU3 -define HAL_GPIO_RADIO_IRQ 15 // PD15 + +# setup for radio IRQ on PD15 +undef PD15 +PD15 MPU_DRDY INPUT GPIO(100) +define HAL_GPIO_RADIO_IRQ 100 # setup defines for ArduCopter config define TOY_MODE_ENABLED ENABLED