fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 14:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

pwm_out, dshot: add dynamic mixing support

Browse Source
This commit is contained in:
Beat Küng
2021-09-17 11:56:42 +02:00
committed by Daniel Agar
parent 3ff6014a3c
commit c1e5e666f0
6 changed files with 305 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/dshot/DShot.cpp
+102 -27
View File
@@ -33,6 +33,8 @@
#include "DShot.h"
#include <px4_arch/io_timer.h>
char DShot::_telemetry_device[] {};
px4::atomic_bool DShot::_request_telemetry_init{false};
@@ -43,7 +45,7 @@ DShot::DShot() :
_mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE);
_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
_mixing_output.setAllFailsafeValues(UINT16_MAX);
}
DShot::~DShot()
@@ -114,39 +116,112 @@ int DShot::task_spawn(int argc, char *argv[])
void DShot::enable_dshot_outputs(const bool enabled)
{
if (enabled && !_outputs_initialized) {
DShotConfig config = (DShotConfig)_param_dshot_config.get();
if (_mixing_output.useDynamicMixing()) {
unsigned int dshot_frequency = DSHOT600;
unsigned int dshot_frequency = 0;
uint32_t dshot_frequency_param = 0;
switch (config) {
case DShotConfig::DShot150:
dshot_frequency = DSHOT150;
break;
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = io_timer_get_group(timer);
case DShotConfig::DShot300:
dshot_frequency = DSHOT300;
break;
if (channels == 0) {
continue;
}
case DShotConfig::DShot600:
dshot_frequency = DSHOT600;
break;
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
case DShotConfig::DShot1200:
dshot_frequency = DSHOT1200;
break;
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
unsigned int dshot_frequency_request = 0;
default:
break;
if (tim_config == -5) {
dshot_frequency_request = DSHOT150;
} else if (tim_config == -4) {
dshot_frequency_request = DSHOT300;
} else if (tim_config == -3) {
dshot_frequency_request = DSHOT600;
} else if (tim_config == -2) {
dshot_frequency_request = DSHOT1200;
} else {
_output_mask &= ~channels; // don't use for dshot
}
if (dshot_frequency_request != 0) {
if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) {
PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name);
param_set_no_notification(handle, &dshot_frequency_param);
} else {
dshot_frequency = dshot_frequency_request;
dshot_frequency_param = tim_config;
}
}
}
int ret = up_dshot_init(_output_mask, dshot_frequency);
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
return;
}
_output_mask = ret;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _output_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
if (_output_mask == 0) {
// exit the module if no outputs used
request_stop();
return;
}
} else {
DShotConfig config = (DShotConfig)_param_dshot_config.get();
unsigned int dshot_frequency = DSHOT600;
switch (config) {
case DShotConfig::DShot150:
dshot_frequency = DSHOT150;
break;
case DShotConfig::DShot300:
dshot_frequency = DSHOT300;
break;
case DShotConfig::DShot600:
dshot_frequency = DSHOT600;
break;
case DShotConfig::DShot1200:
dshot_frequency = DSHOT1200;
break;
default:
break;
}
int ret = up_dshot_init(_output_mask, dshot_frequency);
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
return;
}
_output_mask = ret;
}
int ret = up_dshot_init(_output_mask, dshot_frequency);
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
return;
}
_output_mask = ret;
_outputs_initialized = true;
}
@@ -414,7 +489,7 @@ void DShot::Run()
_mixing_output.update();
// update output status if armed or if mixer is loaded
bool outputs_on = _mixing_output.armed().armed || _mixing_output.mixers();
bool outputs_on = _mixing_output.armed().armed || _mixing_output.initialized();
if (_outputs_on != outputs_on) {
enable_dshot_outputs(outputs_on);