From 6f7dcdc39ded6fba0b8772c8a4a389a57229a208 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 4 Jan 2022 12:08:06 -0500 Subject: [PATCH] delete PWM_SERVO_ARM --- src/drivers/drv_pwm_output.h | 3 - src/drivers/pca9685_pwm_out/main.cpp | 1 - src/drivers/pwm_out/PWMOut.cpp | 166 ----------------------- src/drivers/pwm_out/PWMOut.hpp | 2 - src/drivers/pwm_out_sim/PWMSim.cpp | 3 - src/drivers/px4io/px4io.cpp | 6 - src/systemcmds/esc_calib/esc_calib.cpp | 8 -- src/systemcmds/motor_ramp/motor_ramp.cpp | 6 - src/systemcmds/pwm/pwm.cpp | 24 +--- 9 files changed, 1 insertion(+), 218 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 4837487cd7..9be11c91cc 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -137,9 +137,6 @@ typedef uint16_t servo_position_t; */ #define _PWM_SERVO_BASE 0x2a00 -/** arm all servo outputs handle by this driver */ -#define PWM_SERVO_ARM _PX4_IOC(_PWM_SERVO_BASE, 0) - /** disarm all servo outputs (stop generating pulses) */ #define PWM_SERVO_DISARM _PX4_IOC(_PWM_SERVO_BASE, 1) diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index e96abb359b..defaa4a401 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -490,7 +490,6 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: - case PWM_SERVO_ARM: case PWM_SERVO_DISARM: break; diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 177fd175fa..41db3337be 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -411,10 +411,6 @@ bool PWMOut::update_pwm_out_state(bool on) bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - if (_test_mode) { - return false; - } - /* output to the servos */ if (_pwm_initialized) { for (size_t i = 0; i < num_outputs; i++) { @@ -761,10 +757,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) lock(); switch (cmd) { - case PWM_SERVO_ARM: - update_pwm_out_state(true); - break; - case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: break; @@ -1137,165 +1129,8 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) return ret; } -int PWMOut::test(const char *dev) -{ - int fd; - unsigned servo_count = 0; - unsigned pwm_value = 1000; - int direction = 1; - int ret; - int rv = -1; - - fd = ::open(dev, O_RDWR); - - if (fd < 0) { - PX4_ERR("open fail"); - return -1; - } - - 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; - } - - if (::ioctl(fd, PWM_SERVO_ARM, 0) < 0) { - PX4_ERR("servo arm failed"); - goto err_out; - } - - if (::ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { - PX4_ERR("Unable to get servo count"); - goto err_out; - } - - PX4_INFO("Testing %u servos", servo_count); - - struct pollfd fds; - - fds.fd = 0; /* stdin */ - - fds.events = POLLIN; - - PX4_INFO("Press CTRL-C or 'c' to abort."); - - for (;;) { - /* sweep all servos between 1000..2000 */ - servo_position_t servos[servo_count]; - - for (unsigned i = 0; i < servo_count; i++) { - servos[i] = pwm_value; - } - - for (unsigned i = 0; i < servo_count; i++) { - if (::ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { - PX4_ERR("servo %u set failed", i); - goto err_out; - } - } - - if (direction > 0) { - if (pwm_value < 2000) { - pwm_value++; - - } else { - direction = -1; - } - - } else { - if (pwm_value > 1000) { - pwm_value--; - - } else { - direction = 1; - } - } - - /* readback servo values */ - for (unsigned i = 0; i < servo_count; i++) { - servo_position_t value; - - if (::ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) { - PX4_ERR("error reading PWM servo %u", i); - goto err_out; - } - - if (value != servos[i]) { - PX4_ERR("servo %u readback error, got %" PRIu16 " expected %" PRIu16, i, value, servos[i]); - goto err_out; - } - } - - /* Check if user wants to quit */ - char c; - ret = ::poll(&fds, 1, 0); - - if (ret > 0) { - - ::read(0, &c, 1); - - if (c == 0x03 || c == 0x63 || c == 'q') { - PX4_INFO("User abort"); - break; - } - } - } - - rv = 0; - -err_out: - - if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) { - PX4_ERR("Failed to Exit pwm test mode"); - } - -err_out_no_test: - ::close(fd); - return rv; -} - int PWMOut::custom_command(int argc, char *argv[]) { - - int ch = 0; - int myoptind = 0; - const char *myoptarg = nullptr; - const char *dev = PX4FMU_DEVICE_PATH; - - while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'd': - if (nullptr == strstr(myoptarg, "/dev/")) { - PX4_WARN("device %s not valid", myoptarg); - print_usage(nullptr); - return 1; - } - - dev = myoptarg; - break; - } - } - - if (myoptind >= argc) { - print_usage(nullptr); - return 1; - } - - const char *verb = argv[myoptind]; - - /* start pwm_out if not running */ - if (!is_running()) { - - int ret = PWMOut::task_spawn(argc, argv); - - if (ret) { - return ret; - } - } - - if (!strcmp(verb, "test")) { - return test(dev); - } - return print_usage("unknown command"); } @@ -1365,7 +1200,6 @@ By default the module runs on a work queue with a callback on the uORB actuator_ PRINT_MODULE_USAGE_NAME("pwm_out", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test outputs"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 7f72fea8ce..9d6d22a354 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -101,8 +101,6 @@ public: static bool trylock_module() { return (pthread_mutex_trylock(&pwm_out_module_mutex) == 0); } static void unlock_module() { pthread_mutex_unlock(&pwm_out_module_mutex); } - static int test(const char *dev); - int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; int init() override; diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index ebc5a566ef..62fb497a98 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -94,9 +94,6 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) lock(); switch (cmd) { - case PWM_SERVO_ARM: - break; - case PWM_SERVO_DISARM: break; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5cceaf9cb5..f60f02ab9d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1622,12 +1622,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* regular ioctl? */ switch (cmd) { - case PWM_SERVO_ARM: - PX4_DEBUG("PWM_SERVO_ARM"); - /* set the 'armed' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED); - break; - case PWM_SERVO_SET_ARM_OK: PX4_DEBUG("PWM_SERVO_SET_ARM_OK"); /* set the 'OK to arm' bit */ diff --git a/src/systemcmds/esc_calib/esc_calib.cpp b/src/systemcmds/esc_calib/esc_calib.cpp index d90bb62d1d..d6e30031a0 100644 --- a/src/systemcmds/esc_calib/esc_calib.cpp +++ b/src/systemcmds/esc_calib/esc_calib.cpp @@ -278,14 +278,6 @@ extern "C" __EXPORT int esc_calib_main(int argc, char *argv[]) goto cleanup; } - /* tell IO/FMU that the system is armed (it will output values if safety is off) */ - ret = ioctl(fd, PWM_SERVO_ARM, 0); - - if (ret != OK) { - PX4_ERR("PWM_SERVO_ARM"); - goto cleanup; - } - printf("Outputs armed"); diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp index 8d5b549803..4429417ebd 100644 --- a/src/systemcmds/motor_ramp/motor_ramp.cpp +++ b/src/systemcmds/motor_ramp/motor_ramp.cpp @@ -344,12 +344,6 @@ int prepare(int fd, unsigned long *max_channels) return 1; } - /* tell IO/FMU that the system is armed (it will output values if safety is off) */ - if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) { - PX4_ERR("PWM_SERVO_ARM"); - return 1; - } - return 0; } diff --git a/src/systemcmds/pwm/pwm.cpp b/src/systemcmds/pwm/pwm.cpp index 60a41d2d99..b42e88fce4 100644 --- a/src/systemcmds/pwm/pwm.cpp +++ b/src/systemcmds/pwm/pwm.cpp @@ -111,7 +111,6 @@ $ pwm test -c 13 -p 1200 PRINT_MODULE_USAGE_NAME("pwm", "command"); - PRINT_MODULE_USAGE_COMMAND_DESCR("arm", "Arm output"); PRINT_MODULE_USAGE_COMMAND_DESCR("disarm", "Disarm output"); PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print current configuration of all channels"); @@ -306,28 +305,7 @@ pwm_main(int argc, char *argv[]) oneshot = !strcmp(command, "oneshot"); - if (!strcmp(command, "arm")) { - /* tell safety that its ok to disable it with the switch */ - ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - - if (ret != OK) { - err(1, "PWM_SERVO_SET_ARM_OK"); - } - - /* tell IO that the system is armed (it will output values if safety is off) */ - ret = px4_ioctl(fd, PWM_SERVO_ARM, 0); - - if (ret != OK) { - err(1, "PWM_SERVO_ARM"); - } - - if (print_verbose) { - PX4_INFO("Outputs armed"); - } - - return 0; - - } else if (!strcmp(command, "disarm")) { + if (!strcmp(command, "disarm")) { /* disarm, but do not revoke the SET_ARM_OK flag */ ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0);