mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 06:33:33 +08:00
feat(dshot): Extended Telemetry and EEPROM support
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
@@ -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
|
||||
|
||||
@@ -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);
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
+62
-51
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
+1061
-574
File diff suppressed because it is too large
Load Diff
+168
-91
@@ -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
|
||||
)
|
||||
};
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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,6 +34,7 @@
|
||||
#include "DShotTelemetry.h"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <drivers/drv_dshot.h>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
@@ -47,6 +48,14 @@ using namespace time_literals;
|
||||
DShotTelemetry::~DShotTelemetry()
|
||||
{
|
||||
_uart.close();
|
||||
|
||||
// Clean up settings handlers
|
||||
for (int i = 0; i < DSHOT_MAX_MOTORS; i++) {
|
||||
if (_settings_handlers[i]) {
|
||||
delete _settings_handlers[i];
|
||||
_settings_handlers[i] = nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int DShotTelemetry::init(const char *port, bool swap_rxtx)
|
||||
@@ -76,304 +85,224 @@ int DShotTelemetry::init(const char *port, bool swap_rxtx)
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int DShotTelemetry::update(int num_motors)
|
||||
void DShotTelemetry::initSettingsHandlers(ESCType esc_type, uint16_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;
|
||||
return -1;
|
||||
if (_settings_initialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
// read from the uart. This must be non-blocking, so check first if there is data available
|
||||
int bytes_available = _uart.bytesAvailable();
|
||||
_esc_type = esc_type;
|
||||
|
||||
if (bytes_available <= 0) {
|
||||
// no data available. Check for a timeout
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
for (uint8_t i = 0; i < DSHOT_MAX_MOTORS; i++) {
|
||||
|
||||
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;
|
||||
bool output_enabled = (1 << i) & output_mask;
|
||||
|
||||
} 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;
|
||||
if (!output_enabled) {
|
||||
continue;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
ESCSettingsInterface *interface = nullptr;
|
||||
|
||||
uint8_t buf[ESC_FRAME_SIZE];
|
||||
int bytes = _uart.read(buf, sizeof(buf));
|
||||
switch (esc_type) {
|
||||
case ESCType::AM32:
|
||||
interface = new AM32Settings(i);
|
||||
break;
|
||||
|
||||
int ret = -1;
|
||||
case ESCType::Unknown:
|
||||
break;
|
||||
|
||||
for (int i = 0; i < bytes && ret == -1; ++i) {
|
||||
if (_redirect_output) {
|
||||
_redirect_output->buffer[_redirect_output->buf_pos++] = buf[i];
|
||||
default:
|
||||
PX4_WARN("Unsupported ESC type for settings: %d", (int)esc_type);
|
||||
break;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
if (interface) {
|
||||
_settings_handlers[i] = interface;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
_settings_initialized = true;
|
||||
}
|
||||
|
||||
bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
|
||||
void DShotTelemetry::resetCommandResponse()
|
||||
{
|
||||
_frame_buffer[_frame_position++] = byte;
|
||||
successful_decoding = false;
|
||||
_command_response_motor_index = -1;
|
||||
_command_response_start = 0;
|
||||
_command_response_position = 0;
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
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;
|
||||
|
||||
} else {
|
||||
++_num_checksum_errors;
|
||||
}
|
||||
|
||||
return true;
|
||||
void DShotTelemetry::parseCommandResponse()
|
||||
{
|
||||
if (hrt_elapsed_time(&_command_response_start) > 1_s) {
|
||||
PX4_WARN("Command response timed out: %d bytes received", _command_response_position);
|
||||
PX4_WARN("At time %.2fs", (double)hrt_absolute_time() / 1000000.);
|
||||
resetCommandResponse();
|
||||
return;
|
||||
}
|
||||
|
||||
return false;
|
||||
uint8_t buf[COMMAND_RESPONSE_MAX_SIZE] = {};
|
||||
int bytes = _uart.read(buf, sizeof(buf));
|
||||
|
||||
if (bytes <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Handle potential overflow, fail out
|
||||
if (_command_response_position + bytes > COMMAND_RESPONSE_MAX_SIZE) {
|
||||
PX4_ERR("command response overflow");
|
||||
resetCommandResponse();
|
||||
return;
|
||||
}
|
||||
|
||||
// Add bytes to buffer
|
||||
memcpy(&_command_response_buffer[_command_response_position], buf, bytes);
|
||||
_command_response_position += bytes;
|
||||
|
||||
switch (_command_response_command) {
|
||||
case DSHOT_CMD_ESC_INFO: {
|
||||
if (_command_response_motor_index < 0 || _command_response_motor_index >= DSHOT_MAX_MOTORS) {
|
||||
resetCommandResponse();
|
||||
return;
|
||||
}
|
||||
|
||||
auto handler = _settings_handlers[_command_response_motor_index];
|
||||
|
||||
if (!handler) {
|
||||
resetCommandResponse();
|
||||
break;
|
||||
}
|
||||
|
||||
if (_command_response_position == handler->getExpectedResponseSize()) {
|
||||
// TODO: handle command failures
|
||||
handler->decodeInfoResponse(_command_response_buffer, _command_response_position);
|
||||
resetCommandResponse();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
TelemetryStatus DShotTelemetry::parseTelemetryPacket(EscData *esc_data)
|
||||
{
|
||||
if (telemetryResponseFinished()) {
|
||||
return TelemetryStatus::NotStarted;
|
||||
}
|
||||
|
||||
hrt_abstime elapsed = hrt_elapsed_time(&_telemetry_request_start);
|
||||
|
||||
// At 115200 baud the 10-byte response takes ~868us. Skip polling until data could have arrived.
|
||||
if (elapsed < 800) {
|
||||
return TelemetryStatus::NotReady;
|
||||
}
|
||||
|
||||
uint8_t buf[TELEMETRY_FRAME_SIZE];
|
||||
int bytes = _uart.read(buf, sizeof(buf));
|
||||
|
||||
if (bytes <= 0) {
|
||||
if (elapsed > 5_ms) {
|
||||
++_num_timeouts;
|
||||
|
||||
// Mark telemetry request as finished
|
||||
_telemetry_request_start = 0;
|
||||
_frame_position = 0;
|
||||
return TelemetryStatus::Timeout;
|
||||
}
|
||||
|
||||
return TelemetryStatus::NotReady;
|
||||
}
|
||||
|
||||
return decodeTelemetryResponse(buf, bytes, esc_data);
|
||||
}
|
||||
|
||||
TelemetryStatus DShotTelemetry::decodeTelemetryResponse(uint8_t *buffer, int length, EscData *esc_data)
|
||||
{
|
||||
auto status = TelemetryStatus::NotReady;
|
||||
|
||||
for (int i = 0; i < length; i++) {
|
||||
_frame_buffer[_frame_position++] = buffer[i];
|
||||
|
||||
/*
|
||||
* 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
|
||||
*/
|
||||
|
||||
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];
|
||||
uint16_t voltage = (_frame_buffer[1] << 8) | _frame_buffer[2];
|
||||
uint16_t current = (_frame_buffer[3] << 8) | _frame_buffer[4];
|
||||
// int16_t consumption = (_frame_buffer[5]) << 8 | _frame_buffer[6];
|
||||
uint16_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;
|
||||
|
||||
} else {
|
||||
++_num_checksum_errors;
|
||||
status = TelemetryStatus::ParseError;
|
||||
}
|
||||
|
||||
// Mark telemetry request as finished
|
||||
_telemetry_request_start = 0;
|
||||
_frame_position = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
bool DShotTelemetry::commandResponseStarted()
|
||||
{
|
||||
return _command_response_start > 0;
|
||||
}
|
||||
|
||||
void DShotTelemetry::startTelemetryRequest()
|
||||
{
|
||||
_frame_position = 0;
|
||||
_telemetry_request_start = hrt_absolute_time();
|
||||
}
|
||||
|
||||
bool DShotTelemetry::telemetryResponseFinished()
|
||||
{
|
||||
return _telemetry_request_start == 0;
|
||||
}
|
||||
|
||||
void DShotTelemetry::printStatus() const
|
||||
{
|
||||
PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses);
|
||||
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");
|
||||
}
|
||||
}
|
||||
PX4_INFO("Successful ESC frames: %i", _num_successful_responses);
|
||||
PX4_INFO("Timeouts: %i", _num_timeouts);
|
||||
PX4_INFO("CRC errors: %i", _num_checksum_errors);
|
||||
}
|
||||
|
||||
@@ -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};
|
||||
};
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
};
|
||||
@@ -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
|
||||
};
|
||||
@@ -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
|
||||
|
||||
@@ -21,6 +21,9 @@ actuator_output:
|
||||
type: enum
|
||||
default: 400
|
||||
values:
|
||||
-8: BDShot150
|
||||
-7: BDShot300
|
||||
-6: BDShot600
|
||||
-5: DShot150
|
||||
-4: DShot300
|
||||
-3: DShot600
|
||||
|
||||
@@ -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++) {
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user