diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/drv_pwm_output.h | 2 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 57 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 2 |
3 files changed, 34 insertions, 27 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index c3eea310f..7a93e513e 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -160,7 +160,7 @@ ORB_DECLARE(output_pwm); #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ -#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ +#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index efcf4d12b..7c7b3dcb7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -244,8 +244,7 @@ private: volatile int _task; ///< worker task id volatile bool _task_should_exit; ///< worker terminate flag - int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. - int _thread_mavlink_fd; ///< mavlink file descriptor for thread. + int _mavlink_fd; ///< mavlink file descriptor. perf_counter_t _perf_update; ///<local performance counter for status updates perf_counter_t _perf_write; ///<local performance counter for PWM control writes @@ -474,7 +473,6 @@ PX4IO::PX4IO(device::Device *interface) : _task(-1), _task_should_exit(false), _mavlink_fd(-1), - _thread_mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "io update")), _perf_write(perf_alloc(PC_ELAPSED, "io write")), _perf_chan_count(perf_alloc(PC_COUNT, "io rc #")), @@ -507,9 +505,6 @@ PX4IO::PX4IO(device::Device *interface) : /* we need this potentially before it could be set in task_main */ g_dev = this; - /* open MAVLink text channel */ - _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - _debug_enabled = false; _servorail_status.rssi_v = 0; } @@ -785,7 +780,7 @@ PX4IO::task_main() hrt_abstime poll_last = 0; hrt_abstime orb_check_last = 0; - _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* * Subscribe to the appropriate PWM output topic based on whether we are the @@ -880,6 +875,10 @@ PX4IO::task_main() /* run at 5Hz */ orb_check_last = now; + /* try to claim the MAVLink log FD */ + if (_mavlink_fd < 0) + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + /* check updates on uORB topics and handle it */ bool updated = false; @@ -1275,16 +1274,14 @@ void PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { - /* 0: dsm2, 1:dsmx */ - if ((dsmMode == 0) || (dsmMode == 1)) { - mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8")); - ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); - } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); - } + mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8")); + int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); + + if (ret) + mavlink_log_critical(_mavlink_fd, "binding failed."); } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected"); } } @@ -2115,14 +2112,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case DSM_BIND_START: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); - usleep(500000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(72000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); - usleep(50000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + + /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ + if (arg == DSM2_BIND_PULSES || + arg == DSMX_BIND_PULSES || + arg == DSMX8_BIND_PULSES) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(500000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + usleep(72000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); + usleep(50000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + + ret = OK; + } else { + ret = -EINVAL; + } break; case DSM_BIND_POWER_UP: @@ -2615,7 +2622,7 @@ bind(int argc, char *argv[]) #endif if (argc < 3) - errx(0, "needs argument, use dsm2 or dsmx"); + errx(0, "needs argument, use dsm2, dsmx or dsmx8"); if (!strcmp(argv[2], "dsm2")) pulses = DSM2_BIND_PULSES; @@ -2624,7 +2631,7 @@ bind(int argc, char *argv[]) else if (!strcmp(argv[2], "dsmx8")) pulses = DSMX8_BIND_PULSES; else - errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); // Test for custom pulse parameter if (argc > 3) pulses = atoi(argv[3]); diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1335f52e1..97d25bbfa 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -566,7 +566,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_DSM: - dsm_bind(value & 0x0f, (value >> 4) & 7); + dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; default: |