From 212525a78e79f22feca81780a16b11352d73d986 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 4 Jan 2022 12:10:43 -0500 Subject: [PATCH] delete PWM_SERVO_SET_MODE --- src/drivers/drv_pwm_output.h | 5 - src/drivers/pwm_out/PWMOut.cpp | 17 -- src/drivers/pwm_out/PWMOut.hpp | 1 - src/drivers/px4io/px4io.cpp | 23 +-- src/systemcmds/motor_ramp/motor_ramp.cpp | 14 -- src/systemcmds/pwm/pwm.cpp | 243 +---------------------- 6 files changed, 5 insertions(+), 298 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index ba2976abfb..674b2304cd 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -203,11 +203,6 @@ typedef uint16_t servo_position_t; /** make failsafe non-recoverable (termination) if it occurs */ #define PWM_SERVO_SET_TERMINATION_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 27) -/** set auxillary output mode */ -#define PWM_SERVO_ENTER_TEST_MODE 18 -#define PWM_SERVO_EXIT_TEST_MODE 19 -#define PWM_SERVO_SET_MODE _PX4_IOC(_PWM_SERVO_BASE, 34) - /* * * diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 0c01346477..07e660f981 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -1079,23 +1079,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) *(unsigned *)arg = _num_outputs; break; - case PWM_SERVO_SET_MODE: { - switch (arg) { - case PWM_SERVO_ENTER_TEST_MODE: - _test_mode = true; - break; - - case PWM_SERVO_EXIT_TEST_MODE: - _test_mode = false; - break; - - default: - ret = -EINVAL; - } - - break; - } - case MIXERIOCRESET: _mixing_output.resetMixerThreadSafe(); diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 9d6d22a354..147e463c64 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -145,7 +145,6 @@ private: bool _pwm_on{false}; uint32_t _pwm_mask{0}; bool _pwm_initialized{false}; - bool _test_mode{false}; bool _esc_calibration_mode{false}; hrt_abstime _esc_calibration_last{0}; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c790f42ea6..a1fef26d58 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -238,7 +238,6 @@ private: float _analog_rc_rssi_volt{-1.f}; ///< analog RSSI voltage bool _test_fmu_fail{false}; ///< To test what happens if IO loses FMU - bool _in_test_mode{false}; ///< true if PWM_SERVO_ENTER_TEST_MODE is active MixingOutput _mixing_output{"PWM_MAIN", PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true}; @@ -433,7 +432,7 @@ bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], { SmartLock lock_guard(_lock); - if (!_test_fmu_fail && !_in_test_mode) { + if (!_test_fmu_fail) { /* output to the servos */ io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs); } @@ -585,7 +584,7 @@ void PX4IO::Run() SmartLock lock_guard(_lock); // ESC calibration - if (!_mixing_output.armed().armed && !_test_fmu_fail && !_in_test_mode) { + if (!_mixing_output.armed().armed && !_test_fmu_fail) { if (_mixing_output.armed().in_esc_calibration_mode) { _esc_calibration_mode = true; _esc_calibration_last = _mixing_output.armed().timestamp; @@ -1892,7 +1891,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = -EINVAL; } else { - if (!_test_fmu_fail && _in_test_mode) { + if (!_test_fmu_fail) { /* send a direct PWM value */ ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); @@ -1941,22 +1940,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PWM_SERVO_SET_MODE: { - // reset all channels to disarmed when entering/leaving test mode, so that we don't - // accidentially use values from previous tests - pwm_output_values pwm_disarmed; - - if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) { - for (unsigned i = 0; i < _max_actuators; ++i) { - io_reg_set(PX4IO_PAGE_DIRECT_PWM, i, pwm_disarmed.values[i]); - } - } - - _in_test_mode = (arg == PWM_SERVO_ENTER_TEST_MODE); - ret = (arg == PWM_SERVO_ENTER_TEST_MODE || PWM_SERVO_EXIT_TEST_MODE) ? 0 : -EINVAL; - } - break; - case MIXERIOCRESET: PX4_DEBUG("MIXERIOCRESET"); _mixing_output.resetMixerThreadSafe(); diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp index 4429417ebd..6512d6c5a2 100644 --- a/src/systemcmds/motor_ramp/motor_ramp.cpp +++ b/src/systemcmds/motor_ramp/motor_ramp.cpp @@ -392,13 +392,6 @@ int motor_ramp_thread_main(int argc, char *argv[]) return 1; } - if (px4_ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) { - PX4_ERR("Failed to Enter pwm test mode"); - px4_close(fd); - _thread_running = false; - return 1; - } - if (prepare(fd, &max_channels) != OK) { _thread_should_exit = true; } @@ -505,13 +498,6 @@ int motor_ramp_thread_main(int argc, char *argv[]) } } - if (px4_ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) { - PX4_ERR("Failed to Exit pwm test mode"); - px4_close(fd); - _thread_running = false; - return 1; - } - px4_close(fd); } diff --git a/src/systemcmds/pwm/pwm.cpp b/src/systemcmds/pwm/pwm.cpp index 325c01beac..cd402686ad 100644 --- a/src/systemcmds/pwm/pwm.cpp +++ b/src/systemcmds/pwm/pwm.cpp @@ -103,10 +103,6 @@ Note that in OneShot mode, the PWM range [1000, 2000] is automatically mapped to Set the PWM rate for all channels to 400 Hz: $ pwm rate -a -r 400 -Test the outputs of eg. channels 1 and 3, and set the PWM value to 1200 us: -$ pwm arm -$ pwm test -c 13 -p 1200 - )DESCR_STR"); @@ -130,15 +126,11 @@ $ pwm test -c 13 -p 1200 PRINT_MODULE_USAGE_COMMAND_DESCR("disarmed", "Set Disarmed PWM value"); PRINT_MODULE_USAGE_COMMAND_DESCR("min", "Set Minimum PWM value"); PRINT_MODULE_USAGE_COMMAND_DESCR("max", "Set Maximum PWM value"); - PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Set Output to a specific value until 'q' or 'c' or 'ctrl-c' pressed"); - PRINT_MODULE_USAGE_COMMAND_DESCR("steps", "Run 5 steps from 0 to 100%"); - - - PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'failsafe', 'disarmed', 'min', 'max' and 'test' require a PWM value:"); + PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'failsafe', 'disarmed', 'min', 'max' require a PWM value:"); PRINT_MODULE_USAGE_PARAM_INT('p', -1, 0, 4000, "PWM value (eg. 1100)", false); - PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'rate', 'oneshot', 'failsafe', 'disarmed', 'min', 'max', 'test' and 'steps' " + PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'rate', 'oneshot', 'failsafe', 'disarmed', 'min', 'max' " "additionally require to specify the channels with one of the following commands:"); PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, nullptr, "select channels in the form: 1234 (1 digit per channel, 1=first)", true); @@ -576,237 +568,6 @@ pwm_main(int argc, char *argv[]) return 0; - } else if (!strcmp(command, "test")) { - - if (set_mask == 0) { - usage("no channels set"); - return 1; - } - - if (pwm_value == 0) { - usage("no PWM provided"); - return 1; - } - - /* get current servo values */ - struct pwm_output_values last_spos; - - for (unsigned i = 0; i < servo_count; i++) { - - - ret = px4_ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]); - - if (ret != OK) { - PX4_ERR("PWM_SERVO_GET(%d)", i); - return 1; - } - } - - /* perform PWM output */ - - /* Open console directly to grab CTRL-C signal */ - struct pollfd fds; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - - if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) { - PX4_ERR("Failed to Enter pwm test mode"); - goto err_out_no_test; - } - - PX4_INFO("Press CTRL-C or 'c' to abort."); - - while (1) { - for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1 << i) { - ret = px4_ioctl(fd, PWM_SERVO_SET(i), pwm_value); - - if (ret != OK) { - PX4_ERR("PWM_SERVO_SET(%d)", i); - goto err_out; - } - } - } - - /* abort on user request */ - char c; - ret = poll(&fds, 1, 0); - - if (ret > 0) { - - ret = read(0, &c, 1); - - if (c == 0x03 || c == 0x63 || c == 'q') { - /* reset output to the last value */ - for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1 << i) { - ret = px4_ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]); - - if (ret != OK) { - PX4_ERR("PWM_SERVO_SET(%d)", i); - goto err_out; - } - } - } - - PX4_INFO("User abort\n"); - rv = 0; - goto err_out; - } - } - - /* Delay longer than the max Oneshot duration */ - - px4_usleep(2542); - -#ifdef __PX4_NUTTX - /* Trigger all timer's channels in Oneshot mode to fire - * the oneshots with updated values. - */ - - up_pwm_update(0xff); -#endif - } - rv = 0; -err_out: - if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) { - rv = 1; - PX4_ERR("Failed to Exit pwm test mode"); - } - -err_out_no_test: - return rv; - - - } else if (!strcmp(command, "steps")) { - - if (set_mask == 0) { - usage("no channels set"); - return 1; - } - - /* get current servo values */ - struct pwm_output_values last_spos; - - for (unsigned i = 0; i < servo_count; i++) { - - ret = px4_ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]); - - if (ret != OK) { - PX4_ERR("PWM_SERVO_GET(%d)", i); - return 1; - } - } - - /* perform PWM output */ - - /* Open console directly to grab CTRL-C signal */ - struct pollfd fds; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - - PX4_WARN("Running 5 steps. WARNING! Motors will be live in 5 seconds\nPress any key to abort now."); - px4_sleep(5); - - if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) { - PX4_ERR("Failed to Enter pwm test mode"); - goto err_out_no_test; - } - - unsigned off = 900; - unsigned idle = 1300; - unsigned full = 2000; - unsigned steps_timings_us[] = {2000, 5000, 20000, 50000}; - - unsigned phase = 0; - unsigned phase_counter = 0; - unsigned const phase_maxcount = 20; - - for (unsigned steps_timing_index = 0; - steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]); - steps_timing_index++) { - - PX4_INFO("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]); - - while (1) { - for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1 << i) { - - unsigned val; - - if (phase == 0) { - val = idle; - - } else if (phase == 1) { - /* ramp - depending how steep it is this ramp will look instantaneous on the output */ - val = idle + (full - idle) * ((float)phase_counter / phase_maxcount); - - } else { - val = off; - } - - ret = px4_ioctl(fd, PWM_SERVO_SET(i), val); - - if (ret != OK) { - PX4_ERR("PWM_SERVO_SET(%d)", i); - goto err_out; - } - } - } - - /* abort on user request */ - char c; - ret = poll(&fds, 1, 0); - - if (ret > 0) { - - ret = read(0, &c, 1); - - if (ret > 0) { - /* reset output to the last value */ - for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1 << i) { - ret = px4_ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]); - - if (ret != OK) { - PX4_ERR("PWM_SERVO_SET(%d)", i); - goto err_out; - } - } - } - - PX4_INFO("User abort\n"); - rv = 0; - goto err_out; - } - } - - if (phase == 1) { - px4_usleep(steps_timings_us[steps_timing_index] / phase_maxcount); - - } else if (phase == 0) { - px4_usleep(50000); - - } else if (phase == 2) { - px4_usleep(50000); - - } else { - break; - } - - phase_counter++; - - if (phase_counter > phase_maxcount) { - phase++; - phase_counter = 0; - } - } - } - - rv = 0; - goto err_out; - - } else if (!strcmp(command, "status") || !strcmp(command, "info")) { printf("device: %s\n", dev);