fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 21:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

dshot: handle telemetry index correctly when dynamic mixer is used

Browse Source
This commit is contained in:
Beat Küng 2021-09-16 10:54:00 +02:00 committed by Daniel Agar
parent d951c74a7d
commit f5fbcb0770
1 changed files with 20 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/drivers/dshot/DShot.cpp
View File

@ -164,8 +164,15 @@ void DShot::update_telemetry_num_motors()
int motor_count = 0;
if (_mixing_output.mixers()) {
motor_count = _mixing_output.mixers()->get_multirotor_count();
if (_mixing_output.useDynamicMixing()) {
for (unsigned i = 0; i < _num_outputs; ++i) {
motor_count += _mixing_output.isFunctionSet(i);
}
} else {
if (_mixing_output.mixers()) {
motor_count = _mixing_output.mixers()->get_multirotor_count();
}
}
_telemetry->handler.setNumMotors(motor_count);
@ -331,6 +338,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
if (stop_motors) {
int telemetry_index = 0;
// when motors are stopped we check if we have other commands to send
for (int i = 0; i < (int)num_outputs; i++) {
if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) {
@ -338,8 +347,10 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
up_dshot_motor_command(i, _current_command.command, true);
} else {
up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index);
up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
}
telemetry_index += _mixing_output.isFunctionSet(i);
}
if (_current_command.valid()) {
@ -347,6 +358,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
}
} else {
int telemetry_index = 0;
for (int i = 0; i < (int)num_outputs; i++) {
uint16_t output = outputs[i];
@ -365,12 +378,14 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
}
if (output == DSHOT_DISARM_VALUE) {
up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index);
up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
} else {
up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
i == requested_telemetry_index);
telemetry_index == requested_telemetry_index);
}
telemetry_index += _mixing_output.isFunctionSet(i);
}
// clear commands when motors are running