Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

feat(dshot): Extended Telemetry and EEPROM support

Browse Source 
Overhauls the DShot driver with per-timer BDShot selection, multi-timer                                                                                                                                  
sequential capture, Extended DShot Telemetry (EDT), and AM32 ESC EEPROM                                                                                                                                  
read/write via MAVLink. Expands ESC support from 8 to 12 channels.                                                                                                                                       
                                                                                                                                                                                                   
BDShot:                                                                                                                                                                                                  
- Per-timer BDShot protocol selection via actuator config UI                                                                                                                                             
- Multi-timer sequential burst/capture on any DMA-capable timer                                                                                                                                          
- Adaptive per-channel GCR bitstream decoding                                                                                                                                                            
- Per-channel online/offline detection with hysteresis                                                                                                                                                   
                                                                                                                                                                                                   
Extended DShot Telemetry (EDT):                                                                                                                                                                          
- Temperature, voltage, current from BDShot frames (no serial wire)                                                                                                                                      
- New DSHOT_BIDIR_EDT parameter                                                                                                                                     
- EDT data merged with serial telemetry when both available                                                                                                                                              
                                                                                                                                                                                                   
AM32 EEPROM:                                                                                                                                                                                             
- Read/write AM32 ESC settings via MAVLink ESC_EEPROM message                                                                                                                                            
- ESCSettingsInterface abstraction for future ESC firmware types                                                                                                                                         
- New DSHOT_ESC_TYPE parameter                                                                                                                                                                           
                                                                                                                                                                                                   
