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/>.
|
2019-10-20 10:31:12 -03:00
|
|
|
*
|
2018-01-05 02:19:51 -04:00
|
|
|
* Code by Andrew Tridgell and Siddharth Bharat Purohit
|
|
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include "AP_HAL_ChibiOS.h"
|
2021-01-16 05:05:09 -04:00
|
|
|
#include <AP_HAL/Semaphores.h>
|
2021-03-01 16:40:39 -04:00
|
|
|
#include <AP_ESC_Telem/AP_ESC_Telem.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
|
|
|
|
|
2021-12-05 20:31:24 -04:00
|
|
|
#if !STM32_DMA_ADVANCED && !defined(STM32G4) && !defined(STM32L4)
|
2018-08-29 10:14:12 -03:00
|
|
|
#define DISABLE_DSHOT
|
|
|
|
#endif
|
|
|
|
|
2020-10-12 16:57:29 -03:00
|
|
|
#define RCOU_DSHOT_TIMING_DEBUG 0
|
|
|
|
|
2021-03-01 16:40:39 -04:00
|
|
|
class ChibiOS::RCOutput : public AP_HAL::RCOutput
|
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
, AP_ESC_Telem_Backend
|
|
|
|
#endif
|
|
|
|
{
|
2018-01-05 02:19:51 -04:00
|
|
|
public:
|
2021-04-03 19:00:38 -03:00
|
|
|
// disabled channel marker
|
|
|
|
const static uint8_t CHAN_DISABLED = 255;
|
|
|
|
|
2018-11-07 06:58:46 -04:00
|
|
|
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;
|
2018-01-05 02:19:51 -04:00
|
|
|
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;
|
|
|
|
}
|
2020-10-12 16:57:29 -03:00
|
|
|
// 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
|
2021-01-11 22:01:48 -04:00
|
|
|
void set_output_mode(uint32_t mask, const enum output_mode mode) override;
|
2020-01-28 04:28:36 -04:00
|
|
|
bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const override;
|
2018-01-05 02:19:51 -04:00
|
|
|
|
2022-04-19 14:16:25 -03:00
|
|
|
/*
|
|
|
|
* return mask of channels that must be disabled because they share a group with a digital channel
|
|
|
|
*/
|
2022-05-15 18:30:16 -03:00
|
|
|
uint32_t get_disabled_channels(uint32_t digital_mask) override;
|
2022-04-19 14:16:25 -03: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;
|
|
|
|
}
|
2019-10-20 10:31:12 -03:00
|
|
|
|
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;
|
|
|
|
|
|
|
|
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)
|
|
|
|
*/
|
2023-03-23 01:28:56 -03:00
|
|
|
void timer_tick(uint64_t last_run_us);
|
2018-03-16 18:49:40 -03:00
|
|
|
|
2023-02-23 08:35:34 -04:00
|
|
|
/*
|
|
|
|
LED push
|
|
|
|
*/
|
|
|
|
void led_timer_tick(uint64_t last_run_us);
|
|
|
|
|
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
|
|
|
|
*/
|
2021-01-11 22:01:48 -04:00
|
|
|
bool setup_serial_output(uint32_t chan_mask, ByteBuffer *buffer, uint32_t baudrate);
|
2018-03-16 18:49:40 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
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
|
|
|
|
*/
|
2021-01-11 22:01:48 -04:00
|
|
|
bool serial_setup_output(uint8_t chan, uint32_t baudrate, uint32_t motor_mask) override;
|
2018-03-16 18:49:40 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
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;
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2018-03-16 18:49:40 -03:00
|
|
|
/*
|
|
|
|
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
|
2021-04-03 19:00:38 -03:00
|
|
|
with Dshot to get telemetry feedback
|
2021-07-29 17:46:15 -03:00
|
|
|
The mask uses servo channel numbering
|
2018-04-02 01:20:58 -03:00
|
|
|
*/
|
2021-01-11 22:01:48 -04:00
|
|
|
void set_telem_request_mask(uint32_t mask) override { telem_request_mask = (mask >> chan_offset); }
|
2018-04-14 00:55:03 -03:00
|
|
|
|
2020-10-12 16:57:29 -03:00
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
/*
|
|
|
|
enable bi-directional telemetry request for a mask of channels. This is used
|
2021-04-03 19:00:38 -03:00
|
|
|
with Dshot to get telemetry feedback
|
2021-07-29 17:46:15 -03:00
|
|
|
The mask uses servo channel numbering
|
2020-10-12 16:57:29 -03:00
|
|
|
*/
|
2021-01-11 22:01:48 -04:00
|
|
|
void set_bidir_dshot_mask(uint32_t mask) override;
|
2021-03-01 16:40:39 -04:00
|
|
|
|
|
|
|
void set_motor_poles(uint8_t poles) override { _bdshot.motor_poles = poles; }
|
2020-10-12 16:57:29 -03:00
|
|
|
#endif
|
|
|
|
|
2021-03-02 15:02:38 -04:00
|
|
|
/*
|
|
|
|
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;
|
|
|
|
|
2021-04-03 19:00:38 -03:00
|
|
|
#ifndef DISABLE_DSHOT
|
|
|
|
/*
|
|
|
|
Set/get the dshot esc_type
|
|
|
|
*/
|
2022-05-23 07:42:48 -03:00
|
|
|
void set_dshot_esc_type(DshotEscType dshot_esc_type) override;
|
2021-04-03 19:00:38 -03:00
|
|
|
|
|
|
|
DshotEscType get_dshot_esc_type() const override { return _dshot_esc_type; }
|
|
|
|
#endif
|
|
|
|
|
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-09-12 17:23:01 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
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;
|
2018-10-31 01:44:44 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
set safety mask for IOMCU
|
|
|
|
*/
|
2021-01-11 22:01:48 -04:00
|
|
|
void set_safety_mask(uint32_t mask) { safety_mask = mask; }
|
2018-10-31 01:44:44 -03:00
|
|
|
|
2021-04-03 19:00:38 -03:00
|
|
|
#ifndef DISABLE_DSHOT
|
2018-11-09 06:25:58 -04:00
|
|
|
/*
|
2021-04-03 19:00:38 -03:00
|
|
|
* mark the channels in chanmask as reversible. This is needed for some ESC types (such as Dshot)
|
2021-07-29 17:46:15 -03:00
|
|
|
* so that output scaling can be performed correctly. The chanmask passed is added (ORed) into any existing mask.
|
|
|
|
* The mask uses servo channel numbering
|
2018-11-09 06:25:58 -04:00
|
|
|
*/
|
2021-01-11 22:01:48 -04:00
|
|
|
void set_reversible_mask(uint32_t chanmask) override;
|
2021-04-03 19:00:38 -03:00
|
|
|
|
|
|
|
/*
|
2021-07-29 17:46:15 -03:00
|
|
|
* mark the channels in chanmask as reversed.
|
|
|
|
* The chanmask passed is added (ORed) into any existing mask.
|
|
|
|
* The mask uses servo channel numbering
|
2021-04-03 19:00:38 -03:00
|
|
|
*/
|
2021-01-11 22:01:48 -04:00
|
|
|
void set_reversed_mask(uint32_t chanmask) override;
|
|
|
|
uint32_t get_reversed_mask() override { return _reversed_mask << chan_offset; }
|
2021-04-03 19:00:38 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
mark escs as active for the purpose of sending dshot commands
|
2021-07-29 17:46:15 -03:00
|
|
|
The mask uses servo channel numbering
|
2021-04-03 19:00:38 -03:00
|
|
|
*/
|
2021-01-11 22:01:48 -04:00
|
|
|
void set_active_escs_mask(uint32_t chanmask) override { _active_escs_mask |= (chanmask >> chan_offset); }
|
2021-04-03 19:00:38 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
Send a dshot command, if command timout is 0 then 10 commands are sent
|
|
|
|
chan is the servo channel to send the command to
|
|
|
|
*/
|
|
|
|
void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) override;
|
|
|
|
|
|
|
|
/*
|
2021-06-16 06:27:23 -03:00
|
|
|
* Update channel masks at 1Hz allowing for actions such as dshot commands to be sent
|
2021-04-03 19:00:38 -03:00
|
|
|
*/
|
2021-06-16 06:27:23 -03:00
|
|
|
void update_channel_masks() override;
|
2021-07-29 17:46:15 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
* 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; }
|
2021-06-16 06:27:23 -03:00
|
|
|
#endif
|
2019-03-29 18:45:12 -03:00
|
|
|
|
2019-09-09 06:23:47 -03:00
|
|
|
/*
|
2020-02-22 19:55:47 -04:00
|
|
|
setup serial LED output for a given channel number, with
|
2019-09-09 06:23:47 -03:00
|
|
|
the given max number of LEDs in the chain.
|
|
|
|
*/
|
2021-01-11 22:01:48 -04:00
|
|
|
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;
|
2019-09-09 06:23:47 -03:00
|
|
|
|
|
|
|
/*
|
2020-02-22 19:55:47 -04:00
|
|
|
setup serial LED output data for a given output channel
|
2020-02-09 06:34:46 -04:00
|
|
|
and LEDs number. LED -1 is all LEDs
|
2019-09-09 06:23:47 -03:00
|
|
|
*/
|
2020-02-22 19:55:47 -04:00
|
|
|
void set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue) override;
|
2019-09-09 06:23:47 -03:00
|
|
|
|
|
|
|
/*
|
2020-02-22 19:55:47 -04:00
|
|
|
trigger send of serial LED data
|
2019-09-09 06:23:47 -03:00
|
|
|
*/
|
2020-02-22 19:55:47 -04:00
|
|
|
void serial_led_send(const uint16_t chan) override;
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2020-12-05 15:16:27 -04:00
|
|
|
/*
|
|
|
|
rcout thread
|
|
|
|
*/
|
|
|
|
void rcout_thread();
|
|
|
|
|
2022-02-10 10:42:33 -04:00
|
|
|
/*
|
|
|
|
timer information
|
|
|
|
*/
|
|
|
|
void timer_info(ExpandingString &str) override;
|
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
private:
|
2020-10-12 16:57:29 -03:00
|
|
|
enum class DshotState {
|
|
|
|
IDLE = 0,
|
|
|
|
SEND_START = 1,
|
|
|
|
SEND_COMPLETE = 2,
|
|
|
|
RECV_START = 3,
|
|
|
|
RECV_COMPLETE = 4
|
|
|
|
};
|
|
|
|
|
2021-01-16 05:05:09 -04:00
|
|
|
struct PACKED SerialLed {
|
|
|
|
uint8_t red;
|
|
|
|
uint8_t green;
|
|
|
|
uint8_t blue;
|
|
|
|
};
|
|
|
|
|
2020-10-12 16:57:29 -03:00
|
|
|
/*
|
|
|
|
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(uint32_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);
|
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
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;
|
2022-02-10 10:42:33 -04:00
|
|
|
uint8_t timer_id;
|
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;
|
2020-10-12 16:57:29 -03:00
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
struct {
|
|
|
|
bool have_dma;
|
|
|
|
uint8_t stream_id;
|
|
|
|
uint8_t channel;
|
|
|
|
} dma_ch[4];
|
|
|
|
#endif
|
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;
|
2021-05-21 18:28:13 -03:00
|
|
|
// mask of channels that are able to be enabled
|
2021-01-11 22:01:48 -04:00
|
|
|
uint32_t ch_mask;
|
2021-05-21 18:28:13 -03:00
|
|
|
// mask of channels that are enabled and active
|
2021-01-11 22:01:48 -04:00
|
|
|
uint32_t en_mask;
|
2018-03-14 03:06:30 -03:00
|
|
|
const stm32_dma_stream_t *dma;
|
|
|
|
Shared_DMA *dma_handle;
|
|
|
|
uint32_t *dma_buffer;
|
2019-09-09 06:23:47 -03:00
|
|
|
uint16_t dma_buffer_len;
|
2018-03-16 18:49:40 -03:00
|
|
|
bool pwm_started;
|
|
|
|
uint32_t bit_width_mul;
|
|
|
|
uint32_t rc_frequency;
|
|
|
|
bool in_serial_dma;
|
2023-03-23 01:28:56 -03:00
|
|
|
uint64_t last_dmar_send_us;
|
|
|
|
uint64_t dshot_pulse_time_us;
|
|
|
|
uint64_t dshot_pulse_send_time_us;
|
2018-08-04 02:29:22 -03:00
|
|
|
virtual_timer_t dma_timeout;
|
2021-01-16 05:05:09 -04:00
|
|
|
|
|
|
|
// serial LED support
|
|
|
|
volatile uint8_t serial_nleds;
|
2020-02-22 19:55:47 -04:00
|
|
|
uint8_t clock_mask;
|
2021-03-21 22:27:17 -03:00
|
|
|
enum output_mode led_mode;
|
2021-01-16 05:05:09 -04:00
|
|
|
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];
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2020-12-05 15:16:27 -04:00
|
|
|
eventmask_t dshot_event_mask;
|
|
|
|
thread_t* dshot_waiter;
|
|
|
|
|
2018-03-16 18:49:40 -03:00
|
|
|
// serial output
|
|
|
|
struct {
|
|
|
|
// expected time per bit
|
2021-10-02 02:40:07 -03:00
|
|
|
uint16_t bit_time_us;
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2018-03-16 18:49:40 -03:00
|
|
|
// channel to output to within group (0 to 3)
|
|
|
|
uint8_t chan;
|
|
|
|
|
|
|
|
// thread waiting for byte to be written
|
2019-10-20 10:31:12 -03:00
|
|
|
thread_t *waiter;
|
2018-03-16 18:49:40 -03:00
|
|
|
} serial;
|
2020-10-12 16:57:29 -03:00
|
|
|
|
|
|
|
// support for bi-directional dshot
|
|
|
|
volatile DshotState dshot_state;
|
|
|
|
|
|
|
|
struct {
|
|
|
|
uint16_t erpm[4];
|
|
|
|
volatile bool enabled;
|
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
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
|
|
|
|
#endif
|
|
|
|
} bdshot;
|
|
|
|
|
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
// 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
|
|
|
|
// are we safe to send another pulse?
|
|
|
|
bool can_send_dshot_pulse() const {
|
2020-12-05 15:16:27 -04:00
|
|
|
return is_dshot_protocol(current_mode) && AP_HAL::micros() - last_dmar_send_us > (dshot_pulse_time_us + 50);
|
2020-10-12 16:57:29 -03:00
|
|
|
}
|
2021-04-03 19:00:38 -03:00
|
|
|
|
|
|
|
// return whether the group channel is both enabled in the group and for output
|
|
|
|
bool is_chan_enabled(uint8_t c) const {
|
2021-05-21 18:28:13 -03:00
|
|
|
return chan[c] != CHAN_DISABLED && (en_mask & (1U << chan[c]));
|
2021-04-03 19:00:38 -03:00
|
|
|
}
|
2018-01-05 02:19:51 -04:00
|
|
|
};
|
2020-12-05 15:16:27 -04:00
|
|
|
/*
|
|
|
|
timer thread for use by dshot events
|
|
|
|
*/
|
|
|
|
thread_t *rcout_thread_ctx;
|
2018-01-05 02:19:51 -04:00
|
|
|
|
2023-02-23 08:35:34 -04:00
|
|
|
/*
|
|
|
|
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;
|
|
|
|
|
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
|
2021-10-02 02:40:07 -03:00
|
|
|
uint16_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;
|
|
|
|
|
2019-10-20 10:31:12 -03:00
|
|
|
// value of completed byte (includes start and stop bits)
|
2018-03-16 18:49:40 -03:00
|
|
|
uint16_t byteval;
|
|
|
|
|
2021-03-16 23:23:05 -03:00
|
|
|
// expected time per bit in micros
|
2021-10-02 02:40:07 -03:00
|
|
|
uint16_t bit_time_tick;
|
2018-03-16 18:49:40 -03:00
|
|
|
|
|
|
|
// the bit value of the last bit received
|
|
|
|
uint8_t last_bit;
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2018-03-16 18:49:40 -03:00
|
|
|
// thread waiting for byte to be read
|
|
|
|
thread_t *waiter;
|
2018-08-04 00:34:00 -03:00
|
|
|
|
|
|
|
// timeout for byte read
|
|
|
|
virtual_timer_t serial_timeout;
|
|
|
|
bool timed_out;
|
2018-03-16 18:49:40 -03:00
|
|
|
} irq;
|
|
|
|
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2018-03-16 18:49:40 -03:00
|
|
|
// 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;
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2018-01-12 23:53:29 -04:00
|
|
|
static pwm_group pwm_group_list[];
|
2020-10-12 16:57:29 -03:00
|
|
|
static const uint8_t NUM_GROUPS;
|
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;
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2021-01-11 22:01:48 -04:00
|
|
|
#if NUM_SERVO_CHANNELS >= 17
|
|
|
|
static const uint8_t max_channels = 32;
|
|
|
|
#else
|
2018-05-05 05:54:12 -03:00
|
|
|
static const uint8_t max_channels = 16;
|
2021-01-11 22:01:48 -04:00
|
|
|
#endif
|
2019-10-20 10:31:12 -03: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];
|
2019-10-20 10:31:12 -03:00
|
|
|
|
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];
|
2021-04-03 19:00:38 -03:00
|
|
|
|
2020-10-12 16:57:29 -03:00
|
|
|
// handling of bi-directional dshot
|
|
|
|
struct {
|
2021-01-11 22:01:48 -04:00
|
|
|
uint32_t mask;
|
2020-10-12 16:57:29 -03:00
|
|
|
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];
|
2021-03-01 16:40:39 -04:00
|
|
|
uint8_t motor_poles;
|
2020-10-12 16:57:29 -03:00
|
|
|
#endif
|
|
|
|
} _bdshot;
|
|
|
|
|
2021-03-02 15:02:38 -04:00
|
|
|
// dshot period
|
2021-05-02 14:35:19 -03:00
|
|
|
uint32_t _dshot_period_us = 400;
|
2021-03-02 15:02:38 -04:00
|
|
|
// 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;
|
|
|
|
|
2021-04-03 19:00:38 -03:00
|
|
|
#ifndef DISABLE_DSHOT
|
|
|
|
// 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;
|
|
|
|
|
2021-07-29 17:46:15 -03:00
|
|
|
// control updates to channel masks
|
|
|
|
bool _disable_channel_mask_updates;
|
|
|
|
|
2021-04-03 19:00:38 -03:00
|
|
|
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
|
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
|
2021-01-11 22:01:48 -04:00
|
|
|
uint32_t fast_channel_mask;
|
|
|
|
uint32_t io_fast_channel_mask;
|
2021-04-03 19:00:38 -03:00
|
|
|
// mask of channels that are 3D capable
|
2021-01-11 22:01:48 -04:00
|
|
|
uint32_t _reversible_mask;
|
2021-04-03 19:00:38 -03:00
|
|
|
// mask of channels that should be reversed at startup
|
2021-01-11 22:01:48 -04:00
|
|
|
uint32_t _reversed_mask;
|
2021-04-03 19:00:38 -03:00
|
|
|
// mask of active ESCs
|
2021-01-11 22:01:48 -04:00
|
|
|
uint32_t _active_escs_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
|
|
|
|
2020-01-28 04:28:36 -04:00
|
|
|
// iomcu output mode (pwm, oneshot or oneshot125)
|
|
|
|
enum output_mode iomcu_mode = MODE_PWM_NORMAL;
|
2018-04-03 05:13:41 -03:00
|
|
|
|
2021-01-16 05:05:09 -04:00
|
|
|
volatile bool _initialised;
|
|
|
|
|
2020-10-12 16:57:29 -03:00
|
|
|
bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; }
|
|
|
|
|
2021-04-03 19:00:38 -03:00
|
|
|
// are all the ESCs returning data
|
|
|
|
bool group_escs_active(const pwm_group& group) const {
|
2021-05-21 18:28:13 -03:00
|
|
|
return group.en_mask > 0 && (group.en_mask & _active_escs_mask) == group.en_mask;
|
2021-04-03 19:00:38 -03:00
|
|
|
}
|
|
|
|
|
2019-09-09 06:23:47 -03:00
|
|
|
// find a channel group given a channel number
|
|
|
|
struct pwm_group *find_chan(uint8_t chan, uint8_t &group_idx);
|
|
|
|
|
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;
|
2019-09-02 23:24:34 -03:00
|
|
|
uint8_t safety_press_count; // 0.1s units
|
2018-04-14 00:55:03 -03:00
|
|
|
|
|
|
|
// mask of channels to allow when safety on
|
2021-01-11 22:01:48 -04:00
|
|
|
uint32_t safety_mask;
|
2018-04-14 00:55:03 -03:00
|
|
|
|
|
|
|
// update safety switch and LED
|
|
|
|
void safety_update(void);
|
2019-10-20 10:31:12 -03:00
|
|
|
|
2023-02-23 08:35:34 -04:00
|
|
|
// LED thread
|
|
|
|
void led_thread();
|
|
|
|
bool start_led_thread();
|
|
|
|
|
2021-01-11 22:01:48 -04:00
|
|
|
uint32_t telem_request_mask;
|
2019-03-29 18:45:12 -03:00
|
|
|
|
2019-09-09 06:23:47 -03:00
|
|
|
/*
|
2020-02-22 19:55:47 -04:00
|
|
|
Serial lED handling. Max of 32 LEDs uses max 12k of memory per group
|
2020-02-18 18:04:32 -04:00
|
|
|
return true if send was successful
|
2019-09-09 06:23:47 -03:00
|
|
|
*/
|
2020-12-05 15:16:27 -04:00
|
|
|
static const eventmask_t serial_event_mask = EVENT_MASK(10);
|
2020-02-22 19:55:47 -04:00
|
|
|
bool serial_led_send(pwm_group &group);
|
2021-01-16 05:05:09 -04:00
|
|
|
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;
|
2019-03-29 18:45:12 -03:00
|
|
|
|
2018-03-14 03:06:30 -03:00
|
|
|
void dma_allocate(Shared_DMA *ctx);
|
2019-10-20 10:31:12 -03:00
|
|
|
void dma_deallocate(Shared_DMA *ctx);
|
2020-10-12 16:57:29 -03:00
|
|
|
uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem);
|
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);
|
2020-10-12 16:57:29 -03:00
|
|
|
|
2023-03-23 01:28:56 -03:00
|
|
|
void dshot_send_groups(uint64_t time_out_us);
|
|
|
|
void dshot_send(pwm_group &group, uint64_t time_out_us);
|
2021-05-02 14:35:19 -03:00
|
|
|
bool dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan);
|
2022-11-10 12:49:54 -04:00
|
|
|
static void dshot_update_tick(virtual_timer_t*, void* p);
|
2021-03-17 18:02:18 -03:00
|
|
|
static void dshot_send_next_group(void* p);
|
2020-12-05 15:16:27 -04:00
|
|
|
// release locks on the groups that are pending in reverse order
|
2023-02-23 08:35:34 -04:00
|
|
|
void dshot_collect_dma_locks(uint64_t last_run_us, bool led_thread = false);
|
2020-10-12 16:57:29 -03:00
|
|
|
static void dma_up_irq_callback(void *p, uint32_t flags);
|
2022-11-10 12:49:54 -04:00
|
|
|
static void dma_unlock(virtual_timer_t*, void *p);
|
2020-12-05 15:16:27 -04:00
|
|
|
void dma_cancel(pwm_group& group);
|
2018-03-16 18:49:40 -03:00
|
|
|
bool mode_requires_dma(enum output_mode mode) const;
|
2021-02-06 13:59:04 -04:00
|
|
|
bool setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high,
|
2022-02-10 10:42:33 -04:00
|
|
|
const uint16_t buffer_length, uint32_t pulse_time_us,
|
2022-01-25 05:58:35 -04:00
|
|
|
bool is_dshot);
|
2018-03-16 18:49:40 -03:00
|
|
|
void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);
|
|
|
|
void set_group_mode(pwm_group &group);
|
2020-10-12 16:57:29 -03:00
|
|
|
static uint32_t protocol_bitrate(const enum output_mode mode);
|
2021-03-24 15:52:39 -03:00
|
|
|
void print_group_setup_error(pwm_group &group, const char* error_string);
|
2020-10-12 16:57:29 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
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);
|
2021-04-03 19:00:38 -03:00
|
|
|
bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan);
|
2020-10-12 16:57:29 -03:00
|
|
|
static uint8_t bdshot_find_next_ic_channel(const pwm_group& group);
|
|
|
|
static void bdshot_dma_ic_irq_callback(void *p, uint32_t flags);
|
2022-11-10 12:49:54 -04:00
|
|
|
static void bdshot_finish_dshot_gcr_transaction(virtual_timer_t* vt, void *p);
|
2020-10-12 16:57:29 -03:00
|
|
|
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);
|
2018-03-16 18:49:40 -03:00
|
|
|
|
2020-02-09 06:34:46 -04:00
|
|
|
/*
|
|
|
|
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);
|
|
|
|
|
2020-02-22 19:55:47 -04:00
|
|
|
/*
|
|
|
|
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);
|
|
|
|
|
2018-03-16 18:49:40 -03:00
|
|
|
// serial output support
|
|
|
|
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);
|
2022-11-10 12:49:54 -04:00
|
|
|
static void serial_byte_timeout(virtual_timer_t* vt, 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
|
|
|
|
2021-04-03 19:00:38 -03:00
|
|
|
#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
|
|
|
|
|
2018-03-01 20:46:30 -04:00
|
|
|
#endif // HAL_USE_PWM
|