diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0589639fa5..440b7cad68 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -82,6 +82,7 @@ #include #include #include +#include #include #include #include @@ -134,6 +135,8 @@ public: virtual int init(); + void dsm_bind_ioctl(); + int set_mode(Mode mode); Mode get_mode() { return _mode; } @@ -180,6 +183,7 @@ private: uint32_t _pwm_alt_rate_channels; unsigned _current_update_rate; struct work_s _work; + int _vehicle_cmd_sub; int _armed_sub; int _param_sub; int _adc_sub; @@ -918,7 +922,7 @@ PX4FMU::cycle() // power radio on RF_RADIO_POWER_CONTROL(true); #endif - + _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); // dsm_init sets some file static variables and returns a file descriptor _rcs_fd = dsm_init(RC_SERIAL_PORT); // assume SBUS input @@ -1183,6 +1187,23 @@ PX4FMU::cycle() } } +#ifdef RC_SERIAL_PORT + /* vehicle command */ + orb_check(_vehicle_cmd_sub, &updated); + + if (updated) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub, &cmd); + + // Check for a DSM pairing command + if (((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { + dsm_bind_ioctl((int)cmd.param2); + } + + } + +#endif + orb_check(_param_sub, &updated); if (updated) { @@ -2086,7 +2107,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ - warnx("fmu pwm_ioctl: DSM_BIND_START, arg: %lu", arg); + PX4_INFO("pwm_ioctl: DSM_BIND_START, arg: %lu", arg); if (arg == DSM2_BIND_PULSES || arg == DSMX_BIND_PULSES || @@ -2549,6 +2570,33 @@ PX4FMU::dsm_bind_ioctl(int dsmMode) namespace { +void +bind_spektrum() +{ + int fd; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); + + if (fd < 0) { + errx(1, "open fail"); + } + + if (true) { + PX4_INFO("bind_Spektrum RX"); + + /* specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary */ + if (ioctl(fd, DSM_BIND_START, DSMX8_BIND_PULSES)) { + PX4_ERR("binding failed."); + } + + } else { + PX4_WARN("system armed, bind request rejected"); + } + + close(fd); + +} + enum PortMode { PORT_MODE_UNSET = 0, PORT_FULL_GPIO, @@ -2983,6 +3031,11 @@ fmu_main(int argc, char *argv[]) PortMode new_mode = PORT_MODE_UNSET; const char *verb = argv[1]; + if (!strcmp(verb, "bind")) { + bind_spektrum(); + exit(0); + } + /* does not operate on a FMU instance */ if (!strcmp(verb, "i2c")) { if (argc > 3) { @@ -3126,7 +3179,7 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, " , mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n"); #elif defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c \n"); + fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c , bind\n"); #endif fprintf(stderr, "\n"); exit(1);