From 02833c6075b688ed7c17e0d4f21d97012f7e1e1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 21 Oct 2019 12:19:41 +0200 Subject: [PATCH] px4io: add support for test_motor And fix some IO bugs: - PX4IO_P_STATUS_FLAGS_RAW_PWM was never cleared. This meant that after a 'pwm test' command, normal mixing was not possible anymore. Fixed by remembering when we are in test mode and not sending PX4IO_PAGE_CONTROLS during that time. PX4IO_P_STATUS_FLAGS_RAW_PWM is cleared when PX4IO_PAGE_CONTROLS are received. - when entering test mode w/o specifying all channels, the other channels were not set at all, which means they could still be set to values from a previous test call. This is fixed by setting all channels to disarmed when entering/leaving test mode. --- src/drivers/px4io/px4io.cpp | 111 ++++++++++++++++++++++++-- src/modules/px4iofirmware/registers.c | 2 + 2 files changed, 108 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4f7592709d..6842bbb3cd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -91,6 +91,7 @@ #include #include #include +#include #include @@ -113,6 +114,8 @@ 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 +using namespace time_literals; + /** * The PX4IO class. * @@ -223,7 +226,6 @@ public: private: device::Device *_interface; - // XXX unsigned _hardware; ///< Hardware revision unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO unsigned _max_controls; ///< Maximum # of controls supported by PX4IO @@ -288,6 +290,13 @@ private: bool _test_fmu_fail; ///< To test what happens if IO looses FMU + struct MotorTest { + uORB::Subscription test_motor_sub{ORB_ID(test_motor)}; + bool in_test_mode{false}; + hrt_abstime timeout{0}; + }; + MotorTest _motor_test; + /** * Trampoline to the worker task */ @@ -440,6 +449,11 @@ private: */ void io_handle_vservo(uint16_t vservo, uint16_t vrssi); + /** + * check and handle test_motor topic updates + */ + void handle_motor_test(); + /* do not allow to copy this class due to ptr data members */ PX4IO(const PX4IO &); PX4IO operator=(const PX4IO &); @@ -947,6 +961,13 @@ PX4IO::task_main() (void)io_set_control_groups(); } + if (!_armed && !_lockdown_override) { + handle_motor_test(); + + } else { + _motor_test.in_test_mode = false; + } + if (now >= poll_last + IO_POLL_INTERVAL) { /* run at 50-250Hz */ poll_last = now; @@ -1296,7 +1317,7 @@ PX4IO::io_set_control_state(unsigned group) } - if (!_test_fmu_fail) { + if (!_test_fmu_fail && !_motor_test.in_test_mode) { /* copy values to registers in IO */ return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); @@ -1305,6 +1326,74 @@ PX4IO::io_set_control_state(unsigned group) } } +void +PX4IO::handle_motor_test() +{ + test_motor_s test_motor; + + while (_motor_test.test_motor_sub.update(&test_motor)) { + if (test_motor.driver_instance != 0 || + hrt_elapsed_time(&test_motor.timestamp) > 100_ms) { + continue; + } + + bool in_test_mode = test_motor.action == test_motor_s::ACTION_RUN; + + if (in_test_mode != _motor_test.in_test_mode) { + // reset all outputs to disarmed on state change + 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]); + } + } + } + + if (in_test_mode) { + unsigned idx = test_motor.motor_number; + + if (idx < _max_actuators) { + if (test_motor.value < 0.f) { + pwm_output_values pwm_disarmed; + + if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) { + io_reg_set(PX4IO_PAGE_DIRECT_PWM, idx, pwm_disarmed.values[idx]); + } + + } else { + pwm_output_values pwm_min; + pwm_output_values pwm_max; + + if (io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm_min.values, _max_actuators) == 0 && + io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm_max.values, _max_actuators) == 0) { + + uint16_t value = math::constrain(pwm_min.values[idx] + + (uint16_t)((pwm_max.values[idx] - pwm_min.values[idx]) * test_motor.value), + pwm_min.values[idx], pwm_max.values[idx]); + io_reg_set(PX4IO_PAGE_DIRECT_PWM, idx, value); + } + } + } + + if (test_motor.timeout_ms > 0) { + _motor_test.timeout = test_motor.timestamp + test_motor.timeout_ms * 1000; + + } else { + _motor_test.timeout = 0; + } + } + + _motor_test.in_test_mode = in_test_mode; + } + + // check for timeouts + if (_motor_test.timeout != 0 && hrt_absolute_time() > _motor_test.timeout) { + _motor_test.in_test_mode = false; + _motor_test.timeout = 0; + } +} + int PX4IO::io_set_arming_state() { @@ -2643,7 +2732,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = -EINVAL; } else { - if (!_test_fmu_fail) { + if (!_test_fmu_fail && _motor_test.in_test_mode) { /* send a direct PWM value */ ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); @@ -2692,8 +2781,20 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PWM_SERVO_SET_MODE: - ret = (arg == PWM_SERVO_ENTER_TEST_MODE || PWM_SERVO_EXIT_TEST_MODE) ? 0 : -EINVAL; + 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]); + } + } + + _motor_test.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 MIXERIOCGETOUTPUTCOUNT: diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1e43a5fb80..d254165391 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -291,6 +291,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num values++; } + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; + system_state.fmu_data_received_time = hrt_absolute_time(); break;