From 0e8c73f4af77160336bbaa2ec9b74d3d5d278a27 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 25 Feb 2021 11:30:45 +0100 Subject: [PATCH] drivers: remove snapdragon_pwm_out - it would need to be refactored to use mixer_module - rather than having a separate driver it should use linux_pwm_out --- Tools/uorb_graph/create.py | 1 - boards/atlflight/eagle/qurt.cmake | 1 - boards/atlflight/excelsior/qurt.cmake | 1 - posix-configs/eagle/flight/px4.config | 3 +- src/drivers/snapdragon_pwm_out/CMakeLists.txt | 42 -- .../snapdragon_pwm_out/snapdragon_pwm_out.cpp | 595 ------------------ 6 files changed, 2 insertions(+), 641 deletions(-) delete mode 100644 src/drivers/snapdragon_pwm_out/CMakeLists.txt delete mode 100644 src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 42bedff664..8eadc05796 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -223,7 +223,6 @@ class Graph(object): ('listener', r'.*', None, r'^(id)$'), ('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'), - ('snapdragon_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/atlflight/eagle/qurt.cmake b/boards/atlflight/eagle/qurt.cmake index c7993cc03e..8f092994a6 100644 --- a/boards/atlflight/eagle/qurt.cmake +++ b/boards/atlflight/eagle/qurt.cmake @@ -49,7 +49,6 @@ px4_add_board( imu/invensense/mpu9250 #magnetometer/hmc5883 qshell/qurt - snapdragon_pwm_out spektrum_rc MODULES airspeed_selector diff --git a/boards/atlflight/excelsior/qurt.cmake b/boards/atlflight/excelsior/qurt.cmake index be8ecd5b7b..f3b8b42816 100644 --- a/boards/atlflight/excelsior/qurt.cmake +++ b/boards/atlflight/excelsior/qurt.cmake @@ -49,7 +49,6 @@ px4_add_board( imu/invensense/mpu9250 magnetometer/hmc5883 qshell/qurt - snapdragon_pwm_out spektrum_rc MODULES airspeed_selector diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index 0ae7e48503..de030a0217 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -19,5 +19,6 @@ flight_mode_manager start mc_pos_control start mc_att_control start mc_rate_control start -snapdragon_pwm_out start +# TODO +#linux_pwm_out start spektrum_rc start diff --git a/src/drivers/snapdragon_pwm_out/CMakeLists.txt b/src/drivers/snapdragon_pwm_out/CMakeLists.txt deleted file mode 100644 index 6aed921c7a..0000000000 --- a/src/drivers/snapdragon_pwm_out/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015-2016 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. -# -############################################################################ -px4_add_module( - MODULE drivers__snapdragon_pwm_out - MAIN snapdragon_pwm_out - COMPILE_FLAGS - SRCS - snapdragon_pwm_out.cpp - DEPENDS - mixer - output_limit - ) diff --git a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp deleted file mode 100644 index 883a94d8df..0000000000 --- a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp +++ /dev/null @@ -1,595 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-2016 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. - * - ****************************************************************************/ - -#include - -#include -#include -#include -#include -#include // NAN -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -/* - * PWM Driver for snapdragon. - * @author: Dennis Mannhart - */ -namespace snapdragon_pwm -{ - -static px4_task_t _task_handle = -1; -volatile bool _task_should_exit = false; -static bool _is_running = false; -static const int NUM_PWM = 4; -static char _device[] = "/dev/pwm-1"; - -// snapdragon pins 27,28,29,30 configured for pwm (port J13) -static const int PIN_GPIO = 27; -static struct ::dspal_pwm_ioctl_signal_definition _signal_definition; -static struct ::dspal_pwm _pwm_gpio[NUM_PWM]; -static struct ::dspal_pwm_ioctl_update_buffer *_update_buffer; -static struct ::dspal_pwm *_pwm; - -// TODO: check frequency required -static const int FREQUENCY_PWM_HZ = 400; - -/* TODO: copied from pwm_out_rc_in: check how that works - * filename: /dev/fs is mapped to /usr/share/data/adsp */ -static char _mixer_filename[32] = "/dev/fs/quad_x.main.mix"; - -// subscriptions -int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; -int _armed_sub; -int _fd = -1; - -// publications -orb_advert_t _outputs_pub = nullptr; - -// 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; - -// polling -uint8_t _poll_fds_num = 0; -px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; - -// control group -uint32_t _groups_required = 0; - -// limit for pwm -output_limit_t _pwm_limit; - -// esc parameters -int32_t _pwm_disarmed; -int32_t _pwm_min; -int32_t _pwm_max; - -MultirotorMixer *_mixer = nullptr; - -perf_counter_t _perf_control_latency = nullptr; - -/* - * forward declaration - */ - -static void usage(); - -static void start(); - -static void stop(); - -static int pwm_initialize(const char *device); - -static void pwm_deinitialize(); - -static void send_outputs_pwm(const uint16_t *pwm); - -static void task_main_trampoline(int argc, char *argv[]); - -static void subscribe(); - -static void task_main(int argc, char *argv[]); - -static void update_params(Mixer::Airmode &airmode); - -int initialize_mixer(const char *mixer_filename); - -int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - - - -/* - * functions - */ - -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]; - - return 0; -} - -void update_params(Mixer::Airmode &airmode) -{ - // multicopter air-mode - param_t param_handle = param_find("MC_AIRMODE"); - - if (param_handle != PARAM_INVALID) { - param_get(param_handle, &airmode); - } -} - -int initialize_mixer(const char *mixer_filename) -{ - char buf[2048]; - size_t buflen = sizeof(buf); - PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); - int fd_load = ::open(mixer_filename, O_RDONLY); - - int nRead = ::read(fd_load, buf, buflen); - close(fd_load); - - if (nRead > 0) { - _mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); - - if (_mixer != nullptr) { - PX4_INFO("Successfully initialized mixer from config file"); - return 0; - - } else { - PX4_ERR("Unable to parse from mixer config file"); - return -1; - } - - } else { - PX4_WARN("Unable to read from mixer config file"); - return -2; - } -} - -void subscribe() -{ - memset(_controls, 0, sizeof(_controls)); - memset(_poll_fds, 0, sizeof(_poll_fds)); - - /* - * ORB topic names to msg actuator_controls - */ - _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); - - - /* - * subscibe to orb topic - */ - 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; - - } - - if (_controls_subs[i] >= 0) { - - _poll_fds[_poll_fds_num].fd = _controls_subs[i]; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num++; - - } - - } - - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - -} - - - -int pwm_initialize(const char *device) -{ - /* - * open PWM device - */ - _fd = open(_device, 0); - - if (_fd < 0) { - PX4_ERR("failed to open PWM device!"); - return -1; - } - - /* - * configure PWM - */ - for (int i = 0; i < NUM_PWM; i++) { - _pwm_gpio[i].gpio_id = PIN_GPIO + i; - } - - /* - * description of signal - */ - _signal_definition.num_gpios = NUM_PWM; - _signal_definition.period_in_usecs = 1000000 / FREQUENCY_PWM_HZ; - _signal_definition.pwm_signal = _pwm_gpio; - - - /* - * send signal definition to DSP - */ - if (::ioctl(_fd, PWM_IOCTL_SIGNAL_DEFINITION, &_signal_definition) != 0) { - PX4_ERR("failed to send signal to DSP"); - return -1; - } - - /* - * retrieve shared buffer which will be used to update desired pulse width - */ - if (::ioctl(_fd, PWM_IOCTL_GET_UPDATE_BUFFER, &_update_buffer) != 0) { - PX4_ERR("failed to receive update buffer "); - return -1; - } - - _pwm = _update_buffer->pwm_signal; - - - return 0; -} - -void pwm_deinitialize() -{ - delete _mixer; - close(_fd); - -} - -void send_outputs_pwm(const uint16_t *pwm) -{ - /* - * send pwm in us: TODO: check if it is in us - */ - for (unsigned i = 0; i < NUM_PWM; ++i) { - _pwm[i].pulse_width_in_usecs = pwm[i]; - } -} - -void task_main(int argc, char *argv[]) -{ - if (pwm_initialize(_device) < 0) { - PX4_ERR("Failed to initialize PWM."); - return; - } - - if (initialize_mixer(_mixer_filename) < 0) { - PX4_ERR("Mixer initialization failed."); - return; - } - - _mixer->groups_required(_groups_required); - - // subscribe and set up polling - subscribe(); - - Mixer::Airmode airmode = Mixer::Airmode::disabled; - update_params(airmode); - uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - // Start disarmed - _armed.armed = false; - _armed.prearmed = false; - - // set max min pwm - output_limit_init(&_pwm_limit); - - _perf_control_latency = perf_alloc(PC_ELAPSED, "snapdragon_pwm_out control latency"); - - _is_running = true; - - // Main loop - while (!_task_should_exit) { - - if (_mixer) { - _mixer->set_airmode(airmode); - } - - /* wait up to 10ms for data */ - int pret = px4_poll(_poll_fds, _poll_fds_num, 10); - - /* Timed out, do a periodic check for _task_should_exit. */ - bool timeout = false; - - if (pret == 0) { - timeout = true; - } - - /* 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; - } - - bool updated; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } - - /* get controls for required topics */ - unsigned poll_id = 0; - - 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]); - } - - poll_id++; - } - } - - if (_mixer == nullptr) { - PX4_ERR("Could not mix output! Exiting..."); - _task_should_exit = true; - continue; - } - - /* do mixing for virtual control group */ - _outputs.noutputs = _mixer->mix(_outputs.output, _outputs.NUM_ACTUATOR_OUTPUTS); - - //set max, min and disarmed pwm - const uint16_t reverse_mask = 0; - uint16_t disarmed_pwm[4]; - uint16_t min_pwm[4]; - uint16_t max_pwm[4]; - uint16_t pwm[4]; - - for (unsigned int i = 0; i < 4; i++) { - disarmed_pwm[i] = _pwm_disarmed; - min_pwm[i] = _pwm_min; - max_pwm[i] = _pwm_max; - } - - - // TODO FIXME: pre-armed seems broken -> copied and pasted from pwm_out_rc_in: needs to be tested - output_limit_calc(_armed.armed, - false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm, - min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit); - - // send and publish outputs - if (_armed.lockdown || _armed.manual_lockdown || timeout) { - send_outputs_pwm(disarmed_pwm); - - } else { - send_outputs_pwm(pwm); - } - - _outputs.timestamp = hrt_absolute_time(); - - if (_outputs_pub != nullptr) { - orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); - - } 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 (required && (timestamp_sample > 0)) { - perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample); - break; - } - } - - // 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); - } - } - - pwm_deinitialize(); - - for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_controls_subs[i] >= 0) { - orb_unsubscribe(_controls_subs[i]); - } - } - - orb_unsubscribe(_armed_sub); - - perf_free(_perf_control_latency); - - _is_running = false; -} - -void task_main_trampoline(int argc, char *argv[]) -{ - task_main(argc, argv); -} - -void start() -{ - _task_should_exit = false; - - /* start the task */ - _task_handle = px4_task_spawn_cmd("snapdragon_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; - } - -} - -void stop() -{ - _task_should_exit = true; - - while (_is_running) { - usleep(200000); - PX4_INFO("."); - } - - _task_handle = -1; -} - -void usage() -{ - - PX4_INFO("usage: snapdragon_pwm_out start [-d pwmdevice] [-m mixerfile]"); - PX4_INFO(" -d pwmdevice : device for pwm generation"); - PX4_INFO(" (default /dev/pwm-1)"); - PX4_INFO(" -m mixerfile : path to mixerfile"); - PX4_INFO(" (default /dev/fs/quad_x.main.mix"); - PX4_INFO(" pwm_out stop"); - PX4_INFO(" pwm_out status"); - -} - -} // namespace snapdragon_pwm - -/* driver 'main' command */ -extern "C" __EXPORT int snapdragon_pwm_out_main(int argc, char *argv[]); - -int snapdragon_pwm_out_main(int argc, char *argv[]) -{ - - - 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, "d:m", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'd': - strncpy(snapdragon_pwm::_device, myoptarg, sizeof(snapdragon_pwm::_device)); - break; - - case 'm': - strncpy(snapdragon_pwm::_mixer_filename, myoptarg, sizeof(snapdragon_pwm::_mixer_filename)); - break; - } - } - - // gets the parameters for the esc's pwm - param_get(param_find("PWM_MAIN_DISARM"), &snapdragon_pwm::_pwm_disarmed); - param_get(param_find("PWM_MAIN_MIN"), &snapdragon_pwm::_pwm_min); - param_get(param_find("PWM_MAIN_MAX"), &snapdragon_pwm::_pwm_max); - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - if (snapdragon_pwm::_is_running) { - PX4_WARN("pwm_out already running"); - return 1; - } - - snapdragon_pwm::start(); - } - - else if (!strcmp(verb, "stop")) { - if (!snapdragon_pwm::_is_running) { - PX4_WARN("pwm_out is not running"); - return 1; - } - - snapdragon_pwm::stop(); - } - - else if (!strcmp(verb, "status")) { - PX4_INFO("pwm_out is %s", snapdragon_pwm::_is_running ? "running" : "not running"); - return 0; - - } else { - snapdragon_pwm::usage(); - return 1; - } - - return 0; -}