Other changes:                                                                                                                                                                                           
- Per-motor pole count params DSHOT_MOT_POL1–12 (replaces MOT_POLE_COUNT)                                                                                                                              
- EscStatus/EscReport expanded to 12 ESCs with uint16 bitmasks                                                                                                                                           
- Numerous bounds-check, overflow, and concurrency fixes                                                                                                                                                 
- Updated DShot documentation
This commit is contained in:
Jacob Dahl
2026-03-17 16:38:33 -08:00
committed by GitHub
parent c9ee06ff17
commit f00e46f618
48 changed files with 2997 additions and 1692 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp
-2
View File
@@ -83,7 +83,6 @@ void VertiqTelemetryManager::StartPublishing(uORB::Publication<esc_status_s> *es
_esc_status.esc[i].esc_address = 0;
_esc_status.esc[i].esc_rpm = 0;
_esc_status.esc[i].esc_state = 0;
_esc_status.esc[i].esc_cmdcount = 0;
_esc_status.esc[i].esc_voltage = 0;
_esc_status.esc[i].esc_current = 0;
_esc_status.esc[i].esc_temperature = 0;
@@ -124,7 +123,6 @@ uint16_t VertiqTelemetryManager::UpdateTelemetry()
_esc_status.esc[_current_module_id_target_index].esc_temperature = telem_response.mcu_temp *
0.01; //"If you ask other escs for their temp, they're giving you the micro temp, so go with that"
_esc_status.esc[_current_module_id_target_index].esc_state = 0; //not implemented
_esc_status.esc[_current_module_id_target_index].esc_cmdcount = 0; //not implemented
_esc_status.esc[_current_module_id_target_index].failures = 0; //not implemented
//Update the overall _esc_status timestamp and our counter
src/drivers/actuators/voxl_esc/voxl_esc.cpp
+2 -4
View File
@@ -66,7 +66,6 @@ VoxlEsc::VoxlEsc() :
_esc_status.esc[i].esc_address = 0;
_esc_status.esc[i].esc_rpm = 0;
_esc_status.esc[i].esc_state = 0;
_esc_status.esc[i].esc_cmdcount = 0;
_esc_status.esc[i].esc_voltage = 0;
_esc_status.esc[i].esc_current = 0;
_esc_status.esc[i].esc_temperature = 0;
@@ -526,7 +525,6 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
_esc_status.esc[id].esc_rpm = fb.rpm;
_esc_status.esc[id].esc_power = fb.power;
_esc_status.esc[id].esc_state = fb.id_state & 0x0F;
_esc_status.esc[id].esc_cmdcount = fb.cmd_counter;
_esc_status.esc[id].esc_voltage = _esc_chans[id].voltage;
_esc_status.esc[id].esc_current = _esc_chans[id].current;
_esc_status.esc[id].failures = 0; //not implemented
@@ -563,11 +561,11 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
//print ESC status just for debugging
/*
PX4_INFO("[%lld] ID=%d, ADDR %d, STATE=%d, RPM=%5d, PWR=%3d%%, V=%.2fdV, I=%.2fA, T=%+3dC, CNT %d, FAIL %d",
PX4_INFO("[%lld] ID=%d, ADDR %d, STATE=%d, RPM=%5d, PWR=%3d%%, V=%.2fdV, I=%.2fA, T=%+3dC, FAIL %d",
_esc_status.esc[id].timestamp, id, _esc_status.esc[id].esc_address,
_esc_status.esc[id].esc_state, _esc_status.esc[id].esc_rpm, _esc_status.esc[id].esc_power,
(double)_esc_status.esc[id].esc_voltage, (double)_esc_status.esc[id].esc_current, _esc_status.esc[id].esc_temperature,
_esc_status.esc[id].esc_cmdcount, _esc_status.esc[id].failures);
_esc_status.esc[id].failures);
*/
}
}
src/drivers/drv_dshot.h
+62 -51
View File
@@ -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 <px4_platform_common/defines.h>
@@ -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,12 +87,12 @@ 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, uint32_t bdshot_channel_mask, unsigned dshot_pwm_freq, bool edt_enable);
/**
* Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
*/
__EXPORT extern void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry);
__EXPORT extern void dshot_motor_data_set(uint8_t channel, uint16_t throttle, bool telemetry);
/**
* Set the current dshot throttle value for a channel (motor).
@@ -105,9 +101,9 @@ __EXPORT extern void dshot_motor_data_set(unsigned channel, uint16_t throttle, b
* @param throttle The output dshot throttle value in [0 = DSHOT_DISARM_VALUE, 1 = DSHOT_MIN_THROTTLE, 1999 = DSHOT_MAX_THROTTLE].
* @param telemetry If true, request telemetry from that motor
*/
static inline void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
static inline void up_dshot_motor_data_set(uint8_t 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,17 +138,18 @@ __EXPORT extern int up_dshot_arm(bool armed);
*/
__EXPORT extern void up_bdshot_status(void);
/**
* Get bitmask of channels that have processed BDShot data ready to read.
* Each bit corresponds to an output channel index.
*/
__EXPORT extern uint16_t up_bdshot_get_ready_mask(void);
/**
* Get how many bidirectional erpm channels are ready
*
* When we get the erpm round-robin style, we need to get
* and publish the erpms less often.
*
* @return <0 on error, OK on succes
* 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_erpm_ready(void);
__EXPORT extern int up_bdshot_num_errors(uint8_t channel);
/**
* Get bidrectional dshot erpm for a channel
@@ -162,6 +159,14 @@ __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);
/**
* Get bidrectional dshot status for a channel
@@ -169,7 +174,13 @@ __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);
/**
* Check if bidrectional dshot capture is supported for a channel
* @param channel Dshot channel
* @return 0 if not supported (no DMA), 1 if supported
*/
__EXPORT extern int up_bdshot_channel_capture_supported(uint8_t channel);
__END_DECLS
src/drivers/dshot/CMakeLists.txt
+3 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
# Copyright (c) 2019-2026 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
@@ -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
src/drivers/dshot/DShot.cpp
+1061 -574
View File
File diff suppressed because it is too large Load Diff
src/drivers/dshot/DShot.h
+168 -91
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2026 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
@@ -39,23 +39,33 @@
#include <uORB/topics/esc_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/esc_eeprom_write.h>
#include "DShotCommon.h"
#include "DShotTelemetry.h"
using namespace time_literals;
#if !defined(DIRECT_PWM_OUTPUT_CHANNELS)
# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS"
#endif
static_assert(DSHOT_MAXIMUM_CHANNELS <= 16, "DShot driver uses uint16_t bitmasks");
/** Dshot PWM frequency, Hz */
static constexpr unsigned int DSHOT150 = 150000u;
static constexpr unsigned int DSHOT300 = 300000u;
static constexpr unsigned int DSHOT600 = 600000u;
static constexpr hrt_abstime ESC_INIT_TELEM_DELAY = 5_s;
static constexpr int DSHOT_DISARM_VALUE = 0;
static constexpr int DSHOT_MIN_THROTTLE = 1;
static constexpr int DSHOT_MAX_THROTTLE = 1999;
/// Dshot PWM frequency (Hz)
static constexpr uint32_t DSHOT150 = 150000u;
static constexpr uint32_t DSHOT300 = 300000u;
static constexpr uint32_t DSHOT600 = 600000u;
/// Timer config values from PWM_TIM param (matches pwm_out/module.yaml enum)
static constexpr int32_t TIM_CONFIG_DSHOT150 = -5;
static constexpr int32_t TIM_CONFIG_DSHOT300 = -4;
static constexpr int32_t TIM_CONFIG_DSHOT600 = -3;
static constexpr int32_t TIM_CONFIG_BDSHOT150 = -8;
static constexpr int32_t TIM_CONFIG_BDSHOT300 = -7;
static constexpr int32_t TIM_CONFIG_BDSHOT600 = -6;
static constexpr uint16_t DSHOT_DISARM_VALUE = 0;
static constexpr uint16_t DSHOT_MIN_THROTTLE = 1;
static constexpr uint16_t DSHOT_MAX_THROTTLE = 1999;
class DShot final : public ModuleBase, public OutputModuleInterface
{
@@ -65,121 +75,188 @@ 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(float outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override;
bool updateOutputs(float *outputs, unsigned num_outputs, unsigned num_control_groups_updated) override;
private:
/** Disallow copy construction and move assignment. */
// 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);
// Map output channel to motor index [0..DSHOT_MAX_MOTORS-1], or -1 if not a motor
int motor_index_from_output(int output_channel) const
{
if (!_mixing_output.isMotor(output_channel)) { return -1; }
void publish_esc_status(void);
int idx = (int)_mixing_output.outputFunction(output_channel) - (int)OutputFunction::Motor1;
return (idx >= 0 && idx < DSHOT_MAX_MOTORS) ? idx : -1;
}
int handle_new_bdshot_erpm(void);
uint16_t esc_armed_mask(uint16_t *outputs, uint8_t num_outputs);
void Run() override;
void update_motor_outputs(uint16_t *outputs, int num_outputs);
void update_motor_commands(int num_outputs);
void select_next_command();
void update_params();
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 update_num_motors();
void handle_vehicle_commands();
void consume_esc_data(const EscData &data);
uint16_t calculate_output_value(uint16_t raw, int index);
uint16_t convert_output_to_3d_scaling(uint16_t output);
MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
uint32_t _reversible_outputs{};
void Run() override;
void update_params();
DShotTelemetry *_telemetry{nullptr};
// Mavlink command handlers
void handle_vehicle_commands();
void handle_configure_actuator(const vehicle_command_s &command);
void handle_esc_request_eeprom(const vehicle_command_s &command);
uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
// Mixer
MixingOutput _mixing_output{PARAM_PREFIX, DSHOT_MAXIMUM_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
static char _telemetry_device[20];
static bool _telemetry_swap_rxtx;
static px4::atomic_bool _request_telemetry_init;
px4::atomic<Command *> _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};
// Actuator-order masks (indexed by output channel)
uint32_t _output_mask{0};
uint32_t _bdshot_output_mask{0};
int _num_motors{0};
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")};
Command _current_command{};
// Motor-order masks (indexed by motor number: Motor1=0, Motor2=1, etc.)
uint32_t _motor_mask{0};
uint32_t _bdshot_motor_mask{0};
uint8_t _motor_count{0};
// uORB
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
uint16_t _esc_status_counter{0};
uORB::Subscription _esc_eeprom_write_sub{ORB_ID(esc_eeprom_write)};
uORB::PublicationMultiData<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
uORB::Publication<vehicle_command_ack_s> _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_MAX_MOTORS] = {};
uint32_t _bdshot_telem_errors[DSHOT_MAX_MOTORS] = {};
uint16_t _bdshot_edt_requested_mask = 0;
uint16_t _settings_requested_mask = 0;
// Array of timestamps indicating when the telemetry came online
hrt_abstime _serial_telem_online_timestamps[DSHOT_MAX_MOTORS] = {};
hrt_abstime _bdshot_telem_online_timestamps[DSHOT_MAX_MOTORS] = {};
// Serial telemetry adaptive skip: stop polling motors that never respond
static constexpr int SERIAL_TELEM_SKIP_THRESHOLD = 10; // consecutive timeouts before skipping
uint16_t _serial_telem_skip_mask = 0; // motors to skip in round-robin
uint8_t _serial_telem_consecutive_timeouts[DSHOT_MAX_MOTORS] = {};
// Serial Telemetry
DShotTelemetry _telemetry;
static char _serial_port_path[20];
static bool _telemetry_swap_rxtx;
static px4::atomic_bool _request_telemetry_init;
int _telemetry_motor_index = -1;
uint32_t _telemetry_requested_mask = 0;
hrt_abstime _serial_telem_delay_until = ESC_INIT_TELEM_DELAY;
// Parameters we must load only at init
bool _serial_telemetry_enabled = false;
bool _bdshot_edt_enabled = false;
// Cached parameters (updated in update_params())
bool _3d_enabled = false;
int _3d_dead_l = 0;
int _3d_dead_h = 0;
float _dshot_min = 0.f;
int _esc_type = 0;
// Hardware initialization state
bool _hardware_initialized = false;
uint32_t _dshot_frequency = 0;
uint32_t _bdshot_timer_channels = 0;
// Perf counters
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _bdshot_recv_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot recv")};
perf_counter_t _bdshot_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot error")};
perf_counter_t _serial_telem_success_perf{perf_alloc(PC_COUNT, MODULE_NAME": serial telem success")};
perf_counter_t _serial_telem_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": serial telem error")};
perf_counter_t _serial_telem_timeout_perf{perf_alloc(PC_COUNT, MODULE_NAME": serial telem timeout")};
perf_counter_t _serial_telem_allsampled_perf{perf_alloc(PC_COUNT, MODULE_NAME": serial telem all sampled")};
// Commands
struct DShotCommand {
uint16_t command{};
int num_repetitions{0};
uint16_t motor_mask{(1u << DSHOT_MAXIMUM_CHANNELS) - 1};
bool save{false};
bool expect_response{false};
bool finished() const { return num_repetitions == 0; }
void clear()
{
command = 0;
num_repetitions = 0;
motor_mask = 0;
save = false;
expect_response = false;
}
};
DShotCommand _current_command{};
// DShot Programming Mode
enum class ProgrammingState {
Idle,
EnterMode,
SendAddress,
SendValue,
ExitMode
};
esc_eeprom_write_s _esc_eeprom_write{};
bool _dshot_programming_active = {false};
uint32_t _settings_written_mask[2] = {};
ProgrammingState _programming_state{ProgrammingState::Idle};
uint16_t _programming_address{};
uint16_t _programming_value{};
param_t _param_pole_count_handles[DSHOT_MAX_MOTORS] {};
int32_t _pole_count_params[DSHOT_MAX_MOTORS] {};
int get_pole_count(int motor_index) const;
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::DSHOT_ESC_TYPE>) _param_dshot_esc_type,
(ParamFloat<px4::params::DSHOT_MIN>) _param_dshot_min,
(ParamBool<px4::params::DSHOT_3D_ENABLE>) _param_dshot_3d_enable,
(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count,
(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable
(ParamBool<px4::params::DSHOT_BIDIR_EDT>) _param_dshot_bidir_edt
)
};
src/drivers/dshot/DShotCommon.h
+93
View File
@@ -0,0 +1,93 @@
/****************************************************************************
*
* Copyright (c) 2026 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 <drivers/drv_hrt.h>
#include <board_config.h>
#include <uORB/topics/esc_status.h>
#if !defined(DIRECT_PWM_OUTPUT_CHANNELS)
# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS"
#endif
static constexpr int DSHOT_MAXIMUM_CHANNELS = DIRECT_PWM_OUTPUT_CHANNELS;
// Motor-indexed arrays use this — bounded by both hardware channels and protocol limit
static constexpr int DSHOT_MAX_MOTORS = DSHOT_MAXIMUM_CHANNELS < esc_status_s::CONNECTED_ESC_MAX
? DSHOT_MAXIMUM_CHANNELS : esc_status_s::CONNECTED_ESC_MAX;
enum class TelemetrySource {
Serial = 0,
BDShot = 1,
};
struct EscData {
int motor_index; // Motor index 0..(CONNECTED_ESC_MAX-1)
hrt_abstime timestamp; // Sample time
TelemetrySource source;
float temperature; // [C]
float voltage; // [V]
float current; // [A]
int32_t erpm; // [eRPM]
};
enum class TelemetryStatus {
NotStarted = 0,
NotReady = 1,
Ready = 2,
Timeout = 3,
ParseError = 4,
};
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;
}
src/drivers/dshot/DShotTelemetry.cpp
+205 -276
View File
File diff suppressed because it is too large Load Diff
src/drivers/dshot/DShotTelemetry.h
+35 -66
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2026 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 <px4_platform_common/Serial.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#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.
void parseCommandResponse();
bool commandResponseFinished();
bool commandResponseStarted();
void setExpectCommandResponse(int motor_index, uint16_t command);
void resetCommandResponse();
void initSettingsHandlers(ESCType esc_type, uint16_t output_mask);
private:
static constexpr int ESC_FRAME_SIZE = 10;
static constexpr int COMMAND_RESPONSE_MAX_SIZE = 49;
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
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_MAX_MOTORS] = {nullptr};
ESCType _esc_type{ESCType::Unknown};
bool _settings_initialized{false};
};
src/drivers/dshot/esc/AM32Settings.cpp
+85
View File
@@ -0,0 +1,85 @@
/****************************************************************************
*
* Copyright (c) 2026 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 <px4_platform_common/log.h>
static constexpr int RESPONSE_SIZE = EEPROM_SIZE + 1; // 48B data + 1B CRC
uORB::Publication<esc_eeprom_read_s> AM32Settings::_esc_eeprom_read_pub{ORB_ID(esc_eeprom_read)};
AM32Settings::AM32Settings(int index)
: _esc_index(index)
{}
int AM32Settings::getExpectedResponseSize()
{
return RESPONSE_SIZE;
}
void AM32Settings::publish_latest()
{
esc_eeprom_read_s data = {};
data.timestamp = hrt_absolute_time();
data.firmware = 1; // ESC_FIRMWARE_AM32
data.index = _esc_index;
memcpy(data.data, &_eeprom_data, sizeof(_eeprom_data));
data.length = sizeof(_eeprom_data);
_esc_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_DEBUG("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 immediately
publish_latest();
return true;
}
src/drivers/dshot/esc/AM32Settings.h
+57
View File
@@ -0,0 +1,57 @@
/****************************************************************************
*
* Copyright (c) 2026 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 <uORB/Publication.hpp>
#include <uORB/topics/esc_eeprom_read.h>
static constexpr int EEPROM_SIZE = 48;
class AM32Settings : public ESCSettingsInterface
{
public:
AM32Settings(int index);
int getExpectedResponseSize() override;
bool decodeInfoResponse(const uint8_t *buf, int size) override;
void publish_latest() override;
private:
int _esc_index{};
uint8_t _eeprom_data[EEPROM_SIZE] {};
static uORB::Publication<esc_eeprom_read_s> _esc_eeprom_read_pub;
};
src/drivers/dshot/esc/ESCSettingsInterface.h
+54
View File
@@ -0,0 +1,54 @@
/****************************************************************************
*
* Copyright (c) 2026 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 <cstdint>
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
};
src/drivers/dshot/module.yaml
+24 -11
View File
@@ -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
default: 0
DSHOT_MIN:
description:
short: Minimum DShot Motor Output
@@ -33,13 +42,13 @@ parameters:
When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent.
type: boolean
default: 0
DSHOT_BIDIR_EN:
DSHOT_BIDIR_EDT:
description:
short: Enable bidirectional DShot
short: Enable Extended DShot Telemetry
long: |
This parameter enables bidirectional DShot which provides RPM feedback.
Note that this requires ESCs that support bidirectional DSHot, e.g. BlHeli32.
This is not the same as DShot telemetry which requires an additional serial connection.
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
@@ -63,14 +72,18 @@ parameters:
min: 0
max: 1000
default: 1000
MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group
DSHOT_MOT_POL${i}:
description:
short: Number of magnetic poles of the motors
short: Number of magnetic poles of motor ${i}
long: |
Specify the number of magnetic poles of the motors.
It is required to compute the RPM value from the eRPM returned with the ESC telemetry.
Either get the number from the motor spec sheet or count the magnets on the bell of the motor (not the stator magnets).
Number of magnetic poles for motor ${i}.
Required to compute RPM from the eRPM returned by ESC telemetry.
Either get the number from the motor spec sheet or count the magnets
on the bell of the motor (not the stator magnets).
Typical motors for 5 inch props have 14 poles.
type: int32
default: 14
min: 2
max: 400
num_instances: 12
instance_start: 1
src/drivers/pwm_out/module.yaml
+3
View File
@@ -21,6 +21,9 @@ actuator_output:
type: enum
default: 400
values:
-8: BDShot150
-7: BDShot300
-6: BDShot600
-5: DShot150
-4: DShot300
-3: DShot600
src/drivers/uavcan/actuators/esc.cpp
+2 -2
View File
@@ -153,9 +153,9 @@ void UavcanEscController::esc_status_extended_sub_cb(const uavcan::ReceivedDataS
}
}
uint8_t UavcanEscController::check_escs_status()
uint16_t UavcanEscController::check_escs_status()
{
int esc_status_flags = 0;
uint16_t esc_status_flags = 0;
const hrt_abstime now = hrt_absolute_time();
for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
src/drivers/uavcan/actuators/esc.hpp
+1 -1
View File
@@ -99,7 +99,7 @@ private:
/**
* Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline.
*/
uint8_t check_escs_status();
uint16_t check_escs_status();
/**
* Gets failure flags for a specific ESC
src/lib/mixer_module/mixer_module.hpp
+2
View File
@@ -140,6 +140,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
src/lib/parameters/param_translation.cpp
+15
View File
@@ -34,6 +34,7 @@
#include "param_translation.h"
#include <inttypes.h>
#include <px4_platform_common/log.h>
#include <lib/drivers/device/Device.hpp>
#include <drivers/drv_sensor.h>
@@ -177,5 +178,19 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
}
}
// 2026-03-11: translate MOT_POLE_COUNT to per-motor DSHOT_MOT_POL1-12
{
if ((node->type == bson_type_t::BSON_INT32) && (strcmp("MOT_POLE_COUNT", node->name) == 0)) {
for (int i = 1; i <= 12; i++) {
char name[20];
snprintf(name, sizeof(name), "DSHOT_MOT_POL%d", i);
param_set(param_find(name), &node->i32);
}
PX4_INFO("migrating %s -> DSHOT_MOT_POL1-12 (value=%" PRId32 ")", "MOT_POLE_COUNT", node->i32);
return param_modify_on_import_ret::PARAM_SKIP_IMPORT;
}
}
return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
}
src/modules/commander/Commander.cpp
+1
View File
@@ -1603,6 +1603,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
case vehicle_command_s::VEHICLE_CMD_ESC_REQUEST_EEPROM:
case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
case vehicle_command_s::VEHICLE_CMD_DO_WINCH:
case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
src/modules/logger/logged_topics.cpp
+1 -1
View File
@@ -64,7 +64,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("external_ins_attitude");
add_optional_topic("external_ins_global_position");
add_optional_topic("external_ins_local_position");
add_topic("esc_status");
add_topic("esc_status", 100);
add_topic("failure_detector_status", 100);
add_topic("failsafe_flags");
add_optional_topic("follow_target", 500);
src/modules/mavlink/mavlink_main.cpp
+15
View File
@@ -1462,6 +1462,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("EFI_STATUS", 2.0f);
configure_stream_local("ESC_INFO", 1.0f);
configure_stream_local("ESC_STATUS", 1.0f);
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
configure_stream_local("ESC_EEPROM", unlimited_rate);
#endif
configure_stream_local("ESTIMATOR_STATUS", 0.5f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f);
@@ -1526,6 +1529,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("DISTANCE_SENSOR", 10.0f);
configure_stream_local("ESC_INFO", 10.0f);
configure_stream_local("ESC_STATUS", 10.0f);
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
configure_stream_local("ESC_EEPROM", unlimited_rate);
#endif
configure_stream_local("MOUNT_ORIENTATION", 10.0f);
configure_stream_local("OBSTACLE_DISTANCE", 10.0f);
configure_stream_local("ODOMETRY", 30.0f);
@@ -1704,6 +1710,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("EFI_STATUS", 10.0f);
configure_stream_local("ESC_INFO", 10.0f);
configure_stream_local("ESC_STATUS", 10.0f);
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
configure_stream_local("ESC_EEPROM", unlimited_rate);
#endif
configure_stream_local("ESTIMATOR_STATUS", 5.0f);
configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
@@ -1799,6 +1808,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
configure_stream_local("ESC_INFO", 1.0f);
configure_stream_local("ESC_STATUS", 5.0f);
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
configure_stream_local("ESC_EEPROM", unlimited_rate);
#endif
configure_stream_local("ADSB_VEHICLE", 5.f);
configure_stream_local("ATTITUDE_TARGET", 2.0f);
@@ -1865,6 +1877,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 2.0f);
configure_stream_local("ESC_INFO", 1.0f);
configure_stream_local("ESC_STATUS", 2.0f);
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
configure_stream_local("ESC_EEPROM", unlimited_rate);
#endif
configure_stream_local("ADSB_VEHICLE", 1.0f);
configure_stream_local("ATTITUDE_TARGET", 0.5f);
configure_stream_local("AVAILABLE_MODES", 0.3f);
src/modules/mavlink/mavlink_messages.cpp
+7
View File
@@ -135,6 +135,10 @@
#include "streams/CURRENT_MODE.hpp"
#endif
#ifdef MAVLINK_MSG_ID_ESC_EEPROM // Only defined if development.xml is used
#include "streams/ESC_EEPROM.hpp"
#endif
#if !defined(CONSTRAINED_FLASH)
# include "streams/ADSB_VEHICLE.hpp"
# include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp"
@@ -466,6 +470,9 @@ static const StreamListItem streams_list[] = {
#if defined(ESC_STATUS_HPP)
create_stream_list_item<MavlinkStreamESCStatus>(),
#endif // ESC_STATUS_HPP
#if defined(ESC_EEPROM_HPP)
create_stream_list_item<MavlinkStreamEscEeprom>(),
#endif // ESC_EEPROM_HPP
#if defined(AUTOPILOT_VERSION_HPP)
create_stream_list_item<MavlinkStreamAutopilotVersion>(),
#endif // AUTOPILOT_VERSION_HPP
src/modules/mavlink/mavlink_receiver.cpp
+67 -7
View File
@@ -333,6 +333,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_SET_VELOCITY_LIMITS:
handle_message_set_velocity_limits(msg);
break;
#endif
#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // For now only defined if development.xml is used
case MAVLINK_MSG_ID_ESC_EEPROM:
handle_message_esc_eeprom(msg);
break;
#endif
default:
@@ -598,14 +606,24 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
if (message_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) {
get_message_interval((int)(cmd_mavlink.param2 + 0.5f));
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
} else {
result = handle_request_message_command(message_id,
vehicle_command.param2, vehicle_command.param3, vehicle_command.param4,
vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
}
// Translate ESC_EEPROM request into the PX4 internal command so the DShot driver handles it
if (message_id == MAVLINK_MSG_ID_ESC_EEPROM) {
vehicle_command_s eeprom_cmd = vehicle_command;
eeprom_cmd.command = vehicle_command_s::VEHICLE_CMD_ESC_REQUEST_EEPROM;
_cmd_pub.publish(eeprom_cmd);
} else
#endif
if (message_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) {
get_message_interval((int)(cmd_mavlink.param2 + 0.5f));
} else {
result = handle_request_message_command(message_id,
vehicle_command.param2, vehicle_command.param3, vehicle_command.param4,
vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
}
} else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) {
if (_mavlink.failure_injection_enabled()) {
@@ -1308,6 +1326,48 @@ void MavlinkReceiver::handle_message_set_velocity_limits(mavlink_message_t *msg)
}
#endif // MAVLINK_MSG_ID_SET_VELOCITY_LIMITS
#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // For now only defined if development.xml is used
void
MavlinkReceiver::handle_message_esc_eeprom(mavlink_message_t *msg)
{
mavlink_esc_eeprom_t message;
mavlink_msg_esc_eeprom_decode(msg, &message);
esc_eeprom_write_s eeprom{};
eeprom.timestamp = hrt_absolute_time();
eeprom.firmware = message.firmware;
eeprom.index = message.esc_index;
if (eeprom.index != 255 && eeprom.index >= esc_status_s::CONNECTED_ESC_MAX) {
PX4_ERR("ESC EEPROM: invalid esc_index %u", eeprom.index);
return;
}
uint8_t min_length = sizeof(eeprom.data);
int length = message.length;
if (length > min_length) {
length = min_length;
}
for (int i = 0; i < length && i < min_length; i++) {
int mask_index = i / 32; // Which uint32_t in the write_mask array
int bit_index = i % 32; // Which bit within that uint32_t
if (message.write_mask[mask_index] & (1U << bit_index)) {
eeprom.data[i] = message.data[i];
}
}
eeprom.length = length;
memcpy(eeprom.write_mask, message.write_mask, sizeof(eeprom.write_mask));
PX4_DEBUG("ESC EEPROM write request for ESC%d", eeprom.index + 1);
_esc_eeprom_write_pub.publish(eeprom);
}
#endif // MAVLINK_MSG_ID_ESC_EEPROM
void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_receiver.h
+12
View File
@@ -64,6 +64,10 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/autotune_attitude_control_status.h>
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
#include <uORB/topics/esc_eeprom_write.h>
#endif
#include <uORB/topics/esc_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/camera_status.h>
#include <uORB/topics/cellular_status.h>
@@ -203,6 +207,9 @@ private:
void handle_message_utm_global_position(mavlink_message_t *msg);
#if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used
void handle_message_set_velocity_limits(mavlink_message_t *msg);
#endif
#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // For now only defined if development.xml is used
void handle_message_esc_eeprom(mavlink_message_t *msg);
#endif
void handle_message_vision_position_estimate(mavlink_message_t *msg);
void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
@@ -330,6 +337,11 @@ private:
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
#if defined(MAVLINK_MSG_ID_ESC_EEPROM)
uORB::Publication<esc_eeprom_write_s> _esc_eeprom_write_pub {ORB_ID(esc_eeprom_write)};
#endif
#if !defined(CONSTRAINED_FLASH)
uORB::Publication<debug_array_s> _debug_array_pub {ORB_ID(debug_array)};
uORB::Publication<debug_key_value_s> _debug_key_value_pub{ORB_ID(debug_key_value)};
src/modules/mavlink/streams/ESC_EEPROM.hpp
+89
View File
@@ -0,0 +1,89 @@
/****************************************************************************
*
* Copyright (c) 2026 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.
*
****************************************************************************/
#ifndef ESC_EEPROM_HPP
#define ESC_EEPROM_HPP
#include <uORB/topics/esc_eeprom_read.h>
class MavlinkStreamEscEeprom : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamEscEeprom(mavlink); }
static constexpr const char *get_name_static() { return "ESC_EEPROM"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ESC_EEPROM; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _esc_eeprom_read_sub.advertised() ? MAVLINK_MSG_ID_ESC_EEPROM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamEscEeprom(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _esc_eeprom_read_sub{ORB_ID(esc_eeprom_read)};
bool emit_message(bool force)
{
esc_eeprom_read_s eeprom = {};
if (_esc_eeprom_read_sub.update(&eeprom) || force) {
mavlink_esc_eeprom_t msg = {};
msg.firmware = eeprom.firmware;
msg.esc_index = eeprom.index;
msg.msg_index = 0;
msg.msg_count = 1;
size_t copy_len = eeprom.length < sizeof(eeprom.data) ? eeprom.length : sizeof(eeprom.data);
memcpy(msg.data, eeprom.data, copy_len);
msg.length = copy_len;
mavlink_msg_esc_eeprom_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
bool send() override
{
return emit_message(false);
}
};
#endif // ESC_EEPROM_HPP
src/modules/mavlink/streams/ESC_INFO.hpp
+41 -46
View File
@@ -52,7 +52,7 @@ public:
unsigned get_size() override
{
static constexpr unsigned message_size = MAVLINK_MSG_ID_ESC_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
return _esc_status_subs.advertised_count() * message_size;
return MAX_NUM_MSGS * message_size;
}
private:
@@ -66,106 +66,101 @@ private:
static constexpr hrt_abstime ESC_TIMEOUT = 100000;
struct EscOutputInterfaceInfo {
uint16_t counter;
uint8_t esc_count;
uint8_t esc_connectiontype;
uint8_t esc_online_flags;
};
struct EscInfo {
hrt_abstime timestamp;
uint16_t failure_flags;
uint32_t error_count;
int16_t temperature;
uint8_t connectiontype;
bool online;
};
int _total_esc_count = {};
EscOutputInterfaceInfo _interface[MAX_NUM_MSGS] = {};
uint16_t _total_counter = {};
uint8_t _total_esc_count = {};
EscInfo _escs[MAX_ESC_OUTPUTS] = {};
uint16_t _instance_counter[ORB_MULTI_MAX_INSTANCES] = {};
uint8_t _instance_esc_count[ORB_MULTI_MAX_INSTANCES] = {};
void update_data() override
{
int subscriber_count = math::min(_esc_status_subs.size(), MAX_NUM_MSGS);
for (int i = 0; i < subscriber_count; i++) {
for (int i = 0; i < _esc_status_subs.size(); i++) {
esc_status_s esc = {};
if (_esc_status_subs[i].update(&esc)) {
_interface[i].counter = esc.counter;
_interface[i].esc_count = esc.esc_count;
_interface[i].esc_connectiontype = esc.esc_connectiontype;
_instance_counter[i] = esc.counter;
_instance_esc_count[i] = esc.esc_count;
// Capture online_flags, we will map from index to motor number
uint8_t online_flags = esc.esc_online_flags;
_interface[i].esc_online_flags = 0;
uint16_t online_flags = esc.esc_online_flags;
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) {
bool is_motor = math::isInRange(esc.esc[j].actuator_function,
esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX);
if (is_motor) {
// Map OutputFunction number to index
int index = (int)esc.esc[j].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
_escs[index].online = online_flags & (1 << j);
_escs[index].failure_flags = esc.esc[j].failures;
_escs[index].error_count = esc.esc[j].esc_errorcount;
_escs[index].timestamp = esc.esc[j].timestamp;
_escs[index].temperature = esc.esc[j].esc_temperature * 100.f;
if (index >= 0 && index < MAX_ESC_OUTPUTS) {
_escs[index].online = online_flags & (1 << j);
_escs[index].failure_flags = esc.esc[j].failures;
_escs[index].error_count = esc.esc[j].esc_errorcount;
_escs[index].timestamp = esc.esc[j].timestamp;
_escs[index].temperature = esc.esc[j].esc_temperature * 100.f;
_escs[index].connectiontype = esc.esc_connectiontype;
}
}
}
}
}
int count = 0;
_total_counter = 0;
_total_esc_count = 0;
for (int i = 0; i < MAX_NUM_MSGS; i++) {
count += _interface[i].esc_count;
for (int i = 0; i < _esc_status_subs.size(); i++) {
_total_counter += _instance_counter[i];
_total_esc_count += _instance_esc_count[i];
}
_total_esc_count = count;
}
bool send() override
{
bool updated = false;
if (_total_esc_count == 0) {
return false;
}
for (int i = 0; i < MAX_NUM_MSGS; i++) {
const int num_msgs = math::min((_total_esc_count + ESCS_PER_MSG - 1) / ESCS_PER_MSG, (int)MAX_NUM_MSGS);
const hrt_abstime now = hrt_absolute_time();
hrt_abstime now = hrt_absolute_time();
for (int i = 0; i < num_msgs; i++) {
mavlink_esc_info_t msg = {};
msg.index = i * ESCS_PER_MSG;
msg.time_usec = now;
msg.counter = _interface[i].counter;
msg.counter = _total_counter;
msg.count = _total_esc_count;
msg.connection_type = _interface[i].esc_connectiontype;
msg.info = _interface[i].esc_online_flags;
bool atleast_one_esc_updated = false;
msg.connection_type = 0;
msg.info = 0;
for (int j = 0; j < ESCS_PER_MSG; j++) {
EscInfo &esc = _escs[i * ESCS_PER_MSG + j];
msg.info |= (esc.online << j);
if ((esc.timestamp != 0) && (esc.timestamp + ESC_TIMEOUT) > now) {
msg.info |= (esc.online << j);
msg.failure_flags[j] = esc.failure_flags;
msg.error_count[j] = esc.error_count;
msg.temperature[j] = esc.temperature;
atleast_one_esc_updated = true;
if (msg.connection_type == 0) {
msg.connection_type = esc.connectiontype;
}
}
}
if (atleast_one_esc_updated) {
mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg);
updated = true;
}
mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg);
}
return updated;
return true;
}
};
src/modules/mavlink/streams/ESC_STATUS.hpp
+28 -19
View File
@@ -52,7 +52,7 @@ public:
unsigned get_size() override
{
static constexpr unsigned message_size = MAVLINK_MSG_ID_ESC_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
return _esc_status_subs.advertised_count() * message_size;
return MAX_NUM_MSGS * message_size;
}
private:
@@ -74,14 +74,17 @@ private:
EscStatus _escs[MAX_ESC_OUTPUTS] = {};
uint8_t _esc_count = {};
uint8_t _instance_esc_count[ORB_MULTI_MAX_INSTANCES] = {};
void update_data() override
{
int subscriber_count = math::min(_esc_status_subs.size(), MAX_NUM_MSGS);
for (int i = 0; i < subscriber_count; i++) {
for (int i = 0; i < _esc_status_subs.size(); i++) {
esc_status_s esc = {};
if (_esc_status_subs[i].update(&esc)) {
_instance_esc_count[i] = esc.esc_count;
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) {
const bool is_motor = math::isInRange(esc.esc[j].actuator_function,
@@ -90,30 +93,40 @@ private:
if (is_motor) {
// Map OutputFunction number to index
int index = (int)esc.esc[j].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
_escs[index].timestamp = esc.esc[j].timestamp;
_escs[index].rpm = esc.esc[j].esc_rpm;
_escs[index].voltage = esc.esc[j].esc_voltage;
_escs[index].current = esc.esc[j].esc_current;
if (index >= 0 && index < MAX_ESC_OUTPUTS) {
_escs[index].timestamp = esc.esc[j].timestamp;
_escs[index].rpm = esc.esc[j].esc_rpm;
_escs[index].voltage = esc.esc[j].esc_voltage;
_escs[index].current = esc.esc[j].esc_current;
}
}
}
}
}
_esc_count = 0;
for (int i = 0; i < _esc_status_subs.size(); i++) {
_esc_count += _instance_esc_count[i];
}
}
bool send() override
{
bool updated = false;
if (_esc_count == 0) {
return false;
}
for (int i = 0; i < MAX_NUM_MSGS; i++) {
const int num_msgs = math::min((_esc_count + ESCS_PER_MSG - 1) / ESCS_PER_MSG, (int)MAX_NUM_MSGS);
const hrt_abstime now = hrt_absolute_time();
hrt_abstime now = hrt_absolute_time();
for (int i = 0; i < num_msgs; i++) {
mavlink_esc_status_t msg = {};
msg.index = i * ESCS_PER_MSG;
msg.time_usec = now;
bool atleast_one_esc_updated = false;
for (int j = 0; j < ESCS_PER_MSG; j++) {
EscStatus &esc = _escs[i * ESCS_PER_MSG + j];
@@ -122,17 +135,13 @@ private:
msg.rpm[j] = esc.rpm;
msg.voltage[j] = esc.voltage;
msg.current[j] = esc.current;
atleast_one_esc_updated = true;
}
}
if (atleast_one_esc_updated) {
mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg);
updated = true;
}
mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg);
}
return updated;
return true;
}
};