Skip to content

Commit 5f8aad8

Browse files
dakejahldakejahl
committed
cleaned up
1 parent 729c1ff commit 5f8aad8

File tree

4 files changed

+56
-56
lines changed

4 files changed

+56
-56
lines changed

‎msg/versioned/VehicleCommand.msg‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,13 @@ uint8 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location.
128128
uint8 VEHICLE_ROI_TARGET = 4 # Point toward target.
129129
uint8 VEHICLE_ROI_ENUM_END = 5
130130

131+
uint8 ACTUATOR_CONFIGURATION_NONE = 0 # Do nothing.
132+
uint8 ACTUATOR_CONFIGURATION_BEEP = 1 # Command the actuator to beep now.
133+
uint8 ACTUATOR_CONFIGURATION_3D_MODE_ON = 2 # Permanently set the actuator (ESC) to 3D mode (reversible thrust).
134+
uint8 ACTUATOR_CONFIGURATION_3D_MODE_OFF = 3 # Permanently set the actuator (ESC) to non 3D mode (non-reversible thrust).
135+
uint8 ACTUATOR_CONFIGURATION_SPIN_DIRECTION1 = 4 # Permanently set the actuator (ESC) to spin direction 1 (which can be clockwise or counter-clockwise).
136+
uint8 ACTUATOR_CONFIGURATION_SPIN_DIRECTION2 = 5 # Permanently set the actuator (ESC) to spin direction 2 (opposite of direction 1).
137+
131138
uint8 PARACHUTE_ACTION_DISABLE = 0
132139
uint8 PARACHUTE_ACTION_ENABLE = 1
133140
uint8 PARACHUTE_ACTION_RELEASE = 2

‎platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -150,7 +150,7 @@ static int _consecutive_successes[MAX_TIMER_IO_CHANNELS] = {};
150150
// Adaptive base interval per channel (ticks per bit)
151151
// Initialized to nominal 21 ticks/bit, then adapted based on measured timing
152152
static float _base_interval[MAX_TIMER_IO_CHANNELS] = {
153-
[0 ... (MAX_TIMER_IO_CHANNELS - 1)] = 21.0f
153+
[0 ...(MAX_TIMER_IO_CHANNELS - 1)] = 21.0f
154154
};
155155

156156
// ePRM data
@@ -813,6 +813,7 @@ uint32_t convert_edge_intervals_to_bitstream(uint8_t channel_index)
813813
// Clamp to valid range (1-4 bits typical in GCR encoding)
814814
if (bits < 1) {
815815
bits = 1;
816+
816817
} else if (bits > 4) {
817818
bits = 4;
818819
}
@@ -836,6 +837,7 @@ uint32_t convert_edge_intervals_to_bitstream(uint8_t channel_index)
836837
// Pad to 21 bits for GCR decoder (which expects exactly 21 bits)
837838
if (shifted < 21) {
838839
value <<= (21 - shifted);
840+
839841
} else if (shifted > 21) {
840842
// Trim excess bits (shouldn't happen often with proper decoding)
841843
value >>= (shifted - 21);

‎src/drivers/dshot/DShot.cpp‎

Lines changed: 37 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,9 @@ DShot::DShot() :
4848

4949
// Avoid using the PWM failsafe params
5050
_mixing_output.setAllFailsafeValues(UINT16_MAX);
51+
52+
// Advertise early to ensure we beat uavcan
53+
_esc_status_pub.advertise();
5154
}
5255

