fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 11:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cleaned up target system handling for some commands

Browse Source
This commit is contained in:
Lorenz Meier
2016-02-11 11:58:06 +01:00
parent a8e3194dfa
commit ebeb9eba4b
2 changed files with 28 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+26 -17
View File
@@ -279,6 +279,27 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
_mavlink->set_has_received_messages(true); _mavlink->set_has_received_messages(true);
} }
bool
MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_component)
{
/* evaluate if this system should accept this command */
bool target_ok = false;
switch (command) {
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
/* broadcast and ignore component */
target_ok = (target_system == 0) || (target_system == mavlink_system.sysid);
break;
default:
target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid)
|| (target_component == MAV_COMP_ID_ALL));
break;
}
return target_ok;
}
void void
MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
{ {
@@ -286,22 +307,9 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
mavlink_command_long_t cmd_mavlink; mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink); mavlink_msg_command_long_decode(msg, &cmd_mavlink);
/* evaluate if this system should accept this command */ bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
bool target_ok;
switch (cmd_mavlink.command) {
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: if (target_ok) {
/* broadcast */
target_ok = (cmd_mavlink.target_system == 0);
break;
default:
target_ok = (cmd_mavlink.target_system == mavlink_system.sysid);
break;
}
if (target_ok && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command //check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */ /* This is the link shutdown command, terminate mavlink */
@@ -362,8 +370,9 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
mavlink_command_int_t cmd_mavlink; mavlink_command_int_t cmd_mavlink;
mavlink_msg_command_int_decode(msg, &cmd_mavlink); mavlink_msg_command_int_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
if (target_ok) {
//check for MAVLINK terminate command //check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */ /* This is the link shutdown command, terminate mavlink */
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -161,6 +161,8 @@ private:
*/ */
int decode_switch_pos_n(uint16_t buttons, unsigned sw); int decode_switch_pos_n(uint16_t buttons, unsigned sw);
bool evaluate_target_ok(int command, int target_system, int target_component);
mavlink_status_t status; mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos; struct vehicle_local_position_s hil_local_pos;
struct vehicle_land_detected_s hil_land_detector; struct vehicle_land_detected_s hil_land_detector;