diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 68d68aa42e..49c7924656 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -399,7 +399,7 @@ void RCOutput::push_local(void) pwmEnableChannel(group.pwm_drv, j, width); } #ifndef DISABLE_DSHOT - else if (is_dshot_protocol(group.current_mode) || group.current_mode == MODE_NEOPIXEL) { + else if (is_dshot_protocol(group.current_mode) || group.current_mode == MODE_NEOPIXEL || group.current_mode == MODE_PROFILED) { // set period_us to time for pulse output, to enable very fast rates period_us = dshot_pulse_time_us; } @@ -410,6 +410,7 @@ void RCOutput::push_local(void) if (group.current_mode == MODE_PWM_ONESHOT || group.current_mode == MODE_PWM_ONESHOT125 || group.current_mode == MODE_NEOPIXEL || + group.current_mode == MODE_PROFILED || is_dshot_protocol(group.current_mode)) { need_trigger |= (1U<lock_nonblock()) { - // doing serial output, don't send Neopixel pulses - return; + // doing serial output, don't send Serial LED pulses + return false; } // start sending the pulses out @@ -1096,6 +1108,7 @@ void RCOutput::neopixel_send(pwm_group &group) group.last_dmar_send_us = AP_HAL::micros64(); #endif //#ifndef DISABLE_DSHOT + return true; } @@ -1666,6 +1679,8 @@ uint32_t RCOutput::protocol_bitrate(const enum output_mode mode) const return 1200000; case MODE_NEOPIXEL: return 800000; + case MODE_PROFILED: + return 1500000; // experiment winding this up 3000000 max from data sheet default: // use 1 to prevent a possible divide-by-zero return 1; @@ -1673,23 +1688,44 @@ uint32_t RCOutput::protocol_bitrate(const enum output_mode mode) const } /* - setup neopixel (WS2812B) output for a given channel number, with + setup serial led output for a given channel number, with the given max number of LEDs in the chain. */ -bool RCOutput::set_neopixel_num_LEDs(const uint16_t chan, uint8_t num_leds) +bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode, uint16_t clock_mask) { uint8_t i; pwm_group *grp = find_chan(chan, i); if (!grp) { return false; } - if (grp->neopixel_nleds == num_leds && grp->current_mode == MODE_NEOPIXEL) { - // no change - return true; + + switch (mode) { + case MODE_NEOPIXEL: { + grp->serial_nleds = MAX(num_leds, grp->serial_nleds); + break; + } + case MODE_PROFILED: { + // ProfiLED requires two dummy LED's to mark end of transmission + grp->serial_nleds = MAX(num_leds + 2, grp->serial_nleds); + + // Enable any clock channels in the same group + grp->clock_mask = 0; + for (uint8_t j = 0; j < 4; j++) { + if ((clock_mask & (1U<<(grp->chan[j] + chan_offset))) != 0) { + grp->clock_mask |= 1U<neopixel_nleds = MAX(num_leds, grp->neopixel_nleds); - set_output_mode(1U<current_mode == MODE_NEOPIXEL; + + set_output_mode(1U<current_mode == mode; } /* @@ -1712,36 +1748,127 @@ void RCOutput::_set_neopixel_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, } /* - setup neopixel (WS2812B) output data for a given output channel + ProfiLED frame for a given output channel + channel is active high and bits inverted to get clock rising edge away from data rising edge +*/ +void RCOutput::_set_profiled_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue) +{ + const uint8_t pad_start_bits = 1; + const uint8_t bit_length = 25; + const uint8_t stride = 4; + uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; + uint32_t bits = 0x1000000 | (blue<<16) | (red<<8) | green; + const uint32_t BIT_1 = 14 * grp->bit_width_mul; + for (uint16_t b=0; b < bit_length; b++) { + buf[b * stride] = (bits & 0x1000000) ? 0 : BIT_1; + bits <<= 1; + } +} + +/* + ProfiLED blank frame for a given output channel + channel is active high and bits inverted to get clock rising edge away from data rising edge +*/ +void RCOutput::_set_profiled_blank_frame(pwm_group *grp, uint8_t idx, uint8_t led) +{ + const uint8_t pad_start_bits = 1; + const uint8_t bit_length = 25; + const uint8_t stride = 4; + uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; + const uint32_t BIT_1 = 14 * grp->bit_width_mul; + for (uint16_t b=0; b < bit_length; b++) { + buf[b * stride] = BIT_1; + } +} + +/* + setup ProfiLED clock frame for a given output channel +*/ +void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led) +{ + const uint8_t pad_start_bits = 1; + const uint8_t bit_length = 25; + const uint8_t stride = 4; + uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; + const uint32_t BIT_1 = 7 * grp->bit_width_mul; + for (uint16_t b=0; b < bit_length; b++) { + buf[b * stride] = BIT_1; + } +} + + +/* + setup serial LED output data for a given output channel and a LED number. LED -1 is all LEDs */ -void RCOutput::set_neopixel_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue) +void RCOutput::set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue) { uint8_t i; pwm_group *grp = find_chan(chan, i); if (!grp) { return; } + + if (led >= grp->serial_nleds || (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED)) { + return; + } + if (led == -1) { - for (uint8_t n=0; nneopixel_nleds; n++) { - set_neopixel_rgb_data(chan, n, red, green, blue); + grp->prepared_send = true; + for (uint8_t n=0; nserial_nleds; n++) { + set_serial_led_rgb_data(chan, n, red, green, blue); } return; + } else if (!grp->prepared_send) { + // if not ouput clock and trailing frames, run through all LED's to do it now + set_serial_led_rgb_data(chan, -1, 0, 0, 0); } - if (led < -1 || led >= grp->neopixel_nleds) { - return; - } + switch (grp->current_mode) { + case MODE_NEOPIXEL: { + _set_neopixel_rgb_data(grp, i, uint8_t(led), red, green, blue); + break; + } - _set_neopixel_rgb_data(grp, i, uint8_t(led), red, green, blue); + case MODE_PROFILED: { + if (led < grp->serial_nleds - 2) { + _set_profiled_rgb_data(grp, i, uint8_t(led), red, green, blue); + } else { + _set_profiled_blank_frame(grp, i, uint8_t(led)); + } + + for (uint8_t j = 0; j < 4; j++) { + if ((grp->clock_mask & 1U<current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED) { + return; + } + if (grp->prepared_send) { + serial_led_pending = true; + grp->serial_led_pending = true; + } } #endif // HAL_USE_PWM diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 973a32963a..78af21d86b 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -157,21 +157,21 @@ public: } /* - setup neopixel (WS2812B) output for a given channel number, with + setup serial LED output for a given channel number, with the given max number of LEDs in the chain. */ - bool set_neopixel_num_LEDs(const uint16_t chan, uint8_t num_leds) override; + bool set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode = MODE_PWM_NONE, uint16_t clock_mask = 0) override; /* - setup neopixel (WS2812B) output data for a given output channel + setup serial LED output data for a given output channel and LEDs number. LED -1 is all LEDs */ - void set_neopixel_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue) override; + void set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue) override; /* - trigger send of neopixel data + trigger send of serial LED data */ - void neopixel_send(void) override; + void serial_led_send(const uint16_t chan) override; private: struct pwm_group { @@ -201,7 +201,10 @@ private: bool in_serial_dma; uint64_t last_dmar_send_us; virtual_timer_t dma_timeout; - uint8_t neopixel_nleds; + uint8_t serial_nleds; + uint8_t clock_mask; + bool serial_led_pending; + bool prepared_send; // serial output struct { @@ -336,10 +339,11 @@ private: uint16_t telem_request_mask; /* - NeoPixel handling. Max of 32 LEDs uses max 12k of memory per group + Serial lED handling. Max of 32 LEDs uses max 12k of memory per group + return true if send was successful */ - void neopixel_send(pwm_group &group); - bool neopixel_pending; + bool serial_led_send(pwm_group &group); + bool serial_led_pending; void dma_allocate(Shared_DMA *ctx); void dma_deallocate(Shared_DMA *ctx); @@ -357,10 +361,16 @@ private: /* setup neopixel (WS2812B) output data for a given output channel - and LEDs number. LED -1 is all LEDs */ void _set_neopixel_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue); + /* + setup ProfiLED output data for a given output channel + */ + void _set_profiled_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue); + void _set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led); + void _set_profiled_blank_frame(pwm_group *grp, uint8_t idx, uint8_t led); + // serial output support static const eventmask_t serial_event_mask = EVENT_MASK(1); bool serial_write_byte(uint8_t b);