diff options
author | Jean Cyr <jcyr@dillobits.com> | 2013-09-11 22:11:37 -0400 |
---|---|---|
committer | Jean Cyr <jcyr@dillobits.com> | 2013-09-11 22:11:37 -0400 |
commit | 3b8039e4e009868ec7722ad559cd5b62c5f7a325 (patch) | |
tree | 9a7a9d1d278da2325255579970254a5c6e3e2190 /src/drivers/px4io | |
parent | 5ece19f66aeb299206c1e1e585df2b43d0ed2160 (diff) | |
download | px4-firmware-3b8039e4e009868ec7722ad559cd5b62c5f7a325.tar.gz px4-firmware-3b8039e4e009868ec7722ad559cd5b62c5f7a325.tar.bz2 px4-firmware-3b8039e4e009868ec7722ad559cd5b62c5f7a325.zip |
Implement message based receiver pairing
Diffstat (limited to 'src/drivers/px4io')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 32 |
1 files changed, 29 insertions, 3 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 78d1d3e63..c93904a3f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -254,6 +254,7 @@ private: int _t_actuator_armed; ///< system armed control topic int _t_vehicle_control_mode;///< vehicle control mode topic int _t_param; ///< parameter update topic + int _t_vehicle_command; ///< vehicle command topic /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io @@ -440,6 +441,7 @@ PX4IO::PX4IO(device::Device *interface) : _t_actuator_armed(-1), _t_vehicle_control_mode(-1), _t_param(-1), + _t_vehicle_command(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), @@ -732,16 +734,20 @@ PX4IO::task_main() _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); + orb_set_interval(_t_param, 1000); /* 1Hz update rate max. */ + if ((_t_actuators < 0) || (_t_actuator_armed < 0) || (_t_vehicle_control_mode < 0) || - (_t_param < 0)) { + (_t_param < 0) || + (_t_vehicle_command < 0)) { log("subscription(s) failed"); goto out; } /* poll descriptor */ - pollfd fds[4]; + pollfd fds[5]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_actuator_armed; @@ -750,8 +756,10 @@ PX4IO::task_main() fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; + fds[4].fd = _t_vehicle_command; + fds[4].events = POLLIN; - debug("ready"); + log("ready"); /* lock against the ioctl handler */ lock(); @@ -791,6 +799,24 @@ PX4IO::task_main() if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) io_set_arming_state(); + /* if we have a vehicle command, handle it */ + if (fds[4].revents & POLLIN) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { + if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if ((cmd.param2 == 0.0f) || (cmd.param2 == 1.0f)) { + mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", (cmd.param2 == 0.0f) == 1 ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, (cmd.param2 == 0.0f) ? 3 : 7); + } else { + mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); + } + } else { + mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); + } + } + } + /* * If it's time for another tick of the polling status machine, * try it now. |