Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_HAL_ChibiOS/RCOutput.h

710 lines
23 KiB
C++
Raw Blame History

/*
* 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 <http://www.gnu.org/licenses/>.
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
*/
#pragma once
#include <hal.h>
#include "AP_HAL_ChibiOS.h"
#include <AP_HAL/Semaphores.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include "shared_dma.h"
#if HAL_USE_PWM == TRUE
#if defined(IOMCU_FW)
typedef uint8_t dmar_uint_t; // save memory to allow dshot on IOMCU
#else
typedef uint32_t dmar_uint_t;
#endif
#define RCOU_DSHOT_TIMING_DEBUG 0
class ChibiOS::RCOutput : public AP_HAL::RCOutput
#ifdef HAL_WITH_BIDIR_DSHOT
, AP_ESC_Telem_Backend
#endif
{
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;
void enable_ch(uint8_t ch) override;
void disable_ch(uint8_t ch) override;
void write(uint8_t ch, uint16_t period_us) override;
uint16_t read(uint8_t ch) override;
void read(uint16_t* period_us, uint8_t len) override;
uint16_t read_last_sent(uint8_t ch) override;
void read_last_sent(uint16_t* period_us, uint8_t len) override;
// surface dshot telemetry for use by the harmonic notch and status information
#ifdef HAL_WITH_BIDIR_DSHOT
uint16_t get_erpm(uint8_t chan) const override { return _bdshot.erpm[chan]; }
float get_erpm_error_rate(uint8_t chan) const override {
return 100.0f * float(_bdshot.erpm_errors[chan]) / (1 + _bdshot.erpm_errors[chan] + _bdshot.erpm_clean_frames[chan]);
}
#endif
void set_output_mode(uint32_t mask, const enum output_mode mode) override;
enum output_mode get_output_mode(uint32_t& mask) override;
bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const override;
/*
* return mask of channels that must be disabled because they share a group with a digital channel
*/
uint32_t get_disabled_channels(uint32_t digital_mask) override;
void cork(void) override;
void push(void) override;
/*
force the safety switch on, disabling PWM output from the IO board
*/
bool force_safety_on(void) override;
/*
force the safety switch off, enabling PWM output from the IO board
*/
void force_safety_off(void) override;
bool enable_px4io_sbus_out(uint16_t rate_hz) override;
/*
set default update rate
*/
void set_default_rate(uint16_t rate_hz) override;
/*
timer push (for oneshot min rate)
*/
void timer_tick(uint64_t last_run_us);
/*
LED push
*/
void led_timer_tick(uint64_t last_run_us);
#if defined(IOMCU_FW) && HAL_DSHOT_ENABLED
void timer_tick() override;
static void dshot_send_trampoline(void *p);
#endif
/*
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
*/
#if HAL_SERIAL_ESC_COMM_ENABLED
bool setup_serial_output(uint32_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, uint32_t motor_mask) 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;
#endif
/*
enable telemetry request for a mask of channels. This is used
with Dshot to get telemetry feedback
The mask uses servo channel numbering
*/
void set_telem_request_mask(uint32_t mask) override;
#ifdef HAL_WITH_BIDIR_DSHOT
/*
enable bi-directional telemetry request for a mask of channels. This is used
with Dshot to get telemetry feedback
The mask uses servo channel numbering
*/
void set_bidir_dshot_mask(uint32_t mask) override;
void set_motor_poles(uint8_t poles) override { _bdshot.motor_poles = poles; }
#endif
/*
Set the dshot rate as a multiple of the loop rate
*/
void set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) override;
#if defined(IOMCU_FW)
/*
Get/Set the dshot period in us, only for use by the IOMCU
*/
void set_dshot_period(uint32_t period_us, uint8_t dshot_rate) override {
_dshot_period_us = period_us;
_dshot_rate = dshot_rate;
}
uint32_t get_dshot_period_us() const override { return _dshot_period_us; }
#endif
#if HAL_DSHOT_ENABLED
/*
Set/get the dshot esc_type
*/
void set_dshot_esc_type(DshotEscType dshot_esc_type) override;
DshotEscType get_dshot_esc_type() const override { return _dshot_esc_type; }
#endif
/*
get safety switch state, used by Util.cpp
*/
AP_HAL::Util::safety_state _safety_switch_state(void);
/*
set PWM to send to a set of channels if the FMU firmware dies
*/
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
/*
set safety mask for IOMCU
*/
void set_safety_mask(uint32_t mask) { safety_mask = mask; }
#if HAL_DSHOT_ENABLED
/*
* 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.
* The mask uses servo channel numbering
*/
void set_reversible_mask(uint32_t chanmask) override;
/*
* mark the channels in chanmask as reversed.
* The chanmask passed is added (ORed) into any existing mask.
* The mask uses servo channel numbering
*/
void set_reversed_mask(uint32_t chanmask) override;
uint32_t get_reversed_mask() override { return _reversed_mask; }
/*
mark escs as active for the purpose of sending dshot commands
The mask uses servo channel numbering
*/
void set_active_escs_mask(uint32_t chanmask) override { _active_escs_mask |= (chanmask >> chan_offset); }
/*
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 = ALL_CHANNELS, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) override;
/*
* Update channel masks at 1Hz allowing for actions such as dshot commands to be sent
*/
void update_channel_masks() override;
/*
* Allow channel mask updates to be temporarily suspended
*/
void disable_channel_mask_updates() override { _disable_channel_mask_updates = true; }
void enable_channel_mask_updates() override { _disable_channel_mask_updates = false; }
#endif
/*
setup serial LED output for a given channel number, with
the given max number of LEDs in the chain.
*/
#if HAL_SERIALLED_ENABLED
bool set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode = MODE_PWM_NONE, uint32_t clock_mask = 0) override;
/*
setup serial LED output data for a given output channel
and LEDs number. LED -1 is all LEDs
*/
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 serial LED data
*/
void serial_led_send(const uint16_t chan) override;
#endif
/*
rcout thread
*/
void rcout_thread();
/*
timer information
*/
void timer_info(ExpandingString &str) override;
private:
enum class DshotState {
IDLE = 0,
SEND_START = 1,
SEND_COMPLETE = 2,
RECV_START = 3,
RECV_COMPLETE = 4
};
struct PACKED SerialLed {
uint8_t red;
uint8_t green;
uint8_t blue;
};
/*
DShot handling
*/
// the pre-bit is needed with TIM5, or we can get some corrupt frames
static const uint8_t dshot_pre = 1;
static const uint8_t dshot_post = 2;
static const uint16_t dshot_bit_length = 16 + dshot_pre + dshot_post;
static const uint16_t DSHOT_BUFFER_LENGTH = dshot_bit_length * 4 * sizeof(dmar_uint_t);
static const uint16_t MIN_GCR_BIT_LEN = 7;
static const uint16_t MAX_GCR_BIT_LEN = 22;
static const uint16_t GCR_TELEMETRY_BIT_LEN = MAX_GCR_BIT_LEN;
static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(uint32_t);
struct pwm_group {
// only advanced timers can do high clocks needed for more than 400Hz
bool advanced_timer;
uint8_t chan[4]; // chan number, zero based, 255 for disabled
PWMConfig pwm_cfg;
PWMDriver* pwm_drv;
uint8_t timer_id;
bool have_up_dma; // can we do DMAR outputs for DShot?
uint8_t dma_up_stream_id;
uint8_t dma_up_channel;
#ifdef HAL_WITH_BIDIR_DSHOT
struct {
bool have_dma;
uint8_t stream_id;
uint8_t channel;
} dma_ch[4];
#endif
uint8_t alt_functions[4];
ioline_t pal_lines[4];
// below this line is not initialised by hwdef.h
enum output_mode current_mode;
uint16_t frequency_hz;
// mask of channels that are able to be enabled
uint32_t ch_mask;
// mask of channels that are enabled and active
uint32_t en_mask;
const stm32_dma_stream_t *dma;
#if AP_HAL_SHARED_DMA_ENABLED
Shared_DMA *dma_handle;
#endif
dmar_uint_t *dma_buffer;
uint16_t dma_buffer_len;
bool pwm_started;
uint32_t bit_width_mul;
uint32_t rc_frequency;
bool in_serial_dma;
uint64_t last_dmar_send_us;
uint64_t dshot_pulse_time_us;
uint64_t dshot_pulse_send_time_us;
virtual_timer_t dma_timeout;
#if HAL_SERIALLED_ENABLED
// serial LED support
volatile uint8_t serial_nleds;
uint8_t clock_mask;
enum output_mode led_mode;
volatile bool serial_led_pending;
volatile bool prepared_send;
HAL_Semaphore serial_led_mutex;
// structure to hold serial LED data until it can be transferred
// to the DMA buffer
SerialLed* serial_led_data[4];
#endif
eventmask_t dshot_event_mask;
thread_t* dshot_waiter;
#if HAL_SERIAL_ESC_COMM_ENABLED
// serial output
struct {
// expected time per bit
uint16_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;
#endif
// support for bi-directional dshot
volatile DshotState dshot_state;
#ifdef HAL_WITH_BIDIR_DSHOT
struct {
uint16_t erpm[4];
volatile bool enabled;
const stm32_dma_stream_t *ic_dma[4];
uint16_t dma_tx_size; // save tx value from last read
Shared_DMA *ic_dma_handle[4];
uint8_t telem_tim_ch[4];
uint8_t curr_telem_chan;
uint8_t prev_telem_chan;
uint16_t telempsc;
uint32_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN];
#if RCOU_DSHOT_TIMING_DEBUG
uint16_t telem_rate[4];
uint16_t telem_err_rate[4];
uint64_t last_print; // debug
#endif
} bdshot;
// do we have an input capture dma channel
bool has_ic_dma() const {
return bdshot.ic_dma_handle[bdshot.curr_telem_chan] != nullptr;
}
bool has_shared_ic_up_dma() const {
return bdshot.ic_dma_handle[bdshot.curr_telem_chan] == dma_handle;
}
// is input capture currently enabled
bool ic_dma_enabled() const {
return bdshot.enabled && has_ic_dma() && bdshot.ic_dma[bdshot.curr_telem_chan] != nullptr;
}
bool has_ic() const {
return has_ic_dma() || has_shared_ic_up_dma();
}
// do we have any kind of input capture
bool ic_enabled() const {
return bdshot.enabled && has_ic();
}
#endif // HAL_WITH_BIDIR_DSHOT
// are we safe to send another pulse?
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 && (en_mask & (1U << chan[c]));
}
};
/*
timer thread for use by dshot events
*/
thread_t *rcout_thread_ctx;
#if HAL_SERIALLED_ENABLED
/*
timer thread for use by led events
*/
thread_t *led_thread_ctx;
/*
mutex to control LED thread creation
*/
HAL_Semaphore led_thread_sem;
bool led_thread_created;
#endif
#if HAL_SERIAL_ESC_COMM_ENABLED
/*
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
uint16_t byte_start_tick;
// 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 in micros
uint16_t bit_time_tick;
// the bit value of the last bit received
uint8_t last_bit;
// thread waiting for byte to be read
thread_t *waiter;
// timeout for byte read
virtual_timer_t serial_timeout;
bool timed_out;
} irq;
// the group being used for serial output
struct pwm_group *serial_group;
thread_t *serial_thread;
tprio_t serial_priority;
#endif // HAL_SERIAL_ESC_COMM_ENABLED
static bool soft_serial_waiting() {
#if HAL_SERIAL_ESC_COMM_ENABLED
return irq.waiter != nullptr;
#else
return false;
#endif
}
bool in_soft_serial() const {
#if HAL_SERIAL_ESC_COMM_ENABLED
return serial_group != nullptr;
#else
return false;
#endif
}
static pwm_group pwm_group_list[];
static const uint8_t NUM_GROUPS;
// offset of first local channel
uint8_t chan_offset;
// total number of channels on FMU
uint8_t num_fmu_channels;
// number of active fmu channels
uint8_t active_fmu_channels;
#if NUM_SERVO_CHANNELS >= 17
static const uint8_t max_channels = 32;
#else
static const uint8_t max_channels = 16;
#endif
// last sent values are for all channels
uint16_t last_sent[max_channels];
// 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 {
uint32_t mask;
uint16_t erpm[max_channels];
#ifdef HAL_WITH_BIDIR_DSHOT
uint16_t erpm_errors[max_channels];
uint16_t erpm_clean_frames[max_channels];
uint32_t erpm_last_stats_ms[max_channels];
uint8_t motor_poles;
#endif
} _bdshot;
// dshot period
uint32_t _dshot_period_us = 400;
// dshot rate as a multiple of loop rate or 0 for 1Khz
uint8_t _dshot_rate;
// dshot periods since the last push()
uint8_t _dshot_cycle;
// virtual timer for post-push() pulses
virtual_timer_t _dshot_rate_timer;
#if HAL_DSHOT_ENABLED
// dshot commands
// RingBuffer to store outgoing request.
struct DshotCommandPacket {
uint8_t command;
uint32_t cycle;
uint8_t chan;
};
ObjectBuffer<DshotCommandPacket> _dshot_command_queue{8};
DshotCommandPacket _dshot_current_command;
DshotEscType _dshot_esc_type;
// control updates to channel masks
bool _disable_channel_mask_updates;
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 // HAL_DSHOT_ENABLED
bool corked;
// mask of channels that are running in high speed
uint32_t fast_channel_mask;
uint32_t io_fast_channel_mask;
// mask of channels that are 3D capable
uint32_t _reversible_mask;
// mask of channels that should be reversed at startup
uint32_t _reversed_mask;
// mask of active ESCs
uint32_t _active_escs_mask;
// min time to trigger next pulse to prevent overlap
uint64_t min_pulse_trigger_us;
// mutex for oneshot triggering
mutex_t trigger_mutex;
// which output groups need triggering
uint8_t trigger_groupmask;
// widest pulse for oneshot triggering
uint16_t trigger_widest_pulse;
bool dshot_timer_setup;
volatile bool _initialised;
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.en_mask > 0 && (group.en_mask & _active_escs_mask) == group.en_mask;
}
// find a channel group given a channel number
struct pwm_group *find_chan(uint8_t chan, uint8_t &group_idx);
// push out values to local PWM
void push_local(void);
// trigger group pulses
void trigger_groups(void);
// setup output frequency for a group
void set_freq_group(pwm_group &group);
// safety switch state
AP_HAL::Util::safety_state safety_state;
uint32_t safety_update_ms;
uint8_t led_counter;
int8_t safety_button_counter;
uint8_t safety_press_count; // 0.1s units
// mask of channels to allow when safety on
uint32_t safety_mask;
// update safety switch and LED
void safety_update(void);
// LED thread
void led_thread();
bool start_led_thread();
uint32_t telem_request_mask;
/*
Serial lED handling. Max of 32 LEDs uses max 12k of memory per group
return true if send was successful
*/
static const eventmask_t serial_event_mask = EVENT_MASK(10);
bool serial_led_send(pwm_group &group);
void serial_led_set_single_rgb_data(pwm_group& group, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue);
void fill_DMA_buffer_serial_led(pwm_group& group);
volatile bool serial_led_pending;
void dma_allocate(Shared_DMA *ctx);
void dma_deallocate(Shared_DMA *ctx);
uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem);
void fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
void dshot_send_groups(uint64_t time_out_us);
void dshot_send(pwm_group &group, uint64_t time_out_us);
bool dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan);
static void dshot_update_tick(virtual_timer_t*, void* p);
static void dshot_send_next_group(void* p);
// release locks on the groups that are pending in reverse order
void dshot_collect_dma_locks(uint64_t last_run_us, bool led_thread = false);
static void dma_up_irq_callback(void *p, uint32_t flags);
static void dma_unlock(virtual_timer_t*, void *p);
void dma_cancel(pwm_group& group);
bool mode_requires_dma(enum output_mode mode) const;
bool setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high,
const uint16_t buffer_length, uint32_t pulse_time_us,
bool is_dshot);
void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);
void set_group_mode(pwm_group &group);
static uint32_t protocol_bitrate(const enum output_mode mode);
void print_group_setup_error(pwm_group &group, const char* error_string);
/*
Support for bi-direction dshot
*/
void bdshot_ic_dma_allocate(Shared_DMA *ctx);
void bdshot_ic_dma_deallocate(Shared_DMA *ctx);
static uint32_t bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count);
bool bdshot_decode_telemetry_from_erpm(uint16_t erpm, uint8_t chan);
bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan);
static uint8_t bdshot_find_next_ic_channel(const pwm_group& group);
static void bdshot_dma_ic_irq_callback(void *p, uint32_t flags);
static void bdshot_finish_dshot_gcr_transaction(virtual_timer_t* vt, void *p);
bool bdshot_setup_group_ic_DMA(pwm_group &group);
static void bdshot_receive_pulses_DMAR(pwm_group* group);
static void bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch);
static uint32_t bdshot_get_output_rate_hz(const enum output_mode mode);
/*
setup neopixel (WS2812B) output data for a given output channel
*/
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);
#if AP_HAL_SHARED_DMA_ENABLED
// serial output support
bool serial_write_byte(uint8_t b);
bool serial_read_byte(uint8_t &b);
void fill_DMA_buffer_byte(dmar_uint_t *buffer, uint8_t stride, uint8_t b , uint32_t bitval);
static void serial_bit_irq(void);
static void serial_byte_timeout(virtual_timer_t* vt, void *ctx);
#endif
};
#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
Reference in New Issue View Git Blame Copy Permalink