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

delete lib/mixer and mixer_module static mixing

Browse Source
This commit is contained in:
Daniel Agar
2022-08-24 16:50:02 -04:00
parent 0019ffbea6
commit a7bbcd5b04
79 changed files with 219 additions and 6835 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/actuators/modalai_esc/modalai_esc.cpp
+1 -25
View File
@@ -38,7 +38,6 @@
#include "modalai_esc.hpp"
#include "modalai_esc_serial.hpp"
#define MODALAI_ESC_DEVICE_PATH "/dev/uart_esc"
#define MODALAI_ESC_DEFAULT_PORT "/dev/ttyS1"
#define MODALAI_ESC_VOXL_PORT "/dev/ttyS4"
@@ -48,7 +47,6 @@
const char *_device;
ModalaiEsc::ModalaiEsc() :
CDev(MODALAI_ESC_DEVICE_PATH),
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval"))
@@ -96,35 +94,15 @@ ModalaiEsc::~ModalaiEsc()
_uart_port_bridge = nullptr;
}
/* clean up the alternate device node */
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
perf_free(_cycle_perf);
perf_free(_output_update_perf);
}
int ModalaiEsc::init()
{
/* do regular cdev init */
int ret = CDev::init();
if (ret != OK) {
return ret;
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
_class_instance = register_class_devname(MODALAI_ESC_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* lets not be too verbose */
} else if (_class_instance < 0) {
PX4_ERR("FAILED registering class device");
}
_mixing_output.setDriverInstance(_class_instance);
/* Getting initial parameter values */
ret = update_params();
int ret = update_params();
if (ret != OK) {
return ret;
@@ -1135,8 +1113,6 @@ void ModalaiEsc::Run()
return;
}
SmartLock lock_guard(_lock);
perf_begin(_cycle_perf);
/* Open serial port in this thread */
src/drivers/actuators/modalai_esc/modalai_esc.hpp
+1 -7
View File
@@ -34,7 +34,6 @@
#pragma once
#include <drivers/device/device.h>
#include <lib/cdev/CDev.hpp>
#include <lib/led/led.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/perf/perf_counter.h>
@@ -51,7 +50,7 @@
#include "qc_esc_packet.h"
#include "qc_esc_packet_types.h"
class ModalaiEsc : public cdev::CDev, public ModuleBase<ModalaiEsc>, public OutputModuleInterface
class ModalaiEsc : public ModuleBase<ModalaiEsc>, public OutputModuleInterface
{
public:
ModalaiEsc();
@@ -76,9 +75,6 @@ public:
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
/** @see OutputModuleInterface */
void mixerChanged() override;
virtual int init();
typedef enum {
@@ -183,8 +179,6 @@ private:
ch_assign_t _output_map[MODALAI_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
MixingOutput _mixing_output{"MODALAI_ESC", MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
int _class_instance{-1};
perf_counter_t _cycle_perf;
perf_counter_t _output_update_perf;
src/drivers/cyphal/Cyphal.hpp
-2
View File
@@ -91,8 +91,6 @@ public:
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
void mixerChanged() override {};
void printInfo() { _mixing_output.printStatus(); }
MixingOutput &mixingOutput() { return _mixing_output; }
src/drivers/dshot/CMakeLists.txt
-1
View File
@@ -48,7 +48,6 @@ px4_add_module(
DEPENDS
arch_io_pins
arch_dshot
mixer
mixer_module
MODULE_CONFIG
module.yaml
src/drivers/dshot/DShot.cpp
+57 -73
View File
@@ -47,10 +47,8 @@ DShot::DShot() :
_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
if (_mixing_output.useDynamicMixing()) {
// Avoid using the PWM failsafe params
_mixing_output.setAllFailsafeValues(UINT16_MAX);
}
// Avoid using the PWM failsafe params
_mixing_output.setAllFailsafeValues(UINT16_MAX);
}
DShot::~DShot()
@@ -64,8 +62,6 @@ DShot::~DShot()
int DShot::init()
{
_mixing_output.setDriverInstance(0);
_output_mask = (1u << _num_outputs) - 1;
// Getting initial parameter values
@@ -102,75 +98,72 @@ int DShot::task_spawn(int argc, char *argv[])
void DShot::enable_dshot_outputs(const bool enabled)
{
if (enabled && !_outputs_initialized) {
if (_mixing_output.useDynamicMixing()) {
unsigned int dshot_frequency = 0;
uint32_t dshot_frequency_param = 0;
unsigned int dshot_frequency = 0;
uint32_t dshot_frequency_param = 0;
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = io_timer_get_group(timer);
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = io_timer_get_group(timer);
if (channels == 0) {
continue;
}
if (channels == 0) {
continue;
}
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
unsigned int dshot_frequency_request = 0;
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
unsigned int dshot_frequency_request = 0;
if (tim_config == -5) {
dshot_frequency_request = DSHOT150;
if (tim_config == -5) {
dshot_frequency_request = DSHOT150;
} else if (tim_config == -4) {
dshot_frequency_request = DSHOT300;
} else if (tim_config == -4) {
dshot_frequency_request = DSHOT300;
} else if (tim_config == -3) {
dshot_frequency_request = DSHOT600;
} else if (tim_config == -3) {
dshot_frequency_request = DSHOT600;
} else if (tim_config == -2) {
dshot_frequency_request = DSHOT1200;
} 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 {
_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;
}
dshot_frequency = dshot_frequency_request;
dshot_frequency_param = tim_config;
}
}
}
int ret = up_dshot_init(_output_mask, dshot_frequency);
int ret = up_dshot_init(_output_mask, dshot_frequency);
if (ret < 0) {
PX4_ERR("up_dshot_init failed (%i)", ret);
return;
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);
}
}
_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;
}
if (_output_mask == 0) {
// exit the module if no outputs used
request_stop();
return;
}
_outputs_initialized = true;
@@ -190,17 +183,10 @@ void DShot::update_telemetry_num_motors()
int motor_count = 0;
if (_mixing_output.useDynamicMixing()) {
for (unsigned i = 0; i < _num_outputs; ++i) {
if (_mixing_output.isFunctionSet(i)) {
_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
++motor_count;
}
}
} else {
if (_mixing_output.mixers()) {
motor_count = _mixing_output.mixers()->get_multirotor_count();
for (unsigned i = 0; i < _num_outputs; ++i) {
if (_mixing_output.isFunctionSet(i)) {
_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
++motor_count;
}
}
@@ -449,9 +435,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
_current_command.clear();
}
if (stop_motors || num_control_groups_updated > 0 || _mixing_output.useDynamicMixing()) {
up_dshot_trigger();
}
up_dshot_trigger();
return true;
}
@@ -471,7 +455,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.initialized();
bool outputs_on = true;
if (_outputs_on != outputs_on) {
enable_dshot_outputs(outputs_on);
src/drivers/linux_pwm_out/linux_pwm_out.cpp
+2 -188
View File
@@ -41,22 +41,14 @@
using namespace pwm_out;
LinuxPWMOut::LinuxPWMOut() :
CDev("/dev/pwm_out"),
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
{
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
}
LinuxPWMOut::~LinuxPWMOut()
{
/* clean up the alternate device node */
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
perf_free(_cycle_perf);
perf_free(_interval_perf);
delete _pwm_out;
@@ -64,21 +56,9 @@ LinuxPWMOut::~LinuxPWMOut()
int LinuxPWMOut::init()
{
/* do regular cdev init */
int ret = CDev::init();
if (ret != OK) {
return ret;
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
_mixing_output.setDriverInstance(_class_instance);
_pwm_out = new BOARD_PWM_OUT_IMPL(MAX_ACTUATORS);
ret = _pwm_out->init();
int ret = _pwm_out->init();
if (ret != 0) {
PX4_ERR("PWM output init failed");
@@ -87,8 +67,6 @@ int LinuxPWMOut::init()
return ret;
}
update_params();
ScheduleNow();
return ret;
@@ -134,8 +112,6 @@ void LinuxPWMOut::Run()
return;
}
SmartLock lock_guard(_lock);
perf_begin(_cycle_perf);
perf_count(_interval_perf);
@@ -148,7 +124,7 @@ void LinuxPWMOut::Run()
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
update_params();
updateParams();
}
_mixing_output.updateSubscriptions(false);
@@ -156,168 +132,6 @@ void LinuxPWMOut::Run()
perf_end(_cycle_perf);
}
void LinuxPWMOut::update_params()
{
updateParams();
// skip update when armed or dynamic mixing enabled
if (_mixing_output.armed().armed || _mixing_output.useDynamicMixing()) {
return;
}
int32_t pwm_min_default = PWM_DEFAULT_MIN;
int32_t pwm_max_default = PWM_DEFAULT_MAX;
int32_t pwm_disarmed_default = 0;
int32_t pwm_default_channels = 0;
const char *prefix;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
prefix = "PWM_MAIN";
param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
prefix = "PWM_AUX";
param_get(param_find("PWM_AUX_MIN"), &pwm_min_default);
param_get(param_find("PWM_AUX_MAX"), &pwm_max_default);
param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
prefix = "PWM_EXTRA";
param_get(param_find("PWM_EXTRA_MIN"), &pwm_min_default);
param_get(param_find("PWM_EXTRA_MAX"), &pwm_max_default);
param_get(param_find("PWM_EXTRA_DISARM"), &pwm_disarmed_default);
} else {
PX4_ERR("invalid class instance %d", _class_instance);
return;
}
uint32_t single_ch = 0;
uint32_t pwm_default_channel_mask = 0;
while ((single_ch = pwm_default_channels % 10)) {
pwm_default_channel_mask |= 1 << (single_ch - 1);
pwm_default_channels /= 10;
}
char str[17];
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
// PWM_MAIN_MINx
{
sprintf(str, "%s_MIN%u", prefix, i + 1);
int32_t pwm_min = -1;
if (param_get(param_find(str), &pwm_min) == PX4_OK && pwm_min >= 0) {
_mixing_output.minValue(i) = math::constrain(pwm_min, PWM_LOWEST_MIN, PWM_HIGHEST_MIN);
if (pwm_min != _mixing_output.minValue(i)) {
int32_t pwm_min_new = _mixing_output.minValue(i);
param_set(param_find(str), &pwm_min_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.minValue(i) = pwm_min_default;
}
}
// PWM_MAIN_MAXx
{
sprintf(str, "%s_MAX%u", prefix, i + 1);
int32_t pwm_max = -1;
if (param_get(param_find(str), &pwm_max) == PX4_OK && pwm_max >= 0) {
_mixing_output.maxValue(i) = math::constrain(pwm_max, PWM_LOWEST_MAX, PWM_HIGHEST_MAX);
if (pwm_max != _mixing_output.maxValue(i)) {
int32_t pwm_max_new = _mixing_output.maxValue(i);
param_set(param_find(str), &pwm_max_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.maxValue(i) = pwm_max_default;
}
}
// PWM_MAIN_DISx
{
sprintf(str, "%s_DIS%u", prefix, i + 1);
int32_t pwm_dis = -1;
if (param_get(param_find(str), &pwm_dis) == PX4_OK && pwm_dis >= 0) {
_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, 0, PWM_HIGHEST_MAX);
if (pwm_dis != _mixing_output.disarmedValue(i)) {
int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
param_set(param_find(str), &pwm_dis_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.disarmedValue(i) = pwm_disarmed_default;
}
}
// PWM_MAIN_FAILx
{
sprintf(str, "%s_FAIL%u", prefix, i + 1);
int32_t pwm_failsafe = -1;
if (param_get(param_find(str), &pwm_failsafe) == PX4_OK && pwm_failsafe >= 0) {
_mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, 0, PWM_HIGHEST_MAX);
if (pwm_failsafe != _mixing_output.failsafeValue(i)) {
int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
param_set(param_find(str), &pwm_fail_new);
}
} else {
// if no channel specific failsafe value is configured, use the disarmed value
_mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i);
}
}
// PWM_MAIN_REVx
{
sprintf(str, "%s_REV%u", prefix, i + 1);
int32_t pwm_rev = 0;
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
if (pwm_rev >= 1) {
reverse_pwm_mask = reverse_pwm_mask | (2 << i);
} else {
reverse_pwm_mask = reverse_pwm_mask & ~(2 << i);
}
}
}
}
if (_mixing_output.mixers()) {
int16_t values[MAX_ACTUATORS] {};
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
sprintf(str, "%s_TRIM%u", prefix, i + 1);
float pval = 0.0f;
param_get(param_find(str), &pval);
values[i] = roundf(10000 * pval);
}
// copy the trim values to the mixer offsets
_mixing_output.mixers()->set_trims(values, MAX_ACTUATORS);
}
}
int LinuxPWMOut::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
src/drivers/linux_pwm_out/linux_pwm_out.hpp
+2 -11
View File
@@ -33,9 +33,7 @@
#pragma once
#include <drivers/device/device.h>
#include <lib/cdev/CDev.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h>
@@ -51,8 +49,7 @@
using namespace time_literals;
class LinuxPWMOut : public cdev::CDev, public ModuleBase<LinuxPWMOut>, public OutputModuleInterface
class LinuxPWMOut : public ModuleBase<LinuxPWMOut>, public OutputModuleInterface
{
public:
LinuxPWMOut();
@@ -72,9 +69,7 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
int init() override;
int init();
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
@@ -82,14 +77,10 @@ public:
private:
static constexpr int MAX_ACTUATORS = 8;
void update_params();
MixingOutput _mixing_output{"PWM_MAIN", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int _class_instance{-1};
pwm_out::PWMOutBase *_pwm_out{nullptr};
perf_counter_t _cycle_perf;
src/drivers/pca9685_pwm_out/CMakeLists.txt
-1
View File
@@ -40,6 +40,5 @@ px4_add_module(
MODULE_CONFIG
module.yaml
DEPENDS
mixer
mixer_module
)
src/drivers/pca9685_pwm_out/main.cpp
-226
View File
@@ -59,14 +59,11 @@ using namespace time_literals;
class PCA9685Wrapper : public ModuleBase<PCA9685Wrapper>, public OutputModuleInterface
{
public:
PCA9685Wrapper(int schd_rate_limit = 400);
~PCA9685Wrapper() override ;
int init();
void mixerChanged() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
@@ -85,8 +82,6 @@ public:
private:
perf_counter_t _cycle_perf;
int _class_instance{-1};
enum class STATE : uint8_t {
INIT,
WAIT_FOR_OSC,
@@ -102,12 +97,6 @@ private:
void Run() override;
protected:
void updateParams() override;
void updatePWMParams();
void updatePWMParamTrim();
int _schd_rate_limit = 400;
PCA9685 *pca9685 = nullptr; // driver handle.
@@ -122,10 +111,6 @@ PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) :
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_schd_rate_limit(schd_rate_limit)
{
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
}
PCA9685Wrapper::~PCA9685Wrapper()
@@ -157,205 +142,6 @@ int PCA9685Wrapper::init()
return PX4_OK;
}
void PCA9685Wrapper::updateParams()
{
updatePWMParams();
ModuleParams::updateParams();
}
void PCA9685Wrapper::updatePWMParams()
{
if (_mixing_output.useDynamicMixing()) {
return;
}
// update pwm params
const char *pname_format_pwm_ch_max[2] = {"PWM_MAIN_MAX%d", "PWM_AUX_MAX%d"};
const char *pname_format_pwm_ch_min[2] = {"PWM_MAIN_MIN%d", "PWM_AUX_MIN%d"};
const char *pname_format_pwm_ch_fail[2] = {"PWM_MAIN_FAIL%d", "PWM_AUX_FAIL%d"};
const char *pname_format_pwm_ch_dis[2] = {"PWM_MAIN_DIS%d", "PWM_AUX_DIS%d"};
const char *pname_format_pwm_ch_rev[2] = {"PWM_MAIN_REV%d", "PWM_AUX_REV%d"};
int32_t default_pwm_max = PWM_DEFAULT_MAX,
default_pwm_min = PWM_DEFAULT_MIN,
default_pwm_fail = PWM_DEFAULT_MIN,
default_pwm_dis = PWM_MOTOR_OFF;
param_t param_h = param_find("PWM_MAIN_MAX");
if (param_h != PARAM_INVALID) {
param_get(param_h, &default_pwm_max);
} else {
PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_MAX");
}
param_h = param_find("PWM_MAIN_MIN");
if (param_h != PARAM_INVALID) {
param_get(param_h, &default_pwm_min);
} else {
PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_MIN");
}
param_h = param_find("PWM_MAIN_RATE");
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (_last_fetched_Freq != pval) {
_last_fetched_Freq = pval;
_targetFreq = (float)pval; // update only if changed
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAIN_RATE");
}
for (int i = 0; i < PCA9685_PWM_CHANNEL_COUNT; i++) {
char pname[16];
uint8_t param_group, param_index;
if (i <= 7) { // Main channel
param_group = 0;
param_index = i + 1;
} else { // AUX
param_group = 1;
param_index = i - 8 + 1;
}
sprintf(pname, pname_format_pwm_ch_max[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (pval != -1) {
_mixing_output.maxValue(i) = pval;
} else {
_mixing_output.maxValue(i) = default_pwm_max;
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
sprintf(pname, pname_format_pwm_ch_min[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (pval != -1) {
_mixing_output.minValue(i) = pval;
} else {
_mixing_output.minValue(i) = default_pwm_min;
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
sprintf(pname, pname_format_pwm_ch_fail[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (pval != -1) {
_mixing_output.failsafeValue(i) = pval;
} else {
_mixing_output.failsafeValue(i) = default_pwm_fail;
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
sprintf(pname, pname_format_pwm_ch_dis[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
int32_t pval = 0;
param_get(param_h, &pval);
if (pval != -1) {
_mixing_output.disarmedValue(i) = pval;
} else {
_mixing_output.disarmedValue(i) = default_pwm_dis;
}
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
sprintf(pname, pname_format_pwm_ch_rev[param_group], param_index);
param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
int32_t pval = 0;
param_get(param_h, &pval);
reverse_pwm_mask &= (0xfffe << i); // clear this bit
reverse_pwm_mask |= (((uint16_t)(pval != 0)) << i); // set to new val
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
}
if (_mixing_output.mixers()) { // only update trims if mixer loaded
updatePWMParamTrim();
}
}
void PCA9685Wrapper::updatePWMParamTrim()
{
const char *pname_format_pwm_ch_trim[2] = {"PWM_MAIN_TRIM%d", "PWM_AUX_TRIM%d"};
int16_t trim_values[PCA9685_PWM_CHANNEL_COUNT] = {};
for (int i = 0; i < PCA9685_PWM_CHANNEL_COUNT; i++) {
char pname[16];
uint8_t param_group, param_index;
if (i <= 7) { // Main channel
param_group = 0;
param_index = i + 1;
} else { // AUX
param_group = 1;
param_index = i - 8 + 1;
}
sprintf(pname, pname_format_pwm_ch_trim[param_group], param_index);
param_t param_h = param_find(pname);
int32_t val;
if (param_h != PARAM_INVALID) {
param_get(param_h, &val);
trim_values[i] = (int16_t)val;
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
}
unsigned n_out = _mixing_output.mixers()->set_trims(trim_values, PCA9685_PWM_CHANNEL_COUNT);
PX4_DEBUG("set %d trims", n_out);
}
bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs,
unsigned num_control_groups_updated)
{
@@ -381,7 +167,6 @@ void PCA9685Wrapper::Run()
switch (_state) {
case STATE::INIT:
pca9685->initReg();
updatePWMParams(); // target frequency fetched, immediately apply it
if (_targetFreq > 0.0f) {
if (pca9685->setFreq(_targetFreq) != PX4_OK) {
@@ -564,17 +349,6 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) {
return PX4_ERROR;
}
void PCA9685Wrapper::mixerChanged()
{
OutputModuleInterface::mixerChanged();
if (_mixing_output.mixers()) { // only update trims if mixer loaded
updatePWMParamTrim();
}
_mixing_output.updateSubscriptions(false);
}
extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){
return PCA9685Wrapper::main(argc, argv);
}
src/drivers/pwm_out/CMakeLists.txt
-1
View File
@@ -49,6 +49,5 @@ px4_add_module(
module.yaml
DEPENDS
arch_io_pins
mixer
mixer_module
)
src/drivers/pwm_out/PWMOut.cpp
+65 -490
View File
@@ -58,10 +58,6 @@ PWMOut::PWMOut(int instance, uint8_t output_base) :
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
{
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
}
PWMOut::~PWMOut()
@@ -94,14 +90,7 @@ int PWMOut::init()
PX4_ERR("FAILED registering class device");
}
_mixing_output.setDriverInstance(_class_instance);
if (_mixing_output.useDynamicMixing()) {
_num_outputs = FMU_MAX_ACTUATORS;
} else {
_num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE);
}
_num_outputs = FMU_MAX_ACTUATORS;
_pwm_mask = ((1u << _num_outputs) - 1) << _output_base;
_mixing_output.setMaxNumOutputs(_num_outputs);
@@ -114,116 +103,6 @@ int PWMOut::init()
return 0;
}
/* When set_pwm_rate is called from either of the 2 IOCTLs:
*
* PWM_SERVO_SET_UPDATE_RATE - Sets the "alternate" channel's rate to the callers's rate specified
* and the non "alternate" channels to the _pwm_default_rate.
*
* rate_map = _pwm_alt_rate_channels
* default_rate = _pwm_default_rate
* alt_rate = arg of IOCTL (see rates)
*
* PWM_SERVO_SET_SELECT_UPDATE_RATE - The caller's specified rate map selects the "alternate" channels
* to be set to the alt rate. (_pwm_alt_rate)
* All other channels are set to the default rate. (_pwm_default_rate)
*
* rate_map = arg of IOCTL
* default_rate = _pwm_default_rate
* alt_rate = _pwm_alt_rate
* rate_map - A mask of 1's for the channels to be set to the
* alternate rate.
* N.B. All channels is a given group must be set
* to the same rate/mode. (default or alt)
* rates:
* alt_rate, default_rate For PWM is 25 or 400Hz
* For Oneshot there is no rate, 0 is therefore used
* to select Oneshot mode
*/
int PWMOut::set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate)
{
if (_mixing_output.useDynamicMixing()) {
return -EINVAL;
}
PX4_DEBUG("pwm_out%u set_pwm_rate %x %u %u", _instance, rate_map, default_rate, alt_rate);
for (unsigned pass = 0; pass < 2; pass++) {
/* We should note that group is iterated over from 0 to FMU_MAX_ACTUATORS.
* This allows for the ideal worlds situation: 1 channel per group
* configuration.
*
* This is typically not what HW supports. A group represents a timer
* and channels belongs to a timer.
* Therefore all channels in a group are dependent on the timer's
* common settings and can not be independent in terms of count frequency
* (granularity of pulse width) and rate (period of repetition).
*
* To say it another way, all channels in a group must have the same
* rate and mode. (See rates above.)
*/
for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) {
// get the channel mask for this rate group
uint32_t mask = _pwm_mask & up_pwm_servo_get_rate_group(group);
if (mask == 0) {
continue;
}
// all channels in the group must be either default or alt-rate
uint32_t alt = rate_map & mask;
if (pass == 0) {
// preflight
if ((alt != 0) && (alt != mask)) {
PX4_WARN("rate group %u mask %" PRIx32 " bad overlap %" PRIx32, group, mask, alt);
// not a legal map, bail
return -EINVAL;
}
} else {
// set it - errors here are unexpected
if (alt != 0) {
if (up_pwm_servo_set_rate_group_update(group, alt_rate) != OK) {
PX4_WARN("rate group set alt failed");
return -EINVAL;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, default_rate) != OK) {
PX4_WARN("rate group set default failed");
return -EINVAL;
}
}
}
}
}
_pwm_alt_rate_channels = rate_map;
_pwm_default_rate = default_rate;
_pwm_alt_rate = alt_rate;
// minimum rate for backup schedule
unsigned backup_schedule_rate_hz = math::min(_pwm_default_rate, _pwm_alt_rate);
if (backup_schedule_rate_hz == 0) {
// OneShot rate is 0
backup_schedule_rate_hz = 50;
}
// constrain reasonably (1 to 50 Hz)
backup_schedule_rate_hz = math::constrain(backup_schedule_rate_hz, 1u, 50u);
_backup_schedule_interval_us = roundf(1e6f / backup_schedule_rate_hz);
_current_update_rate = 0; // force update
return OK;
}
void PWMOut::update_current_rate()
{
/*
@@ -274,7 +153,8 @@ int PWMOut::task_spawn(int argc, char *argv[])
}
// only start one instance with dynamic mixing
if (dev->_mixing_output.useDynamicMixing()) {
//if (dev->_mixing_output.useDynamicMixing()) {
if (true) {
break;
}
@@ -297,110 +177,65 @@ bool PWMOut::update_pwm_out_state(bool on)
{
if (on && !_pwm_initialized && _pwm_mask != 0) {
if (_mixing_output.useDynamicMixing()) {
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
_timer_rates[timer] = -1;
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
_timer_rates[timer] = -1;
uint32_t channels = io_timer_get_group(timer);
uint32_t channels = io_timer_get_group(timer);
if (channels == 0) {
continue;
}
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
if (tim_config > 0) {
_timer_rates[timer] = tim_config;
} else if (tim_config == -1) { // OneShot
_timer_rates[timer] = 0;
} else {
_pwm_mask &= ~channels; // don't use for pwm
}
if (channels == 0) {
continue;
}
int ret = up_pwm_servo_init(_pwm_mask);
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
if (ret < 0) {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
return false;
}
int32_t tim_config = 0;
param_t handle = param_find(param_name);
param_get(handle, &tim_config);
_pwm_mask = ret;
if (tim_config > 0) {
_timer_rates[timer] = tim_config;
// set the timer rates
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
if (channels == 0) {
continue;
}
ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]);
if (ret != 0) {
PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret);
_timer_rates[timer] = -1;
_pwm_mask &= ~channels;
}
}
_pwm_initialized = true;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _pwm_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
} else {
// Collect all PWM masks from all instances
uint32_t pwm_mask_new = 0;
// Collect the PWM alt rate channels across all instances
uint32_t pwm_alt_rate_channels_new = 0;
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (_objects[i].load()) {
pwm_mask_new |= _objects[i].load()->get_pwm_mask();
pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels();
}
}
// Initialize the PWM output state for all instances
// this is re-done once per instance, but harmless
int ret = up_pwm_servo_init(pwm_mask_new);
if (ret >= 0) {
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (_objects[i].load()) {
_objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret);
}
}
// Set rate is not affecting non-masked channels, so can be called
// individually
set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate());
_pwm_initialized = true;
// Other instances need to call up_pwm_servo_arm again after we initialized
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
if (i != _instance) {
_require_arming[i].store(true);
}
}
} else if (tim_config == -1) { // OneShot
_timer_rates[timer] = 0;
} else {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
_pwm_mask &= ~channels; // don't use for pwm
}
}
int ret = up_pwm_servo_init(_pwm_mask);
if (ret < 0) {
PX4_ERR("up_pwm_servo_init failed (%i)", ret);
return false;
}
_pwm_mask = ret;
// set the timer rates
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
if (channels == 0) {
continue;
}
ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]);
if (ret != 0) {
PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret);
_timer_rates[timer] = -1;
_pwm_mask &= ~channels;
}
}
_pwm_initialized = true;
// disable unused functions
for (unsigned i = 0; i < _num_outputs; ++i) {
if (((1 << i) & _pwm_mask) == 0) {
_mixing_output.disableFunction(i);
}
}
}
@@ -452,16 +287,10 @@ void PWMOut::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
if (!_mixing_output.useDynamicMixing()) {
// push backup schedule
ScheduleDelayed(_backup_schedule_interval_us);
}
_mixing_output.update();
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing()
|| _mixing_output.armed().in_esc_calibration_mode;
bool pwm_on = true;
if (_pwm_on != pwm_on || _require_arming[_instance].load()) {
@@ -477,13 +306,7 @@ void PWMOut::Run()
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings)
update_params();
}
}
if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) {
update_current_rate();
update_params();
}
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
@@ -503,7 +326,7 @@ void PWMOut::update_params()
updateParams();
// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
if (_mixing_output.useDynamicMixing() && !_first_param_update) {
if (!_first_param_update) {
for (size_t i = 0; i < _num_outputs; i++) {
if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
int32_t output_function;
@@ -544,203 +367,6 @@ void PWMOut::update_params()
}
_first_param_update = false;
if (_mixing_output.useDynamicMixing()) {
return;
}
int32_t pwm_min_default = PWM_DEFAULT_MIN;
int32_t pwm_max_default = PWM_DEFAULT_MAX;
int32_t pwm_disarmed_default = 0;
int32_t pwm_rate_default = 50;
int32_t pwm_default_channels = 0;
const char *prefix;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
prefix = "PWM_MAIN";
param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default);
param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
prefix = "PWM_AUX";
param_get(param_find("PWM_AUX_MIN"), &pwm_min_default);
param_get(param_find("PWM_AUX_MAX"), &pwm_max_default);
param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_AUX_RATE"), &pwm_rate_default);
param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels);
} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
prefix = "PWM_EXTRA";
param_get(param_find("PWM_EXTRA_MIN"), &pwm_min_default);
param_get(param_find("PWM_EXTRA_MAX"), &pwm_max_default);
param_get(param_find("PWM_EXTRA_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_EXTRA_RATE"), &pwm_rate_default);
} else {
PX4_ERR("invalid class instance %d", _class_instance);
return;
}
uint32_t single_ch = 0;
uint32_t pwm_default_channel_mask = 0;
while ((single_ch = pwm_default_channels % 10)) {
pwm_default_channel_mask |= 1 << (single_ch - 1);
pwm_default_channels /= 10;
}
// update the counter
// this is needed to decide if disarmed PWM output should be turned on or not
int num_disarmed_set = 0;
char str[17];
for (unsigned i = 0; i < _num_outputs; i++) {
// PWM_MAIN_MINx
{
sprintf(str, "%s_MIN%u", prefix, i + 1);
int32_t pwm_min = -1;
if (param_get(param_find(str), &pwm_min) == PX4_OK) {
if (pwm_min >= 0 && pwm_min != 1000) {
_mixing_output.minValue(i) = math::constrain(pwm_min, (int32_t) PWM_LOWEST_MIN, (int32_t) PWM_HIGHEST_MIN);
if (pwm_min != _mixing_output.minValue(i)) {
int32_t pwm_min_new = _mixing_output.minValue(i);
param_set(param_find(str), &pwm_min_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.minValue(i) = pwm_min_default;
}
} else {
PX4_ERR("param %s not found", str);
}
}
// PWM_MAIN_MAXx
{
sprintf(str, "%s_MAX%u", prefix, i + 1);
int32_t pwm_max = -1;
if (param_get(param_find(str), &pwm_max) == PX4_OK) {
if (pwm_max >= 0 && pwm_max != 2000) {
_mixing_output.maxValue(i) = math::constrain(pwm_max, (int32_t) PWM_LOWEST_MAX, (int32_t) PWM_HIGHEST_MAX);
if (pwm_max != _mixing_output.maxValue(i)) {
int32_t pwm_max_new = _mixing_output.maxValue(i);
param_set(param_find(str), &pwm_max_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.maxValue(i) = pwm_max_default;
}
} else {
PX4_ERR("param %s not found", str);
}
}
// PWM_MAIN_DISx
{
sprintf(str, "%s_DIS%u", prefix, i + 1);
int32_t pwm_dis = -1;
if (param_get(param_find(str), &pwm_dis) == PX4_OK) {
if (pwm_dis >= 0 && pwm_dis != 900) {
_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
if (pwm_dis != _mixing_output.disarmedValue(i)) {
int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
param_set(param_find(str), &pwm_dis_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.disarmedValue(i) = pwm_disarmed_default;
}
} else {
PX4_ERR("param %s not found", str);
}
if (_mixing_output.disarmedValue(i) > 0) {
num_disarmed_set++;
}
}
// PWM_MAIN_FAILx
{
sprintf(str, "%s_FAIL%u", prefix, i + 1);
int32_t pwm_failsafe = -1;
if (param_get(param_find(str), &pwm_failsafe) == PX4_OK) {
if (pwm_failsafe >= 0) {
_mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX);
if (pwm_failsafe != _mixing_output.failsafeValue(i)) {
int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
param_set(param_find(str), &pwm_fail_new);
}
} else {
// if no channel specific failsafe value is configured, use the disarmed value
_mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i);
}
} else {
PX4_ERR("param %s not found", str);
}
}
// PWM_MAIN_REVx
{
sprintf(str, "%s_REV%u", prefix, i + 1);
int32_t pwm_rev = 0;
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
if (pwm_rev >= 1) {
reverse_pwm_mask = reverse_pwm_mask | (1 << i);
} else {
reverse_pwm_mask = reverse_pwm_mask & ~(1 << i);
}
} else {
PX4_ERR("param %s not found", str);
}
}
}
if (_mixing_output.mixers()) {
int16_t values[FMU_MAX_ACTUATORS] {};
for (unsigned i = 0; i < _num_outputs; i++) {
sprintf(str, "%s_TRIM%u", prefix, i + 1);
float pval = 0.0f;
if (param_get(param_find(str), &pval) != PX4_OK) {
PX4_ERR("param %s not found", str);
}
values[i] = roundf(10000 * pval);
}
// copy the trim values to the mixer offsets
_mixing_output.mixers()->set_trims(values, _num_outputs);
}
_num_disarmed_set = num_disarmed_set;
}
int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -786,7 +412,7 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_UPDATE_RATE:
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
ret = -EINVAL;
break;
case PWM_SERVO_GET_UPDATE_RATE:
@@ -794,7 +420,7 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
ret = -EINVAL;
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
@@ -823,38 +449,9 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_mixing_output.minValue(i) = PWM_HIGHEST_MIN;
}
#if PWM_LOWEST_MIN > 0
else if (pwm->values[i] < PWM_LOWEST_MIN) {
_mixing_output.minValue(i) = PWM_LOWEST_MIN;
}
#endif
else {
_mixing_output.minValue(i) = pwm->values[i];
}
}
break;
}
case PWM_SERVO_SET_MIN_PWM:
ret = -EINVAL;
break;
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
@@ -868,31 +465,9 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_mixing_output.maxValue(i) = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_mixing_output.maxValue(i) = PWM_HIGHEST_MAX;
} else {
_mixing_output.maxValue(i) = pwm->values[i];
}
}
break;
}
case PWM_SERVO_SET_MAX_PWM:
ret = -EINVAL;
break;
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
@@ -997,7 +572,7 @@ int PWMOut::print_status()
perf_print_counter(_interval_perf);
_mixing_output.printStatus();
if (_mixing_output.useDynamicMixing() && _pwm_initialized) {
if (_pwm_initialized) {
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
if (_timer_rates[timer] >= 0) {
PX4_INFO_RAW("Timer %i: rate: %3i", timer, _timer_rates[timer]);
src/drivers/pwm_out/PWMOut.hpp
-2
View File
@@ -128,8 +128,6 @@ private:
MixingOutput _mixing_output {PARAM_PREFIX, FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
uint32_t _backup_schedule_interval_us{1_s};
unsigned _pwm_default_rate{50};
unsigned _pwm_alt_rate{50};
uint32_t _pwm_alt_rate_channels{0};
src/drivers/px4io/px4io.cpp
+6 -230
View File
@@ -306,11 +306,6 @@ private:
*/
int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
/**
* Send mixer definition text to IO
*/
int mixer_send(const char *buf, unsigned buflen, unsigned retries = 3);
/**
* Handle a status update from IO.
*
@@ -355,11 +350,6 @@ PX4IO::PX4IO(device::Device *interface) :
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(PX4IO_SERIAL_DEVICE)),
_interface(interface)
{
if (!_mixing_output.useDynamicMixing()) {
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
_mixing_output.setLowrateSchedulingInterval(20_ms);
}
@@ -487,7 +477,6 @@ int PX4IO::init()
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
_mixing_output.setDriverInstance(_class_instance);
_mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL);
}
@@ -712,7 +701,7 @@ void PX4IO::update_params()
updateParams();
if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) {
if (!_mixing_output.armed().armed) {
// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
if (!_first_param_update) {
@@ -764,177 +753,6 @@ void PX4IO::update_params()
}
_first_param_update = false;
// skip update when armed or PWM disabled
if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) {
return;
}
int32_t pwm_min_default = PWM_DEFAULT_MIN;
int32_t pwm_max_default = PWM_DEFAULT_MAX;
int32_t pwm_disarmed_default = 0;
int32_t pwm_rate_default = 50;
int32_t pwm_default_channels = 0;
const char *prefix = "PWM_MAIN";
param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default);
param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default);
param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default);
param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default);
param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels);
uint32_t single_ch = 0;
uint32_t pwm_default_channel_mask = 0;
while ((single_ch = pwm_default_channels % 10)) {
pwm_default_channel_mask |= 1 << (single_ch - 1);
pwm_default_channels /= 10;
}
char str[17];
// PWM_MAIN_MINx
if (!_pwm_min_configured) {
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_MIN%u", prefix, i + 1);
int32_t pwm_min = -1;
if (param_get(param_find(str), &pwm_min) == PX4_OK) {
if (pwm_min >= 0 && pwm_min != 1000) {
_mixing_output.minValue(i) = math::constrain(pwm_min, static_cast<int32_t>(PWM_LOWEST_MIN),
static_cast<int32_t>(PWM_HIGHEST_MIN));
if (pwm_min != _mixing_output.minValue(i)) {
int32_t pwm_min_new = _mixing_output.minValue(i);
param_set(param_find(str), &pwm_min_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.minValue(i) = pwm_min_default;
}
}
}
_pwm_min_configured = true;
}
// PWM_MAIN_MAXx
if (!_pwm_max_configured) {
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_MAX%u", prefix, i + 1);
int32_t pwm_max = -1;
if (param_get(param_find(str), &pwm_max) == PX4_OK) {
if (pwm_max >= 0 && pwm_max != 2000) {
_mixing_output.maxValue(i) = math::constrain(pwm_max, static_cast<int32_t>(PWM_LOWEST_MAX),
static_cast<int32_t>(PWM_HIGHEST_MAX));
if (pwm_max != _mixing_output.maxValue(i)) {
int32_t pwm_max_new = _mixing_output.maxValue(i);
param_set(param_find(str), &pwm_max_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.maxValue(i) = pwm_max_default;
}
}
}
_pwm_max_configured = true;
}
// PWM_MAIN_DISx
if (!_pwm_dis_configured) {
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_DIS%u", prefix, i + 1);
int32_t pwm_dis = -1;
if (param_get(param_find(str), &pwm_dis) == PX4_OK) {
if (pwm_dis >= 0 && pwm_dis != 900) {
_mixing_output.disarmedValue(i) = math::constrain(pwm_dis, static_cast<int32_t>(0),
static_cast<int32_t>(PWM_HIGHEST_MAX));
if (pwm_dis != _mixing_output.disarmedValue(i)) {
int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
param_set(param_find(str), &pwm_dis_new);
}
} else if (pwm_default_channel_mask & 1 << i) {
_mixing_output.disarmedValue(i) = pwm_disarmed_default;
}
}
}
_pwm_dis_configured = true;
updateDisarmed();
}
// PWM_MAIN_FAILx
if (!_pwm_fail_configured) {
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_FAIL%u", prefix, i + 1);
int32_t pwm_fail = -1;
if (param_get(param_find(str), &pwm_fail) == PX4_OK) {
if (pwm_fail >= 0) {
_mixing_output.failsafeValue(i) = math::constrain(pwm_fail, static_cast<int32_t>(0),
static_cast<int32_t>(PWM_HIGHEST_MAX));
if (pwm_fail != _mixing_output.failsafeValue(i)) {
int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
param_set(param_find(str), &pwm_fail_new);
}
} else {
// if no channel specific failsafe value is configured, use the disarmed value
_mixing_output.failsafeValue(i) = _mixing_output.disarmedValue(i);
}
}
}
_pwm_fail_configured = true;
updateFailsafe();
}
// PWM_MAIN_REVx
if (!_pwm_rev_configured) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
reverse_pwm_mask = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_REV%u", prefix, i + 1);
int32_t pwm_rev = -1;
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
if (pwm_rev >= 1) {
reverse_pwm_mask |= (1 << i);
}
}
}
_pwm_rev_configured = true;
}
// PWM_MAIN_TRIMx
{
int16_t values[8] {};
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_TRIM%u", prefix, i + 1);
float pwm_trim = 0.f;
if (param_get(param_find(str), &pwm_trim) == PX4_OK) {
values[i] = roundf(10000 * pwm_trim);
}
}
if (_mixing_output.mixers()) {
// copy the trim values to the mixer offsets
_mixing_output.mixers()->set_trims(values, _max_actuators);
}
}
}
void PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result)
@@ -1187,30 +1005,13 @@ int PX4IO::io_get_status()
status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i);
}
if (_mixing_output.useDynamicMixing()) {
{
int i = 0;
for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) {
// This is a bit different than below, setting the groups, not the channels
status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset);
}
} else {
// PWM rates, 0 = low rate, 1 = high rate
const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
const int pwm_low_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
const int pwm_high_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
for (unsigned i = 0; i < _max_actuators; i++) {
if (pwm_rate & (1 << i)) {
status.pwm_rate_hz[i] = pwm_high_rate;
} else {
status.pwm_rate_hz[i] = pwm_low_rate;
}
}
}
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
@@ -1544,13 +1345,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_SET_UPDATE_RATE:
PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE");
if (_mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
/* set the requested alternate rate */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
ret = -EINVAL;
break;
case PWM_SERVO_GET_UPDATE_RATE:
@@ -1562,26 +1357,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE");
if (_mixing_output.useDynamicMixing()) {
ret = -EINVAL;
break;
}
/* blindly clear the PWM update alarm - might be set for some other reason */
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
/* attempt to set the rate map */
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg);
/* check that the changes took */
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) {
ret = -EINVAL;
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
PX4_ERR("failed setting PWM rate on IO");
}
ret = -EINVAL;
break;
}
@@ -1624,7 +1400,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
if (pwm->values[i] != 0 && false) {
_mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN);
}
}
@@ -1654,7 +1430,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) {
if (pwm->values[i] != 0 && false) {
_mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX);
}
}
src/drivers/tap_esc/CMakeLists.txt
-1
View File
@@ -45,7 +45,6 @@ px4_add_module(
module.yaml
DEPENDS
led
mixer
mixer_module
tunes
)
src/drivers/tap_esc/TAP_ESC.cpp
+1 -5
View File
@@ -36,7 +36,6 @@
#include <px4_platform_common/sem.hpp>
TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count):
CDev(TAP_ESC_DEVICE_PATH),
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(device)),
_mixing_output{"TAP_ESC", channels_count, *this, MixingOutput::SchedulingPolicy::Auto, true},
_channels_count(channels_count)
@@ -154,8 +153,7 @@ int TAP_ESC::init()
usleep(2000);
}
/* do regular cdev init */
return CDev::init();
return 0;
}
void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const uint8_t motor_cnt)
@@ -337,8 +335,6 @@ void TAP_ESC::Run()
return;
}
SmartLock lock_guard(_lock);
// push backup schedule
ScheduleDelayed(20_ms);
src/drivers/tap_esc/TAP_ESC.hpp
+2 -3
View File
@@ -52,7 +52,6 @@
#include <lib/drivers/device/device.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/cdev/CDev.hpp>
#include <lib/led/led.h>
#include <lib/tunes/tunes.h>
@@ -80,7 +79,7 @@ using namespace time_literals;
/*
* This driver connects to TAP ESCs via serial.
*/
class TAP_ESC : public cdev::CDev, public ModuleBase<TAP_ESC>, public OutputModuleInterface
class TAP_ESC : public ModuleBase<TAP_ESC>, public OutputModuleInterface
{
public:
TAP_ESC(const char *device, uint8_t channels_count);
@@ -98,7 +97,7 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
int init() override;
int init();
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
src/drivers/tap_esc/drv_tap_esc.h
-2
View File
@@ -37,8 +37,6 @@
#include <stdint.h>
#define TAP_ESC_DEVICE_PATH "/dev/tap_esc"
/* At the moment the only known use is with a current sensor */
#define ESC_HAVE_CURRENT_SENSOR
src/drivers/uavcan/CMakeLists.txt
-1
View File
@@ -177,7 +177,6 @@ px4_add_module(
button_publisher
drivers_rangefinder
led
mixer
mixer_module
version
src/drivers/uavcan/uavcan_main.cpp
-16
View File
@@ -575,15 +575,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
_mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
// these are configurable with dynamic mixing
_mixing_interface_esc.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT
_mixing_interface_esc.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value());
param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param);
enable_idle_throttle_when_armed(true);
}
/* Set up shared service clients */
_param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset));
_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));
@@ -931,13 +922,6 @@ void
UavcanNode::enable_idle_throttle_when_armed(bool value)
{
value &= _idle_throttle_when_armed_param > 0;
if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
if (value != _idle_throttle_when_armed) {
_mixing_interface_esc.mixingOutput().setAllMinValues(value ? 1 : 0);
_idle_throttle_when_armed = value;
}
}
}
bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,