diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a136dc4a888e..1b5f1e102e29 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -504,7 +504,7 @@ then if [ $OUTPUT_MODE == hil ] then - if pwm_out_sim mode_pwm16 + if pwm_out_sim start then else tune_control play -m ${TUNE_ERR} diff --git a/platforms/qurt/src/px4_layer/commands_hil.c b/platforms/qurt/src/px4_layer/commands_hil.c index 30c4707f776a..a2f6629a51ae 100644 --- a/platforms/qurt/src/px4_layer/commands_hil.c +++ b/platforms/qurt/src/px4_layer/commands_hil.c @@ -56,7 +56,7 @@ const char *get_commands() "mc_pos_control start\n" "mc_att_control start\n" "sleep 1\n" - "pwm_out_sim mode_pwm\n" + "pwm_out_sim start\n" "param set RC1_MAX 2015\n" "param set RC1_MIN 996\n" "param set RC1_TRIM 1502\n" diff --git a/posix-configs/SITL/init/ekf2/hippocampus b/posix-configs/SITL/init/ekf2/hippocampus index aa2b8ac78956..ebb279197ae7 100644 --- a/posix-configs/SITL/init/ekf2/hippocampus +++ b/posix-configs/SITL/init/ekf2/hippocampus @@ -13,7 +13,7 @@ dataman start simulator start -s -pwm_out_sim mode_pwm +pwm_out_sim start mixer load /dev/pwm_output0 ROMFS/px4fmu_test/mixers/uuv_quad_x.mix gyrosim start diff --git a/posix-configs/SITL/init/ekf2/iris b/posix-configs/SITL/init/ekf2/iris index 4a46567a5c28..c1536f10b9f5 100644 --- a/posix-configs/SITL/init/ekf2/iris +++ b/posix-configs/SITL/init/ekf2/iris @@ -53,7 +53,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/ekf2/iris_1 b/posix-configs/SITL/init/ekf2/iris_1 index 2b512462ea98..6079d6fbc16f 100644 --- a/posix-configs/SITL/init/ekf2/iris_1 +++ b/posix-configs/SITL/init/ekf2/iris_1 @@ -55,7 +55,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/ekf2/iris_2 b/posix-configs/SITL/init/ekf2/iris_2 index 454e7e4309bf..73f56db89a31 100644 --- a/posix-configs/SITL/init/ekf2/iris_2 +++ b/posix-configs/SITL/init/ekf2/iris_2 @@ -55,7 +55,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/ekf2/iris_irlock b/posix-configs/SITL/init/ekf2/iris_irlock index a7edb5584df6..1bed0e6d8bed 100644 --- a/posix-configs/SITL/init/ekf2/iris_irlock +++ b/posix-configs/SITL/init/ekf2/iris_irlock @@ -54,7 +54,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/ekf2/iris_opt_flow b/posix-configs/SITL/init/ekf2/iris_opt_flow index cb2c15ece336..6ef121cccc26 100644 --- a/posix-configs/SITL/init/ekf2/iris_opt_flow +++ b/posix-configs/SITL/init/ekf2/iris_opt_flow @@ -56,7 +56,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/ekf2/iris_rplidar b/posix-configs/SITL/init/ekf2/iris_rplidar index 4a46567a5c28..c1536f10b9f5 100644 --- a/posix-configs/SITL/init/ekf2/iris_rplidar +++ b/posix-configs/SITL/init/ekf2/iris_rplidar @@ -53,7 +53,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/ekf2/multiple_iris b/posix-configs/SITL/init/ekf2/multiple_iris index b60f823b3665..dca2c392f7b0 100644 --- a/posix-configs/SITL/init/ekf2/multiple_iris +++ b/posix-configs/SITL/init/ekf2/multiple_iris @@ -53,7 +53,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/ekf2/plane b/posix-configs/SITL/init/ekf2/plane index bb97c6d0f961..de15cdfa9134 100644 --- a/posix-configs/SITL/init/ekf2/plane +++ b/posix-configs/SITL/init/ekf2/plane @@ -61,7 +61,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start navigator start diff --git a/posix-configs/SITL/init/ekf2/rover b/posix-configs/SITL/init/ekf2/rover index 9bde3abedbe6..2ed44cef1bfb 100644 --- a/posix-configs/SITL/init/ekf2/rover +++ b/posix-configs/SITL/init/ekf2/rover @@ -57,7 +57,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start sleep 1 sensors start commander start diff --git a/posix-configs/SITL/init/ekf2/solo b/posix-configs/SITL/init/ekf2/solo index 546753077c25..96aaa3063036 100644 --- a/posix-configs/SITL/init/ekf2/solo +++ b/posix-configs/SITL/init/ekf2/solo @@ -50,7 +50,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol index 8241d4fc8899..fdb208b87973 100644 --- a/posix-configs/SITL/init/ekf2/standard_vtol +++ b/posix-configs/SITL/init/ekf2/standard_vtol @@ -68,7 +68,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start start sensors start commander start land_detector start vtol diff --git a/posix-configs/SITL/init/ekf2/tailsitter b/posix-configs/SITL/init/ekf2/tailsitter index 42554edc2133..1202328e90bf 100644 --- a/posix-configs/SITL/init/ekf2/tailsitter +++ b/posix-configs/SITL/init/ekf2/tailsitter @@ -49,7 +49,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start vtol diff --git a/posix-configs/SITL/init/ekf2/tiltrotor b/posix-configs/SITL/init/ekf2/tiltrotor index 2fcce30b83b8..21d40b253d5c 100644 --- a/posix-configs/SITL/init/ekf2/tiltrotor +++ b/posix-configs/SITL/init/ekf2/tiltrotor @@ -70,7 +70,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start vtol diff --git a/posix-configs/SITL/init/ekf2/typhoon_h480 b/posix-configs/SITL/init/ekf2/typhoon_h480 index 8057dac7c969..d9750fa8d70f 100644 --- a/posix-configs/SITL/init/ekf2/typhoon_h480 +++ b/posix-configs/SITL/init/ekf2/typhoon_h480 @@ -56,7 +56,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm16 +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/inav/iris b/posix-configs/SITL/init/inav/iris index 57e6713887cb..6fdebe6c4df3 100644 --- a/posix-configs/SITL/init/inav/iris +++ b/posix-configs/SITL/init/inav/iris @@ -51,7 +51,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/inav/iris_opt_flow b/posix-configs/SITL/init/inav/iris_opt_flow index 84c9adb3d48c..78283a2e9bb6 100644 --- a/posix-configs/SITL/init/inav/iris_opt_flow +++ b/posix-configs/SITL/init/inav/iris_opt_flow @@ -53,7 +53,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/lpe/hippocampus b/posix-configs/SITL/init/lpe/hippocampus index 99281b32cc0b..226c11326214 100644 --- a/posix-configs/SITL/init/lpe/hippocampus +++ b/posix-configs/SITL/init/lpe/hippocampus @@ -13,7 +13,7 @@ dataman start simulator start -s -pwm_out_sim mode_pwm +pwm_out_sim start mixer load /dev/pwm_output0 ROMFS/px4fmu_test/mixers/uuv_quad_x.mix gyrosim start diff --git a/posix-configs/SITL/init/lpe/iris b/posix-configs/SITL/init/lpe/iris index 1904cc119f34..b2d270d7a60a 100644 --- a/posix-configs/SITL/init/lpe/iris +++ b/posix-configs/SITL/init/lpe/iris @@ -53,7 +53,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/lpe/iris_1 b/posix-configs/SITL/init/lpe/iris_1 index 003c8dfbfa67..62420a38676e 100644 --- a/posix-configs/SITL/init/lpe/iris_1 +++ b/posix-configs/SITL/init/lpe/iris_1 @@ -55,7 +55,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/lpe/iris_2 b/posix-configs/SITL/init/lpe/iris_2 index 1e1d43fc04a8..9c7516a2fc1a 100644 --- a/posix-configs/SITL/init/lpe/iris_2 +++ b/posix-configs/SITL/init/lpe/iris_2 @@ -55,7 +55,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/lpe/iris_irlock b/posix-configs/SITL/init/lpe/iris_irlock index c038a044ae8a..b10a556f3bc6 100644 --- a/posix-configs/SITL/init/lpe/iris_irlock +++ b/posix-configs/SITL/init/lpe/iris_irlock @@ -61,7 +61,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/lpe/iris_opt_flow b/posix-configs/SITL/init/lpe/iris_opt_flow index fea6ce94c56d..8986c0d39494 100644 --- a/posix-configs/SITL/init/lpe/iris_opt_flow +++ b/posix-configs/SITL/init/lpe/iris_opt_flow @@ -71,7 +71,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/lpe/iris_rplidar b/posix-configs/SITL/init/lpe/iris_rplidar index d8cf251b2b16..e0d014944f7a 100644 --- a/posix-configs/SITL/init/lpe/iris_rplidar +++ b/posix-configs/SITL/init/lpe/iris_rplidar @@ -52,7 +52,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sleep 1 sensors start commander start diff --git a/posix-configs/SITL/init/lpe/plane b/posix-configs/SITL/init/lpe/plane index 5768cf55ec46..7ad438a7c036 100644 --- a/posix-configs/SITL/init/lpe/plane +++ b/posix-configs/SITL/init/lpe/plane @@ -59,7 +59,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start navigator start diff --git a/posix-configs/SITL/init/lpe/rover b/posix-configs/SITL/init/lpe/rover index 5bafa8bc9f84..2fce364e7de1 100644 --- a/posix-configs/SITL/init/lpe/rover +++ b/posix-configs/SITL/init/lpe/rover @@ -53,7 +53,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start sleep 1 sensors start commander start diff --git a/posix-configs/SITL/init/lpe/solo b/posix-configs/SITL/init/lpe/solo index 73028f645b6a..25f8f8ae97fc 100644 --- a/posix-configs/SITL/init/lpe/solo +++ b/posix-configs/SITL/init/lpe/solo @@ -53,7 +53,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/lpe/standard_vtol b/posix-configs/SITL/init/lpe/standard_vtol index 252cd8535d2e..84be2365a153 100644 --- a/posix-configs/SITL/init/lpe/standard_vtol +++ b/posix-configs/SITL/init/lpe/standard_vtol @@ -63,7 +63,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start land_detector start vtol diff --git a/posix-configs/SITL/init/lpe/typhoon_h480 b/posix-configs/SITL/init/lpe/typhoon_h480 index 1ea5ebaaa8de..150a5459b02d 100644 --- a/posix-configs/SITL/init/lpe/typhoon_h480 +++ b/posix-configs/SITL/init/lpe/typhoon_h480 @@ -50,7 +50,7 @@ accelsim start barosim start adcsim start gpssim start -pwm_out_sim mode_pwm16 +pwm_out_sim start sensors start commander start land_detector start multicopter diff --git a/posix-configs/SITL/init/rcS_gazebo_delta_wing b/posix-configs/SITL/init/rcS_gazebo_delta_wing index e72a2f322f30..43b82509cab3 100644 --- a/posix-configs/SITL/init/rcS_gazebo_delta_wing +++ b/posix-configs/SITL/init/rcS_gazebo_delta_wing @@ -35,7 +35,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start sensors start commander start navigator start diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index dc2d46a702ab..13245ff56569 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -13,7 +13,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start ver all diff --git a/posix-configs/SITL/init/test/tests_template.in b/posix-configs/SITL/init/test/tests_template.in index 51e0bdc75bba..b6fff53ca640 100644 --- a/posix-configs/SITL/init/test/tests_template.in +++ b/posix-configs/SITL/init/test/tests_template.in @@ -13,7 +13,7 @@ barosim start adcsim start gpssim start measairspeedsim start -pwm_out_sim mode_pwm +pwm_out_sim start ver all diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config index f545e140a8e3..2f1977396fa5 100644 --- a/posix-configs/eagle/hil/px4.config +++ b/posix-configs/eagle/hil/px4.config @@ -47,7 +47,7 @@ mc_pos_control start mc_att_control start land_detector start multicopter sleep 1 -pwm_out_sim mode_pwm +pwm_out_sim start mixer load /dev/pwm_output0 /startup/quad_x.main.mix list_devices list_files diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index 28053fe2fe5e..b20f1ec1bacd 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -18,7 +18,7 @@ mc_pos_control start mc_att_control start mavlink start -x -u 14577 -r 1000000 navio_sysfs_rc_in start -pwm_out_sim mode_pwm16 +pwm_out_sim start mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix logger start -t -b 200 mavlink boot_complete diff --git a/src/drivers/pwm_out_sim/CMakeLists.txt b/src/drivers/pwm_out_sim/CMakeLists.txt index dafb6a27ca99..8c29e4a7d3c1 100644 --- a/src/drivers/pwm_out_sim/CMakeLists.txt +++ b/src/drivers/pwm_out_sim/CMakeLists.txt @@ -33,12 +33,6 @@ px4_add_module( MODULE drivers__pwm_out_sim MAIN pwm_out_sim - STACK_MAIN 1200 - STACK_MAX 1200 - COMPILE_FLAGS SRCS - pwm_out_sim.cpp - DEPENDS - platforms__common + PWMSim.cpp ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp new file mode 100644 index 000000000000..62325be2433d --- /dev/null +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -0,0 +1,666 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2018 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 "PWMSim.hpp" + +PWMSim::PWMSim() : + CDev("pwm_out_sim", PWM_OUTPUT0_DEVICE_PATH) +{ + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _pwm_min[i] = PWM_SIM_PWM_MIN_MAGIC; + _pwm_max[i] = PWM_SIM_PWM_MAX_MAGIC; + } + + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + _control_subs[i] = -1; + } + + CDev::init(); + + // default to MODE_16PWM + set_mode(MODE_16PWM); +} + +int +PWMSim::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_8PWM: + PX4_INFO("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 400; /* default output rate */ + _num_outputs = 8; + break; + + case MODE_16PWM: + PX4_INFO("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 400; /* default output rate */ + _num_outputs = 16; + break; + + case MODE_NONE: + PX4_INFO("MODE_NONE"); + /* disable servo outputs and set a very low update rate */ + _update_rate = 10; + _num_outputs = 0; + break; + + default: + return -EINVAL; + } + + _mode = mode; + return OK; +} + +int +PWMSim::set_pwm_rate(unsigned rate) +{ + if ((rate > 500) || (rate < 10)) { + return -EINVAL; + } + + _update_rate = rate; + return OK; +} + +void +PWMSim::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + PX4_DEBUG("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + + if (unsub_groups & (1 << i)) { + PX4_DEBUG("unsubscribe from actuator_controls_%d", i); + orb_unsubscribe(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] >= 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +void +PWMSim::run() +{ + /* force a reset of the update rate */ + _current_update_rate = 0; + + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* advertise the mixed control outputs, insist on the first group output */ + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_actuator_outputs); + + /* loop until killed */ + while (!should_exit()) { + + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + } + + /* handle update rate changes */ + if (_current_update_rate != _update_rate) { + int update_rate_in_ms = int(1000 / _update_rate); + + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + } + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] >= 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } + + // up_pwm_servo_set_rate(_update_rate); + _current_update_rate = _update_rate; + } + + /* this can happen during boot, but after the sleep its likely resolved */ + if (_poll_fds_num == 0) { + usleep(1000 * 1000); + + PX4_DEBUG("no valid fds"); + continue; + } + + /* sleep waiting for data, but no more than a second */ + int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); + + /* this would be bad... */ + if (ret < 0) { + PX4_ERR("poll error %d", errno); + continue; + } + + if (ret == 0) { + // timeout + continue; + } + + /* get controls for required topics */ + unsigned poll_id = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] >= 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + } + + poll_id++; + } + } + + /* can we mix? */ + if (_armed && _mixers != nullptr) { + + /* do mixing */ + unsigned num_outputs = _mixers->mix(&_actuator_outputs.output[0], _num_outputs); + _actuator_outputs.noutputs = num_outputs; + _actuator_outputs.timestamp = hrt_absolute_time(); + + /* disable unused ports by setting their output to NaN */ + for (size_t i = 0; i < sizeof(_actuator_outputs.output) / sizeof(_actuator_outputs.output[0]); i++) { + if (i >= num_outputs) { + _actuator_outputs.output[i] = NAN; + } + } + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < _actuator_outputs.noutputs && + PX4_ISFINITE(_actuator_outputs.output[i]) && + _actuator_outputs.output[i] >= -1.0f && + _actuator_outputs.output[i] <= 1.0f) { + /* scale for PWM output 1000 - 2000us */ + _actuator_outputs.output[i] = 1500 + (500 * _actuator_outputs.output[i]); + + if (_actuator_outputs.output[i] > _pwm_max[i]) { + _actuator_outputs.output[i] = _pwm_max[i]; + } + + if (_actuator_outputs.output[i] < _pwm_min[i]) { + _actuator_outputs.output[i] = _pwm_min[i]; + } + + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + _actuator_outputs.output[i] = PWM_SIM_DISARMED_MAGIC; + } + } + + /* overwrite outputs in case of force_failsafe */ + if (_failsafe) { + for (size_t i = 0; i < num_outputs; i++) { + _actuator_outputs.output[i] = PWM_SIM_FAILSAFE_MAGIC; + } + } + + /* overwrite outputs in case of lockdown */ + if (_lockdown) { + for (size_t i = 0; i < num_outputs; i++) { + _actuator_outputs.output[i] = 0.0; + } + } + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_actuator_outputs); + } + + /* how about an arming update? */ + bool updated; + + orb_check(_armed_sub, &updated); + + if (updated) { + actuator_armed_s aa = {}; + + if (orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa) == PX4_OK) { + /* do not obey the lockdown value, as lockdown is for PWMSim. Only obey manual lockdown */ + _armed = aa.armed; + _failsafe = aa.force_failsafe; + _lockdown = aa.manual_lockdown; + } + } + } + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] >= 0) { + orb_unsubscribe(_control_subs[i]); + } + } + + orb_unsubscribe(_armed_sub); +} + +int +PWMSim::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; +} + +int +PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + // up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + // up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < pwm->channel_count; i++) { + + if (i <= MAX_ACTUATORS) { + _pwm_min[i] = pwm->values[i]; + } + } + + break; + } + + case PWM_SERVO_SET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < pwm->channel_count; i++) { + + if (i <= MAX_ACTUATORS) { + _pwm_max[i] = pwm->values[i]; + } + } + + break; + } + + case PWM_SERVO_SET_UPDATE_RATE: + // PWMSim always outputs at the alternate (usually faster) rate + set_pwm_rate(arg); + break; + + case PWM_SERVO_SET_SELECT_UPDATE_RATE: + // PWMSim always outputs at the alternate (usually faster) rate + break; + + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + *(uint32_t *)arg = 400; + break; + + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = _current_update_rate; + break; + + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + *(uint32_t *)arg = 0; + break; + + case PWM_SERVO_GET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = PWM_SIM_FAILSAFE_MAGIC; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = PWM_SIM_DISARMED_MAGIC; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = PWM_SIM_PWM_MIN_MAGIC; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_TRIM_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = PWM_SIM_PWM_MAX_MAGIC; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { + + unsigned channel = cmd - PWM_SERVO_GET(0); + + if (channel >= _num_outputs) { + ret = -EINVAL; + + } else { + /* fetch a current PWM value */ + *(servo_position_t *)arg = _actuator_outputs.output[channel]; + } + + break; + } + + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + // no restrictions on output grouping + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + + *(uint32_t *)arg = (1 << channel); + break; + } + + case PWM_SERVO_GET_COUNT: + case MIXERIOCGETOUTPUTCOUNT: + if (_mode == MODE_16PWM) { + *(unsigned *)arg = 16; + + } else if (_mode == MODE_8PWM) { + + *(unsigned *)arg = 8; + } + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + _groups_required = 0; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) { + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + } + + _mixers->add_mixer(mixer); + _mixers->groups_required(_groups_required); + } + + break; + } + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) { + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + } + + if (_mixers == nullptr) { + _groups_required = 0; + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + PX4_ERR("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + ret = -EINVAL; + + } else { + _mixers->groups_required(_groups_required); + } + } + + break; + } + + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +int +PWMSim::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("pwmsim", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 1310, + (px4_main_t)&run_trampoline, + nullptr); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + // wait until task is up & running (the mode_* commands depend on it) + if (wait_until_running() < 0) { + _task_id = -1; + return -1; + } + + return PX4_OK; +} + +PWMSim *PWMSim::instantiate(int argc, char *argv[]) +{ + return new PWMSim(); +} + +int PWMSim::custom_command(int argc, char *argv[]) +{ + const char *verb = argv[0]; + + /* start the FMU if not running */ + if (!is_running()) { + int ret = PWMSim::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + Mode servo_mode = MODE_NONE; + + if (!strcmp(verb, "mode_pwm")) { + servo_mode = PWMSim::MODE_8PWM; + + } else if (!strcmp(verb, "mode_pwm16")) { + servo_mode = PWMSim::MODE_16PWM; + } + + /* was a new mode set? */ + if (servo_mode != MODE_NONE) { + /* switch modes */ + PWMSim *object = get_instance(); + + if (servo_mode != object->get_mode()) { + /* (re)set the PWM output mode */ + return object->set_mode(servo_mode); + } + } + + return print_usage("unknown command"); +} + +int PWMSim::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Driver/configurator for the virtual PWMSim port. + +This virtual driver emulates PWM / servo outputs for setups where +the connected hardware does not provide enough or no PWM outputs. + +Its only function is to take actuator_control uORB messages, +mix them with any loaded mixer and output the result to the +actuator_output uORB topic. PWMSim can also be performed with normal +PWM outputs, a special flag prevents the outputs to be operated +during PWMSim mode. If PWMSim is not performed with a standalone FMU, +but in a real system, it is NOT recommended to use this virtual +driver. Use instead the normal FMU or IO driver. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("pwmsim", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)"); + + PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the pwm sim if not running already"); + + PRINT_MODULE_USAGE_COMMAND("mode_pwm"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm16"); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int PWMSim::print_status() +{ + PX4_INFO("Running max update rate: %i Hz", _current_update_rate); + PX4_INFO("Polling %i actuator controls", _poll_fds_num); + + const char *mode_str = nullptr; + + switch (_mode) { + case MODE_8PWM: mode_str = "pwm8"; break; + case MODE_16PWM: mode_str = "pwm16"; break; + + case MODE_NONE: mode_str = "no pwm"; break; + + default: + break; + } + + if (mode_str) { + PX4_INFO("PWM Mode: %s", mode_str); + } + + return 0; +} + +extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]); + +int +pwm_out_sim_main(int argc, char *argv[]) +{ + return PWMSim::main(argc, argv); +} diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp new file mode 100644 index 000000000000..5331ac18e596 --- /dev/null +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2018 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. + * + ****************************************************************************/ + +#ifndef DRIVERS_PWM_OUT_SIM_PWMSIM_HPP_ +#define DRIVERS_PWM_OUT_SIM_PWMSIM_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class PWMSim : public device::CDev, public ModuleBase +{ + static constexpr uint32_t PWM_SIM_DISARMED_MAGIC = 900; + static constexpr uint32_t PWM_SIM_FAILSAFE_MAGIC = 600; + static constexpr uint32_t PWM_SIM_PWM_MIN_MAGIC = 1000; + static constexpr uint32_t PWM_SIM_PWM_MAX_MAGIC = 2000; + +public: + + enum Mode { + MODE_8PWM, + MODE_16PWM, + MODE_NONE + }; + + PWMSim(); + virtual ~PWMSim() = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static PWMSim *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + + + int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; + + int set_pwm_rate(unsigned rate); + + int set_mode(Mode mode); + Mode get_mode() { return _mode; } + +private: + static constexpr unsigned MAX_ACTUATORS = 16; + + Mode _mode{MODE_NONE}; + + int _update_rate{400}; + int _current_update_rate{0}; + + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; + + px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; + unsigned _poll_fds_num{0}; + + int _armed_sub{-1}; + + actuator_outputs_s _actuator_outputs = {}; + orb_advert_t _outputs_pub{nullptr}; + + unsigned _num_outputs{0}; + + unsigned _pwm_min[MAX_ACTUATORS] {}; + unsigned _pwm_max[MAX_ACTUATORS] {}; + + uint32_t _groups_required{0}; + uint32_t _groups_subscribed{0}; + + bool _armed{false}; + bool _lockdown{false}; + bool _failsafe{false}; + + MixerGroup *_mixers{nullptr}; + + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; + + static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); + + void subscribe(); + +}; + +#endif /* DRIVERS_PWM_OUT_SIM_PWMSIM_HPP_ */ diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp deleted file mode 100644 index 7091ccca55e1..000000000000 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ /dev/null @@ -1,1097 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2017 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. - * - ****************************************************************************/ - -/** - * @file hil.cpp - * - * Driver/configurator for the virtual PWMSim port. - * - * This virtual driver emulates PWM / servo outputs for setups where - * the connected hardware does not provide enough or no PWM outputs. - * - * Its only function is to take actuator_control uORB messages, - * mix them with any loaded mixer and output the result to the - * actuator_output uORB topic. PWMSim can also be performed with normal - * PWM outputs, a special flag prevents the outputs to be operated - * during PWMSim mode. If PWMSim is not performed with a standalone FMU, - * but in a real system, it is NOT recommended to use this virtual - * driver. Use instead the normal FMU or IO driver. - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include - -class PWMSim : public device::CDev -{ - const uint32_t PWM_SIM_DISARMED_MAGIC = 900; - const uint32_t PWM_SIM_FAILSAFE_MAGIC = 600; - const uint32_t PWM_SIM_PWM_MIN_MAGIC = 1000; - const uint32_t PWM_SIM_PWM_MAX_MAGIC = 2000; -public: - enum Mode { - MODE_2PWM, - MODE_4PWM, - MODE_6PWM, - MODE_8PWM, - MODE_12PWM, - MODE_16PWM, - MODE_NONE - }; - PWMSim(); - virtual ~PWMSim(); - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - - virtual int init(); - - int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); - int _task; - -private: - static const unsigned _max_actuators = 16; - - Mode _mode; - int _update_rate; - int _current_update_rate; - int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; - px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; - unsigned _poll_fds_num; - int _armed_sub; - orb_advert_t _outputs_pub; - unsigned _num_outputs; - bool _primary_pwm_device; - unsigned _pwm_min[_max_actuators]; - unsigned _pwm_max[_max_actuators]; - - uint32_t _groups_required; - uint32_t _groups_subscribed; - - volatile bool _task_should_exit; - static bool _armed; - static bool _lockdown; - static bool _failsafe; - - MixerGroup *_mixers; - - actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; - - static void task_main_trampoline(int argc, char *argv[]); - void task_main(); - - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); - - int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); - void subscribe(); - - struct GPIOConfig { - uint32_t input; - uint32_t output; - uint32_t alt; - }; - - static const GPIOConfig _gpio_tab[]; - static const unsigned _ngpio; - - void gpio_reset(); - void gpio_set_function(uint32_t gpios, int function); - void gpio_write(uint32_t gpios, int function); - uint32_t gpio_read(); - int gpio_ioctl(device::file_t *filp, int cmd, unsigned long arg); - -}; - -namespace -{ - -PWMSim *g_pwm_sim = nullptr; - -} // namespace - -bool PWMSim::_armed = false; -bool PWMSim::_lockdown = false; -bool PWMSim::_failsafe = false; - -PWMSim::PWMSim() : - CDev("pwm_out_sim", PWM_OUTPUT0_DEVICE_PATH), - _task(-1), - _mode(MODE_NONE), - _update_rate(50), - _current_update_rate(0), - _poll_fds{}, - _poll_fds_num(0), - _armed_sub(-1), - _outputs_pub(nullptr), - _num_outputs(0), - _primary_pwm_device(false), - _groups_required(0), - _groups_subscribed(0), - _task_should_exit(false), - _mixers(nullptr) -{ - memset(_controls, 0, sizeof(_controls)); - - for (unsigned i = 0; i < _max_actuators; i++) { - _pwm_min[i] = PWM_SIM_PWM_MIN_MAGIC; - _pwm_max[i] = PWM_SIM_PWM_MAX_MAGIC; - } - - _control_topics[0] = ORB_ID(actuator_controls_0); - _control_topics[1] = ORB_ID(actuator_controls_1); - _control_topics[2] = ORB_ID(actuator_controls_2); - _control_topics[3] = ORB_ID(actuator_controls_3); - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - _control_subs[i] = -1; - } -} - -PWMSim::~PWMSim() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - unsigned i = 10; - - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - px4_task_delete(_task); - break; - } - - } while (_task != -1); - } - - g_pwm_sim = nullptr; -} - -int -PWMSim::init() -{ - int ret; - - ASSERT(_task == -1); - - /* do regular cdev init */ - ret = CDev::init(); - - if (ret != OK) { - return ret; - - } else { - _primary_pwm_device = true; - } - - /* start the PWMSim interface task */ - _task = px4_task_spawn_cmd("pwm_out_sim", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1300, - (px4_main_t)&PWMSim::task_main_trampoline, - nullptr); - - if (_task < 0) { - PX4_INFO("task start failed: %d", errno); - return -errno; - } - - return OK; -} - -void -PWMSim::task_main_trampoline(int argc, char *argv[]) -{ - g_pwm_sim->task_main(); -} - -int -PWMSim::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: - PX4_INFO("MODE_2PWM"); - /* multi-port with flow control lines as PWM */ - _update_rate = 50; /* default output rate */ - _num_outputs = 2; - break; - - case MODE_4PWM: - PX4_INFO("MODE_4PWM"); - /* multi-port as 4 PWM outs */ - _update_rate = 50; /* default output rate */ - _num_outputs = 4; - break; - - case MODE_8PWM: - PX4_INFO("MODE_8PWM"); - /* multi-port as 8 PWM outs */ - _update_rate = 50; /* default output rate */ - _num_outputs = 8; - break; - - case MODE_12PWM: - PX4_INFO("MODE_12PWM"); - /* multi-port as 12 PWM outs */ - _update_rate = 50; /* default output rate */ - _num_outputs = 12; - break; - - case MODE_16PWM: - PX4_INFO("MODE_16PWM"); - /* multi-port as 16 PWM outs */ - _update_rate = 50; /* default output rate */ - _num_outputs = 16; - break; - - case MODE_NONE: - PX4_INFO("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ - _update_rate = 10; - _num_outputs = 0; - break; - - default: - return -EINVAL; - } - - _mode = mode; - return OK; -} - -int -PWMSim::set_pwm_rate(unsigned rate) -{ - if ((rate > 500) || (rate < 10)) { - return -EINVAL; - } - - _update_rate = rate; - return OK; -} - -void -PWMSim::subscribe() -{ - /* subscribe/unsubscribe to required actuator control groups */ - uint32_t sub_groups = _groups_required & ~_groups_subscribed; - uint32_t unsub_groups = _groups_subscribed & ~_groups_required; - _poll_fds_num = 0; - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (sub_groups & (1 << i)) { - PX4_DEBUG("subscribe to actuator_controls_%d", i); - _control_subs[i] = orb_subscribe(_control_topics[i]); - } - - if (unsub_groups & (1 << i)) { - PX4_DEBUG("unsubscribe from actuator_controls_%d", i); - orb_unsubscribe(_control_subs[i]); - _control_subs[i] = -1; - } - - if (_control_subs[i] >= 0) { - _poll_fds[_poll_fds_num].fd = _control_subs[i]; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num++; - } - } -} - -void -PWMSim::task_main() -{ - /* force a reset of the update rate */ - _current_update_rate = 0; - - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - - /* advertise the mixed control outputs */ - actuator_outputs_s outputs = {}; - - /* advertise the mixed control outputs, insist on the first group output */ - _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); - - - /* loop until killed */ - while (!_task_should_exit) { - - if (_groups_subscribed != _groups_required) { - subscribe(); - _groups_subscribed = _groups_required; - } - - /* handle update rate changes */ - if (_current_update_rate != _update_rate) { - int update_rate_in_ms = int(1000 / _update_rate); - - if (update_rate_in_ms < 2) { - update_rate_in_ms = 2; - } - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - orb_set_interval(_control_subs[i], update_rate_in_ms); - } - } - - // up_pwm_servo_set_rate(_update_rate); - _current_update_rate = _update_rate; - } - - /* this can happen during boot, but after the sleep its likely resolved */ - if (_poll_fds_num == 0) { - usleep(1000 * 1000); - - PX4_DEBUG("no valid fds"); - continue; - } - - /* sleep waiting for data, but no more than a second */ - int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); - - /* this would be bad... */ - if (ret < 0) { - DEVICE_LOG("poll error %d", errno); - continue; - } - - if (ret == 0) { - // timeout - continue; - } - - /* get controls for required topics */ - unsigned poll_id = 0; - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - if (_poll_fds[poll_id].revents & POLLIN) { - orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); - } - - poll_id++; - } - } - - /* can we mix? */ - if (_armed && _mixers != nullptr) { - - size_t num_outputs; - - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_6PWM: - num_outputs = 6; - break; - - case MODE_8PWM: - num_outputs = 8; - break; - - case MODE_12PWM: - num_outputs = 12; - break; - - case MODE_16PWM: - num_outputs = 16; - break; - - default: - num_outputs = 0; - break; - } - - /* do mixing */ - num_outputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.noutputs = num_outputs; - outputs.timestamp = hrt_absolute_time(); - - /* disable unused ports by setting their output to NaN */ - for (size_t i = 0; i < sizeof(outputs.output) / sizeof(outputs.output[0]); i++) { - if (i >= num_outputs) { - outputs.output[i] = NAN; - } - } - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - PX4_ISFINITE(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 1000 - 2000us */ - outputs.output[i] = 1500 + (500 * outputs.output[i]); - - if (outputs.output[i] > _pwm_max[i]) { - outputs.output[i] = _pwm_max[i]; - } - - if (outputs.output[i] < _pwm_min[i]) { - outputs.output[i] = _pwm_min[i]; - } - - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = PWM_SIM_DISARMED_MAGIC; - } - } - - /* overwrite outputs in case of force_failsafe */ - if (_failsafe) { - for (size_t i = 0; i < num_outputs; i++) { - outputs.output[i] = PWM_SIM_FAILSAFE_MAGIC; - } - } - - /* overwrite outputs in case of lockdown */ - if (_lockdown) { - for (size_t i = 0; i < num_outputs; i++) { - outputs.output[i] = 0.0; - } - } - - /* and publish for anyone that cares to see */ - orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); - } - - /* how about an arming update? */ - bool updated; - actuator_armed_s aa; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa); - /* do not obey the lockdown value, as lockdown is for PWMSim. Only obey manual lockdown */ - _armed = aa.armed; - _failsafe = aa.force_failsafe; - _lockdown = aa.manual_lockdown; - } - } - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - orb_unsubscribe(_control_subs[i]); - } - } - - orb_unsubscribe(_armed_sub); - - /* make sure servos are off */ - // up_pwm_servo_deinit(); - - /* note - someone else is responsible for restoring the GPIO config */ - - /* tell the dtor that we are exiting */ - _task = -1; -} - -int -PWMSim::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) -{ - const actuator_controls_s *controls = (actuator_controls_s *)handle; - - if (_armed) { - input = controls[control_group].control[control_index]; - - } else { - /* clamp actuator to zero if not armed */ - input = 0.0f; - } - - return 0; -} - -int -PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - int ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch (_mode) { - case MODE_2PWM: - case MODE_4PWM: - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - ret = PWMSim::pwm_ioctl(filp, cmd, arg); - break; - - default: - ret = -ENOTTY; - PX4_INFO("not in a PWM mode"); - break; - } - - /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) { - ret = CDev::ioctl(filp, cmd, arg); - } - - return ret; -} - -int -PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - int ret = OK; - // int channel; - - lock(); - - switch (cmd) { - case PWM_SERVO_ARM: - // up_pwm_servo_arm(true); - break; - - case PWM_SERVO_DISARM: - // up_pwm_servo_arm(false); - break; - - case PWM_SERVO_SET_MIN_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < pwm->channel_count; i++) { - - if (i <= _max_actuators) { - _pwm_min[i] = pwm->values[i]; - } - } - - break; - } - - case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < pwm->channel_count; i++) { - - if (i <= _max_actuators) { - _pwm_max[i] = pwm->values[i]; - } - } - - break; - } - - case PWM_SERVO_SET_UPDATE_RATE: - // PWMSim always outputs at the alternate (usually faster) rate - g_pwm_sim->set_pwm_rate(arg); - break; - - case PWM_SERVO_SET_SELECT_UPDATE_RATE: - // PWMSim always outputs at the alternate (usually faster) rate - break; - - case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: - *(uint32_t *)arg = 400; - break; - - case PWM_SERVO_GET_UPDATE_RATE: - *(uint32_t *)arg = 400; - break; - - case PWM_SERVO_GET_SELECT_UPDATE_RATE: - *(uint32_t *)arg = 0; - break; - - case PWM_SERVO_GET_FAILSAFE_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < _num_outputs; i++) { - pwm->values[i] = 850; - } - - pwm->channel_count = _num_outputs; - break; - } - - case PWM_SERVO_GET_DISARMED_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < _num_outputs; i++) { - pwm->values[i] = 900; - } - - pwm->channel_count = _num_outputs; - break; - } - - case PWM_SERVO_GET_MIN_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < _num_outputs; i++) { - pwm->values[i] = 1000; - } - - pwm->channel_count = _num_outputs; - break; - } - - case PWM_SERVO_GET_TRIM_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < _num_outputs; i++) { - pwm->values[i] = 1500; - } - - pwm->channel_count = _num_outputs; - break; - } - - case PWM_SERVO_GET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < _num_outputs; i++) { - pwm->values[i] = 2000; - } - - pwm->channel_count = _num_outputs; - break; - } - - case PWM_SERVO_SET(2): - case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - - case PWM_SERVO_SET(0): - case PWM_SERVO_SET(1): - if (arg < 2100) { - // channel = cmd - PWM_SERVO_SET(0); -// up_pwm_servo_set(channel, arg); XXX - - } else { - ret = -EINVAL; - } - - break; - - case PWM_SERVO_GET(7): - case PWM_SERVO_GET(6): - case PWM_SERVO_GET(5): - case PWM_SERVO_GET(4): - if (_num_outputs < 8) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - - case PWM_SERVO_GET(3): - case PWM_SERVO_GET(2): - if (_num_outputs < 4) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - - case PWM_SERVO_GET(1): - case PWM_SERVO_GET(0): { - *(servo_position_t *)arg = 1500; - break; - } - - case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - // no restrictions on output grouping - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - - *(uint32_t *)arg = (1 << channel); - break; - } - - case PWM_SERVO_GET_COUNT: - case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_16PWM) { - *(unsigned *)arg = 16; - - } else if (_mode == MODE_12PWM) { - - *(unsigned *)arg = 12; - - } else if (_mode == MODE_8PWM) { - - *(unsigned *)arg = 8; - - } else if (_mode == MODE_4PWM) { - - *(unsigned *)arg = 4; - - } else { - - *(unsigned *)arg = 2; - } - - break; - - case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - _groups_required = 0; - } - - break; - - case MIXERIOCADDSIMPLE: { - mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); - - if (mixer->check()) { - delete mixer; - _groups_required = 0; - ret = -EINVAL; - - } else { - if (_mixers == nullptr) { - _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); - } - - _mixers->add_mixer(mixer); - _mixers->groups_required(_groups_required); - } - - break; - } - - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strnlen(buf, 1024); - - if (_mixers == nullptr) { - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - } - - if (_mixers == nullptr) { - _groups_required = 0; - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - PX4_ERR("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - _groups_required = 0; - ret = -EINVAL; - - } else { - _mixers->groups_required(_groups_required); - } - } - - break; - } - - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - return ret; -} - -namespace -{ - -enum PortMode { - PORT_MODE_UNDEFINED = 0, - PORT1_MODE_UNSET, - PORT1_FULL_PWM, - PORT2_MODE_UNSET, - PORT2_8PWM, - PORT2_12PWM, - PORT2_16PWM, -}; - -PortMode g_port_mode = PORT_MODE_UNDEFINED; - -int -hil_new_mode(PortMode new_mode) -{ - // uint32_t gpio_bits; - - -// /* reset to all-inputs */ -// g_pwm_sim->ioctl(0, GPIO_RESET, 0); - - // gpio_bits = 0; - - PWMSim::Mode servo_mode = PWMSim::MODE_NONE; - - switch (new_mode) { - case PORT_MODE_UNDEFINED: - case PORT1_MODE_UNSET: - case PORT2_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT1_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = PWMSim::MODE_8PWM; - break; - - case PORT2_8PWM: - /* select 8-pin PWM mode */ - servo_mode = PWMSim::MODE_8PWM; - break; - - case PORT2_12PWM: - /* select 12-pin PWM mode */ - servo_mode = PWMSim::MODE_12PWM; - break; - - case PORT2_16PWM: - /* select 16-pin PWM mode */ - servo_mode = PWMSim::MODE_16PWM; - break; - } - - /* (re)set the PWM output mode */ - g_pwm_sim->set_mode(servo_mode); - - return OK; -} - -int -test() -{ - int fd; - - fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); - - if (fd < 0) { - puts("open fail"); - return -ENODEV; - } - - px4_ioctl(fd, PWM_SERVO_ARM, 0); - px4_ioctl(fd, PWM_SERVO_SET(0), 1000); - - px4_close(fd); - - return OK; -} - -int -fake(int argc, char *argv[]) -{ - if (argc < 5) { - puts("pwm_out_sim fake (values -100 .. 100)"); - return -EINVAL; - } - - actuator_controls_s ac; - - ac.control[0] = strtol(argv[1], nullptr, 0) / 100.0f; - - ac.control[1] = strtol(argv[2], nullptr, 0) / 100.0f; - - ac.control[2] = strtol(argv[3], nullptr, 0) / 100.0f; - - ac.control[3] = strtol(argv[4], nullptr, 0) / 100.0f; - - orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - - if (handle == nullptr) { - puts("advertise failed"); - return 1; - } - - return 0; -} - -} // namespace - -extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]); - -static void -usage() -{ - PX4_WARN("unrecognized command, try:"); - PX4_WARN(" mode_pwm, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16"); -} - -int -pwm_out_sim_main(int argc, char *argv[]) -{ - PortMode new_mode = PORT_MODE_UNDEFINED; - const char *verb; - int ret = OK; - - if (argc < 2) { - usage(); - return -EINVAL; - } - - verb = argv[1]; - - if (g_pwm_sim == nullptr) { - g_pwm_sim = new PWMSim; - - if (g_pwm_sim == nullptr) { - return -ENOMEM; - } - } - - /* - * Mode switches. - */ - - // this was all cut-and-pasted from the FMU driver; it's junk - if (!strcmp(verb, "mode_pwm")) { - new_mode = PORT2_12PWM; - - } else if (!strcmp(verb, "mode_port2_pwm8")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm12")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm16")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_pwm16")) { - new_mode = PORT2_16PWM; - } - - /* was a new mode set? */ - if (new_mode != PORT_MODE_UNDEFINED) { - - /* yes but it's the same mode */ - if (new_mode == g_port_mode) { - return OK; - } - - /* switch modes */ - ret = hil_new_mode(new_mode); - - } else if (!strcmp(verb, "test")) { - ret = test(); - } - - else if (!strcmp(verb, "fake")) { - ret = fake(argc - 1, argv + 1); - } - - else { - usage(); - ret = -EINVAL; - } - - if (ret == OK && g_pwm_sim->_task == -1) { - ret = g_pwm_sim->init(); - - if (ret != OK) { - warnx("failed to start the pwm_out_sim driver"); - delete g_pwm_sim; - g_pwm_sim = nullptr; - } - } - - return ret; -}