dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Implement message based receiver pairing

This commit is contained in:
Jean Cyr 2013-09-11 22:11:37 -04:00
parent 5ece19f66a
commit 3b8039e4e0
2 changed files with 31 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/drivers/px4io/px4io.cpp
View File

@ -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.

3
src/modules/uORB/topics/vehicle_command.h
View File

@ -86,7 +86,8 @@ enum VEHICLE_CMD
VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */
VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */
VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */
VEHICLE_CMD_ENUM_END=401, /* | */
VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */
VEHICLE_CMD_ENUM_END=501, /* | */
};
/**