diff --git a/libraries/AP_HAL_PX4/RCInput.cpp b/libraries/AP_HAL_PX4/RCInput.cpp index aae097fce3..d7aadf2ac3 100644 --- a/libraries/AP_HAL_PX4/RCInput.cpp +++ b/libraries/AP_HAL_PX4/RCInput.cpp @@ -2,6 +2,9 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #include "RCInput.h" +#include +#include +#include #include #include @@ -114,4 +117,22 @@ void PX4RCInput::_timer_tick(void) perf_end(_perf_rcin); } +bool PX4RCInput::rc_bind(int dsmMode) +{ + int fd = open("/dev/px4io", 0); + if (fd == -1) { + hal.console->printf("RCInput: failed to open /dev/px4io\n"); + return false; + } + + uint32_t mode = (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES); + int ret = ioctl(fd, DSM_BIND_START, mode); + close(fd); + if (ret != 0) { + hal.console->printf("RCInput: Unable to start DSM bind\n"); + return false; + } + return true; +} + #endif diff --git a/libraries/AP_HAL_PX4/RCInput.h b/libraries/AP_HAL_PX4/RCInput.h index 249b6d7c0e..1b8602cb7b 100644 --- a/libraries/AP_HAL_PX4/RCInput.h +++ b/libraries/AP_HAL_PX4/RCInput.h @@ -21,6 +21,8 @@ public: void _timer_tick(void); + bool rc_bind(int dsmMode); + private: /* override state */ uint16_t _override[RC_INPUT_MAX_CHANNELS];