zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

linux_pwm_out: refactor to run on a work queue and use mixer_module

release/1.12
Beat Küng 4 years ago committed by Daniel Agar
parent
439fb00aed
commit
f0cc8a344b
12 changed files with 334 additions and 422 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      Tools/uorb_graph/create.py
  2. 1
      boards/aerotenna/ocpoc/default.cmake
  3. 1
      boards/beaglebone/blue/default.cmake
  4. 1
      boards/emlid/navio2/default.cmake
  5. 5
      posix-configs/bbblue/px4.config
  6. 5
      posix-configs/bbblue/px4_fw.config
  7. 3
      posix-configs/ocpoc/px4.config
  8. 1
      posix-configs/rpi/px4.config
  9. 3
      posix-configs/rpi/px4_fw.config
  10. 1
      posix-configs/rpi/px4_test.config
  11. 636
      src/drivers/linux_pwm_out/linux_pwm_out.cpp
  12. 98
      src/drivers/linux_pwm_out/linux_pwm_out.hpp

1
Tools/uorb_graph/create.py
Unescape View File

@ -225,7 +225,6 @@ class Graph(object): @@ -225,7 +225,6 @@ class Graph(object):
('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
('snapdragon_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
('linux_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
]
special_cases_sub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d))
for a,b,c,d in special_cases_sub]

1
boards/aerotenna/ocpoc/default.cmake
Unescape View File

