diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 7633884ea1..30fee02863 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -16,9 +16,6 @@ then then # Allow PX4IO to recover from midair restarts. px4io recovery - - # Adjust PX4IO update rate limit. - px4io limit 400 else echo "PX4IO start failed" >> $LOG_FILE tune_control play -t 20 diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 464313e6a6..84c59ab758 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -109,9 +109,6 @@ #define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) #define PX4IO_CHECK_CRC _IOC(0xff00, 3) -static constexpr unsigned UPDATE_INTERVAL_MIN{2}; // 2 ms -> 500 Hz -static constexpr unsigned UPDATE_INTERVAL_MAX{100}; // 100 ms -> 10 Hz - #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz #define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz @@ -174,26 +171,6 @@ public: */ virtual int ioctl(file *filp, int cmd, unsigned long arg); - /** - * write handler. - * - * Handle writes to the PX4IO file descriptor. - * - * @param[in] filp file handle (not used). This function is always called directly through object reference - * @param[in] buffer pointer to the data buffer to be written - * @param[in] len size in bytes to be written - * @return number of bytes written - */ - virtual ssize_t write(file *filp, const char *buffer, size_t len); - - /** - * Set the update rate for actuator outputs from FMU to IO. - * - * @param[in] rate The rate in Hz actuator outpus are sent to IO. - * Min 10 Hz, max 400 Hz - */ - int set_update_rate(int rate); - /** * Disable RC input handling */ @@ -235,7 +212,6 @@ private: unsigned _max_relays; ///< Maximum relays supported by PX4IO unsigned _max_transfer; ///< Maximum number of I2C transfers supported by PX4IO - unsigned _update_interval; ///< Subscription interval limiting send rate bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels uint64_t _rc_last_valid; ///< last valid timestamp @@ -464,7 +440,6 @@ PX4IO::PX4IO(device::Device *interface) : _max_rc_input(0), _max_relays(0), _max_transfer(16), /* sensible default */ - _update_interval(0), _rc_handling_disabled(false), _rc_chan_count(0), _rc_last_valid(0), @@ -893,7 +868,7 @@ PX4IO::task_main() * primary PWM output or not. */ _t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0)); - orb_set_interval(_t_actuator_controls_0, 20); /* default to 50Hz */ + orb_set_interval(_t_actuator_controls_0, 2); /* default to 500Hz */ if (_t_actuator_controls_0 < 0) { PX4_ERR("actuator subscription failed"); @@ -916,18 +891,6 @@ PX4IO::task_main() /* loop talking to IO */ while (!_task_should_exit) { - /* adjust update interval */ - if (_update_interval != 0) { - _update_interval = math::constrain(_update_interval, UPDATE_INTERVAL_MIN, UPDATE_INTERVAL_MAX); - - orb_set_interval(_t_actuator_controls_0, _update_interval); - /* - * NOT changing the rate of groups 1-3 here, because only attitude - * really needs to run fast. - */ - _update_interval = 0; - } - /* sleep waiting for topic updates, but no more than 20ms */ unlock(); int ret = ::poll(fds, 1, 20); @@ -2897,47 +2860,6 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return ret; } -ssize_t -PX4IO::write(file * /*filp*/, const char *buffer, size_t len) -/* Make it obvious that file * isn't used here */ -{ - unsigned count = len / 2; - - if (count > _max_actuators) { - count = _max_actuators; - } - - if (count > 0) { - - perf_begin(_perf_write); - - int ret = OK; - - /* The write() is silently ignored in test mode. */ - if (!_test_fmu_fail) { - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); - } - - perf_end(_perf_write); - - if (ret != OK) { - return ret; - } - } - - return count * 2; -} - -int -PX4IO::set_update_rate(int rate) -{ - unsigned interval_ms = 1000 / rate; - - _update_interval = math::constrain(interval_ms, UPDATE_INTERVAL_MIN, UPDATE_INTERVAL_MAX); - - return 0; -} - extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace @@ -3156,107 +3078,6 @@ bind(int argc, char *argv[]) } -void -test(void) -{ - int fd; - unsigned servo_count = 0; - unsigned pwm_value = 1000; - int direction = 1; - int ret; - - fd = open(PX4IO_DEVICE_PATH, O_WRONLY); - - if (fd < 0) { - err(1, "failed to open device"); - } - - if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) { - err(1, "failed to get servo count"); - } - - /* tell IO that its ok to disable its safety with the switch */ - ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - - if (ret != OK) { - err(1, "PWM_SERVO_SET_ARM_OK"); - } - - if (ioctl(fd, PWM_SERVO_ARM, 0)) { - err(1, "failed to arm servos"); - } - - struct pollfd fds; - - fds.fd = 0; /* stdin */ - - fds.events = POLLIN; - - warnx("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; - } - - ret = write(fd, servos, sizeof(servos)); - - if (ret != (int)sizeof(servos)) { - err(1, "error writing PWM servo data, wrote %zu got %d", sizeof(servos), ret); - } - - if (direction > 0) { - if (pwm_value < 2000) { - pwm_value++; - - } else { - direction = -1; - } - - } else { - if (pwm_value > 1000) { - pwm_value--; - - } else { - direction = 1; - } - } - - px4_usleep(250); - - /* 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)) { - err(1, "error reading PWM servo %u", i); - } - - if (value != servos[i]) { - warnx("servo %u readback error, got %hu expected %hu", i, value, servos[i]); - } - } - - /* 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') { - warnx("User abort\n"); - exit(0); - } - } - } -} - void monitor(void) { @@ -3299,23 +3120,6 @@ monitor(void) } } -void -if_test(unsigned mode) -{ - device::Device *interface = get_interface(); - int result; - - if (interface) { - result = interface->ioctl(1, mode); /* XXX magic numbers */ - delete interface; - - } else { - errx(1, "interface not loaded, exiting"); - } - - errx(0, "test returned %d", result); -} - void lockdown(int argc, char *argv[]) { @@ -3450,14 +3254,6 @@ px4io_main(int argc, char *argv[]) return ret; } - if (!strcmp(argv[1], "iftest")) { - if (g_dev != nullptr) { - errx(1, "can't iftest when started"); - } - - if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); - } - if (!strcmp(argv[1], "forceupdate")) { /* force update of the IO firmware without requiring @@ -3511,26 +3307,6 @@ px4io_main(int argc, char *argv[]) errx(1, "not started"); } - if (!strcmp(argv[1], "limit")) { - - if ((argc > 2)) { - int limitrate = atoi(argv[2]); - - if (limitrate > 0) { - g_dev->set_update_rate(limitrate); - - } else { - errx(1, "invalid rate: %d", limitrate); - } - - } else { - errx(1, "missing argument (50 - 500 Hz)"); - return 1; - } - - exit(0); - } - if (!strcmp(argv[1], "safety_off")) { int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); @@ -3615,10 +3391,6 @@ px4io_main(int argc, char *argv[]) errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); } - if (!strcmp(argv[1], "test")) { - test(); - } - if (!strcmp(argv[1], "monitor")) { monitor(); } @@ -3706,8 +3478,8 @@ px4io_main(int argc, char *argv[]) } out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" - "'recovery', 'limit ', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" + errx(1, "need a command, try 'start', 'stop', 'status', 'monitor', 'debug ',\n" + "'recovery', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm',\n" "'test_fmu_fail', 'test_fmu_ok'"); }