5356
DShot::~DShot()
@@ -58,7 +61,6 @@ DShot::~DShot()
5861
perf_free(_cycle_perf);
5962
perf_free(_bdshot_success_perf);
6063
perf_free(_bdshot_error_perf);
61-
perf_free(_bdshot_timeout_perf);
6264
perf_free(_telem_success_perf);
6365
perf_free(_telem_error_perf);
6466
perf_free(_telem_timeout_perf);
@@ -69,6 +71,10 @@ int DShot::init()
6971
{
7072
update_params();
7173

74+
_serial_telemetry_enabled = _param_dshot_tel_cfg.get();
75+
_bdshot_telemetry_enabled = _param_dshot_bidir_en.get();
76+
_bdshot_edt_enabled = _param_dshot_bidir_edt.get();
77+
7278
if (initialize_dshot()) {
7379
ScheduleNow();
7480
return PX4_OK;
@@ -196,7 +202,7 @@ void DShot::select_next_command()
196202

197203
_current_command.clear();
198204

199-
if (_param_dshot_bidir_en.get() && _param_dshot_bidir_edt.get() && needs_edt_request_mask) {
205+
if (_bdshot_telemetry_enabled && _bdshot_edt_enabled && needs_edt_request_mask) {
200206
// EDT Request first
201207
int next_motor_index = 0;
202208

@@ -214,7 +220,7 @@ void DShot::select_next_command()
214220
_bdshot_edt_requested_mask |= (1 << next_motor_index);
215221
PX4_INFO("ESC%d: requesting EDT at time %.2fs", next_motor_index + 1, (double)now / 1000000.);
216222

217-
} else if (_param_dshot_tel_cfg.get() && _param_dshot_esc_type.get() && needs_settings_request_mask) {
223+
} else if (_serial_telemetry_enabled && _bdshot_telemetry_enabled && needs_settings_request_mask) {
218224
// Settings Request next
219225
int next_motor_index = 0;
220226

@@ -293,7 +299,7 @@ void DShot::select_next_command()
293299
// _serial_telem_online_mask &= ~(_am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index));
294300
_settings_requested_mask &= ~(_am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index));
295301

296-
_telem_delay_until = hrt_absolute_time() + 100_ms;
302+
_serial_telem_delay_until = hrt_absolute_time() + 100_ms;
297303
}
298304
}
299305

@@ -344,8 +350,8 @@ void DShot::update_motor_outputs(uint16_t outputs[MAX_ACTUATORS], int num_output
344350
bool set_telemetry_bit = false;
345351

346352
if (_telemetry_motor_index == i) {
347-
if (_param_dshot_tel_cfg.get() && _telemetry.telemetryResponseFinished() && _telemetry.commandResponseFinished()) {
348-
if (hrt_absolute_time() > _telem_delay_until) {
353+
if (_serial_telemetry_enabled && _telemetry.telemetryResponseFinished() && _telemetry.commandResponseFinished()) {
354+
if (hrt_absolute_time() > _serial_telem_delay_until) {
349355
set_telemetry_bit = true;
350356
_telemetry.startTelemetryRequest();
351357
}
@@ -431,7 +437,7 @@ uint16_t DShot::calculate_output_value(uint16_t raw, int index)
431437

432438
bool DShot::process_serial_telemetry()
433439
{
434-
if (!_param_dshot_tel_cfg.get()) {
440+
if (!_serial_telemetry_enabled) {
435441
return false;
436442
}
437443

@@ -497,7 +503,7 @@ bool DShot::process_serial_telemetry()
497503
// Consume an empty EscData to zero the data
498504
consume_esc_data(esc, TelemetrySource::Serial);
499505
all_telem_sampled = set_next_telemetry_index();
500-
_telem_delay_until = hrt_absolute_time() + 100_ms; // TODO: how long do we need to wait?
506+
_serial_telem_delay_until = hrt_absolute_time() + 100_ms;
501507
perf_count(_telem_error_perf);
502508
break;
503509
}
@@ -536,14 +542,14 @@ bool DShot::set_next_telemetry_index()
536542

537543
bool DShot::process_bdshot_telemetry()
538544
{
539-
if (!_param_dshot_bidir_en.get()) {
545+
if (!_bdshot_telemetry_enabled) {
540546
return false;
541547
}
542548

543549
hrt_abstime now = hrt_absolute_time();
544550

545551
// Don't try to process any telem data until after ESCs have been given time to boot
546-
if (now < _telem_delay_until) {
552+
if (now < ESC_INIT_TELEM_DELAY) {
547553
return false;
548554
}
549555

@@ -557,10 +563,6 @@ bool DShot::process_bdshot_telemetry()
557563
continue;
558564
}
559565

560-
// TODO: handle Extended Telemetry -- Volt/Curr/Temp
561-
// We won't want to zero out the EscData, and instead
562-
// use the previously set values.
563-
564566
EscData esc = {};
565567

566568
// NOTE: dshot erpm order is actuator channel order, so we map to motor index here
@@ -588,29 +590,26 @@ bool DShot::process_bdshot_telemetry()
588590
}
589591

590592
// Extended DShot Telemetry
591-
if (_param_dshot_bidir_edt.get()) {
593+
if (_bdshot_edt_enabled) {
592594

593595
uint8_t value = 0;
594596

595597
if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_TEMPERATURE, &value) == PX4_OK) {
596598
esc.temperature = value; // BDShot temperature is in C
597-
// PX4_INFO("ESC%d: temperature: %f", motor_index, (double)esc.temperature);
598599

599600
} else {
600601
esc.temperature = _esc_status.esc[motor_index].esc_temperature; // use previous
601602
}
602603

603604
if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_VOLTAGE, &value) == PX4_OK) {
604605
esc.voltage = value * 0.25f; // BDShot voltage is in 0.25V
605-
// PX4_INFO("ESC%d: voltage: %f", motor_index, (double)esc.voltage);
606606

607607
} else {
608608
esc.voltage = _esc_status.esc[motor_index].esc_voltage; // use previous
609609
}
610610

611611
if (up_bdshot_get_extended_telemetry(output_channel, DSHOT_EDT_CURRENT, &value) == PX4_OK) {
612612
esc.current = value * 0.5f; // BDShot current is in 0.5V
613-
// PX4_INFO("ESC%d: current: %f", motor_index, (double)esc.current);
614613

615614
} else {
616615
esc.current = _esc_status.esc[motor_index].esc_current; // use previous
@@ -634,21 +633,18 @@ bool DShot::process_bdshot_telemetry()
634633

635634
void DShot::consume_esc_data(const EscData &esc, TelemetrySource source)
636635
{
637-
bool bdshot_telemetry_enabled = _param_dshot_bidir_en.get();
638-
bool serial_telemetry_enabled = _param_dshot_tel_cfg.get();
639-
640636
if (esc.motor_index >= esc_status_s::CONNECTED_ESC_MAX) {
641637
return;
642638
}
643639

644640
// Require both sources online when enabled
645641
uint8_t online_mask = 0xFF;
646642

647-
if (bdshot_telemetry_enabled) {
643+
if (_bdshot_telemetry_enabled) {
648644
online_mask &= _bdshot_telem_online_mask;
649645
}
650646

651-
if (serial_telemetry_enabled) {
647+
if (_serial_telemetry_enabled) {
652648
online_mask &= _serial_telem_online_mask;
653649
}
654650

@@ -660,7 +656,7 @@ void DShot::consume_esc_data(const EscData &esc, TelemetrySource source)
660656

661657
if (source == TelemetrySource::Serial) {
662658
// Only use SerialTelemetry eRPM when BDShot is disabled
663-
if (!bdshot_telemetry_enabled) {
659+
if (!_bdshot_telemetry_enabled) {
664660
_esc_status.esc[esc.motor_index].timestamp = esc.timestamp;
665661
_esc_status.esc[esc.motor_index].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2);
666662
}
@@ -674,7 +670,7 @@ void DShot::consume_esc_data(const EscData &esc, TelemetrySource source)
674670
_esc_status.esc[esc.motor_index].esc_rpm = esc.erpm / (_param_mot_pole_count.get() / 2);
675671

676672
// Only use BDShot Volt/Curr/Temp when Serial Telemetry is disabled
677-
if (!serial_telemetry_enabled) {
673+
if (!_serial_telemetry_enabled) {
678674
_esc_status.esc[esc.motor_index].esc_voltage = esc.voltage;
679675
_esc_status.esc[esc.motor_index].esc_current = esc.current;
680676
_esc_status.esc[esc.motor_index].esc_temperature = esc.temperature;
@@ -765,26 +761,26 @@ void DShot::handle_configure_actuator(const vehicle_command_s &command)
765761

766762

767763
switch (type) {
768-
case ACTUATOR_CONFIGURATION_BEEP:
764+
case vehicle_command_s::ACTUATOR_CONFIGURATION_BEEP:
769765
_current_command.command = DSHOT_CMD_BEEP1;
770766
break;
771767

772-
case ACTUATOR_CONFIGURATION_3D_MODE_OFF:
768+
case vehicle_command_s::ACTUATOR_CONFIGURATION_3D_MODE_OFF:
773769
_current_command.command = DSHOT_CMD_3D_MODE_OFF;
774770
_current_command.save = true;
775771
break;
776772

777-
case ACTUATOR_CONFIGURATION_3D_MODE_ON:
773+
case vehicle_command_s::ACTUATOR_CONFIGURATION_3D_MODE_ON:
778774
_current_command.command = DSHOT_CMD_3D_MODE_ON;
779775
_current_command.save = true;
780776
break;
781777

782-
case ACTUATOR_CONFIGURATION_SPIN_DIRECTION1:
778+
case vehicle_command_s::ACTUATOR_CONFIGURATION_SPIN_DIRECTION1:
783779
_current_command.command = DSHOT_CMD_SPIN_DIRECTION_1;
784780
_current_command.save = true;
785781
break;
786782

787-
case ACTUATOR_CONFIGURATION_SPIN_DIRECTION2:
783+
case vehicle_command_s::ACTUATOR_CONFIGURATION_SPIN_DIRECTION2:
788784
_current_command.command = DSHOT_CMD_SPIN_DIRECTION_2;
789785
_current_command.save = true;
790786
break;
@@ -928,7 +924,7 @@ bool DShot::initialize_dshot()
928924
}
929925
}
930926

931-
int ret = up_dshot_init(_output_mask, dshot_frequency, _param_dshot_bidir_en.get(), _param_dshot_bidir_edt.get());
927+
int ret = up_dshot_init(_output_mask, dshot_frequency, _bdshot_telemetry_enabled, _bdshot_edt_enabled);
932928

933929
if (ret < 0) {
934930
PX4_ERR("up_dshot_init failed (%i)", ret);
@@ -965,40 +961,40 @@ void DShot::init_telemetry(const char *device, bool swap_rxtx)
965961
return;
966962
}
967963

964+
if (!_serial_telemetry_enabled) {
965+
PX4_ERR("serial telemetry not enabled");
966+
return;
967+
}
968+
968969
if (_telemetry.init(device, swap_rxtx) != PX4_OK) {
969970
PX4_ERR("telemetry init failed");
971+
return;
970972
}
971973

972974
// Initialize ESC settings handlers based on ESC type
973975
ESCType esc_type = static_cast<ESCType>(_param_dshot_esc_type.get());
974976
_telemetry.initSettingsHandlers(esc_type, _output_mask);
975-
976-
// TODO: enforce uavcan init ordering in some way
977-
// Advertise early to ensure we beat uavcan
978-
_esc_status_pub.advertise();
979977
}
980978

981979
int DShot::print_status()
982980
{
983-
PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask);
981+
_mixing_output.printStatus();
982+
984983
perf_print_counter(_cycle_perf);
985984
perf_print_counter(_bdshot_success_perf);
986985
perf_print_counter(_bdshot_error_perf);
987-
perf_print_counter(_bdshot_timeout_perf);
988986

989987
perf_print_counter(_telem_success_perf);
990988
perf_print_counter(_telem_error_perf);
991989
perf_print_counter(_telem_timeout_perf);
992990
perf_print_counter(_telem_allsampled_perf);
993991

994-
_mixing_output.printStatus();
995-
996-
if (_param_dshot_tel_cfg.get()) {
992+
if (_serial_telemetry_enabled) {
997993
PX4_INFO("telemetry on: %s", _telemetry_device);
998994
_telemetry.printStatus();
999995
}
1000996

1001-
if (_param_dshot_bidir_en.get()) {
997+
if (_bdshot_telemetry_enabled) {
1002998
up_bdshot_status();
1003999
}
10041000

‎src/drivers/dshot/DShot.h‎

Lines changed: 9 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -49,9 +49,9 @@ using namespace time_literals;
4949
# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS"
5050
#endif
5151

52-
static constexpr hrt_abstime ESC_INIT_TELEM_WAIT_TIME = 5_s;
52+
static constexpr hrt_abstime ESC_INIT_TELEM_DELAY = 5_s;
5353

54-
/** Dshot PWM frequency, Hz */
54+
/// Dshot PWM frequency (Hz)
5555
static constexpr unsigned int DSHOT150 = 150000u;
5656
static constexpr unsigned int DSHOT300 = 300000u;
5757
static constexpr unsigned int DSHOT600 = 600000u;
@@ -60,14 +60,6 @@ static constexpr uint16_t DSHOT_DISARM_VALUE = 0;
6060
static constexpr uint16_t DSHOT_MIN_THROTTLE = 1;
6161
static constexpr uint16_t DSHOT_MAX_THROTTLE = 1999;
6262

63-
// We do this to avoid bringing in mavlink.h
64-
// #include <mavlink/common/mavlink.h>
65-
#define ACTUATOR_CONFIGURATION_BEEP 1
66-
#define ACTUATOR_CONFIGURATION_3D_MODE_OFF 2
67-
#define ACTUATOR_CONFIGURATION_3D_MODE_ON 3
68-
#define ACTUATOR_CONFIGURATION_SPIN_DIRECTION1 4
69-
#define ACTUATOR_CONFIGURATION_SPIN_DIRECTION2 5
70-
7163
class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
7264
{
7365
public:
@@ -99,7 +91,7 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
9991
Armed
10092
} _state = State::Disarmed;
10193

102-
/** Disallow copy construction and move assignment. */
94+
// Disallow copy construction and move assignment
10395
DShot(const DShot &) = delete;
10496
DShot operator=(const DShot &) = delete;
10597

@@ -121,7 +113,6 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
121113
uint16_t calculate_output_value(uint16_t raw, int index);
122114
uint16_t convert_output_to_3d_scaling(uint16_t output);
123115

124-
125116
void Run() override;
126117
void update_params();
127118

@@ -163,13 +154,17 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
163154
static px4::atomic_bool _request_telemetry_init;
164155
int _telemetry_motor_index = 0;
165156
uint32_t _telemetry_requested_mask = 0;
166-
hrt_abstime _telem_delay_until = ESC_INIT_TELEM_WAIT_TIME;
157+
hrt_abstime _serial_telem_delay_until = ESC_INIT_TELEM_DELAY;
158+
159+
// Parameters we must load only at init
160+
bool _serial_telemetry_enabled = false;
161+
bool _bdshot_telemetry_enabled = false;
162+
bool _bdshot_edt_enabled = false;
167163

168164
// Perf counters
169165
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
170166
perf_counter_t _bdshot_success_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot success")};
171167
perf_counter_t _bdshot_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot error")};
172-
perf_counter_t _bdshot_timeout_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot timeout")};
173168
perf_counter_t _telem_success_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem success")};
174169
perf_counter_t _telem_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem error")};
175170
perf_counter_t _telem_timeout_perf{perf_alloc(PC_COUNT, MODULE_NAME": telem timeout")};

0 commit comments

Comments
 (0)