@ -73,6 +73,7 @@ px4_add_board( @@ -73,6 +73,7 @@ px4_add_board(
led_control
mixer
motor_ramp
motor_test
param
perf
pwm

1
boards/beaglebone/blue/default.cmake
Unescape View File

@ -69,6 +69,7 @@ px4_add_board( @@ -69,6 +69,7 @@ px4_add_board(
led_control
mixer
motor_ramp
motor_test
param
perf
pwm

1
boards/emlid/navio2/default.cmake
Unescape View File

@ -69,6 +69,7 @@ px4_add_board( @@ -69,6 +69,7 @@ px4_add_board(
led_control
mixer
motor_ramp
motor_test
param
perf
pwm

5
posix-configs/bbblue/px4.config
Unescape View File

@ -82,9 +82,8 @@ sleep 1 @@ -82,9 +82,8 @@ sleep 1
# RC port is mapped to /dev/ttyS4 (auto-detected)
#rc_input start -d /dev/ttyS4
# default: etc/mixers/quad_x.main.mix, 8 output channels
linux_pwm_out start -m etc/mixers/quad_x.main.mix
#linux_pwm_out start -m etc/mixers/AETRFG.main.mix
linux_pwm_out start
mixer load /dev/pwm_out etc/mixers/quad_x.main.mix
logger start -t -b 200

5
posix-configs/bbblue/px4_fw.config
Unescape View File

@ -77,9 +77,8 @@ sleep 1 @@ -77,9 +77,8 @@ sleep 1
# RC port is mapped to /dev/ttyS4 (auto-detected)
rc_input start -d /dev/ttyS4
# default: etc/mixers/quad_x.main.mix, 8 output channels
#linux_pwm_out start -m etc/mixers/quad_x.main.mix
linux_pwm_out start -m etc/mixers/AETRFG.main.mix
linux_pwm_out start
mixer load /dev/pwm_out etc/mixers/AETRFG.main.mix
logger start -t -b 200

3
posix-configs/ocpoc/px4.config
Unescape View File

@ -45,6 +45,7 @@ mavlink stream -d /dev/ttyPS1 -s ATTITUDE -r 50 @@ -45,6 +45,7 @@ mavlink stream -d /dev/ttyPS1 -s ATTITUDE -r 50
rc_input start -d /dev/ttyS2
linux_pwm_out start -m etc/mixers/quad_x.main.mix
linux_pwm_out start
mixer load /dev/pwm_out etc/mixers/quad_x.main.mix
logger start -t -b 200
mavlink boot_complete

1
posix-configs/rpi/px4.config
Unescape View File

@ -60,6 +60,7 @@ fi @@ -60,6 +60,7 @@ fi
navio_sysfs_rc_in start
linux_pwm_out start
mixer load /dev/pwm_out etc/mixers/quad_x.main.mix
logger start -t -b 200

3
posix-configs/rpi/px4_fw.config
Unescape View File

@ -57,7 +57,8 @@ then @@ -57,7 +57,8 @@ then
fi
navio_sysfs_rc_in start
linux_pwm_out start -m etc/mixers/AETRFG.main.mix
linux_pwm_out start
mixer load /dev/pwm_out etc/mixers/AETRFG.main.mix
logger start -t -b 200

1
posix-configs/rpi/px4_test.config
Unescape View File

@ -59,6 +59,7 @@ fi @@ -59,6 +59,7 @@ fi
navio_sysfs_rc_in start
linux_pwm_out start
mixer load /dev/pwm_out etc/mixers/quad_x.main.mix
logger start -t -f -b 200

636
src/drivers/linux_pwm_out/linux_pwm_out.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2021 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,530 +31,340 @@ @@ -31,530 +31,340 @@
*
****************************************************************************/
#include <errno.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/posix.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/mixer/mixer_load.h>
#include <parameters/param.h>
#include <output_limit/output_limit.h>
#include <perf/perf_counter.h>
#include "linux_pwm_out.hpp"
#include <board_pwm_out.h>
#include <drivers/drv_hrt.h>
using namespace time_literals;
using namespace pwm_out;
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"))
{
static px4_task_t _task_handle = -1;
volatile bool _task_should_exit = false;
static bool _is_running = false;
static int _max_num_outputs = 8; ///< maximum number of outputs the driver should use
static char _mixer_filename[64] = "etc/mixers/quad_x.main.mix";
// subscriptions
int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
int _armed_sub = -1;
// publications
orb_advert_t _outputs_pub = nullptr;
orb_advert_t _rc_pub = nullptr;
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
}
perf_counter_t _perf_control_latency = nullptr;
LinuxPWMOut::~LinuxPWMOut()
{
/* clean up the alternate device node */
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
// topic structures
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
actuator_outputs_s _outputs;
actuator_armed_s _armed;
perf_free(_cycle_perf);
perf_free(_interval_perf);
delete _pwm_out;
}
// polling
uint8_t _poll_fds_num = 0;
px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
int LinuxPWMOut::init()
{
/* do regular cdev init */
int ret = CDev::init();
// control groups related
uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0;
if (ret != OK) {
return ret;
}
output_limit_t _pwm_limit;
/* 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);
// esc parameters
int32_t _pwm_disarmed;
int32_t _pwm_min;
int32_t _pwm_max;
_mixing_output.setDriverInstance(_class_instance);
MixerGroup *_mixer_group = nullptr;
_pwm_out = new BOARD_PWM_OUT_IMPL(MAX_ACTUATORS);
static void usage();
ret = _pwm_out->init();
static void start();
if (ret != 0) {
PX4_ERR("PWM output init failed");
delete _pwm_out;
_pwm_out = nullptr;
return ret;
}
static void stop();
update_params();
static int task_main_trampoline(int argc, char *argv[]);
ScheduleNow();
static void subscribe();
return ret;
}
static void task_main(int argc, char *argv[]);
int LinuxPWMOut::task_spawn(int argc, char *argv[])
{
LinuxPWMOut *instance = new LinuxPWMOut();
static void update_params(Mixer::Airmode &airmode);
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
/* mixer initialization */
int initialize_mixer(const char *mixer_filename);
int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
if (instance->init() == PX4_OK) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
int mixer_control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls[control_group].control[control_index];
delete instance;
_object.store(nullptr);
_task_id = -1;
return 0;
return PX4_ERROR;
}
void update_params(Mixer::Airmode &airmode)
bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
// multicopter air-mode
param_t param_handle = param_find("MC_AIRMODE");
if (param_handle != PARAM_INVALID) {
param_get(param_handle, (int32_t *)&airmode);
}
_pwm_out->send_output_pwm(outputs, num_outputs);
return true;
}
int initialize_mixer(const char *mixer_filename)
void LinuxPWMOut::Run()
{
char buf[4096];
unsigned buflen = sizeof(buf);
memset(buf, '\0', buflen);
_mixer_group = new MixerGroup();
// PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
if (load_mixer_file(mixer_filename, buf, buflen) == 0) {
if (_mixer_group->load_from_buf(mixer_control_callback, (uintptr_t) &_controls, buf, buflen) == 0) {
PX4_INFO("Loaded mixer from file %s", mixer_filename);
return 0;
if (should_exit()) {
ScheduleClear();
_mixing_output.unregister();
} else {
PX4_ERR("Unable to parse from mixer config file %s", mixer_filename);
}
} else {
PX4_ERR("Unable to load config file %s", mixer_filename);
}
if (_mixer_group->count() <= 0) {
PX4_ERR("Mixer initialization failed");
return -1;
exit_and_cleanup();
return;
}
return 0;
}
perf_begin(_cycle_perf);
perf_count(_interval_perf);
_mixing_output.update();
void subscribe()
{
memset(_controls, 0, sizeof(_controls));
memset(_poll_fds, 0, sizeof(_poll_fds));
/* set up ORB topic names */
_controls_topics[0] = ORB_ID(actuator_controls_0);
_controls_topics[1] = ORB_ID(actuator_controls_1);
_controls_topics[2] = ORB_ID(actuator_controls_2);
_controls_topics[3] = ORB_ID(actuator_controls_3);
// Subscribe for orb topics
for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_groups_required & (1 << i)) {
PX4_DEBUG("subscribe to actuator_controls_%d", i);
_controls_subs[i] = orb_subscribe(_controls_topics[i]);
} else {
_controls_subs[i] = -1;
}
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
if (_controls_subs[i] >= 0) {
_poll_fds[_poll_fds_num].fd = _controls_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
}
// update parameters from storage
update_params();
}
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_mixing_output.updateSubscriptions(false);
}
perf_end(_cycle_perf);
}
void task_main(int argc, char *argv[])
void LinuxPWMOut::update_params()
{
_is_running = true;
updateParams();
_perf_control_latency = perf_alloc(PC_ELAPSED, "linux_pwm_out control latency");
// Set up mixer
if (initialize_mixer(_mixer_filename) < 0) {
PX4_ERR("Mixer initialization failed.");
return;
}
PWMOutBase *pwm_out = new BOARD_PWM_OUT_IMPL(_max_num_outputs);
if (pwm_out->init() != 0) {
PX4_ERR("PWM output init failed");
delete pwm_out;
// skip update when armed
if (_mixing_output.armed().armed) {
return;
}
_mixer_group->groups_required(_groups_required);
int32_t pwm_min_default = PWM_DEFAULT_MIN;
int32_t pwm_max_default = PWM_DEFAULT_MAX;
int32_t pwm_disarmed_default = 0;
// subscribe and set up polling
subscribe();
const char *prefix;
Mixer::Airmode airmode = Mixer::Airmode::disabled;
update_params(airmode);
uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s};
if (_class_instance == CLASS_DEVICE_PRIMARY) {
prefix = "PWM_MAIN";
int rc_channels_sub = -1;
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);
// Start disarmed
_armed.armed = false;
_armed.prearmed = false;
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
prefix = "PWM_AUX";
output_limit_init(&_pwm_limit);
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);
while (!_task_should_exit) {
} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
prefix = "PWM_EXTRA";
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
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);
if (_mixer_group) {
_mixer_group->set_airmode(airmode);
}
int pret = px4_poll(_poll_fds, _poll_fds_num, 10);
} else {
PX4_ERR("invalid class instance %d", _class_instance);
return;
}
/* Timed out, do a periodic check for _task_should_exit. */
if (pret == 0 && !_armed.in_esc_calibration_mode) {
continue;
}
char str[17];
/* This is undesirable but not much we can do. */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(10000);
continue;
}
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
// PWM_MAIN_MINx
{
sprintf(str, "%s_MIN%u", prefix, i + 1);
int32_t pwm_min = -1;
/* get controls for required topics */
unsigned poll_id = 0;
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);
for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_controls_subs[i] >= 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_controls_topics[i], _controls_subs[i], &_controls[i]);
if (pwm_min != _mixing_output.minValue(i)) {
int32_t pwm_min_new = _mixing_output.minValue(i);
param_set(param_find(str), &pwm_min_new);
}
poll_id++;
} else {
_mixing_output.minValue(i) = pwm_min_default;
}
}
if (_armed.in_esc_calibration_mode) {
if (rc_channels_sub == -1) {
// only subscribe when really needed: esc calibration is not something we use regularily
rc_channels_sub = orb_subscribe(ORB_ID(rc_channels));
}
// PWM_MAIN_MAXx
{
sprintf(str, "%s_MAX%u", prefix, i + 1);
int32_t pwm_max = -1;
rc_channels_s rc_channels;
int ret = orb_copy(ORB_ID(rc_channels), rc_channels_sub, &rc_channels);
_controls[0].control[0] = 0.f;
_controls[0].control[1] = 0.f;
_controls[0].control[2] = 0.f;
int channel = rc_channels.function[rc_channels_s::FUNCTION_THROTTLE];
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 (ret == 0 && channel >= 0 && channel < (int)(sizeof(rc_channels.channels) / sizeof(rc_channels.channels[0]))) {
_controls[0].control[3] = rc_channels.channels[channel];
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 {
_controls[0].control[3] = 1.f;
_mixing_output.maxValue(i) = pwm_max_default;
}
/* Switch off the PWM limit ramp for the calibration. */
_pwm_limit.state = OUTPUT_LIMIT_STATE_ON;
}
if (_mixer_group != nullptr) {
/* do mixing */
_outputs.noutputs = _mixer_group->mix(_outputs.output, actuator_outputs_s::NUM_ACTUATOR_OUTPUTS);
/* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs; i < _outputs.NUM_ACTUATOR_OUTPUTS; i++) {
_outputs.output[i] = NAN;
}
// PWM_MAIN_FAILx
{
sprintf(str, "%s_FAIL%u", prefix, i + 1);
int32_t pwm_failsafe = -1;
const uint16_t reverse_mask = 0;
uint16_t disarmed_pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
uint16_t min_pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
uint16_t max_pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
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);
for (unsigned int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
disarmed_pwm[i] = _pwm_disarmed;
min_pwm[i] = _pwm_min;
max_pwm[i] = _pwm_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);
}
}
}
uint16_t pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
// TODO FIXME: pre-armed seems broken
output_limit_calc(_armed.armed,
false/*_armed.prearmed*/,
_outputs.noutputs,
reverse_mask,
disarmed_pwm,
min_pwm,
max_pwm,
_outputs.output,
pwm,
&_pwm_limit);
if (_armed.lockdown || _armed.manual_lockdown) {
pwm_out->send_output_pwm(disarmed_pwm, _outputs.noutputs);
} else if (_armed.in_esc_calibration_mode) {
uint16_t pwm_value;
// PWM_MAIN_DISx
{
sprintf(str, "%s_DIS%u", prefix, i + 1);
int32_t pwm_dis = -1;
if (_controls[0].control[3] > 0.5f) { // use throttle to decide which value to use
pwm_value = _pwm_max;
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);
} else {
pwm_value = _pwm_min;
if (pwm_dis != _mixing_output.disarmedValue(i)) {
int32_t pwm_dis_new = _mixing_output.disarmedValue(i);
param_set(param_find(str), &pwm_dis_new);
}
for (uint32_t i = 0; i < _outputs.noutputs; ++i) {
pwm[i] = pwm_value;
}
pwm_out->send_output_pwm(pwm, _outputs.noutputs);
} else {
pwm_out->send_output_pwm(pwm, _outputs.noutputs);
_mixing_output.disarmedValue(i) = pwm_disarmed_default;
}
}
_outputs.timestamp = hrt_absolute_time();
// PWM_MAIN_REVx
{
sprintf(str, "%s_REV%u", prefix, i + 1);
int32_t pwm_rev = 0;
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
// use first valid timestamp_sample for latency tracking
for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
const bool required = _groups_required & (1 << i);
const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
if (pwm_rev >= 1) {
reverse_pwm_mask = reverse_pwm_mask | (2 << i);
if (required && (timestamp_sample > 0)) {
perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample);
break;
} else {
reverse_pwm_mask = reverse_pwm_mask & ~(2 << i);
}
}
} else {
PX4_ERR("Could not mix output! Exiting...");
_task_should_exit = true;
}
// check for parameter updates
if (parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
parameter_update_sub.copy(&pupdate);
// update parameters from storage
update_params(airmode);
}
}
delete pwm_out;
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);
for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_controls_subs[i] >= 0) {
orb_unsubscribe(_controls_subs[i]);
float pval = 0.0f;
param_get(param_find(str), &pval);
values[i] = roundf(10000 * pval);
}
}
if (_armed_sub != -1) {
orb_unsubscribe(_armed_sub);
_armed_sub = -1;
// copy the trim values to the mixer offsets
_mixing_output.mixers()->set_trims(values, MAX_ACTUATORS);
}
}
if (rc_channels_sub != -1) {
orb_unsubscribe(rc_channels_sub);
}
int LinuxPWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
int ret = OK;
perf_free(_perf_control_latency);
PX4_DEBUG("ioctl cmd: %d, arg: %ld", cmd, arg);
_is_running = false;
lock();
}
switch (cmd) {
case MIXERIOCRESET:
_mixing_output.resetMixerThreadSafe();
break;
int task_main_trampoline(int argc, char *argv[])
{
task_main(argc, argv);
return 0;
}
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strlen(buf);
ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
update_params();
break;
}
void start()
{
_task_should_exit = false;
/* start the task */
_task_handle = px4_task_spawn_cmd("pwm_out_main",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
(px4_main_t)&task_main_trampoline,
nullptr);
if (_task_handle < 0) {
PX4_ERR("task start failed");
return;
default:
ret = -ENOTTY;
break;
}
}
void stop()
{
_task_should_exit = true;
unlock();
while (_is_running) {
usleep(200000);
PX4_INFO(".");
if (ret == -ENOTTY) {
ret = CDev::ioctl(filp, cmd, arg);
}
_task_handle = -1;
return ret;
}
void usage()
int LinuxPWMOut::custom_command(int argc, char *argv[])
{
PX4_INFO("usage: linux_pwm_out start [-m mixerfile]");
PX4_INFO(" -m mixerfile : path to mixerfile");
PX4_INFO(" (default etc/mixers/quad_x.main.mix)");
PX4_INFO(" -n num_outputs : maximum number of outputs the driver should use");
PX4_INFO(" (default is 8)");
PX4_INFO(" linux_pwm_out stop");
PX4_INFO(" linux_pwm_out status");
return print_usage("unknown command");
}
} // namespace pwm_out
/* driver 'main' command */
extern "C" __EXPORT int linux_pwm_out_main(int argc, char *argv[]);
int linux_pwm_out_main(int argc, char *argv[])
int LinuxPWMOut::print_status()
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
char *verb = nullptr;
if (argc >= 2) {
verb = argv[1];
} else {
return 1;
}
while ((ch = px4_getopt(argc, argv, "m:n:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'm':
strncpy(pwm_out::_mixer_filename, myoptarg, sizeof(pwm_out::_mixer_filename) - 1);
break;
case 'n': {
unsigned long max_num = strtoul(myoptarg, nullptr, 10);
if (max_num <= 0) {
max_num = 8;
}
if (max_num > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
max_num = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
}
pwm_out::_max_num_outputs = max_num;
}
break;
}
}
/** gets the parameters for the esc's pwm */
param_get(param_find("PWM_MAIN_DISARM"), &pwm_out::_pwm_disarmed);
param_get(param_find("PWM_MAIN_MIN"), &pwm_out::_pwm_min);
param_get(param_find("PWM_MAIN_MAX"), &pwm_out::_pwm_max);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
if (pwm_out::_is_running) {
PX4_WARN("pwm_out already running");
return 1;
}
pwm_out::start();
}
else if (!strcmp(verb, "stop")) {
if (!pwm_out::_is_running) {
PX4_WARN("pwm_out is not running");
return 1;
}
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
_mixing_output.printStatus();
return 0;
}
pwm_out::stop();
int LinuxPWMOut::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
else if (!strcmp(verb, "status")) {
PX4_WARN("pwm_out is %s", pwm_out::_is_running ? "running" : "not running");
return 0;
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Linux PWM output driver with board-specific backend implementation.
)DESCR_STR");
} else {
pwm_out::usage();
return 1;
}
PRINT_MODULE_USAGE_NAME("linux_pwm_out", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int linux_pwm_out_main(int argc, char *argv[])
{
return LinuxPWMOut::main(argc, argv);
}

98
src/drivers/linux_pwm_out/linux_pwm_out.hpp
Unescape View File

@ -0,0 +1,98 @@ @@ -0,0 +1,98 @@
/****************************************************************************
*
* Copyright (c) 2021 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/device/device.h>
#include <drivers/drv_mixer.h>
#include <lib/cdev/CDev.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <px4_platform/pwm_out_base.h>
using namespace time_literals;
class LinuxPWMOut : public cdev::CDev, public ModuleBase<LinuxPWMOut>, public OutputModuleInterface
{
public:
LinuxPWMOut();
virtual ~LinuxPWMOut();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
void Run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
int init() override;
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
private:
static constexpr int MAX_ACTUATORS = 8;
void update_params();
MixingOutput _mixing_output{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;
perf_counter_t _interval_perf;
};
Write Preview
Loading…
Cancel
Save