diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 2b0ee14062..2f7a09eadb 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -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] diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index 0a8ec6719d..ffab2821b8 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -73,6 +73,7 @@ px4_add_board( led_control mixer motor_ramp + motor_test param perf pwm diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index cdcd011127..816d020380 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -69,6 +69,7 @@ px4_add_board( led_control mixer motor_ramp + motor_test param perf pwm diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index ee0d6c30a4..52bd7ace5f 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -69,6 +69,7 @@ px4_add_board( led_control mixer motor_ramp + motor_test param perf pwm diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index b00a6f2d30..da94108f2c 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -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 diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 26e00c37c6..030173a4bc 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -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 diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 09bcafbd1f..cf44f727b8 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -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 diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index d9a63b25ae..1a8ec392b6 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -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 diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index b814e624d0..81a4d0e2f7 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -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 diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index 07562034f9..efb0fafc03 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -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 diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 78dd4e3b4c..d19da24c67 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -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 @@ * ****************************************************************************/ -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include +#include "linux_pwm_out.hpp" #include +#include -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 ×tamp_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); +} diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.hpp b/src/drivers/linux_pwm_out/linux_pwm_out.hpp new file mode 100644 index 0000000000..79755dcd69 --- /dev/null +++ b/src/drivers/linux_pwm_out/linux_pwm_out.hpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + + +class LinuxPWMOut : public cdev::CDev, public ModuleBase, 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; +};