2018-01-05 02:19:51 -04:00
|
|
|
/*
|
|
|
|
* 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 "AP_HAL_ChibiOS.h"
|
2018-03-14 03:06:30 -03:00
|
|
|
#include "shared_dma.h"
|
2018-01-05 02:19:51 -04:00
|
|
|
#include "ch.h"
|
|
|
|
#include "hal.h"
|
|
|
|
|
2018-03-01 20:46:30 -04:00
|
|
|
#if HAL_USE_PWM == TRUE
|
|
|
|
|
2018-01-13 00:02:05 -04:00
|
|
|
class ChibiOS::RCOutput : public AP_HAL::RCOutput {
|
2018-01-05 02:19:51 -04:00
|
|
|
public:
|
|
|
|
void init();
|
|
|
|
void set_freq(uint32_t chmask, uint16_t freq_hz);
|
|
|
|
uint16_t get_freq(uint8_t ch);
|
|
|
|
void enable_ch(uint8_t ch);
|
|
|
|
void disable_ch(uint8_t ch);
|
|
|
|
void write(uint8_t ch, uint16_t period_us);
|
|
|
|
uint16_t read(uint8_t ch);
|
|
|
|
void read(uint16_t* period_us, uint8_t len);
|
|
|
|
uint16_t read_last_sent(uint8_t ch) override;
|
|
|
|
void read_last_sent(uint16_t* period_us, uint8_t len) override;
|
|
|
|
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override {
|
|
|
|
_esc_pwm_min = min_pwm;
|
|
|
|
_esc_pwm_max = max_pwm;
|
|
|
|
}
|
2018-04-02 04:57:45 -03:00
|
|
|
bool get_esc_scaling(uint16_t &min_pwm, uint16_t &max_pwm) override {
|
|
|
|
min_pwm = _esc_pwm_min;
|
|
|
|
max_pwm = _esc_pwm_max;
|
|
|
|
return true;
|
|
|
|
}
|
2018-03-14 03:06:30 -03:00
|
|
|
void set_output_mode(uint16_t mask, enum output_mode mode) override;
|
2018-01-05 02:19:51 -04:00
|
|
|
|
2018-02-23 04:53:56 -04:00
|
|
|
float scale_esc_to_unity(uint16_t pwm) override {
|
|
|
|
return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0;
|
|
|
|
}
|
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
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;
|
|
|
|
|
2018-04-14 00:55:03 -03:00
|
|
|
/*
|
|
|
|
set PWM to send to a set of channels when the safety switch is
|
|
|
|
in the safe state
|
|
|
|
*/
|
|
|
|
void set_safety_pwm(uint32_t chmask, uint16_t period_us) override;
|
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
bool enable_px4io_sbus_out(uint16_t rate_hz) override;
|
2018-01-12 23:53:29 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
set default update rate
|
|
|
|
*/
|
|
|
|
void set_default_rate(uint16_t rate_hz) override;
|
2018-01-17 06:25:02 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
timer push (for oneshot min rate)
|
|
|
|
*/
|
|
|
|
void timer_tick(void) override;
|
2018-03-16 18:49:40 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
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;
|
2018-04-02 01:20:58 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
enable telemetry request for a mask of channels. This is used
|
|
|
|
with DShot to get telemetry feedback
|
|
|
|
*/
|
|
|
|
void set_telem_request_mask(uint16_t mask) { telem_request_mask = (mask >> chan_offset); }
|
2018-04-14 00:55:03 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
get safety switch state, used by Util.cpp
|
|
|
|
*/
|
|
|
|
AP_HAL::Util::safety_state _safety_switch_state(void);
|
2018-01-05 02:19:51 -04:00
|
|
|
|
|
|
|
private:
|
|
|
|
struct pwm_group {
|
2018-01-12 23:53:29 -04:00
|
|
|
// only advanced timers can do high clocks needed for more than 400Hz
|
|
|
|
bool advanced_timer;
|
2018-01-12 04:29:16 -04:00
|
|
|
uint8_t chan[4]; // chan number, zero based, 255 for disabled
|
2018-01-05 02:19:51 -04:00
|
|
|
PWMConfig pwm_cfg;
|
|
|
|
PWMDriver* pwm_drv;
|
2018-03-14 03:06:30 -03:00
|
|
|
bool have_up_dma; // can we do DMAR outputs for DShot?
|
|
|
|
uint8_t dma_up_stream_id;
|
|
|
|
uint8_t dma_up_channel;
|
2018-03-16 18:49:40 -03:00
|
|
|
uint8_t alt_functions[4];
|
|
|
|
ioline_t pal_lines[4];
|
|
|
|
|
|
|
|
// below this line is not initialised by hwdef.h
|
2018-03-14 03:06:30 -03:00
|
|
|
enum output_mode current_mode;
|
2018-03-16 18:49:40 -03:00
|
|
|
uint16_t frequency_hz;
|
2018-03-14 03:06:30 -03:00
|
|
|
uint16_t ch_mask;
|
|
|
|
const stm32_dma_stream_t *dma;
|
|
|
|
Shared_DMA *dma_handle;
|
|
|
|
uint32_t *dma_buffer;
|
|
|
|
bool have_lock;
|
2018-03-16 18:49:40 -03:00
|
|
|
bool pwm_started;
|
|
|
|
uint32_t bit_width_mul;
|
|
|
|
uint32_t rc_frequency;
|
|
|
|
bool in_serial_dma;
|
2018-04-01 21:39:17 -03:00
|
|
|
uint64_t last_dshot_send_us;
|
2018-03-16 18:49:40 -03:00
|
|
|
|
|
|
|
// 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;
|
2018-01-05 02:19:51 -04:00
|
|
|
};
|
|
|
|
|
2018-03-16 18:49:40 -03:00
|
|
|
/*
|
|
|
|
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
|
2018-04-15 02:33:40 -03:00
|
|
|
systime_t byte_start_tick;
|
2018-03-16 18:49:40 -03:00
|
|
|
|
|
|
|
// 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;
|
|
|
|
|
2018-04-15 02:33:40 -03:00
|
|
|
// expected time per bit in system ticks
|
|
|
|
systime_t bit_time_tick;
|
2018-03-16 18:49:40 -03:00
|
|
|
|
|
|
|
// the bit value of the last bit received
|
|
|
|
uint8_t last_bit;
|
|
|
|
|
|
|
|
// thread waiting for byte to be read
|
|
|
|
thread_t *waiter;
|
2018-08-05 22:17:05 -03:00
|
|
|
|
|
|
|
// timeout for byte read
|
|
|
|
virtual_timer_t serial_timeout;
|
|
|
|
bool timed_out;
|
2018-03-16 18:49:40 -03:00
|
|
|
} irq;
|
|
|
|
|
|
|
|
|
|
|
|
// the group being used for serial output
|
|
|
|
struct pwm_group *serial_group;
|
2018-04-03 07:41:24 -03:00
|
|
|
thread_t *serial_thread;
|
|
|
|
tprio_t serial_priority;
|
2018-08-05 22:17:05 -03:00
|
|
|
|
2018-01-12 23:53:29 -04:00
|
|
|
static pwm_group pwm_group_list[];
|
2018-01-05 02:19:51 -04:00
|
|
|
uint16_t _esc_pwm_min;
|
|
|
|
uint16_t _esc_pwm_max;
|
|
|
|
|
|
|
|
// offset of first local channel
|
|
|
|
uint8_t chan_offset;
|
|
|
|
|
2018-05-05 05:54:12 -03:00
|
|
|
// total number of channels on FMU
|
|
|
|
uint8_t num_fmu_channels;
|
|
|
|
|
|
|
|
// number of active fmu channels
|
|
|
|
uint8_t active_fmu_channels;
|
|
|
|
|
|
|
|
static const uint8_t max_channels = 16;
|
2018-01-12 04:29:16 -04:00
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
// last sent values are for all channels
|
2018-05-05 05:54:12 -03:00
|
|
|
uint16_t last_sent[max_channels];
|
2018-01-05 02:19:51 -04:00
|
|
|
|
|
|
|
// these values are for the local channels. Non-local channels are handled by IOMCU
|
|
|
|
uint32_t en_mask;
|
2018-05-05 05:54:12 -03:00
|
|
|
uint16_t period[max_channels];
|
|
|
|
uint16_t safe_pwm[max_channels]; // pwm to use when safety is on
|
2018-01-05 02:19:51 -04:00
|
|
|
bool corked;
|
2018-01-12 17:13:00 -04:00
|
|
|
// mask of channels that are running in high speed
|
|
|
|
uint16_t fast_channel_mask;
|
2018-05-11 03:30:18 -03:00
|
|
|
uint16_t io_fast_channel_mask;
|
2018-01-05 02:19:51 -04:00
|
|
|
|
2018-01-16 03:58:58 -04:00
|
|
|
// min time to trigger next pulse to prevent overlap
|
|
|
|
uint64_t min_pulse_trigger_us;
|
2018-01-17 06:25:02 -04:00
|
|
|
|
|
|
|
// mutex for oneshot triggering
|
|
|
|
mutex_t trigger_mutex;
|
|
|
|
|
|
|
|
// which output groups need triggering
|
2018-03-14 03:06:30 -03:00
|
|
|
uint8_t trigger_groupmask;
|
2018-01-17 06:25:02 -04:00
|
|
|
|
|
|
|
// widest pulse for oneshot triggering
|
|
|
|
uint16_t trigger_widest_pulse;
|
2018-03-14 03:06:30 -03:00
|
|
|
|
2018-04-03 05:13:41 -03:00
|
|
|
// are we using oneshot125 for the iomcu?
|
|
|
|
bool iomcu_oneshot125;
|
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
// push out values to local PWM
|
|
|
|
void push_local(void);
|
2018-01-17 06:25:02 -04:00
|
|
|
|
2018-03-14 03:06:30 -03:00
|
|
|
// trigger group pulses
|
|
|
|
void trigger_groups(void);
|
|
|
|
|
2018-03-16 18:49:40 -03:00
|
|
|
// setup output frequency for a group
|
|
|
|
void set_freq_group(pwm_group &group);
|
2018-04-14 00:55:03 -03:00
|
|
|
|
|
|
|
// safety switch state
|
|
|
|
AP_HAL::Util::safety_state safety_state;
|
|
|
|
uint32_t safety_update_ms;
|
|
|
|
uint8_t led_counter;
|
|
|
|
int8_t safety_button_counter;
|
|
|
|
|
|
|
|
// mask of channels to allow when safety on
|
|
|
|
uint16_t safety_mask;
|
|
|
|
|
|
|
|
// update safety switch and LED
|
|
|
|
void safety_update(void);
|
2018-03-16 18:49:40 -03:00
|
|
|
|
2018-03-14 03:06:30 -03:00
|
|
|
/*
|
|
|
|
DShot handling
|
|
|
|
*/
|
2018-04-01 21:39:17 -03:00
|
|
|
const uint8_t dshot_post = 6;
|
2018-03-14 03:06:30 -03:00
|
|
|
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;
|
2018-04-02 01:20:58 -03:00
|
|
|
uint16_t telem_request_mask;
|
|
|
|
|
2018-03-14 03:06:30 -03:00
|
|
|
void dma_allocate(Shared_DMA *ctx);
|
|
|
|
void dma_deallocate(Shared_DMA *ctx);
|
2018-04-02 01:20:58 -03:00
|
|
|
uint16_t create_dshot_packet(const uint16_t value, bool telem_request);
|
2018-03-16 18:49:40 -03:00
|
|
|
void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
|
2018-03-14 03:06:30 -03:00
|
|
|
void dshot_send(pwm_group &group, bool blocking);
|
|
|
|
static void dma_irq_callback(void *p, uint32_t flags);
|
2018-03-16 18:49:40 -03:00
|
|
|
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);
|
2018-08-05 22:17:05 -03:00
|
|
|
static void serial_byte_timeout(void *ctx);
|
2018-03-16 18:49:40 -03:00
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
};
|
2018-03-01 20:46:30 -04:00
|
|
|
|
|
|
|
#endif // HAL_USE_PWM
|