From b93cb96c4f188bf1f33dd08b3802aa813f722765 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 20 Nov 2025 11:01:43 -0900 Subject: [PATCH] rebased --- msg/Am32EepromRead.msg | 5 + msg/Am32EepromWrite.msg | 6 + msg/CMakeLists.txt | 2 + msg/EscReport.msg | 47 +- msg/EscStatus.msg | 26 +- msg/versioned/VehicleCommand.msg | 1 + .../nuttx/src/px4/nxp/imxrt/dshot/dshot.c | 21 +- .../src/px4/stm/stm32_common/dshot/dshot.c | 652 +++++--- src/drivers/drv_dshot.h | 99 +- src/drivers/dshot/CMakeLists.txt | 2 + src/drivers/dshot/DShot.cpp | 1451 ++++++++++------- src/drivers/dshot/DShot.h | 211 ++- src/drivers/dshot/DShotCommon.h | 96 ++ src/drivers/dshot/DShotTelemetry.cpp | 460 +++--- src/drivers/dshot/DShotTelemetry.h | 101 +- src/drivers/dshot/esc/AM32Settings.cpp | 85 + src/drivers/dshot/esc/AM32Settings.h | 103 ++ src/drivers/dshot/esc/ESCSettingsInterface.h | 53 + src/drivers/dshot/module.yaml | 19 + src/lib/mixer_module/mixer_module.hpp | 2 + src/modules/logger/logged_topics.cpp | 2 + 21 files changed, 2142 insertions(+), 1302 deletions(-) create mode 100644 msg/Am32EepromRead.msg create mode 100644 msg/Am32EepromWrite.msg create mode 100644 src/drivers/dshot/DShotCommon.h create mode 100644 src/drivers/dshot/esc/AM32Settings.cpp create mode 100644 src/drivers/dshot/esc/AM32Settings.h create mode 100644 src/drivers/dshot/esc/ESCSettingsInterface.h diff --git a/msg/Am32EepromRead.msg b/msg/Am32EepromRead.msg new file mode 100644 index 0000000000..786cdb541e --- /dev/null +++ b/msg/Am32EepromRead.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +uint8 index # Index of the ESC (0 = ESC1, 1 = ESC2, etc.) +uint8[48] data # Raw AM32 EEPROM data + +uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up reponses diff --git a/msg/Am32EepromWrite.msg b/msg/Am32EepromWrite.msg new file mode 100644 index 0000000000..c1515f73d9 --- /dev/null +++ b/msg/Am32EepromWrite.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +uint8 index # Index of the ESC (0 = ESC1, 1 = ESC2, etc, 255 = All) +uint8[48] data # Raw AM32 EEPROM data +uint32[2] write_mask # Bitmask indicating which bytes in the data array should be written (max 64 values, am32 is currently 48) + +uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up requests diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index dbdaf79b7f..a8de0e829e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -69,6 +69,8 @@ set(msg_files DistanceSensorModeChangeRequest.msg DronecanNodeStatus.msg Ekf2Timestamps.msg + Am32EepromRead.msg + Am32EepromWrite.msg EscReport.msg EscStatus.msg EstimatorAidSource1d.msg diff --git a/msg/EscReport.msg b/msg/EscReport.msg index ca80753f14..4792724583 100644 --- a/msg/EscReport.msg +++ b/msg/EscReport.msg @@ -1,15 +1,16 @@ -uint64 timestamp # time since system start (microseconds) -uint32 esc_errorcount # Number of reported errors by ESC - if supported -int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if supported -float32 esc_voltage # Voltage measured from current ESC [V] - if supported -float32 esc_current # Current measured from current ESC [A] - if supported -float32 esc_temperature # Temperature measured from current ESC [degC] - if supported -uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver) -uint8 esc_cmdcount # Counter of number of commands +uint64 timestamp # time since system start (microseconds) -uint8 esc_state # State of ESC - depend on Vendor +uint32 esc_errorcount # Number of reported errors by ESC - if supported +int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if supported +float32 esc_voltage # Voltage measured from current ESC [V] - if supported +float32 esc_current # Current measured from current ESC [A] - if supported +float32 esc_temperature # Temperature measured from current ESC [degC] - if supported +uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver) +uint8 esc_cmdcount # Counter of number of commands -uint8 actuator_function # actuator output function (one of Motor1...MotorN) +uint8 esc_state # State of ESC - depend on Vendor + +uint8 actuator_function # actuator output function (one of Motor1...MotorN) uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 uint8 ACTUATOR_FUNCTION_MOTOR2 = 102 @@ -24,17 +25,17 @@ uint8 ACTUATOR_FUNCTION_MOTOR10 = 110 uint8 ACTUATOR_FUNCTION_MOTOR11 = 111 uint8 ACTUATOR_FUNCTION_MOTOR12 = 112 -uint16 failures # Bitmask to indicate the internal ESC faults -int8 esc_power # Applied power 0-100 in % (negative values reserved) +uint16 failures # Bitmask to indicate the internal ESC faults +int8 esc_power # Applied power 0-100 in % (negative values reserved) -uint8 FAILURE_OVER_CURRENT = 0 # (1 << 0) -uint8 FAILURE_OVER_VOLTAGE = 1 # (1 << 1) -uint8 FAILURE_MOTOR_OVER_TEMPERATURE = 2 # (1 << 2) -uint8 FAILURE_OVER_RPM = 3 # (1 << 3) -uint8 FAILURE_INCONSISTENT_CMD = 4 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries) -uint8 FAILURE_MOTOR_STUCK = 5 # (1 << 5) -uint8 FAILURE_GENERIC = 6 # (1 << 6) -uint8 FAILURE_MOTOR_WARN_TEMPERATURE = 7 # (1 << 7) -uint8 FAILURE_WARN_ESC_TEMPERATURE = 8 # (1 << 8) -uint8 FAILURE_OVER_ESC_TEMPERATURE = 9 # (1 << 9) -uint8 ESC_FAILURE_COUNT = 10 # Counter - keep it as last element! +uint8 FAILURE_OVER_CURRENT = 0 # (1 << 0) +uint8 FAILURE_OVER_VOLTAGE = 1 # (1 << 1) +uint8 FAILURE_MOTOR_OVER_TEMPERATURE = 2 # (1 << 2) +uint8 FAILURE_OVER_RPM = 3 # (1 << 3) +uint8 FAILURE_INCONSISTENT_CMD = 4 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries) +uint8 FAILURE_MOTOR_STUCK = 5 # (1 << 5) +uint8 FAILURE_GENERIC = 6 # (1 << 6) +uint8 FAILURE_MOTOR_WARN_TEMPERATURE = 7 # (1 << 7) +uint8 FAILURE_WARN_ESC_TEMPERATURE = 8 # (1 << 8) +uint8 FAILURE_OVER_ESC_TEMPERATURE = 9 # (1 << 9) +uint8 ESC_FAILURE_COUNT = 10 # Counter - keep it as last element! diff --git a/msg/EscStatus.msg b/msg/EscStatus.msg index e5e220ce0d..cd98429941 100644 --- a/msg/EscStatus.msg +++ b/msg/EscStatus.msg @@ -1,19 +1,19 @@ -uint64 timestamp # time since system start (microseconds) -uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) we support 8 ESCs +uint64 timestamp # time since system start (microseconds) +uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) we support 8 ESCs -uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC -uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC -uint8 ESC_CONNECTION_TYPE_ONESHOT = 2 # One Shot PPM -uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C -uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus -uint8 ESC_CONNECTION_TYPE_DSHOT = 5 # DShot +uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC +uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC +uint8 ESC_CONNECTION_TYPE_ONESHOT = 2 # One Shot PPM +uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C +uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus +uint8 ESC_CONNECTION_TYPE_DSHOT = 5 # DShot -uint16 counter # incremented by the writing thread everytime new data is stored +uint16 counter # incremented by the writing thread everytime new data is stored -uint8 esc_count # number of connected ESCs -uint8 esc_connectiontype # how ESCs connected to the system +uint8 esc_count # number of connected ESCs +uint8 esc_connectiontype # how ESCs connected to the system -uint8 esc_online_flags # Bitmask indicating which ESC is online/offline +uint8 esc_online_flags # Bitmask indicating which ESC is online/offline # esc_online_flags bit 0 : Set to 1 if ESC0 is online # esc_online_flags bit 1 : Set to 1 if ESC1 is online # esc_online_flags bit 2 : Set to 1 if ESC2 is online @@ -23,6 +23,6 @@ uint8 esc_online_flags # Bitmask indicating which ESC is online/offline # esc_online_flags bit 6 : Set to 1 if ESC6 is online # esc_online_flags bit 7 : Set to 1 if ESC7 is online -uint8 esc_armed_flags # Bitmask indicating which ESC is armed. For ESC's where the arming state is not known (returned by the ESC), the arming bits should always be set. +uint8 esc_armed_flags # Bitmask indicating which ESC is armed. For ESC's where the arming state is not known (returned by the ESC), the arming bits should always be set. EscReport[8] esc diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 9503477ce4..4388656bb9 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -80,6 +80,7 @@ uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information uint16 VEHICLE_CMD_MISSION_START = 300 # Start running a mission. |first_item: the first mission item to run|last_item: the last mission item to run (after this item is run, the mission ends)| uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command. |[@range -1,1] value|[s] timeout|Unused|Unused|output function| uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command. |configuration|Unused|Unused|Unused|output function| +uint16 VEHICLE_CMD_AM32_REQUEST_EEPROM = 312 # Request EEPROM data from an AM32 ESC. |esc index| uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component. |1 to arm, 0 to disarm. uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401 # Instructs a target system to run pre-arm checks. uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes. diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c index 7bd2508ddf..f8e714dfa8 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -321,8 +321,11 @@ static int flexio_irq_handler(int irq, void *context, void *arg) } -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool bdshot_enable, + bool enable_extended_dshot_telemetry) { + (void)enable_extended_dshot_telemetry; // Not implemented + /* Calculate dshot timings based on dshot_pwm_freq */ dshot_tcmp = 0x2F00 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF); dshot_speed = dshot_pwm_freq; @@ -497,7 +500,7 @@ void up_bdshot_erpm(void) } -int up_bdshot_num_erpm_ready(void) +int up_bdshot_num_channels_ready(void) { int num_ready = 0; @@ -512,6 +515,10 @@ int up_bdshot_num_erpm_ready(void) return num_ready; } +int up_bdshot_num_errors(uint8_t channel) +{ + return dshot_inst[channel].crc_error_cnt + dshot_inst[channel].frame_error_cnt + dshot_inst[channel].no_response_cnt; +} int up_bdshot_get_erpm(uint8_t channel, int *erpm) { @@ -523,7 +530,13 @@ int up_bdshot_get_erpm(uint8_t channel, int *erpm) return -1; } -int up_bdshot_channel_status(uint8_t channel) +int up_bdshot_get_extended_telemetry(uint8_t channel, int type, uint8_t *value) +{ + // NOT IMPLEMENTED + return -1; +} + +int up_bdshot_channel_online(uint8_t channel) { if (channel < DSHOT_TIMERS) { return ((dshot_inst[channel].no_response_cnt - dshot_inst[channel].last_no_response_cnt) < BDSHOT_OFFLINE_COUNT); @@ -538,7 +551,7 @@ void up_bdshot_status(void) for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { if (dshot_inst[channel].init) { - PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_status(channel) ? "online" : "offline", + PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_online(channel) ? "online" : "offline", dshot_inst[channel].erpm); PX4_INFO("BDSHOT Training done: %s TCMP offset: %d", dshot_inst[channel].bdshot_training_done ? "YES" : "NO", dshot_inst[channel].bdshot_tcmp_offset); diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index a667537d9f..f295d7a5cb 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2024 PX4 Development Team. All rights reserved. - * Author: Igor Misic + * Copyright (C) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -97,7 +96,8 @@ static void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void static void capture_complete_callback(void *arg); static void process_capture_results(uint8_t timer_index, uint8_t channel_index); -static unsigned calculate_period(uint8_t timer_index, uint8_t channel_index); +static uint32_t convert_edge_intervals_to_bitstream(uint8_t channel_index); +static void decode_dshot_telemetry(uint32_t payload, struct BDShotTelemetry *packet); // Timer configuration struct typedef struct timer_config_t { @@ -122,24 +122,65 @@ static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; static uint16_t dshot_capture_buffer[MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE] px4_cache_aligned_data() = {}; -static bool _bidirectional = false; +static const uint32_t gcr_decode[32] = { + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x9, 0xA, 0xB, 0x0, 0xD, 0xE, 0xF, + 0x0, 0x0, 0x2, 0x3, 0x0, 0x5, 0x6, 0x7, + 0x0, 0x0, 0x8, 0x1, 0x0, 0x4, 0xC, 0x0 +}; + +// Indicates when the bdshot capture cycle is finished. This is necessary since the captured data is +// processed after a fixed delay in an hrt callback. System jitter can delay the firing of the hrt callback +// and thus delay the processing of the data. This should never happen in a properly working system, as the +// jitter would have to be longer than the control allocator update interval. A warning is issued if this +// ever does occur. +static bool _bdshot_cycle_complete = true; +static bool _bdshot_enabled = false; +static bool _extended_dshot_telem = false; static uint8_t _bidi_timer_index = 0; // TODO: BDSHOT_TIM param to select timer index? static uint32_t _dshot_frequency = 0; -// eRPM data for channels on the singular timer -static int32_t _erpms[MAX_TIMER_IO_CHANNELS] = {}; -static bool _erpms_ready[MAX_TIMER_IO_CHANNELS] = {}; +// Online flags, set if ESC is reponding with valid BDShot frames +#define BDSHOT_OFFLINE_COUNT 200 +static bool _bdshot_online[MAX_TIMER_IO_CHANNELS] = {}; +static bool _bdshot_processed[MAX_TIMER_IO_CHANNELS] = {}; +static int _consecutive_failures[MAX_TIMER_IO_CHANNELS] = {}; +static int _consecutive_successes[MAX_TIMER_IO_CHANNELS] = {}; + +// ePRM data +typedef struct erpm_data_t { + int32_t erpm; + bool ready; + float rate_hz; + uint64_t last_timestamp; +} erpm_data_t; + +erpm_data_t _erpms[MAX_TIMER_IO_CHANNELS] = {}; + +// EDT data +typedef struct edt_data_t { + uint8_t value; + bool ready; + float rate_hz; + uint64_t last_timestamp; +} edt_data_t; + +edt_data_t _edt_temp[MAX_TIMER_IO_CHANNELS] = {}; +edt_data_t _edt_volt[MAX_TIMER_IO_CHANNELS] = {}; +edt_data_t _edt_curr[MAX_TIMER_IO_CHANNELS] = {}; + +static float calculate_rate_hz(uint64_t last_timestamp, float last_rate_hz, uint64_t timestamp); // hrt callback handle for captcomp post dma processing static struct hrt_call _cc_call; // decoding status for each channel static uint32_t read_ok[MAX_NUM_CHANNELS_PER_TIMER] = {}; -static uint32_t read_fail_nibble[MAX_NUM_CHANNELS_PER_TIMER] = {}; static uint32_t read_fail_crc[MAX_NUM_CHANNELS_PER_TIMER] = {}; -static uint32_t read_fail_zero[MAX_NUM_CHANNELS_PER_TIMER] = {}; static perf_counter_t hrt_callback_perf = NULL; +static perf_counter_t capture_cycle_perf = NULL; +static perf_counter_t capture_cycle_perf2 = NULL; static void init_timer_config(uint32_t channel_mask) { @@ -163,7 +204,7 @@ static void init_timer_config(uint32_t channel_mask) } // NOTE: only 1 timer can be used if Bidirectional DShot is enabled - if (_bidirectional && (timer_index != _bidi_timer_index)) { + if (_bdshot_enabled && (timer_index != _bidi_timer_index)) { continue; } @@ -175,7 +216,7 @@ static void init_timer_config(uint32_t channel_mask) timer_configs[timer_index].enabled_channels[timer_channel_index] = true; // Mark timer as bidirectional - if (_bidirectional && timer_index == _bidi_timer_index) { + if (_bdshot_enabled && timer_index == _bidi_timer_index) { timer_configs[timer_index].bidirectional = true; } } @@ -198,7 +239,7 @@ static void init_timers_dma_up(void) } // NOTE: only 1 timer can be used if Bidirectional DShot is enabled - if (_bidirectional && (timer_index != _bidi_timer_index)) { + if (_bdshot_enabled && (timer_index != _bidi_timer_index)) { continue; } @@ -216,7 +257,7 @@ static void init_timers_dma_up(void) // Bidirectional DShot will free/allocate DMA stream on every update event. This is required // in order to reconfigure the DMA stream between Timer Burst and CaptureCompare. - if (_bidirectional) { + if (_bdshot_enabled) { // Free the allocated DMA channels for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { if (timer_configs[timer_index].dma_handle != NULL) { @@ -276,14 +317,18 @@ static int32_t init_timer_channels(uint8_t timer_index) return channels_init_mask; } -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool bdshot_enable, + bool enable_extended_dshot_telemetry) { _dshot_frequency = dshot_pwm_freq; - _bidirectional = enable_bidirectional_dshot; + _bdshot_enabled = bdshot_enable; + _extended_dshot_telem = enable_extended_dshot_telemetry; - if (_bidirectional) { + if (_bdshot_enabled) { PX4_INFO("Bidirectional DShot enabled, only one timer will be used"); hrt_callback_perf = perf_alloc(PC_ELAPSED, "dshot: callback perf"); + capture_cycle_perf = perf_alloc(PC_INTERVAL, "dshot: cycle perf"); + capture_cycle_perf2 = perf_alloc(PC_INTERVAL, "dshot: cycle perf2"); } // NOTE: if bidirectional is enabled only 1 timer can be used. This is because Burst mode uses 1 DMA channel per timer @@ -333,8 +378,18 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // Kicks off a DMA transmit for each configured timer and the associated channels void up_dshot_trigger() { + if (_bdshot_enabled) { + + if (!_bdshot_cycle_complete) { + PX4_WARN("Cylce not complete! Check system jitter"); + return; + } + + _bdshot_cycle_complete = false; + } + // Enable DShot inverted on all channels - io_timer_set_enable(true, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + io_timer_set_enable(true, _bdshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); // For each timer, begin DMA transmit @@ -345,7 +400,7 @@ void up_dshot_trigger() io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count); - if (_bidirectional) { + if (_bdshot_enabled) { // Deallocate DMA from previous transaction if (timer_configs[timer_index].dma_handle != NULL) { stm32_dmastop(timer_configs[timer_index].dma_handle); @@ -376,14 +431,20 @@ void up_dshot_trigger() // Clean UDE flag before DMA is started io_timer_update_dma_req(timer_index, false); - // Trigger DMA (DShot Outputs) - if (timer_configs[timer_index].bidirectional) { + // Trigger DMA (DShot Outputs). Only capture compare afte the system has had time to boot. + if (timer_configs[timer_index].bidirectional && (hrt_absolute_time() > 3000000)) { + + perf_begin(capture_cycle_perf); stm32_dmastart(timer_configs[timer_index].dma_handle, dma_burst_finished_callback, &timer_configs[timer_index].timer_index, false); } else { stm32_dmastart(timer_configs[timer_index].dma_handle, NULL, NULL, false); + + if (_bdshot_enabled) { + _bdshot_cycle_complete = true; + } } // Enable DMA update request @@ -453,11 +514,21 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg) // Unallocate timer channel for currently selected capture_channel uint8_t capture_channel = timer_configs[timer_index].capture_channel_index; - uint8_t output_channel = output_channel_from_timer_channel(timer_index, capture_channel); - // Re-initialize output for CaptureDMA for next time - io_timer_unallocate_channel(output_channel); - io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); + // Re-initialize all output channels on this timer as CaptureDMA to ensure all lines idle high + for (uint8_t channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { + + bool is_this_timer = timer_index == timer_io_channels[channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1; + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + + if (is_this_timer && channel_initialized) { + + io_timer_unallocate_channel(channel); + // Initialize back to DShotInverted to bring IO back to the expected idle state + io_timer_channel_init(channel, IOTimerChanMode_CaptureDMA, NULL, NULL); + } + } // Select the next capture channel select_next_capture_channel(timer_index); @@ -493,14 +564,19 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg) // Enable CaptureDMA and on all configured channels io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS); - // 30us to switch regardless of DShot frequency + eRPM frame time + 10us for good measure + // Measuring the time it takes from when we start the DMA to when we enable CaptureDMA + perf_end(capture_cycle_perf); + perf_begin(capture_cycle_perf2); + + // 30us to switch regardless of DShot frequency + eRPM frame time + 20us for good measure hrt_abstime frame_us = (16 * 1000000) / _dshot_frequency; // 16 bits * us_per_s / bits_per_s - hrt_abstime delay = 30 + frame_us + 10; + hrt_abstime delay = 30 + frame_us + 20; hrt_call_after(&_cc_call, delay, capture_complete_callback, arg); } static void capture_complete_callback(void *arg) { + perf_end(capture_cycle_perf2); perf_begin(hrt_callback_perf); uint8_t timer_index = *((uint8_t *)arg); @@ -524,7 +600,6 @@ static void capture_complete_callback(void *arg) bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; if (is_this_timer && channel_initialized) { - io_timer_unallocate_channel(output_channel); // Initialize back to DShotInverted to bring IO back to the expected idle state io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); @@ -542,213 +617,137 @@ static void capture_complete_callback(void *arg) io_timer_set_enable(true, IOTimerChanMode_DshotInverted, IO_TIMER_ALL_MODES_CHANNELS); perf_end(hrt_callback_perf); + + _bdshot_cycle_complete = true; } void process_capture_results(uint8_t timer_index, uint8_t channel_index) { - const unsigned period = calculate_period(timer_index, channel_index); - + (void)timer_index; // NOTE: in the current implementation only 1 timer is used uint8_t output_channel = output_channel_from_timer_channel(timer_index, channel_index); + uint32_t value = convert_edge_intervals_to_bitstream(channel_index); + // Decode RLL + value = (value ^ (value >> 1)); - if (period == 0) { - // If the parsing failed, set the eRPM to 0 - _erpms[output_channel] = 0; + // Decode GCR + uint32_t payload = gcr_decode[value & 0x1f]; + payload |= gcr_decode[(value >> 5) & 0x1f] << 4; + payload |= gcr_decode[(value >> 10) & 0x1f] << 8; + payload |= gcr_decode[(value >> 15) & 0x1f] << 12; - } else if (period == 65408) { - // Special case for zero motion (e.g., stationary motor) - _erpms[output_channel] = 0; + // Calculate checksum + uint32_t checksum = payload; + checksum = checksum ^ (checksum >> 8); + checksum = checksum ^ (checksum >> NIBBLES_SIZE); - } else { - // Convert the period to eRPM - _erpms[output_channel] = (1000000 * 60 / 100 + period / 2) / period; - } + if ((checksum & 0xF) != 0xF) { + ++read_fail_crc[output_channel]; - // We set it ready anyway, not to hold up other channels when used in round robin. - _erpms_ready[output_channel] = true; -} + if (_consecutive_failures[output_channel]++ > BDSHOT_OFFLINE_COUNT) { + _consecutive_failures[output_channel] = BDSHOT_OFFLINE_COUNT; + _consecutive_successes[output_channel] = 0; + _bdshot_online[output_channel] = false; + } -/** -* bits 1-11 - throttle value (0-47 are reserved for commands, 48-2047 give 2000 steps of throttle resolution) -* bit 12 - dshot telemetry enable/disable -* bits 13-16 - XOR checksum -**/ -void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry) -{ - uint8_t timer_index = timer_io_channels[channel].timer_index; - uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1; - bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; - - if (!channel_initialized) { + _bdshot_processed[output_channel] = true; return; } - uint16_t packet = 0; - uint16_t checksum = 0; + ++read_ok[output_channel]; - packet |= data << DSHOT_THROTTLE_POSITION; - packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; - - uint16_t csum_data = packet; - - /* XOR checksum calculation */ - csum_data >>= NIBBLES_SIZE; - - for (uint8_t i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { - checksum ^= (csum_data & 0x0F); // XOR data by nibbles - csum_data >>= NIBBLES_SIZE; + if (_consecutive_successes[output_channel]++ > BDSHOT_OFFLINE_COUNT) { + _consecutive_successes[output_channel] = BDSHOT_OFFLINE_COUNT; + _consecutive_failures[output_channel] = 0; + _bdshot_online[output_channel] = true; } - if (_bidirectional) { - packet |= ((~checksum) & 0x0F); + // Convert payload into telem type/value + struct BDShotTelemetry packet = {}; + payload = (payload >> 4) & 0xFFF; + decode_dshot_telemetry(payload, &packet); - } else { - packet |= ((checksum) & 0x0F); - } + hrt_abstime now = hrt_absolute_time(); + switch (packet.type) { + case DSHOT_EDT_ERPM: { + _erpms[output_channel].erpm = packet.value; + _erpms[output_channel].ready = true; - const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer_index]; - uint8_t num_motors = mapping->channel_count_including_gaps; - uint8_t timer_channel = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel; - - for (uint8_t motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) { - dshot_output_buffer[timer_index][motor_data_index * num_motors + timer_channel] = - (packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first - packet <<= 1; - } -} - -int up_dshot_arm(bool armed) -{ - return io_timer_set_enable(armed, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, - IO_TIMER_ALL_MODES_CHANNELS); -} - -int up_bdshot_num_erpm_ready(void) -{ - int num_ready = 0; - - for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; ++i) { - if (_erpms_ready[i]) { - ++num_ready; + uint64_t last_timestamp = _erpms[output_channel].last_timestamp; + float last_rate_hz = _erpms[output_channel].rate_hz; + _erpms[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now); + _erpms[output_channel].last_timestamp = now; + break; } - } - return num_ready; -} + case DSHOT_EDT_TEMPERATURE: { + _edt_temp[output_channel].value = packet.value; + _edt_temp[output_channel].ready = true; -int up_bdshot_get_erpm(uint8_t output_channel, int *erpm) -{ - uint8_t timer_index = timer_io_channels[output_channel].timer_index; - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; - bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; - - if (channel_initialized) { - *erpm = _erpms[output_channel]; - _erpms_ready[output_channel] = false; - return PX4_OK; - } - - // this channel is not configured for dshot - return PX4_ERROR; -} - -int up_bdshot_channel_status(uint8_t channel) -{ - uint8_t timer_index = timer_io_channels[channel].timer_index; - uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1; - bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; - - // TODO: track that each channel is communicating using the decode stats - if (channel_initialized) { - return 1; - } - - return 0; -} - -void up_bdshot_status(void) -{ - PX4_INFO("dshot driver stats:"); - - if (_bidirectional) { - PX4_INFO("Bidirectional DShot enabled"); - } - - uint8_t timer_index = _bidi_timer_index; - - for (uint8_t timer_channel_index = 0; timer_channel_index < MAX_NUM_CHANNELS_PER_TIMER; timer_channel_index++) { - bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; - - if (channel_initialized) { - PX4_INFO("Timer %u, Channel %u: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", - timer_index, timer_channel_index, - read_ok[timer_channel_index], - read_fail_nibble[timer_channel_index], - read_fail_crc[timer_channel_index], - read_fail_zero[timer_channel_index]); + uint64_t last_timestamp = _edt_temp[output_channel].last_timestamp; + float last_rate_hz = _edt_temp[output_channel].rate_hz; + _edt_temp[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now); + _edt_temp[output_channel].last_timestamp = now; + break; } - } -} -uint8_t nibbles_from_mapped(uint8_t mapped) -{ - switch (mapped) { - case 0x19: - return 0x00; + case DSHOT_EDT_VOLTAGE: { + _edt_volt[output_channel].value = packet.value; + _edt_volt[output_channel].ready = true; - case 0x1B: - return 0x01; + uint64_t last_timestamp = _edt_volt[output_channel].last_timestamp; + float last_rate_hz = _edt_volt[output_channel].rate_hz; + _edt_volt[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now); + _edt_volt[output_channel].last_timestamp = now; + break; + } - case 0x12: - return 0x02; + case DSHOT_EDT_CURRENT: { + _edt_curr[output_channel].value = packet.value; + _edt_curr[output_channel].ready = true; - case 0x13: - return 0x03; + uint64_t last_timestamp = _edt_curr[output_channel].last_timestamp; + float last_rate_hz = _edt_curr[output_channel].rate_hz; + _edt_curr[output_channel].rate_hz = calculate_rate_hz(last_timestamp, last_rate_hz, now); + _edt_curr[output_channel].last_timestamp = now; + break; + } - case 0x1D: - return 0x04; - - case 0x15: - return 0x05; - - case 0x16: - return 0x06; - - case 0x17: - return 0x07; - - case 0x1a: - return 0x08; - - case 0x09: - return 0x09; - - case 0x0A: - return 0x0A; - - case 0x0B: - return 0x0B; - - case 0x1E: - return 0x0C; - - case 0x0D: - return 0x0D; - - case 0x0E: - return 0x0E; - - case 0x0F: - return 0x0F; + case DSHOT_EDT_STATE_EVENT: + // TODO: Handle these? + break; default: - // Unknown mapped - return 0xFF; + PX4_WARN("unknown EDT type %d", packet.type); + break; } + + _bdshot_processed[output_channel] = true; } -unsigned calculate_period(uint8_t timer_index, uint8_t channel_index) +float calculate_rate_hz(uint64_t last_timestamp, float last_rate_hz, uint64_t timestamp) +{ + if (last_timestamp == 0 || timestamp <= last_timestamp) { + return last_rate_hz; + } + + uint64_t dt_us = timestamp - last_timestamp; + + float instant_rate = 1000000.0f / dt_us; + + // Simple exponential moving average with fixed alpha + // Alpha = 0.125 (1/8) works well across all rates + float rate_hz = instant_rate * 0.125f + last_rate_hz * 0.875f; + + return rate_hz; +} + +// Converts captured edge timestamps into a raw bit stream. +// Measures the time intervals between signal edges to determine how many consecutive +// 1s or 0s to shift in, alternating the bit value with each edge transition. +// Returns a 20 bit raw value that still needs RLL and GCR decoding. +uint32_t convert_edge_intervals_to_bitstream(uint8_t channel_index) { uint32_t value = 0; uint32_t high = 1; // We start off with high @@ -783,46 +782,255 @@ unsigned calculate_period(uint8_t timer_index, uint8_t channel_index) if (shifted == 0) { // no data yet, or this time - ++read_fail_zero[channel_index]; return 0; } // We need to make sure we shifted 21 times. We might have missed some low "pulses" at the very end. value <<= (21 - shifted); - // From GCR to eRPM according to: - // https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#erpm-transmission - unsigned gcr = (value ^ (value >> 1)); + return value; +} - uint32_t data = 0; +void decode_dshot_telemetry(uint32_t payload, struct BDShotTelemetry *packet) +{ + // Extended DShot Telemetry + bool edt_enabled = _extended_dshot_telem; + uint32_t mantissa = payload & 0x01FF; + bool is_telemetry = (mantissa & 0x0100) == + 0; // if the msb of the mantissa is zero, then this is an extended telemetry packet - // 20bits -> 5 mapped -> 4 nibbles - for (unsigned i = 0; i < 4; ++i) { - uint32_t nibble = nibbles_from_mapped(gcr & 0x1F) << (4 * i); + if (edt_enabled && is_telemetry) { + packet->type = (payload & 0x0F00) >> 8; + packet->value = payload & 0x00FF; // extended telemetry value is 8 bits wide - if (nibble == 0xFF) { - ++read_fail_nibble[channel_index];; - return 0; + } else { + // otherwise it's an eRPM frame + uint8_t exponent = ((payload >> 9) & 0x7); // 3 bit: exponent + uint16_t period = (payload & 0x1FF); // 9 bit: period base + period = period << exponent; // Period in usec + + packet->type = DSHOT_EDT_ERPM; + + if (period == 65408) { + // Special case for zero motion (e.g., stationary motor) + packet->value = 0; + + } else { + packet->value = (1000000 * 60 / 100 + period / 2) / period; } + } +} - data |= nibble; - gcr >>= 5; +// bits 1-11 - throttle value (0-47 are reserved for commands, 48-2047 give 2000 steps of throttle resolution) +// bit 12 - dshot telemetry enable/disable +// bits 13-16 - XOR checksum +void dshot_motor_data_set(unsigned channel, uint16_t data, bool telemetry) +{ + uint8_t timer_index = timer_io_channels[channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1; + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + + if (!channel_initialized) { + return; } - unsigned shift = (data & 0xE000) >> 13; - unsigned period = ((data & 0x1FF0) >> 4) << shift; - unsigned crc = data & 0xF; + uint16_t packet = 0; + uint16_t checksum = 0; - unsigned payload = (data & 0xFFF0) >> 4; - unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F; + packet |= data << DSHOT_THROTTLE_POSITION; + packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; - if (crc != calculated_crc) { - ++read_fail_crc[channel_index];; + uint16_t csum_data = packet; + + // XOR checksum calculation + csum_data >>= NIBBLES_SIZE; + + for (uint8_t i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { + checksum ^= (csum_data & 0x0F); // XOR data by nibbles + csum_data >>= NIBBLES_SIZE; + } + + if (_bdshot_enabled) { + packet |= ((~checksum) & 0x0F); + + } else { + packet |= ((checksum) & 0x0F); + } + + const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer_index]; + uint8_t num_motors = mapping->channel_count_including_gaps; + uint8_t timer_channel = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel; + + for (uint8_t motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) { + dshot_output_buffer[timer_index][motor_data_index * num_motors + timer_channel] = + (packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first + packet <<= 1; + } +} + +int up_dshot_arm(bool armed) +{ + return io_timer_set_enable(armed, _bdshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); +} + +int up_bdshot_num_channels_ready(void) +{ + int num_ready = 0; + + for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; ++i) { + if (_bdshot_processed[i]) { + ++num_ready; + } + } + + return num_ready; +} + +int up_bdshot_num_errors(uint8_t channel) +{ + return read_fail_crc[channel]; +} + +int up_bdshot_get_erpm(uint8_t channel, int *erpm) +{ + uint8_t timer_index = timer_io_channels[channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1; + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + + int status = PX4_ERROR; + + if (channel_initialized && _erpms[channel].ready) { + *erpm = _erpms[channel].erpm; + status = PX4_OK; + } + + // Mark sample read + _bdshot_processed[channel] = false; + + return status; +} + +int up_bdshot_get_extended_telemetry(uint8_t channel, int type, uint8_t *value) +{ + int result = PX4_ERROR; + + switch (type) { + case DSHOT_EDT_TEMPERATURE: + if (_edt_temp[channel].ready) { + *value = _edt_temp[channel].value; + _edt_temp[channel].ready = false; + result = PX4_OK; + } + + break; + + case DSHOT_EDT_VOLTAGE: + if (_edt_volt[channel].ready) { + *value = _edt_volt[channel].value; + _edt_volt[channel].ready = false; + result = PX4_OK; + } + + break; + + case DSHOT_EDT_CURRENT: + if (_edt_curr[channel].ready) { + *value = _edt_curr[channel].value; + _edt_curr[channel].ready = false; + result = PX4_OK; + } + + break; + + default: + break; + } + + return result; +} + +int up_bdshot_get_extended_telemetry_rate(uint8_t channel, int type, int *value) +{ + int result = PX4_ERROR; + + switch (type) { + case DSHOT_EDT_TEMPERATURE: + if (_bdshot_online[channel]) { + *value = 0; + result = PX4_OK; + } + + break; + + case DSHOT_EDT_VOLTAGE: + if (_bdshot_online[channel]) { + *value = 0; + result = PX4_OK; + } + + break; + + case DSHOT_EDT_CURRENT: + if (_bdshot_online[channel]) { + *value = 0; + result = PX4_OK; + } + + break; + + default: + break; + } + + return result; +} + +int up_bdshot_channel_online(uint8_t channel) +{ + if (channel >= MAX_TIMER_IO_CHANNELS) { return 0; } - ++read_ok[channel_index];; - return period; + return _bdshot_online[channel]; +} + +void up_bdshot_status(void) +{ + PX4_INFO("dshot driver stats:"); + + if (_bdshot_enabled) { + PX4_INFO("BDShot enabled"); + } + + if (_extended_dshot_telem) { + PX4_INFO("BDShot EDT rates"); + + for (int i = 0; i < MAX_TIMER_IO_CHANNELS; i++) { + + if (_bdshot_online[i]) { + PX4_INFO("Ch%d: eRPM: %dHz Temp: %.2fHz Volt: %.2fHz Curr: %.2fHz", + i, + (int)_erpms[i].rate_hz, + (double)_edt_temp[i].rate_hz, + (double)_edt_volt[i].rate_hz, + (double)_edt_curr[i].rate_hz); + } + } + } + + uint8_t timer_index = _bidi_timer_index; + + for (uint8_t timer_channel_index = 0; timer_channel_index < MAX_NUM_CHANNELS_PER_TIMER; timer_channel_index++) { + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + + if (channel_initialized) { + PX4_INFO("Timer %u, Channel %u: read %lu, failed CRC %lu", + timer_index, timer_channel_index, + read_ok[timer_channel_index], + read_fail_crc[timer_channel_index]); + } + } } #endif diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 792d6076fc..7e10672850 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,11 +31,6 @@ * ****************************************************************************/ -/** - * @file drv_dshot.h - * - */ - #pragma once #include @@ -48,39 +43,40 @@ __BEGIN_DECLS -typedef enum { - DShot_cmd_motor_stop = 0, - DShot_cmd_beacon1, - DShot_cmd_beacon2, - DShot_cmd_beacon3, - DShot_cmd_beacon4, - DShot_cmd_beacon5, - DShot_cmd_esc_info, // V2 includes settings - DShot_cmd_spin_direction_1, - DShot_cmd_spin_direction_2, - DShot_cmd_3d_mode_off, - DShot_cmd_3d_mode_on, - DShot_cmd_settings_request, // Currently not implemented - DShot_cmd_save_settings, - DShot_cmd_spin_direction_normal = 20, - DShot_cmd_spin_direction_reversed = 21, - DShot_cmd_led0_on, // BLHeli32 only - DShot_cmd_led1_on, // BLHeli32 only - DShot_cmd_led2_on, // BLHeli32 only - DShot_cmd_led3_on, // BLHeli32 only - DShot_cmd_led0_off, // BLHeli32 only - DShot_cmd_led1_off, // BLHeli32 only - DShot_cmd_led2_off, // BLHeli32 only - DShot_cmd_led4_off, // BLHeli32 only - DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off - DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off - DShot_cmd_signal_line_telemetry_disable = 32, - DShot_cmd_signal_line_continuous_erpm_telemetry = 33, - DShot_cmd_MAX = 47, // >47 are throttle values - DShot_cmd_MIN_throttle = 48, - DShot_cmd_MAX_throttle = 2047 -} dshot_command_t; +// https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#special-commands +enum { + DSHOT_CMD_MOTOR_STOP = 0, + DSHOT_CMD_BEEP1 = 1, + DSHOT_CMD_ESC_INFO = 6, + DSHOT_CMD_SPIN_DIRECTION_1 = 7, + DSHOT_CMD_SPIN_DIRECTION_2 = 8, + DSHOT_CMD_3D_MODE_OFF = 9, + DSHOT_CMD_3D_MODE_ON = 10, + DSHOT_CMD_SAVE_SETTINGS = 12, + DSHOT_EXTENDED_TELEMETRY_ENABLE = 13, + DSHOT_CMD_ENTER_PROGRAMMING_MODE = 36, + DSHOT_CMD_EXIT_PROGRAMMING_MODE = 37, + DSHOT_CMD_MAX = 47, // >47 are throttle values + DSHOT_CMD_MIN_THROTTLE = 48, + DSHOT_CMD_MAX_THROTTLE = 2047 +}; +// Extended DShot Telemetry +enum { + DSHOT_EDT_ERPM = 0x00, + DSHOT_EDT_TEMPERATURE = 0x02, // C + DSHOT_EDT_VOLTAGE = 0x04, // 0.25V per step + DSHOT_EDT_CURRENT = 0x06, // A + DSHOT_EDT_DEBUG1 = 0x08, + DSHOT_EDT_DEBUG2 = 0x0A, + DSHOT_EDT_DEBUG3 = 0x0C, + DSHOT_EDT_STATE_EVENT = 0x0E, +}; + +struct BDShotTelemetry { + int type; + int32_t value; +}; /** * Intialise the Dshot outputs using the specified configuration. @@ -91,7 +87,8 @@ typedef enum { * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, or DSHOT600 * @return <0 on error, the initialized channels mask. */ -__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot); +__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool bdshot_enable, + bool enable_extended_dshot_telemetry); /** * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) @@ -107,7 +104,7 @@ __EXPORT extern void dshot_motor_data_set(unsigned channel, uint16_t throttle, b */ static inline void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) { - dshot_motor_data_set(channel, throttle + DShot_cmd_MIN_throttle, telemetry); + dshot_motor_data_set(channel, throttle + DSHOT_CMD_MIN_THROTTLE, telemetry); } /** @@ -142,7 +139,6 @@ __EXPORT extern int up_dshot_arm(bool armed); */ __EXPORT extern void up_bdshot_status(void); - /** * Get how many bidirectional erpm channels are ready * @@ -151,8 +147,14 @@ __EXPORT extern void up_bdshot_status(void); * * @return <0 on error, OK on succes */ -__EXPORT extern int up_bdshot_num_erpm_ready(void); +__EXPORT extern int up_bdshot_num_channels_ready(void); +/** + * Get the total number of errors for a channel + * @param channel Dshot channel + * @return The total number of recorded errors + */ +__EXPORT extern int up_bdshot_num_errors(uint8_t channel); /** * Get bidrectional dshot erpm for a channel @@ -162,6 +164,16 @@ __EXPORT extern int up_bdshot_num_erpm_ready(void); */ __EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm); +/** + * Get bidrectional dshot extended telemetry for a channel + * @param channel Dshot channel + * @param type The type of telemetry value to get + * @param value pointer to write the telemetry value + * @return <0 on error, OK on succes + */ +__EXPORT extern int up_bdshot_get_extended_telemetry(uint8_t channel, int type, uint8_t *value); + +__EXPORT extern int up_bdshot_get_extended_telemetry_rate(uint8_t channel, int type, int *value); /** * Get bidrectional dshot status for a channel @@ -169,7 +181,6 @@ __EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm); * @param erpm pointer to write the erpm value * @return <0 on error / not supported, 0 on offline, 1 on online */ -__EXPORT extern int up_bdshot_channel_status(uint8_t channel); - +__EXPORT extern int up_bdshot_channel_online(uint8_t channel); __END_DECLS diff --git a/src/drivers/dshot/CMakeLists.txt b/src/drivers/dshot/CMakeLists.txt index 82f7c3db51..856f2c12eb 100644 --- a/src/drivers/dshot/CMakeLists.txt +++ b/src/drivers/dshot/CMakeLists.txt @@ -42,9 +42,11 @@ px4_add_module( MAIN dshot COMPILE_FLAGS -DPARAM_PREFIX="${PARAM_PREFIX}" + # -DDEBUG_BUILD SRCS DShot.cpp DShotTelemetry.cpp + esc/AM32Settings.cpp DEPENDS arch_io_pins arch_dshot diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 92fec8e76e..a1dc731817 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +32,7 @@ ****************************************************************************/ #include "DShot.h" - #include - #include char DShot::_telemetry_device[] {}; @@ -58,370 +56,101 @@ DShot::~DShot() up_dshot_arm(false); perf_free(_cycle_perf); - perf_free(_bdshot_rpm_perf); - perf_free(_dshot_telem_perf); - - delete _telemetry; + perf_free(_bdshot_success_perf); + perf_free(_bdshot_error_perf); + perf_free(_bdshot_timeout_perf); + perf_free(_telem_success_perf); + perf_free(_telem_error_perf); + perf_free(_telem_timeout_perf); + perf_free(_telem_allsampled_perf); } int DShot::init() { - _output_mask = (1u << _num_outputs) - 1; - - // Getting initial parameter values update_params(); - ScheduleNow(); - - return OK; -} - -int DShot::task_spawn(int argc, char *argv[]) -{ - DShot *instance = new DShot(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init() == PX4_OK) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); + if (initialize_dshot()) { + ScheduleNow(); + return PX4_OK; } - delete instance; - _object.store(nullptr); - _task_id = -1; - return PX4_ERROR; } -void DShot::enable_dshot_outputs(const bool enabled) +void DShot::Run() { - if (enabled && !_outputs_initialized) { - unsigned int dshot_frequency = 0; - uint32_t dshot_frequency_param = 0; - - for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { - uint32_t channels = io_timer_get_group(timer); - - if (channels == 0) { - continue; - } - - char param_name[17]; - snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); - - int32_t tim_config = 0; - param_t handle = param_find(param_name); - param_get(handle, &tim_config); - unsigned int dshot_frequency_request = 0; - - if (tim_config == -5) { - dshot_frequency_request = DSHOT150; - - } else if (tim_config == -4) { - dshot_frequency_request = DSHOT300; - - } else if (tim_config == -3) { - dshot_frequency_request = DSHOT600; - - } else { - _output_mask &= ~channels; // don't use for dshot - } - - if (dshot_frequency_request != 0) { - if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) { - PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name); - param_set_no_notification(handle, &dshot_frequency_param); - - } else { - dshot_frequency = dshot_frequency_request; - dshot_frequency_param = tim_config; - } - } - } - - _bidirectional_dshot_enabled = _param_bidirectional_enable.get(); - - int ret = up_dshot_init(_output_mask, dshot_frequency, _bidirectional_dshot_enabled); - - if (ret < 0) { - PX4_ERR("up_dshot_init failed (%i)", ret); - return; - } - - _output_mask = ret; - - // disable unused functions - for (unsigned i = 0; i < _num_outputs; ++i) { - if (((1 << i) & _output_mask) == 0) { - _mixing_output.disableFunction(i); - - } - } - - if (_output_mask == 0) { - // exit the module if no outputs used - request_stop(); - return; - } - - _outputs_initialized = true; + if (should_exit()) { + ScheduleClear(); + _mixing_output.unregister(); + exit_and_cleanup(); + return; } - if (_outputs_initialized) { - up_dshot_arm(enabled); - _outputs_on = enabled; + perf_begin(_cycle_perf); + + _mixing_output.update(); + + if (process_serial_telemetry() || process_bdshot_telemetry()) { + _esc_status.timestamp = hrt_absolute_time(); + _esc_status.esc_count = count_set_bits(_output_mask); + _esc_status.counter++; + _esc_status_pub.publish(_esc_status); } + + if (_parameter_update_sub.updated()) { + update_params(); + } + + // Telemetry init hook + if (_request_telemetry_init.load()) { + init_telemetry(_telemetry_device, _telemetry_swap_rxtx); + _request_telemetry_init.store(false); + } + + handle_vehicle_commands(); + + // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) + _mixing_output.updateSubscriptions(true); + + perf_end(_cycle_perf); } -void DShot::update_num_motors() +bool DShot::updateOutputs(uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) { - int motor_count = 0; - - for (unsigned i = 0; i < _num_outputs; ++i) { - if (_mixing_output.isFunctionSet(i)) { - _actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i); - ++motor_count; - } - } - - _num_motors = motor_count; -} - -void DShot::init_telemetry(const char *device, bool swap_rxtx) -{ - if (!_telemetry) { - _telemetry = new DShotTelemetry{}; - - if (!_telemetry) { - PX4_ERR("alloc failed"); - return; - } - } - - if (device != NULL) { - int ret = _telemetry->init(device, swap_rxtx); - - if (ret != 0) { - PX4_ERR("telemetry init failed (%i)", ret); - } - } - - update_num_motors(); -} - -int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm) -{ - int ret = 0; - // fill in new motor data - esc_status_s &esc_status = esc_status_pub.get(); - - if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) { - esc_status.esc_online_flags |= 1 << telemetry_index; - - esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index]; - - if (!ignore_rpm) { - // If we also have bidirectional dshot, we use rpm and timestamps from there. - esc_status.esc[telemetry_index].timestamp = data.time; - esc_status.esc[telemetry_index].esc_rpm = (static_cast(data.erpm) * 100) / - (_param_mot_pole_count.get() / 2); - } - - esc_status.esc[telemetry_index].esc_voltage = static_cast(data.voltage) * 0.01f; - esc_status.esc[telemetry_index].esc_current = static_cast(data.current) * 0.01f; - esc_status.esc[telemetry_index].esc_temperature = static_cast(data.temperature); - // TODO: accumulate consumption and use for battery estimation - } - - // publish when motor index wraps (which is robust against motor timeouts) - if (telemetry_index <= _last_telemetry_index) { - esc_status.timestamp = hrt_absolute_time(); - esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; - esc_status.esc_count = _num_motors; - ++esc_status.counter; - - ret = 1; // Indicate we wrapped, so we publish data - } - - _last_telemetry_index = telemetry_index; - - perf_count(_dshot_telem_perf); - - return ret; -} - -void DShot::publish_esc_status(void) -{ - esc_status_s &esc_status = esc_status_pub.get(); - int telemetry_index = 0; - - // clear data of the esc that are offline - for (int index = 0; (index < _last_telemetry_index); index++) { - if ((esc_status.esc_online_flags & (1 << index)) == 0) { - memset(&esc_status.esc[index], 0, sizeof(struct esc_report_s)); - } - } - - // FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout - esc_status.esc_count = _num_motors; - esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; - esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; - - if (_bidirectional_dshot_enabled) { - for (unsigned i = 0; i < _num_outputs; i++) { - if (_mixing_output.isFunctionSet(i)) { - if (up_bdshot_channel_status(i)) { - esc_status.esc_online_flags |= 1 << i; - - } else { - esc_status.esc_online_flags &= ~(1 << i); - } - - ++telemetry_index; - } - } - } - - if (!esc_status_pub.advertised()) { - esc_status_pub.advertise(); - - } else { - esc_status_pub.update(); - } - - // reset esc online flags - esc_status.esc_online_flags = 0; -} - -int DShot::handle_new_bdshot_erpm(void) -{ - int num_erpms = 0; - int telemetry_index = 0; - int erpm; - esc_status_s &esc_status = esc_status_pub.get(); - - esc_status.timestamp = hrt_absolute_time(); - esc_status.counter = _esc_status_counter++; - esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; - esc_status.esc_armed_flags = _outputs_on; - - // We wait until all are ready. - if (up_bdshot_num_erpm_ready() < _num_motors) { - return 0; - } - - for (unsigned i = 0; i < _num_outputs; i++) { - if (_mixing_output.isFunctionSet(i)) { - if (up_bdshot_get_erpm(i, &erpm) == 0) { - num_erpms++; - esc_status.esc_online_flags |= 1 << telemetry_index; - esc_status.esc[telemetry_index].timestamp = hrt_absolute_time(); - esc_status.esc[telemetry_index].esc_rpm = (erpm * 100) / (_param_mot_pole_count.get() / 2); - esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index]; - } - - ++telemetry_index; - } - } - - perf_count(_bdshot_rpm_perf); - - return num_erpms; -} - -int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index) -{ - Command cmd{}; - cmd.command = command; - - if (motor_index == -1) { - cmd.motor_mask = 0xff; - - } else { - cmd.motor_mask = 1 << motor_index; - } - - cmd.num_repetitions = num_repetitions; - _new_command.store(&cmd); - - hrt_abstime timestamp_for_timeout = hrt_absolute_time(); - - // wait until main thread processed it - while (_new_command.load()) { - - if (hrt_elapsed_time(×tamp_for_timeout) < 2_s) { - px4_usleep(1000); - - } else { - _new_command.store(nullptr); - PX4_WARN("DShot command timeout!"); - } - } - - return 0; -} - -void DShot::mixerChanged() -{ - update_num_motors(); -} - -bool DShot::updateOutputs(uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) -{ - if (!_outputs_on) { + if (!count_set_bits(_output_mask)) { return false; } - int requested_telemetry_index = -1; + // Get the armed mask + _esc_status.esc_armed_flags = esc_armed_mask(outputs, num_outputs); - if (_telemetry) { - requested_telemetry_index = _telemetry->getRequestMotorIndex(); - } + // Set the state + _state = _esc_status.esc_armed_flags ? State::Armed : State::Disarmed; - int telemetry_index = 0; - - for (int i = 0; i < (int)num_outputs; i++) { - - uint16_t output = outputs[i]; - - if (output == DSHOT_DISARM_VALUE) { - - if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) { - up_dshot_motor_command(i, _current_command.command, true); - - } else { - up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index); - } - - } else { - - if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) { - output = convert_output_to_3d_scaling(output); - } - - up_dshot_motor_data_set(i, math::min(output, static_cast(DSHOT_MAX_THROTTLE)), - telemetry_index == requested_telemetry_index); + switch (_state) { + case State::Armed: { + update_motor_outputs(outputs, num_outputs); + break; } - telemetry_index += _mixing_output.isFunctionSet(i); - } + case State::Disarmed: { - // Decrement the command counter - if (_current_command.valid()) { - --_current_command.num_repetitions; + // Select next command to send (if any) + if (_telemetry.telemetryResponseFinished() && + _current_command.finished() && _telemetry.commandResponseFinished()) { + select_next_command(); + } - // Queue a save command after the burst if save has been requested - if (_current_command.num_repetitions == 0 && _current_command.save) { - _current_command.save = false; - _current_command.num_repetitions = 10; - _current_command.command = dshot_command_t::DShot_cmd_save_settings; + // Send command if available + if (!_current_command.finished()) { + update_motor_commands(num_outputs); + + } else { + // Otherwise idle + update_motor_outputs(outputs, num_outputs); + } + + break; } } @@ -430,6 +159,529 @@ bool DShot::updateOutputs(uint16_t outputs[MAX_ACTUATORS], return true; } +// TODO: this needs a refactor +void DShot::select_next_command() +{ + // Settings Programming + // NOTE: only update when we're not actively programming an ESC + if (!_dshot_programming_active) { + + // Get the next update from the queue? + if (_am32_eeprom_write_sub.updated()) { + + // TODO: because uORB can't queue?? (what is the point of the ORB_QUEUE_LENGTH then??) + auto last = _am32_eeprom_write_sub.get_last_generation(); + _am32_eeprom_write_sub.copy(&_am32_eeprom_write); + auto current = _am32_eeprom_write_sub.get_last_generation(); + + if (current != last + 1) { + PX4_ERR("am32_eeprom_write lost, generation %u -> %u", last, current); + } + + PX4_INFO("ESC%u: starting programming mode", _am32_eeprom_write.index + 1); + _dshot_programming_active = true; + } + } + + // Command order or priority: + // - EDT Request + // - Settings Request + // - Settings Programming + + // EDT Request mask + uint8_t needs_edt_request_mask = _bdshot_telem_online_mask & ~_bdshot_edt_requested_mask; + + // Settings Request mask + uint8_t needs_settings_request_mask = _serial_telem_online_mask & ~_settings_requested_mask; + + _current_command.clear(); + + if (_param_dshot_bidir_en.get() && _param_dshot_bidir_edt.get() && needs_edt_request_mask) { + // EDT Request first + int next_motor_index = 0; + + for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { + if (needs_edt_request_mask & (1 << i)) { + next_motor_index = i; + break; + } + } + + auto now = hrt_absolute_time(); + _current_command.num_repetitions = 10; + _current_command.command = DSHOT_EXTENDED_TELEMETRY_ENABLE; + _current_command.motor_mask = (1 << next_motor_index); + _bdshot_edt_requested_mask |= (1 << next_motor_index); + PX4_INFO("ESC%d: requesting EDT at time %.2fs", next_motor_index + 1, (double)now / 1000000.); + + } else if (_param_dshot_tel_cfg.get() && _param_dshot_esc_type.get() && needs_settings_request_mask) { + // Settings Request next + int next_motor_index = 0; + + for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { + if (needs_settings_request_mask & (1 << i)) { + next_motor_index = i; + break; + } + } + + auto now = hrt_absolute_time(); + _current_command.num_repetitions = 6; + _current_command.command = DSHOT_CMD_ESC_INFO; + _current_command.motor_mask = (1 << next_motor_index); + _current_command.expect_response = true; + _settings_requested_mask |= (1 << next_motor_index); + PX4_INFO("ESC%d: requesting Settings at time %.2fs", next_motor_index + 1, (double)now / 1000000.); + + } else if (_dshot_programming_active) { + // Settings programming state machine + if (_programming_state == ProgrammingState::Idle) { + // Get next setting address/value to program + int next_index = -1; + + // Find settings that need to be written but haven't been yet + for (int i = 0; i < 48; i++) { + int array_index = i / 32; + int bit_index = i % 32; + + bool needs_write = _am32_eeprom_write.write_mask[array_index] & (1 << bit_index); + bool already_written = _settings_written_mask[array_index] & (1 << bit_index); + + if (needs_write && !already_written) { + next_index = i; + break; + } + } + + if (next_index >= 0) { + // Set up the motor mask based on the index in the write request + if (_am32_eeprom_write.index == 255) { + // _current_command.motor_mask = 0xFF; // Apply to all ESCs + PX4_INFO("ESC ALL: Writing setting at index %d, value %u", next_index, _am32_eeprom_write.data[next_index]); + + } else { + // _current_command.motor_mask = (1 << _am32_eeprom_write.index); + PX4_INFO("ESC%d: Writing setting at index %d, value %u", _am32_eeprom_write.index + 1, next_index, + _am32_eeprom_write.data[next_index]); + } + + _programming_address = next_index; + _programming_value = _am32_eeprom_write.data[next_index]; + _programming_state = ProgrammingState::EnterMode; + + // Pre-emptively Mark this setting as written + int array_index = next_index / 32; + int bit_index = next_index % 32; + _settings_written_mask[array_index] |= (1 << bit_index); + + } else { + // All settings have been written + PX4_INFO("All settings written!"); + _dshot_programming_active = false; + // _programming_state = ProgrammingState::Save; + _current_command.command = DSHOT_CMD_SAVE_SETTINGS; + _current_command.num_repetitions = 6; + _current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index); + _programming_state = ProgrammingState::Idle; + + // TODO: do we want to re-request and compare? + // Clear the written mask for this motor for next time + _settings_written_mask[0] = 0; + _settings_written_mask[1] = 0; + + // Mark as offline and unread so that we read again + // _serial_telem_online_mask &= ~(_am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index)); + _settings_requested_mask &= ~(_am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index)); + + _telem_delay_until = hrt_absolute_time() + 100_ms; + } + } + + switch (_programming_state) { + case ProgrammingState::EnterMode: + _current_command.command = DSHOT_CMD_ENTER_PROGRAMMING_MODE; + _current_command.num_repetitions = 6; + _current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index); + _programming_state = ProgrammingState::SendAddress; + break; + + case ProgrammingState::SendAddress: + _current_command.command = _programming_address; + _current_command.num_repetitions = 1; + _current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index); + _programming_state = ProgrammingState::SendValue; + break; + + case ProgrammingState::SendValue: + _current_command.command = _programming_value; + _current_command.num_repetitions = 1; + _current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index); + _programming_state = ProgrammingState::ExitMode; + break; + + case ProgrammingState::ExitMode: + _current_command.command = DSHOT_CMD_EXIT_PROGRAMMING_MODE; + _current_command.num_repetitions = 1; + _current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index); + _programming_state = ProgrammingState::Idle; + break; + + default: + break; + } + } +} + +void DShot::update_motor_outputs(uint16_t outputs[MAX_ACTUATORS], int num_outputs) +{ + for (int i = 0; i < num_outputs; i++) { + + if (!_mixing_output.isMotor(i)) { + up_dshot_motor_command(i, DSHOT_CMD_MOTOR_STOP, false); + continue; + } + + bool set_telemetry_bit = false; + + if (_telemetry_motor_index == i) { + if (_param_dshot_tel_cfg.get() && _telemetry.telemetryResponseFinished() && _telemetry.commandResponseFinished()) { + if (hrt_absolute_time() > _telem_delay_until) { + set_telemetry_bit = true; + _telemetry.startTelemetryRequest(); + } + } + } + + if (outputs[i] == DSHOT_DISARM_VALUE) { + up_dshot_motor_command(i, DSHOT_CMD_MOTOR_STOP, set_telemetry_bit); + + } else { + up_dshot_motor_data_set(i, calculate_output_value(outputs[i], i), set_telemetry_bit); + } + } +} + +void DShot::update_motor_commands(int num_outputs) +{ + bool command_sent = false; + + for (int i = 0; i < num_outputs; i++) { + + uint16_t command = DSHOT_CMD_MOTOR_STOP; + + if (_mixing_output.isMotor(i)) { + + int motor_index = (int)_mixing_output.outputFunction(i) - (int)OutputFunction::Motor1; + + if (_current_command.motor_mask & (1 << motor_index)) { + + if (_current_command.expect_response) { + _telemetry.setExpectCommandResponse(motor_index, _current_command.command); + } + + // PX4_INFO("Writing: ESC%d, value: %u", motor_index + 1, _current_command.command); + command = _current_command.command; + command_sent = true; + } + } + + up_dshot_motor_command(i, command, false); + } + + if (command_sent) { + --_current_command.num_repetitions; + + // Queue a save command if it has been requested + if (_current_command.num_repetitions == 0 && _current_command.save) { + _current_command.save = false; + _current_command.num_repetitions = 10; + _current_command.command = DSHOT_CMD_SAVE_SETTINGS; + } + } +} + +uint8_t DShot::esc_armed_mask(uint16_t *outputs, int num_outputs) +{ + uint8_t mask = 0; + + for (int i = 0; i < num_outputs; i++) { + if (_mixing_output.isMotor(i)) { + if (outputs[i] != DSHOT_DISARM_VALUE) { + mask |= (1 << i); + } + } + } + + return mask; +} + +uint16_t DShot::calculate_output_value(uint16_t raw, int index) +{ + uint16_t output = raw; + + // Reverse output if required + if (_param_dshot_3d_enable.get() || (_mixing_output.reversibleOutputs() & (1u << index))) { + output = convert_output_to_3d_scaling(raw); + } + + output = math::min(output, DSHOT_MAX_THROTTLE); + + return output; +} + +bool DShot::process_serial_telemetry() +{ + if (!_param_dshot_tel_cfg.get()) { + return false; + } + + bool all_telem_sampled = false; + + if (!_telemetry.commandResponseFinished()) { + _telemetry.parseCommandResponse(); + + } else { + + EscData esc {}; + esc.motor_index = _telemetry_motor_index; + + switch (_telemetry.parseTelemetryPacket(&esc)) { + case TelemetryStatus::NotStarted: + // no-op, should not hit this case + break; + + case TelemetryStatus::NotReady: + // no-op, will eventually timeout + break; + + case TelemetryStatus::Ready: + + if (_serial_telem_online_mask & (1 << _telemetry_motor_index)) { + consume_esc_data(esc, TelemetrySource::Serial); + all_telem_sampled = set_next_telemetry_index(); + perf_count(_telem_success_perf); + + } else { + hrt_abstime now = hrt_absolute_time(); + + if (_serial_telem_online_timestamps[_telemetry_motor_index] == 0) { + _serial_telem_online_timestamps[_telemetry_motor_index] = now; + } + + // Mark as online only after 100_ms without errors + if (now - _serial_telem_online_timestamps[_telemetry_motor_index] > 100_ms) { + _serial_telem_online_mask |= (1 << _telemetry_motor_index); + } + } + + break; + + case TelemetryStatus::Timeout: + // Set ESC data to zeroes + // PX4_WARN("Telem timeout"); + _serial_telem_errors[_telemetry_motor_index]++; + _serial_telem_online_mask &= ~(1 << _telemetry_motor_index); + _serial_telem_online_timestamps[_telemetry_motor_index] = 0; + // Consume an empty EscData to zero the data + consume_esc_data(esc, TelemetrySource::Serial); + all_telem_sampled = set_next_telemetry_index(); + perf_count(_telem_timeout_perf); + break; + + case TelemetryStatus::ParseError: + // Set ESC data to zeroes + PX4_WARN("Telem parse error"); + _serial_telem_errors[_telemetry_motor_index]++; + _serial_telem_online_mask &= ~(1 << _telemetry_motor_index); + _serial_telem_online_timestamps[_telemetry_motor_index] = 0; + // Consume an empty EscData to zero the data + consume_esc_data(esc, TelemetrySource::Serial); + all_telem_sampled = set_next_telemetry_index(); + _telem_delay_until = hrt_absolute_time() + 100_ms; // TODO: how long do we need to wait? + perf_count(_telem_error_perf); + break; + } + } + + return all_telem_sampled; +} + +bool DShot::set_next_telemetry_index() +{ + int start_index = (_telemetry_motor_index + 1) % DSHOT_MAXIMUM_CHANNELS; + int next_motor_index = (_telemetry_motor_index + 1) % DSHOT_MAXIMUM_CHANNELS; + + do { + bool is_motor = _mixing_output.isMotor(next_motor_index); + bool already_requested = _telemetry_requested_mask & (1 << next_motor_index); + + if (is_motor && !already_requested) { + _telemetry_motor_index = next_motor_index; + _telemetry_requested_mask |= (1 << next_motor_index); + break; + } + + next_motor_index = (next_motor_index + 1) % DSHOT_MAXIMUM_CHANNELS; + } while (next_motor_index != start_index); + + // Check if all motors have been sampled + if (count_set_bits(_telemetry_requested_mask) >= count_set_bits(_output_mask)) { + _telemetry_requested_mask = 0; + perf_count(_telem_allsampled_perf); + return true; + } + + return false; +} + +bool DShot::process_bdshot_telemetry() +{ + if (!_param_dshot_bidir_en.get()) { + return false; + } + + hrt_abstime now = hrt_absolute_time(); + + // Don't try to process any telem data until after ESCs have been given time to boot + if (now < _telem_delay_until) { + return false; + } + + // We wait until all are ready. + if (up_bdshot_num_channels_ready() < count_set_bits(_output_mask)) { + return false; + } + + for (unsigned output_channel = 0; output_channel < DSHOT_MAXIMUM_CHANNELS; output_channel++) { + if (!_mixing_output.isMotor(output_channel)) { + continue; + } + + // TODO: handle Extended Telemetry -- Volt/Curr/Temp + // We won't want to zero out the EscData, and instead + // use the previously set values. + + EscData esc = {}; + + // NOTE: dshot erpm order is actuator channel order, so we map to motor index here + int motor_index = (int)_mixing_output.outputFunction(output_channel) - (int)OutputFunction::Motor1; + + if ((motor_index >= 0) && (motor_index < esc_status_s::CONNECTED_ESC_MAX)) { + + esc.motor_index = motor_index; + esc.timestamp = now; + + _bdshot_telem_errors[motor_index] = up_bdshot_num_errors(output_channel); + + if (up_bdshot_channel_online(output_channel)) { + _bdshot_telem_online_mask |= (1 << motor_index); + + // Only update RPM if online + int erpm = 0; + + if (up_bdshot_get_erpm(output_channel, &erpm) == PX4_OK) { + esc.erpm = erpm * 100; + + } else { + esc.erpm = _esc_status.esc[motor_index].esc_rpm * (_param_mot_pole_count.get() / + 2); // use previous and convert back to rpm + } + + // Extended DShot Telemetry + if (_param_dshot_bidir_edt.get()) { + + uint8_t value = 0; + + if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_TEMPERATURE, &value) == PX4_OK) { + esc.temperature = value; // BDShot temperature is in C + // PX4_INFO("ESC%d: temperature: %f", motor_index, (double)esc.temperature); + + } else { + esc.temperature = _esc_status.esc[motor_index].esc_temperature; // use previous + } + + if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_VOLTAGE, &value) == PX4_OK) { + esc.voltage = value * 0.25f; // BDShot voltage is in 0.25V + // PX4_INFO("ESC%d: voltage: %f", motor_index, (double)esc.voltage); + + } else { + esc.voltage = _esc_status.esc[motor_index].esc_voltage; // use previous + } + + if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_CURRENT, &value) == PX4_OK) { + esc.current = value * 0.5f; // BDShot current is in 0.5V + // PX4_INFO("ESC%d: current: %f", motor_index, (double)esc.current); + + } else { + esc.current = _esc_status.esc[motor_index].esc_current; // use previous + } + } + + } else { + _bdshot_telem_online_mask &= ~(1 << motor_index); + _bdshot_edt_requested_mask &= ~(1 << motor_index); // re-triggers EDT request when it comes back online + perf_count(_bdshot_error_perf); + } + + consume_esc_data(esc, TelemetrySource::BDShot); + } + } + + perf_count(_bdshot_success_perf); + + return true; +} + +void DShot::consume_esc_data(const EscData &esc, TelemetrySource source) +{ + bool bdshot_telemetry_enabled = _param_dshot_bidir_en.get(); + bool serial_telemetry_enabled = _param_dshot_tel_cfg.get(); + + if (esc.motor_index >= esc_status_s::CONNECTED_ESC_MAX) { + return; + } + + // Require both sources online when enabled + uint8_t online_mask = 0xFF; + + if (bdshot_telemetry_enabled) { + online_mask &= _bdshot_telem_online_mask; + } + + if (serial_telemetry_enabled) { + online_mask &= _serial_telem_online_mask; + } + + _esc_status.esc_online_flags = online_mask; + + // Sum the errors from both interfaces + _esc_status.esc[esc.motor_index].esc_errorcount = _serial_telem_errors[esc.motor_index] + + _bdshot_telem_errors[esc.motor_index]; + + if (source == TelemetrySource::Serial) { + // Only use SerialTelemetry eRPM when BDShot is disabled + if (!bdshot_telemetry_enabled) { + _esc_status.esc[esc.motor_index].timestamp = esc.timestamp; + _esc_status.esc[esc.motor_index].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2); + } + + _esc_status.esc[esc.motor_index].esc_voltage = esc.voltage; + _esc_status.esc[esc.motor_index].esc_current = esc.current; + _esc_status.esc[esc.motor_index].esc_temperature = esc.temperature; + + } else if (source == TelemetrySource::BDShot) { + _esc_status.esc[esc.motor_index].timestamp = esc.timestamp; + _esc_status.esc[esc.motor_index].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2); + + // Only use BDShot Volt/Curr/Temp when Serial Telemetry is disabled + if (!serial_telemetry_enabled) { + _esc_status.esc[esc.motor_index].esc_voltage = esc.voltage; + _esc_status.esc[esc.motor_index].esc_current = esc.current; + _esc_status.esc[esc.motor_index].esc_temperature = esc.temperature; + } + } +} + uint16_t DShot::convert_output_to_3d_scaling(uint16_t output) { // DShot 3D splits the throttle ranges in two. @@ -460,161 +712,122 @@ uint16_t DShot::convert_output_to_3d_scaling(uint16_t output) return output; } -void DShot::Run() -{ - if (should_exit()) { - ScheduleClear(); - _mixing_output.unregister(); - - exit_and_cleanup(); - return; - } - - perf_begin(_cycle_perf); - - _mixing_output.update(); - - // update output status if armed or if mixer is loaded - bool outputs_on = true; - - if (_outputs_on != outputs_on) { - enable_dshot_outputs(outputs_on); - } - - if (_telemetry) { - const int telem_update = _telemetry->update(_num_motors); - - if (telem_update >= 0) { - const int need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->latestESCData(), - _bidirectional_dshot_enabled); - - // We don't want to publish twice, once by telemetry and once by bidirectional dishot. - if (!_bidirectional_dshot_enabled && need_to_publish) { - publish_esc_status(); - } - } - } - - if (_bidirectional_dshot_enabled) { - // Add bdshot data to esc status - const int need_to_publish = handle_new_bdshot_erpm(); - - if (need_to_publish) { - publish_esc_status(); - } - } - - if (_parameter_update_sub.updated()) { - update_params(); - } - - // telemetry device update request? - if (_request_telemetry_init.load()) { - init_telemetry(_telemetry_device, _telemetry_swap_rxtx); - _request_telemetry_init.store(false); - } - - // new command? - if (!_current_command.valid()) { - Command *new_command = _new_command.load(); - - if (new_command) { - _current_command = *new_command; - _new_command.store(nullptr); - } - } - - handle_vehicle_commands(); - - if (!_mixing_output.armed().armed) { - if (_reversible_outputs != _mixing_output.reversibleOutputs()) { - _reversible_outputs = _mixing_output.reversibleOutputs(); - update_params(); - } - } - - // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) - _mixing_output.updateSubscriptions(true); - - perf_end(_cycle_perf); -} - void DShot::handle_vehicle_commands() { - vehicle_command_s vehicle_command; + vehicle_command_s command = {}; - while (!_current_command.valid() && _vehicle_command_sub.update(&vehicle_command)) { + while (_current_command.finished() && _vehicle_command_sub.update(&command)) { - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR) { - int function = (int)(vehicle_command.param5 + 0.5); + switch (command.command) { + case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: + handle_configure_actuator(command); + break; - if (function < 1000) { - const int first_motor_function = 1; // from MAVLink ACTUATOR_OUTPUT_FUNCTION - const int first_servo_function = 33; + case vehicle_command_s::VEHICLE_CMD_AM32_REQUEST_EEPROM: + handle_am32_request_eeprom(command); + break; - if (function >= first_motor_function && function < first_motor_function + actuator_test_s::MAX_NUM_MOTORS) { - function = function - first_motor_function + actuator_test_s::FUNCTION_MOTOR1; - - } else if (function >= first_servo_function && function < first_servo_function + actuator_test_s::MAX_NUM_SERVOS) { - function = function - first_servo_function + actuator_test_s::FUNCTION_SERVO1; - - } else { - function = INT32_MAX; - } - - } else { - function -= 1000; - } - - int type = (int)(vehicle_command.param1 + 0.5f); - int index = -1; - - for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { - if ((int)_mixing_output.outputFunction(i) == function) { - index = i; - } - } - - vehicle_command_ack_s command_ack{}; - command_ack.command = vehicle_command.command; - command_ack.target_system = vehicle_command.source_system; - command_ack.target_component = vehicle_command.source_component; - command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; - - if (index != -1) { - PX4_DEBUG("setting command: index: %i type: %i", index, type); - _current_command.command = dshot_command_t::DShot_cmd_motor_stop; - - switch (type) { - case 1: _current_command.command = dshot_command_t::DShot_cmd_beacon1; break; - - case 2: _current_command.command = dshot_command_t::DShot_cmd_3d_mode_on; break; - - case 3: _current_command.command = dshot_command_t::DShot_cmd_3d_mode_off; break; - - case 4: _current_command.command = dshot_command_t::DShot_cmd_spin_direction_1; break; - - case 5: _current_command.command = dshot_command_t::DShot_cmd_spin_direction_2; break; - } - - if (_current_command.command == dshot_command_t::DShot_cmd_motor_stop) { - PX4_WARN("unknown command: %i", type); - - } else { - command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - _current_command.motor_mask = 1 << index; - _current_command.num_repetitions = 10; - _current_command.save = true; - } - - } - - command_ack.timestamp = hrt_absolute_time(); - _command_ack_pub.publish(command_ack); + default: + break; } } } +void DShot::handle_configure_actuator(const vehicle_command_s &command) +{ + int function = (int)(command.param5 + 0.5); + + PX4_INFO("Received VEHICLE_CMD_CONFIGURE_ACTUATOR"); + + int motor_index = -1; + + if (function >= (int)OutputFunction::Motor1 && function < ((int)OutputFunction::Motor1 + DSHOT_MAXIMUM_CHANNELS)) { + for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; ++i) { + if ((int)_mixing_output.outputFunction(i) == function && _mixing_output.isMotor(i)) { + motor_index = (int)_mixing_output.outputFunction(i) - (int)OutputFunction::Motor1; + } + } + } + + vehicle_command_ack_s command_ack = {}; + command_ack.command = command.command; + command_ack.target_system = command.source_system; + command_ack.target_component = command.source_component; + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + + if ((motor_index >= 0) || (motor_index < DSHOT_MAXIMUM_CHANNELS)) { + int type = (int)(command.param1 + 0.5f); + PX4_INFO("motor_index: %i type: %i", motor_index, type); + _current_command.clear(); + _current_command.command = DSHOT_CMD_MOTOR_STOP; + _current_command.num_repetitions = 10; + _current_command.save = false; + + + switch (type) { + case ACTUATOR_CONFIGURATION_BEEP: + _current_command.command = DSHOT_CMD_BEEP1; + break; + + case ACTUATOR_CONFIGURATION_3D_MODE_OFF: + _current_command.command = DSHOT_CMD_3D_MODE_OFF; + _current_command.save = true; + break; + + case ACTUATOR_CONFIGURATION_3D_MODE_ON: + _current_command.command = DSHOT_CMD_3D_MODE_ON; + _current_command.save = true; + break; + + case ACTUATOR_CONFIGURATION_SPIN_DIRECTION1: + _current_command.command = DSHOT_CMD_SPIN_DIRECTION_1; + _current_command.save = true; + break; + + case ACTUATOR_CONFIGURATION_SPIN_DIRECTION2: + _current_command.command = DSHOT_CMD_SPIN_DIRECTION_2; + _current_command.save = true; + break; + + default: + PX4_WARN("unknown command: %i", type); + break; + } + + if (_current_command.command != DSHOT_CMD_MOTOR_STOP) { + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + _current_command.motor_mask = 1 << motor_index; + } + } + + command_ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(command_ack); +} + +void DShot::handle_am32_request_eeprom(const vehicle_command_s &command) +{ + PX4_INFO("Received AM32_REQUEST_EEPROM"); + PX4_INFO("index: %d", (int)command.param1); + + int index = command.param1; + + // Mark as unread to re-trigger settings request + if (index == 255) { + _settings_requested_mask = 0; + + } else { + _settings_requested_mask &= ~(1 << index); + } + + vehicle_command_ack_s command_ack = {}; + command_ack.command = command.command; + command_ack.target_system = command.source_system; + command_ack.target_component = command.source_component; + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + command_ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(command_ack); +} + void DShot::update_params() { parameter_update_s pupdate; @@ -622,24 +835,180 @@ void DShot::update_params() updateParams(); - // we use a minimum value of 1, since 0 is for disarmed - _mixing_output.setAllMinValues(math::constrain(static_cast((_param_dshot_min.get() * - static_cast(DSHOT_MAX_THROTTLE))), - DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE)); + // Calculate minimum DShot output as percent of throttle and constrain. + float min_value = _param_dshot_min.get() * (float)DSHOT_MAX_THROTTLE; + uint16_t dshot_min_value = math::constrain((uint16_t)min_value, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE); + + _mixing_output.setAllMinValues(dshot_min_value); // Do not use the minimum parameter for reversible outputs - for (unsigned i = 0; i < _num_outputs; ++i) { - if ((1 << i) & _reversible_outputs) { + for (unsigned i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + if (_mixing_output.reversibleOutputs() & (1 << i)) { _mixing_output.minValue(i) = DSHOT_MIN_THROTTLE; } } } +void DShot::mixerChanged() +{ + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; + + int motor_count = 0; + + for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { + + if (_mixing_output.isMotor(i)) { + _esc_status.esc[i].actuator_function = (uint8_t)_mixing_output.outputFunction(i); + motor_count++; + + if (!(_output_mask & (1 << i))) { + PX4_INFO("Enabling channel %d", i); + } + + _output_mask |= (1 << i); + + } else { + if ((_output_mask & (1 << i))) { + PX4_INFO("Disabling channel %d", i); + } + + _output_mask &= ~(1 << i); + } + } + + // TODO: re-init dshot.c output channels and stuff +} + +bool DShot::initialize_dshot() +{ + unsigned int dshot_frequency = 0; + uint32_t dshot_frequency_param = 0; + + _output_mask = 0; + + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + + // Get mask of actuator channels associated with this timer group + uint32_t channels = io_timer_get_group(timer); + + if (channels == 0) { + continue; + } + + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); + + int32_t tim_config = 0; + param_t handle = param_find(param_name); + param_get(handle, &tim_config); + unsigned int dshot_frequency_request = 0; + + if (tim_config == -5) { + dshot_frequency_request = DSHOT150; + _output_mask |= channels; + + } else if (tim_config == -4) { + dshot_frequency_request = DSHOT300; + _output_mask |= channels; + + } else if (tim_config == -3) { + dshot_frequency_request = DSHOT600; + _output_mask |= channels; + } + + if (dshot_frequency_request != 0) { + if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) { + PX4_WARN("Only supporting a single frequency (%u), adjusting param %s", dshot_frequency, param_name); + param_set_no_notification(handle, &dshot_frequency_param); + + } else { + dshot_frequency = dshot_frequency_request; + dshot_frequency_param = tim_config; + } + } + } + + int ret = up_dshot_init(_output_mask, dshot_frequency, _param_dshot_bidir_en.get(), _param_dshot_bidir_edt.get()); + + if (ret < 0) { + PX4_ERR("up_dshot_init failed (%i)", ret); + return false; + } + + if ((uint32_t)ret != _output_mask) { + PX4_INFO("Failed to configure some channels"); + PX4_INFO("requested: 0x%lx", _output_mask); + PX4_INFO("configured: 0x%lx", (uint32_t)ret); + _output_mask = ret; + } + + // Set our mixer to explicitly disable channels we do not control + for (unsigned i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + if (((1 << i) & _output_mask) == 0) { + _mixing_output.disableFunction(i); + } + } + + if (_output_mask == 0) { + PX4_WARN("No channels configured"); + return false; + } + + up_dshot_arm(true); + + return true; +} + +void DShot::init_telemetry(const char *device, bool swap_rxtx) +{ + if (!device) { + return; + } + + if (_telemetry.init(device, swap_rxtx) != PX4_OK) { + PX4_ERR("telemetry init failed"); + } + + // Initialize ESC settings handlers based on ESC type + ESCType esc_type = static_cast(_param_dshot_esc_type.get()); + _telemetry.initSettingsHandlers(esc_type, _output_mask); + + // TODO: enforce uavcan init ordering in some way + // Advertise early to ensure we beat uavcan + _esc_status_pub.advertise(); +} + +int DShot::print_status() +{ + PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask); + perf_print_counter(_cycle_perf); + perf_print_counter(_bdshot_success_perf); + perf_print_counter(_bdshot_error_perf); + perf_print_counter(_bdshot_timeout_perf); + + perf_print_counter(_telem_success_perf); + perf_print_counter(_telem_error_perf); + perf_print_counter(_telem_timeout_perf); + perf_print_counter(_telem_allsampled_perf); + + _mixing_output.printStatus(); + + if (_param_dshot_tel_cfg.get()) { + PX4_INFO("telemetry on: %s", _telemetry_device); + _telemetry.printStatus(); + } + + if (_param_dshot_bidir_en.get()) { + up_bdshot_status(); + } + + return 0; +} + int DShot::custom_command(int argc, char *argv[]) { const char *verb = argv[0]; - int motor_index = -1; // select motor index, default: -1=all int myoptind = 1; bool swap_rxtx = false; const char *device_name = nullptr; @@ -648,10 +1017,6 @@ int DShot::custom_command(int argc, char *argv[]) while ((ch = px4_getopt(argc, argv, "m:xd:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - case 'm': - motor_index = strtol(myoptarg, nullptr, 10) - 1; - break; - case 'x': swap_rxtx = true; break; @@ -677,36 +1042,6 @@ int DShot::custom_command(int argc, char *argv[]) return 0; } - struct VerbCommand { - const char *name; - dshot_command_t command; - int num_repetitions; - }; - - constexpr VerbCommand commands[] = { - {"reverse", DShot_cmd_spin_direction_2, 10}, - {"normal", DShot_cmd_spin_direction_1, 10}, - {"save", DShot_cmd_save_settings, 10}, - {"3d_on", DShot_cmd_3d_mode_on, 10}, - {"3d_off", DShot_cmd_3d_mode_off, 10}, - {"beep1", DShot_cmd_beacon1, 1}, - {"beep2", DShot_cmd_beacon2, 1}, - {"beep3", DShot_cmd_beacon3, 1}, - {"beep4", DShot_cmd_beacon4, 1}, - {"beep5", DShot_cmd_beacon5, 1}, - }; - - for (unsigned i = 0; i < sizeof(commands) / sizeof(commands[0]); ++i) { - if (!strcmp(verb, commands[i].name)) { - if (!is_running()) { - PX4_ERR("module not running"); - return -1; - } - - return get_instance()->send_command_thread_safe(commands[i].command, commands[i].num_repetitions, motor_index); - } - } - if (!is_running()) { int ret = DShot::task_spawn(argc, argv); @@ -718,28 +1053,27 @@ int DShot::custom_command(int argc, char *argv[]) return print_usage("unknown command"); } -int DShot::print_status() +int DShot::task_spawn(int argc, char *argv[]) { - PX4_INFO("Outputs initialized: %s", _outputs_initialized ? "yes" : "no"); - PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask); - PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); - perf_print_counter(_cycle_perf); - perf_print_counter(_bdshot_rpm_perf); - perf_print_counter(_dshot_telem_perf); + DShot *instance = new DShot(); - _mixing_output.printStatus(); + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; - if (_telemetry) { - PX4_INFO("telemetry on: %s", _telemetry_device); - _telemetry->printStatus(); + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); } - /* Print dshot status */ - if (_bidirectional_dshot_enabled) { - up_bdshot_status(); - } + delete instance; + _object.store(nullptr); + _task_id = -1; - return 0; + return PX4_ERROR; } int DShot::print_usage(const char *reason) @@ -751,22 +1085,13 @@ int DShot::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This is the DShot output driver. It is similar to the fmu driver, and can be used as drop-in replacement +This is the DShot output driver. It can be used as drop-in replacement to use DShot as ESC communication protocol instead of PWM. -On startup, the module tries to occupy all available pins for DShot output. -It skips all pins already in use (e.g. by a camera trigger module). - It supports: - DShot150, DShot300, DShot600 - telemetry via separate UART and publishing as esc_status message -- sending DShot commands via CLI -### Examples -Permanently reverse motor 1: -$ dshot reverse -m 1 -$ dshot save -m 1 -After saving, the reversed direction will be regarded as the normal one. So to reverse again repeat the same commands. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("dshot", "driver"); @@ -776,28 +1101,6 @@ After saving, the reversed direction will be regarded as the normal one. So to r PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "UART device", false); PRINT_MODULE_USAGE_PARAM_FLAG('x', "Swap RX/TX pins", true); - // DShot commands - PRINT_MODULE_USAGE_COMMAND_DESCR("reverse", "Reverse motor direction"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("normal", "Normal motor direction"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("save", "Save current settings"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("3d_on", "Enable 3D mode"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("3d_off", "Disable 3D mode"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("beep1", "Send Beep pattern 1"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("beep2", "Send Beep pattern 2"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("beep3", "Send Beep pattern 3"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("beep4", "Send Beep pattern 4"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("beep5", "Send Beep pattern 5"); - PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 8137e461e2..4f1e40c081 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,10 +36,11 @@ #include #include #include -#include #include #include +#include +#include "DShotCommon.h" #include "DShotTelemetry.h" using namespace time_literals; @@ -48,14 +49,24 @@ using namespace time_literals; # error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS" #endif +static constexpr hrt_abstime ESC_INIT_TELEM_WAIT_TIME = 3_s; + /** Dshot PWM frequency, Hz */ static constexpr unsigned int DSHOT150 = 150000u; static constexpr unsigned int DSHOT300 = 300000u; static constexpr unsigned int DSHOT600 = 600000u; -static constexpr int DSHOT_DISARM_VALUE = 0; -static constexpr int DSHOT_MIN_THROTTLE = 1; -static constexpr int DSHOT_MAX_THROTTLE = 1999; +static constexpr uint16_t DSHOT_DISARM_VALUE = 0; +static constexpr uint16_t DSHOT_MIN_THROTTLE = 1; +static constexpr uint16_t DSHOT_MAX_THROTTLE = 1999; + +// We do this to avoid bringing in mavlink.h +// #include +#define ACTUATOR_CONFIGURATION_BEEP 1 +#define ACTUATOR_CONFIGURATION_3D_MODE_OFF 2 +#define ACTUATOR_CONFIGURATION_3D_MODE_ON 3 +#define ACTUATOR_CONFIGURATION_SPIN_DIRECTION1 4 +#define ACTUATOR_CONFIGURATION_SPIN_DIRECTION2 5 class DShot final : public ModuleBase, public OutputModuleInterface { @@ -63,122 +74,156 @@ public: DShot(); ~DShot() override; - /** @see ModuleBase */ + // @see ModuleBase static int custom_command(int argc, char *argv[]); + // @see ModuleBase + int print_status() override; + + // @see ModuleBase + static int print_usage(const char *reason = nullptr); + + // @see ModuleBase + static int task_spawn(int argc, char *argv[]); + int init(); void mixerChanged() override; - /** @see ModuleBase::print_status() */ - int print_status() override; - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - /** - * Send a dshot command to one or all motors - * This is expected to be called from another thread. - * @param num_repetitions number of times to repeat, set at least to 1 - * @param motor_index index or -1 for all - * @return 0 on success, <0 error otherwise - */ - int send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index); - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - bool telemetry_enabled() const { return _telemetry != nullptr; } - - bool updateOutputs(uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + bool updateOutputs(uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) override; private: + enum class State { + Disarmed, + Armed + } _state = State::Disarmed; + /** Disallow copy construction and move assignment. */ DShot(const DShot &) = delete; DShot operator=(const DShot &) = delete; - enum class DShotConfig { - Disabled = 0, - DShot150 = 150, - DShot300 = 300, - DShot600 = 600, - }; - - struct Command { - dshot_command_t command{}; - int num_repetitions{0}; - uint8_t motor_mask{0xff}; - bool save{false}; - - bool valid() const { return num_repetitions > 0; } - void clear() { num_repetitions = 0; } - }; - - int _last_telemetry_index{-1}; - uint8_t _actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {}; - - void enable_dshot_outputs(const bool enabled); - + bool initialize_dshot(); void init_telemetry(const char *device, bool swap_rxtx); - int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm); + uint8_t esc_armed_mask(uint16_t *outputs, int num_outputs); - void publish_esc_status(void); + void update_motor_outputs(uint16_t *outputs, int num_outputs); + void update_motor_commands(int num_outputs); + void select_next_command(); - int handle_new_bdshot_erpm(void); + bool set_next_telemetry_index(); // Returns true when the telemetry index has wrapped, indicating all configured motors have been sampled. + bool process_serial_telemetry(); + bool process_bdshot_telemetry(); - void Run() override; - - void update_params(); - - void update_num_motors(); - - void handle_vehicle_commands(); + void consume_esc_data(const EscData &data, TelemetrySource source); + uint16_t calculate_output_value(uint16_t raw, int index); uint16_t convert_output_to_3d_scaling(uint16_t output); + + void Run() override; + void update_params(); + + // Mavlink command handlers + void handle_vehicle_commands(); + void handle_configure_actuator(const vehicle_command_s &command); + void handle_am32_request_eeprom(const vehicle_command_s &command); + + // Mixer MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; - uint32_t _reversible_outputs{}; + uint32_t _output_mask{0}; // Configured outputs for this (shouldn't this live in OutputModuleInterface?) - DShotTelemetry *_telemetry{nullptr}; + // uORB + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _am32_eeprom_write_sub{ORB_ID(am32_eeprom_write)}; - uORB::PublicationMultiData esc_status_pub{ORB_ID(esc_status)}; + uORB::PublicationMultiData _esc_status_pub{ORB_ID(esc_status)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + esc_status_s _esc_status{}; + + // Status information + uint32_t _bdshot_telem_online_mask = 0; // Mask indicating telem receive status for bidirectional dshot telem + uint32_t _serial_telem_online_mask = 0; // Mask indicating telem receive status for serial telem + uint32_t _serial_telem_errors[DSHOT_MAXIMUM_CHANNELS] = {}; + uint32_t _bdshot_telem_errors[DSHOT_MAXIMUM_CHANNELS] = {}; + uint8_t _bdshot_edt_requested_mask = 0; + uint8_t _settings_requested_mask = 0; + + // Array of timestamps indicating when the telemetry came online + hrt_abstime _serial_telem_online_timestamps[DSHOT_MAXIMUM_CHANNELS] = {}; + hrt_abstime _bdshot_telem_online_timestamps[DSHOT_MAXIMUM_CHANNELS] = {}; + + // Serial Telemetry + DShotTelemetry _telemetry; static char _telemetry_device[20]; static bool _telemetry_swap_rxtx; static px4::atomic_bool _request_telemetry_init; + int _telemetry_motor_index = 0; + uint32_t _telemetry_requested_mask = 0; + hrt_abstime _telem_delay_until = ESC_INIT_TELEM_WAIT_TIME; - px4::atomic _new_command{nullptr}; - - - bool _outputs_initialized{false}; - bool _outputs_on{false}; - bool _bidirectional_dshot_enabled{false}; - - static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS}; - uint32_t _output_mask{0}; - - int _num_motors{0}; - + // Perf counters perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - perf_counter_t _bdshot_rpm_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot rpm")}; - perf_counter_t _dshot_telem_perf{perf_alloc(PC_COUNT, MODULE_NAME": dshot telem")}; + perf_counter_t _bdshot_success_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot success")}; + perf_counter_t _bdshot_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot error")}; + perf_counter_t _bdshot_timeout_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot timeout")}; + perf_counter_t _telem_success_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem success")}; + perf_counter_t _telem_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem error")}; + perf_counter_t _telem_timeout_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem timeout")}; + perf_counter_t _telem_allsampled_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem all sampled")}; - Command _current_command{}; + // Commands + struct DShotCommand { + uint16_t command{}; + int num_repetitions{0}; + uint8_t motor_mask{0xff}; + bool save{false}; + bool expect_response{false}; - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; - uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; - uint16_t _esc_status_counter{0}; + bool finished() const { return num_repetitions == 0; } + void clear() + { + command = 0; + num_repetitions = 0; + motor_mask = 0; + save = 0; + expect_response = 0; + } + }; + DShotCommand _current_command{}; + + // DShot Programming Mode + enum class ProgrammingState { + Idle, + EnterMode, + SendAddress, + SendValue, + ExitMode + }; + + am32_eeprom_write_s _am32_eeprom_write{}; + bool _dshot_programming_active = {}; + uint32_t _settings_written_mask[2] = {}; + + ProgrammingState _programming_state{ProgrammingState::Idle}; + + uint16_t _programming_address{}; + uint16_t _programming_value{}; + + // Parameters DEFINE_PARAMETERS( + (ParamInt) _param_dshot_esc_type, (ParamFloat) _param_dshot_min, (ParamBool) _param_dshot_3d_enable, (ParamInt) _param_dshot_3d_dead_h, (ParamInt) _param_dshot_3d_dead_l, (ParamInt) _param_mot_pole_count, - (ParamBool) _param_bidirectional_enable + (ParamBool) _param_dshot_bidir_en, + (ParamBool) _param_dshot_bidir_edt, + (ParamBool) _param_dshot_tel_cfg ) }; diff --git a/src/drivers/dshot/DShotCommon.h b/src/drivers/dshot/DShotCommon.h new file mode 100644 index 0000000000..382d3769d0 --- /dev/null +++ b/src/drivers/dshot/DShotCommon.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +static constexpr int DSHOT_MAXIMUM_CHANNELS = esc_status_s::CONNECTED_ESC_MAX; + +enum class TelemetrySource { + Serial = 0, + BDShot = 1, +}; + +struct EscData { + int motor_index; // Motors 0-7 + hrt_abstime timestamp; // Sample time + TelemetrySource source; + + float temperature; // [deg C] + float voltage; // [0.01V] + float current; // [0.01A] + int16_t erpm; // [100ERPM] +}; + +enum class TelemetryStatus { + NotStarted = 0, + NotReady = 1, + Ready = 2, + Timeout = 3, + ParseError = 4, +}; + +inline int count_set_bits(int mask) +{ + int count = 0; + + while (mask) { + mask &= mask - 1; + count++; + } + + return count; +} + +inline uint8_t crc8(const uint8_t *buf, unsigned len) +{ + auto update_crc8 = [](uint8_t crc, uint8_t crc_seed) { + uint8_t crc_u = crc ^ crc_seed; + + for (unsigned i = 0; i < 8; ++i) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + + return crc_u; + }; + + uint8_t crc = 0; + + for (unsigned i = 0; i < len; ++i) { + crc = update_crc8(buf[i], crc); + } + + return crc; +} diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp index 0567759226..f4769e01d1 100644 --- a/src/drivers/dshot/DShotTelemetry.cpp +++ b/src/drivers/dshot/DShotTelemetry.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +34,7 @@ #include "DShotTelemetry.h" #include +#include #include #include @@ -47,6 +48,14 @@ using namespace time_literals; DShotTelemetry::~DShotTelemetry() { _uart.close(); + + // Clean up settings handlers + for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { + if (_settings_handlers[i]) { + delete _settings_handlers[i]; + _settings_handlers[i] = nullptr; + } + } } int DShotTelemetry::init(const char *port, bool swap_rxtx) @@ -76,105 +85,204 @@ int DShotTelemetry::init(const char *port, bool swap_rxtx) return PX4_OK; } -int DShotTelemetry::update(int num_motors) +void DShotTelemetry::initSettingsHandlers(ESCType esc_type, uint8_t output_mask) { - if (_current_motor_index_request == -1) { - // nothing in progress, start a request - _current_motor_index_request = 0; - _current_request_start = 0; - _frame_position = 0; + if (_settings_initialized) { + return; + } + + _esc_type = esc_type; + + for (uint8_t i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { + + bool output_enabled = (1 << i) & output_mask; + + if (!output_enabled) { + continue; + } + + ESCSettingsInterface *interface = nullptr; + + switch (esc_type) { + case ESCType::AM32: + interface = new AM32Settings(i); + break; + + default: + PX4_WARN("Unsupported ESC type for settings: %d", (int)esc_type); + break; + } + + if (interface) { + _settings_handlers[i] = interface; + } + } + + _settings_initialized = true; +} + +int DShotTelemetry::parseCommandResponse() +{ + if (hrt_elapsed_time(&_command_response_start) > 1_s) { + PX4_WARN("Command response timed out: %d bytes received", _command_response_position); + _command_response_motor_index = -1; + _command_response_start = 0; + _command_response_position = 0; return -1; } + if (_uart.bytesAvailable() <= 0) { + return -1; + } + + uint8_t buf[COMMAND_RESPONSE_MAX_SIZE]; + int bytes = _uart.read(buf, sizeof(buf)); + + // Add bytes to buffer + for (int i = 0; i < bytes; i++) { + _command_response_buffer[_command_response_position++] = buf[i]; + } + + int index = -1; + + switch (_command_response_command) { + case DSHOT_CMD_ESC_INFO: { + auto handler = _settings_handlers[_command_response_motor_index]; + + if (handler && _command_response_position == handler->getExpectedResponseSize()) { + if (handler->decodeInfoResponse(_command_response_buffer, _command_response_position)) { + index = _command_response_motor_index; + } + + // Reset command state + _command_response_position = 0; + _command_response_start = 0; + _command_response_motor_index = -1; + } + + break; + } + + default: + break; + } + + return index; +} + +TelemetryStatus DShotTelemetry::parseTelemetryPacket(EscData *esc_data) +{ + if (telemetryResponseFinished()) { + return TelemetryStatus::NotStarted; + } + // read from the uart. This must be non-blocking, so check first if there is data available - int bytes_available = _uart.bytesAvailable(); + if (_uart.bytesAvailable() <= 0) { + if (hrt_elapsed_time(&_telemetry_request_start) > 30_ms) { + // NOTE: this happens when sending commands, there's a window after an ESC receives + // a command where it will not respond to any telemetry requests + // PX4_INFO("ESC telemetry timeout: %d", esc_data->motor_index); + ++_num_timeouts; - if (bytes_available <= 0) { - // no data available. Check for a timeout - const hrt_abstime now = hrt_absolute_time(); - - if (_current_request_start > 0 && now - _current_request_start > 30_ms) { - if (_redirect_output) { - // clear and go back to internal buffer - _redirect_output = nullptr; - _current_motor_index_request = -1; - - } else { - PX4_DEBUG("ESC telemetry timeout for motor %i (frame pos=%i)", _current_motor_index_request, _frame_position); - ++_num_timeouts; - } - - requestNextMotor(num_motors); - return -2; + // Mark telemetry request as finished + _telemetry_request_start = 0; + _frame_position = 0; + return TelemetryStatus::Timeout; } - return -1; + return TelemetryStatus::NotReady; } - uint8_t buf[ESC_FRAME_SIZE]; + uint8_t buf[TELEMETRY_FRAME_SIZE]; int bytes = _uart.read(buf, sizeof(buf)); - int ret = -1; - - for (int i = 0; i < bytes && ret == -1; ++i) { - if (_redirect_output) { - _redirect_output->buffer[_redirect_output->buf_pos++] = buf[i]; - - if (_redirect_output->buf_pos == sizeof(_redirect_output->buffer)) { - // buffer full: return & go back to internal buffer - _redirect_output = nullptr; - ret = _current_motor_index_request; - _current_motor_index_request = -1; - requestNextMotor(num_motors); - } - - } else { - bool successful_decoding; - - if (decodeByte(buf[i], successful_decoding)) { - if (successful_decoding) { - ret = _current_motor_index_request; - } - - requestNextMotor(num_motors); - } - } - } - - return ret; + return decodeTelemetryResponse(buf, bytes, esc_data); } -bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding) +TelemetryStatus DShotTelemetry::decodeTelemetryResponse(uint8_t *buffer, int length, EscData *esc_data) { - _frame_buffer[_frame_position++] = byte; - successful_decoding = false; + auto status = TelemetryStatus::NotReady; - if (_frame_position == ESC_FRAME_SIZE) { - PX4_DEBUG("got ESC frame for motor %i", _current_motor_index_request); - uint8_t checksum = crc8(_frame_buffer, ESC_FRAME_SIZE - 1); - uint8_t checksum_data = _frame_buffer[ESC_FRAME_SIZE - 1]; + for (int i = 0; i < length; i++) { + _frame_buffer[_frame_position++] = buffer[i]; - if (checksum == checksum_data) { - _latest_data.time = hrt_absolute_time(); - _latest_data.temperature = _frame_buffer[0]; - _latest_data.voltage = (_frame_buffer[1] << 8) | _frame_buffer[2]; - _latest_data.current = (_frame_buffer[3] << 8) | _frame_buffer[4]; - _latest_data.consumption = (_frame_buffer[5]) << 8 | _frame_buffer[6]; - _latest_data.erpm = (_frame_buffer[7] << 8) | _frame_buffer[8]; - PX4_DEBUG("Motor %i: temp=%i, V=%i, cur=%i, consumpt=%i, rpm=%i", _current_motor_index_request, - _latest_data.temperature, _latest_data.voltage, _latest_data.current, _latest_data.consumption, - _latest_data.erpm); - ++_num_successful_responses; - successful_decoding = true; + /* + * ESC Telemetry Frame Structure (10 bytes total) + * ============================================= + * Byte 0: Temperature (uint8_t) [deg C] + * Byte 1-2: Voltage (uint16_t, big-endian) [0.01V] + * Byte 3-4: Current (uint16_t, big-endian) [0.01A] + * Byte 5-6: Consumption (uint16_t, big-endian) [mAh] + * Byte 7-8: eRPM (uint16_t, big-endian) [100ERPM] + * Byte 9: CRC8 Checksum + */ - } else { - ++_num_checksum_errors; + if (_frame_position == TELEMETRY_FRAME_SIZE) { + uint8_t checksum = crc8(_frame_buffer, TELEMETRY_FRAME_SIZE - 1); + uint8_t checksum_data = _frame_buffer[TELEMETRY_FRAME_SIZE - 1]; + + if (checksum == checksum_data) { + + uint8_t temperature = _frame_buffer[0]; + int16_t voltage = (_frame_buffer[1] << 8) | _frame_buffer[2]; + int16_t current = (_frame_buffer[3] << 8) | _frame_buffer[4]; + // int16_t consumption = (_frame_buffer[5]) << 8 | _frame_buffer[6]; + int16_t erpm = (_frame_buffer[7] << 8) | _frame_buffer[8]; + + esc_data->timestamp = hrt_absolute_time(); + esc_data->temperature = (float)temperature; + esc_data->voltage = (float)voltage * 0.01f; + esc_data->current = (float)current * 0.01f;; + esc_data->erpm = erpm * 100; + + ++_num_successful_responses; + status = TelemetryStatus::Ready; + _uart.flush(); + + } else { + ++_num_checksum_errors; + status = TelemetryStatus::ParseError; + } + + // Mark telemetry request as finished + _telemetry_request_start = 0; + _frame_position = 0; } - - return true; } - return false; + return status; +} + +void DShotTelemetry::publish_esc_settings() +{ + for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { + if (_settings_handlers[i]) { + _settings_handlers[i]->publish_latest(); + } + } +} + +void DShotTelemetry::setExpectCommandResponse(int motor_index, uint16_t command) +{ + _command_response_motor_index = motor_index; + _command_response_command = command; + _command_response_start = hrt_absolute_time(); + _command_response_position = 0; +} + +bool DShotTelemetry::commandResponseFinished() +{ + return _command_response_motor_index < 0; +} + +void DShotTelemetry::startTelemetryRequest() +{ + _telemetry_request_start = hrt_absolute_time(); +} + +bool DShotTelemetry::telemetryResponseFinished() +{ + return _telemetry_request_start == 0; } void DShotTelemetry::printStatus() const @@ -183,197 +291,3 @@ void DShotTelemetry::printStatus() const PX4_INFO("Number of timeouts: %i", _num_timeouts); PX4_INFO("Number of CRC errors: %i", _num_checksum_errors); } - -uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len) -{ - auto update_crc8 = [](uint8_t crc, uint8_t crc_seed) { - uint8_t crc_u = crc ^ crc_seed; - - for (int i = 0; i < 8; ++i) { - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - } - - return crc_u; - }; - - uint8_t crc = 0; - - for (int i = 0; i < len; ++i) { - crc = update_crc8(buf[i], crc); - } - - return crc; -} - -void DShotTelemetry::requestNextMotor(int num_motors) -{ - _current_motor_index_request = (_current_motor_index_request + 1) % num_motors; - _current_request_start = 0; - _frame_position = 0; -} - -int DShotTelemetry::getRequestMotorIndex() -{ - if (_current_request_start != 0) { - // already in progress, do not send another request - return -1; - } - - _current_request_start = hrt_absolute_time(); - return _current_motor_index_request; -} - -void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer) -{ - static constexpr int version_position = 12; - const uint8_t *data = buffer.buffer; - - if (buffer.buf_pos < version_position) { - PX4_ERR("Not enough data received"); - return; - } - - enum class ESCVersionInfo { - BLHELI32, - KissV1, - KissV2, - }; - ESCVersionInfo version; - int packet_length; - - if (data[version_position] == 254) { - version = ESCVersionInfo::BLHELI32; - packet_length = esc_info_size_blheli32; - - } else if (data[version_position] == 255) { - version = ESCVersionInfo::KissV2; - packet_length = esc_info_size_kiss_v2; - - } else { - version = ESCVersionInfo::KissV1; - packet_length = esc_info_size_kiss_v1; - } - - if (buffer.buf_pos != packet_length) { - PX4_ERR("Packet length mismatch (%i != %i)", buffer.buf_pos, packet_length); - return; - } - - if (DShotTelemetry::crc8(data, packet_length - 1) != data[packet_length - 1]) { - PX4_ERR("Checksum mismatch"); - return; - } - - uint8_t esc_firmware_version = 0; - uint8_t esc_firmware_subversion = 0; - uint8_t esc_type = 0; - - switch (version) { - case ESCVersionInfo::KissV1: - esc_firmware_version = data[12]; - esc_firmware_subversion = (data[13] & 0x1f) + 97; - esc_type = (data[13] & 0xe0) >> 5; - break; - - case ESCVersionInfo::KissV2: - case ESCVersionInfo::BLHELI32: - esc_firmware_version = data[13]; - esc_firmware_subversion = data[14]; - esc_type = data[15]; - break; - } - - const char *esc_type_str = ""; - - switch (version) { - case ESCVersionInfo::KissV1: - case ESCVersionInfo::KissV2: - switch (esc_type) { - case 1: esc_type_str = "KISS8A"; - break; - - case 2: esc_type_str = "KISS16A"; - break; - - case 3: esc_type_str = "KISS24A"; - break; - - case 5: esc_type_str = "KISS Ultralite"; - break; - - default: esc_type_str = "KISS (unknown)"; - break; - } - - break; - - case ESCVersionInfo::BLHELI32: { - char *esc_type_mutable = (char *)(data + 31); - esc_type_mutable[32] = 0; - esc_type_str = esc_type_mutable; - } - break; - } - - PX4_INFO("ESC Type: %s", esc_type_str); - - PX4_INFO("MCU Serial Number: %02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x", - data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8], - data[9], data[10], data[11]); - - switch (version) { - case ESCVersionInfo::KissV1: - case ESCVersionInfo::KissV2: - PX4_INFO("Firmware version: %d.%d%c", esc_firmware_version / 100, esc_firmware_version % 100, - (char)esc_firmware_subversion); - break; - - case ESCVersionInfo::BLHELI32: - PX4_INFO("Firmware version: %d.%d", esc_firmware_version, esc_firmware_subversion); - break; - } - - if (version == ESCVersionInfo::KissV2 || version == ESCVersionInfo::BLHELI32) { - PX4_INFO("Rotation Direction: %s", data[16] ? "reversed" : "normal"); - PX4_INFO("3D Mode: %s", data[17] ? "on" : "off"); - } - - if (version == ESCVersionInfo::BLHELI32) { - uint8_t setting = data[18]; - - switch (setting) { - case 0: - PX4_INFO("Low voltage Limit: off"); - break; - - case 255: - PX4_INFO("Low voltage Limit: unsupported"); - break; - - default: - PX4_INFO("Low voltage Limit: %d.%01d V", setting / 10, setting % 10); - break; - } - - setting = data[19]; - - switch (setting) { - case 0: - PX4_INFO("Current Limit: off"); - break; - - case 255: - PX4_INFO("Current Limit: unsupported"); - break; - - default: - PX4_INFO("Current Limit: %d A", setting); - break; - } - - for (int i = 0; i < 4; ++i) { - setting = data[i + 20]; - PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off"); - } - } -} diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h index ccbe7b1ce0..e47361e55b 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,90 +34,59 @@ #pragma once #include -#include +#include +#include "DShotCommon.h" +#include "esc/AM32Settings.h" class DShotTelemetry { public: - struct EscData { - hrt_abstime time; - int8_t temperature; ///< [deg C] - int16_t voltage; ///< [0.01V] - int16_t current; ///< [0.01A] - int16_t consumption; ///< [mAh] - int16_t erpm; ///< [100ERPM] - }; - - static constexpr int esc_info_size_blheli32 = 64; - static constexpr int esc_info_size_kiss_v1 = 15; - static constexpr int esc_info_size_kiss_v2 = 21; - static constexpr int max_esc_info_size = esc_info_size_blheli32; - - struct OutputBuffer { - uint8_t buffer[max_esc_info_size]; - int buf_pos{0}; - int motor_index; - }; ~DShotTelemetry(); int init(const char *uart_device, bool swap_rxtx); - - /** - * Read telemetry from the UART (non-blocking) and handle timeouts. - * @param num_motors How many DShot enabled motors - * @return -1 if no update, -2 timeout, >= 0 for the motor index. Use @latestESCData() to get the data. - */ - int update(int num_motors); - - bool redirectActive() const { return _redirect_output != nullptr; } - - /** - * Get the motor index for which telemetry should be requested. - * @return -1 if no request should be made, motor index otherwise - */ - int getRequestMotorIndex(); - - const EscData &latestESCData() const { return _latest_data; } - - /** - * Check whether we are currently expecting to read new data from an ESC - */ - bool expectingData() const { return _current_request_start != 0; } - void printStatus() const; - static void decodeAndPrintEscInfoPacket(const OutputBuffer &buffer); + void startTelemetryRequest(); + bool telemetryResponseFinished(); + + TelemetryStatus parseTelemetryPacket(EscData *esc_data); + + // Attempt to parse a command response. Returns the index of the ESC or -1 on failure. + int parseCommandResponse(); + bool commandResponseFinished(); + void setExpectCommandResponse(int motor_index, uint16_t command); + void initSettingsHandlers(ESCType esc_type, uint8_t output_mask); + void publish_esc_settings(); private: - static constexpr int ESC_FRAME_SIZE = 10; + static constexpr int COMMAND_RESPONSE_MAX_SIZE = 128; + static constexpr int COMMAND_RESPONSE_SETTINGS_SIZE = 49; // 48B for EEPROM + 1B for CRC + static constexpr int TELEMETRY_FRAME_SIZE = 10; + TelemetryStatus decodeTelemetryResponse(uint8_t *buffer, int length, EscData *esc_data); - void requestNextMotor(int num_motors); + device::Serial _uart{}; - /** - * Decode a single byte from an ESC feedback frame - * @param byte - * @param successful_decoding set to true if checksum matches - * @return true if received the expected amount of bytes and the next motor can be requested - */ - bool decodeByte(uint8_t byte, bool &successful_decoding); + // Command response + int _command_response_motor_index{-1}; + uint16_t _command_response_command{0}; + uint8_t _command_response_buffer[COMMAND_RESPONSE_MAX_SIZE]; + int _command_response_position{0}; + hrt_abstime _command_response_start{0}; - static uint8_t crc8(const uint8_t *buf, uint8_t len); - - device::Serial _uart {}; - - uint8_t _frame_buffer[ESC_FRAME_SIZE]; + // Telemetry packet + EscData _latest_data{}; + uint8_t _frame_buffer[TELEMETRY_FRAME_SIZE]; int _frame_position{0}; - - EscData _latest_data; - - int _current_motor_index_request{-1}; - hrt_abstime _current_request_start{0}; - - OutputBuffer *_redirect_output{nullptr}; ///< if set, all read bytes are stored here instead of the internal buffer + hrt_abstime _telemetry_request_start{0}; // statistics int _num_timeouts{0}; int _num_successful_responses{0}; int _num_checksum_errors{0}; + + // Settings + ESCSettingsInterface *_settings_handlers[DSHOT_MAXIMUM_CHANNELS] = {nullptr}; + ESCType _esc_type{ESCType::Unknown}; + bool _settings_initialized{false}; }; diff --git a/src/drivers/dshot/esc/AM32Settings.cpp b/src/drivers/dshot/esc/AM32Settings.cpp new file mode 100644 index 0000000000..8e333b6988 --- /dev/null +++ b/src/drivers/dshot/esc/AM32Settings.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "AM32Settings.h" +#include "../DShotCommon.h" +#include + +static constexpr int EEPROM_SIZE = 48; // AM32 sends raw eeprom data +static constexpr int RESPONSE_SIZE = 49; // 48B data + 1B CRC + +uORB::Publication AM32Settings::_am32_eeprom_read_pub{ORB_ID(am32_eeprom_read)}; + +AM32Settings::AM32Settings(int index) + : _esc_index(index) +{} + +int AM32Settings::getExpectedResponseSize() +{ + return RESPONSE_SIZE; +} + +void AM32Settings::publish_latest() +{ + // PX4_INFO("publish_latest()"); + am32_eeprom_read_s data = {}; + data.timestamp = hrt_absolute_time(); + data.index = _esc_index; + memcpy(data.data, &_eeprom_data, sizeof(data.data)); + _am32_eeprom_read_pub.publish(data); +} + +bool AM32Settings::decodeInfoResponse(const uint8_t *buf, int size) +{ + if (size != RESPONSE_SIZE) { + return false; + } + + uint8_t checksum = crc8(buf, EEPROM_SIZE); + uint8_t checksum_data = buf[EEPROM_SIZE]; + + if (checksum != checksum_data) { + PX4_WARN("Command Response checksum failed!"); + return false; + } + + // PX4_INFO("Successfully received AM32 settings from ESC%d", _esc_index + 1); + + // Store data for retrieval later if requested + memcpy(&_eeprom_data, buf, EEPROM_SIZE); + + // Publish data immedietly + publish_latest(); + + return true; +} diff --git a/src/drivers/dshot/esc/AM32Settings.h b/src/drivers/dshot/esc/AM32Settings.h new file mode 100644 index 0000000000..c06940182d --- /dev/null +++ b/src/drivers/dshot/esc/AM32Settings.h @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "ESCSettingsInterface.h" +#include +#include + +class AM32Settings : public ESCSettingsInterface +{ +public: + AM32Settings(int index); + + struct EEPROMData { + uint8_t eeprom_start; // 0: must be 1 + uint8_t eeprom_version; // 1: version 0-255 + uint8_t bootloader_version; // 2: bootloader version 0-255 + uint8_t firmware_major; // 3: firmware version major + uint8_t firmware_minor; // 4: firmware version minor + uint8_t max_ramp_speed; // 5: value/10 percent per ms (default 160 = 16%/ms) + uint8_t min_duty_cycle; // 6: value/2 (default 4 = 2%) + uint8_t stick_calibration; // 7: disable stick calibration (default 0) + uint8_t voltage_cutoff; // 8: absolute voltage cutoff (default 10) + uint8_t current_pid_p; // 9: P value x2 (default 100 = 200) + uint8_t current_pid_i; // 10: I value (default 0) + uint8_t current_pid_d; // 11: D value x10 (default 50 = 500) + uint8_t active_brake_power; // 12: active brake power + uint8_t reserved[4]; // 13-16: reserved bytes + uint8_t direction_reversed; // 17: direction reversed + uint8_t bidirectional_mode; // 18: bidirectional mode (1=on, 0=off) + uint8_t sinusoidal_startup; // 19: sinusoidal startup + uint8_t complementary_pwm; // 20: complementary PWM + uint8_t variable_pwm_freq; // 21: variable PWM frequency + uint8_t stuck_rotor_protection; // 22: stuck rotor protection + uint8_t timing_advance; // 23: timing advance x0.9375 (16 = 15 degrees) + uint8_t pwm_frequency; // 24: PWM freq in kHz (default 24) + uint8_t startup_power; // 25: startup power 50-150% (default 100) + uint8_t motor_kv; // 26: KV in increments of 40 (55 = 2200kv) + uint8_t motor_poles; // 27: motor poles (default 14) + uint8_t brake_on_stop; // 28: brake on stop (default 0) + uint8_t anti_stall; // 29: anti-stall protection + uint8_t beep_volume; // 30: beep volume 0-11 (default 5) + uint8_t telemetry_30ms; // 31: 30ms telemetry output (0 or 1) + uint8_t servo_low; // 32: servo low (value*2)+750us + uint8_t servo_high; // 33: servo high (value*2)+1750us + uint8_t servo_neutral; // 34: servo neutral 1374+value us (128=1500us) + uint8_t servo_deadband; // 35: servo deadband 0-100 + uint8_t low_voltage_cutoff; // 36: low voltage cutoff + uint8_t low_voltage_threshold; // 37: threshold value+250/10V (50=3.0V) + uint8_t rc_car_reversing; // 38: RC car type reversing (default 0) + uint8_t hall_sensors; // 39: hall sensor options + uint8_t sine_mode_range; // 40: sine mode range 5-25% (default 15) + uint8_t drag_brake_strength; // 41: drag brake 1-10 (default 10) + uint8_t running_brake_amount; // 42: brake when running (default 10) + uint8_t temperature_limit; // 43: temp limit 70-140C (141=disabled) + uint8_t current_protection; // 44: current limit value x2 (102=disabled) + uint8_t sine_mode_strength; // 45: sine mode strength 1-10 (default 6) + uint8_t input_type; // 46: input type selector + uint8_t auto_timing; // 47: auto timing + } __attribute__((packed)); + + int getExpectedResponseSize() override; + bool decodeInfoResponse(const uint8_t *buf, int size) override; + + void publish_latest() override; + +private: + int _esc_index{}; + EEPROMData _eeprom_data{}; + + static uORB::Publication _am32_eeprom_read_pub; +}; diff --git a/src/drivers/dshot/esc/ESCSettingsInterface.h b/src/drivers/dshot/esc/ESCSettingsInterface.h new file mode 100644 index 0000000000..34d64191ea --- /dev/null +++ b/src/drivers/dshot/esc/ESCSettingsInterface.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +enum class ESCType : uint8_t { + Unknown = 0, + AM32 = 1, +}; + +class ESCSettingsInterface +{ +public: + virtual ~ESCSettingsInterface() = default; + + virtual bool decodeInfoResponse(const uint8_t *buf, int size) = 0; + virtual int getExpectedResponseSize() = 0; + virtual void publish_latest() { /* no-op */}; + + // TODO: function to read data + // TODO: function to write data +}; + diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml index d25809f87c..394c037259 100644 --- a/src/drivers/dshot/module.yaml +++ b/src/drivers/dshot/module.yaml @@ -8,6 +8,15 @@ serial_config: parameters: - group: DShot definitions: + DSHOT_ESC_TYPE: + description: + short: ESC Type + long: The ESC firmware type + type: enum + values: + 0: Unknown + 1: AM32 + 2: TODO Check Ardupilot DSHOT_MIN: description: short: Minimum DShot Motor Output @@ -43,6 +52,16 @@ parameters: type: boolean default: 0 reboot_required: true + DSHOT_BIDIR_EDT: + description: + short: Enable Extended DShot Telemetry + long: | + This parameter enables Extended DShot Telemetry which allows transmission of + additional telemetry within the eRPM frame. The EDT data is interleaved with + the eRPM frames at a low rate. + type: boolean + default: 0 + reboot_required: true DSHOT_3D_DEAD_H: description: short: DSHOT 3D deadband high diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 3e4cec0d51..e5ed586714 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -141,6 +141,8 @@ public: OutputFunction outputFunction(int index) const { return _function_assignment[index]; } + bool isMotor(int index) const { return isFunctionSet(index) && (_function_assignment[index] >= OutputFunction::Motor1) && (_function_assignment[index] <= OutputFunction::Motor12); } + /** * Call this regularly from Run(). It will call interface.updateOutputs(). * @return true if outputs were updated diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 5e366273d3..212764d735 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -38,6 +38,7 @@ #include #include #include +#include // TODO: debugging #include @@ -45,6 +46,7 @@ using namespace px4::logger; void LoggedTopics::add_default_topics() { + add_topic("am32_eeprom_read"); // TODO: debugging add_topic("action_request"); add_topic("actuator_armed"); add_optional_topic("actuator_controls_status_0", 300);