From b91529622b11fd386544d4608e8ec754b02c2ee4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 10 Dec 2014 14:00:53 +1100 Subject: [PATCH] GCS_MAVLink: initial implementation of MAVLink routing object will be used to auto-route packets over all available links --- libraries/GCS_MAVLink/MAVLink_routing.cpp | 356 ++++++++++++++++++++++ libraries/GCS_MAVLink/MAVLink_routing.h | 56 ++++ 2 files changed, 412 insertions(+) create mode 100644 libraries/GCS_MAVLink/MAVLink_routing.cpp create mode 100644 libraries/GCS_MAVLink/MAVLink_routing.h diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp new file mode 100644 index 0000000000..522437ef84 --- /dev/null +++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp @@ -0,0 +1,356 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/// @file MAVLink_routing.h +/// @brief handle routing of MAVLink packets by sysid/componentid + +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define ROUTING_DEBUG 0 + +// constructor +MAVLink_routing::MAVLink_routing(void) : num_routes(0) {} + +/* + forward a MAVLink message to the right port. This also + automatically learns the route for the sender if it is not + already known. + + This returns true if the message matched a route and was + forwarded. +*/ +bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t* msg) +{ + // learn new routes + learn_route(in_channel, msg); + + if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) { + // heartbeat needs special handling + handle_heartbeat(in_channel, msg); + return false; + } + + // extract the targets for this packet + int16_t target_system =-1; + int16_t target_component =-1; + get_targets(msg, target_system, target_component); + + // see if it is for us + if ((target_system == -1 || target_system == mavlink_system.sysid) && + (target_component == -1 || target_component == mavlink_system.compid)) { + // it is for us + return false; + } + + // forward on any channels matching the targets + for (uint8_t i=0; i= ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) { +#if ROUTING_DEBUG + ::printf("fwd msg %u from chan %u on chan %u sysid=%u compid=%u\n", + msg->msgid, + (unsigned)in_channel, + (unsigned)routes[i].channel, + (unsigned)target_system, + (unsigned)target_component); +#endif + _mavlink_resend_uart(routes[i].channel, msg); + } + } + } + + return true; +} + +/* + see if the message is for a new route and learn it +*/ +void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_message_t* msg) +{ + uint8_t i; + for (i=0; isysid && + routes[i].compid == msg->compid) { + break; + } + } + if (i == num_routes && isysid; + routes[i].compid = msg->compid; + routes[i].channel = in_channel; + num_routes++; +#if ROUTING_DEBUG + ::printf("learned route %u %u via %u\n", + (unsigned)msg->sysid, + (unsigned)msg->compid, + (unsigned)in_channel); +#endif + } +} + + +/* + special handling for heartbeat messages. To ensure routing + propogation heartbeat messages need to be forwarded on all channels + except channels where the sysid/compid of the heartbeat could come from +*/ +void MAVLink_routing::handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t* msg) +{ + uint16_t mask = (1U<sysid && routes[i].compid == msg->compid) { + mask &= ~(1U<<((unsigned)(routes[i].channel-MAVLINK_COMM_0))); + } + } + + if (mask == 0) { + // nothing to send to + return; + } + + // send on the remaining channels + for (uint8_t i=0; i= ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) { +#if ROUTING_DEBUG + ::printf("fwd HB from chan %u on chan %u from sysid=%u compid=%u\n", + (unsigned)in_channel, + (unsigned)channel, + (unsigned)msg->sysid, + (unsigned)msg->compid); +#endif + _mavlink_resend_uart(channel, msg); + } + } + } +} + + +/* + extract target sysid and compid from a message. int16_t is used so + that the caller can set them to -1 and know when a sysid or compid + target is found in the message +*/ +void MAVLink_routing::get_targets(const mavlink_message_t* msg, int16_t &sysid, int16_t &compid) +{ + // unfortunately the targets are not in a consistent position in + // the packets, so we need a switch. Using the single element + // extraction functions (which are inline) makes this a bit faster + // than it would otherwise be. + // This list of messages below was extracted using: + // + // cat ardupilotmega/*h common/*h|egrep + // 'get_target_system|get_target_component' |grep inline | cut + // -d'(' -f1 | cut -d' ' -f4 > /tmp/targets.txt + // + // TODO: we should write a python script to extract this list + // properly + + switch (msg->msgid) { + // these messages only have a target system + case MAVLINK_MSG_ID_CAMERA_FEEDBACK: + sysid = mavlink_msg_camera_feedback_get_target_system(msg); + break; + case MAVLINK_MSG_ID_CAMERA_STATUS: + sysid = mavlink_msg_camera_status_get_target_system(msg); + break; + case MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL: + sysid = mavlink_msg_change_operator_control_get_target_system(msg); + break; + case MAVLINK_MSG_ID_SET_MODE: + sysid = mavlink_msg_set_mode_get_target_system(msg); + break; + case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN: + sysid = mavlink_msg_set_gps_global_origin_get_target_system(msg); + break; + + // these support both target system and target component + case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: + sysid = mavlink_msg_digicam_configure_get_target_system(msg); + compid = mavlink_msg_digicam_configure_get_target_component(msg); + break; + case MAVLINK_MSG_ID_DIGICAM_CONTROL: + sysid = mavlink_msg_digicam_control_get_target_system(msg); + compid = mavlink_msg_digicam_control_get_target_component(msg); + break; + case MAVLINK_MSG_ID_FENCE_FETCH_POINT: + sysid = mavlink_msg_fence_fetch_point_get_target_system(msg); + compid = mavlink_msg_fence_fetch_point_get_target_component(msg); + break; + case MAVLINK_MSG_ID_FENCE_POINT: + sysid = mavlink_msg_fence_point_get_target_system(msg); + compid = mavlink_msg_fence_point_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MOUNT_CONFIGURE: + sysid = mavlink_msg_mount_configure_get_target_system(msg); + compid = mavlink_msg_mount_configure_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MOUNT_CONTROL: + sysid = mavlink_msg_mount_control_get_target_system(msg); + compid = mavlink_msg_mount_control_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MOUNT_STATUS: + sysid = mavlink_msg_mount_status_get_target_system(msg); + compid = mavlink_msg_mount_status_get_target_component(msg); + break; + case MAVLINK_MSG_ID_RALLY_FETCH_POINT: + sysid = mavlink_msg_rally_fetch_point_get_target_system(msg); + compid = mavlink_msg_rally_fetch_point_get_target_component(msg); + break; + case MAVLINK_MSG_ID_RALLY_POINT: + sysid = mavlink_msg_rally_point_get_target_system(msg); + compid = mavlink_msg_rally_point_get_target_component(msg); + break; + case MAVLINK_MSG_ID_SET_MAG_OFFSETS: + sysid = mavlink_msg_set_mag_offsets_get_target_system(msg); + compid = mavlink_msg_set_mag_offsets_get_target_component(msg); + break; + case MAVLINK_MSG_ID_COMMAND_INT: + sysid = mavlink_msg_command_int_get_target_system(msg); + compid = mavlink_msg_command_int_get_target_component(msg); + break; + case MAVLINK_MSG_ID_COMMAND_LONG: + sysid = mavlink_msg_command_long_get_target_system(msg); + compid = mavlink_msg_command_long_get_target_component(msg); + break; + case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: + sysid = mavlink_msg_file_transfer_protocol_get_target_system(msg); + compid = mavlink_msg_file_transfer_protocol_get_target_component(msg); + break; + case MAVLINK_MSG_ID_GPS_INJECT_DATA: + sysid = mavlink_msg_gps_inject_data_get_target_system(msg); + compid = mavlink_msg_gps_inject_data_get_target_component(msg); + break; + case MAVLINK_MSG_ID_LOG_ERASE: + sysid = mavlink_msg_log_erase_get_target_system(msg); + compid = mavlink_msg_log_erase_get_target_component(msg); + break; + case MAVLINK_MSG_ID_LOG_REQUEST_DATA: + sysid = mavlink_msg_log_request_data_get_target_system(msg); + compid = mavlink_msg_log_request_data_get_target_component(msg); + break; + case MAVLINK_MSG_ID_LOG_REQUEST_END: + sysid = mavlink_msg_log_request_end_get_target_system(msg); + compid = mavlink_msg_log_request_end_get_target_component(msg); + break; + case MAVLINK_MSG_ID_LOG_REQUEST_LIST: + sysid = mavlink_msg_log_request_list_get_target_system(msg); + compid = mavlink_msg_log_request_list_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_ACK: + sysid = mavlink_msg_mission_ack_get_target_system(msg); + compid = mavlink_msg_mission_ack_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + sysid = mavlink_msg_mission_clear_all_get_target_system(msg); + compid = mavlink_msg_mission_clear_all_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_COUNT: + sysid = mavlink_msg_mission_count_get_target_system(msg); + compid = mavlink_msg_mission_count_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_ITEM: + sysid = mavlink_msg_mission_item_get_target_system(msg); + compid = mavlink_msg_mission_item_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_ITEM_INT: + sysid = mavlink_msg_mission_item_int_get_target_system(msg); + compid = mavlink_msg_mission_item_int_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_REQUEST: + sysid = mavlink_msg_mission_request_get_target_system(msg); + compid = mavlink_msg_mission_request_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + sysid = mavlink_msg_mission_request_list_get_target_system(msg); + compid = mavlink_msg_mission_request_list_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST: + sysid = mavlink_msg_mission_request_partial_list_get_target_system(msg); + compid = mavlink_msg_mission_request_partial_list_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + sysid = mavlink_msg_mission_set_current_get_target_system(msg); + compid = mavlink_msg_mission_set_current_get_target_component(msg); + break; + case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: + sysid = mavlink_msg_mission_write_partial_list_get_target_system(msg); + compid = mavlink_msg_mission_write_partial_list_get_target_component(msg); + break; + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + sysid = mavlink_msg_param_request_list_get_target_system(msg); + compid = mavlink_msg_param_request_list_get_target_component(msg); + break; + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: + sysid = mavlink_msg_param_request_read_get_target_system(msg); + compid = mavlink_msg_param_request_read_get_target_component(msg); + break; + case MAVLINK_MSG_ID_PARAM_SET: + sysid = mavlink_msg_param_set_get_target_system(msg); + compid = mavlink_msg_param_set_get_target_component(msg); + break; + case MAVLINK_MSG_ID_PING: + sysid = mavlink_msg_ping_get_target_system(msg); + compid = mavlink_msg_ping_get_target_component(msg); + break; + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + sysid = mavlink_msg_rc_channels_override_get_target_system(msg); + compid = mavlink_msg_rc_channels_override_get_target_component(msg); + break; + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + sysid = mavlink_msg_request_data_stream_get_target_system(msg); + compid = mavlink_msg_request_data_stream_get_target_component(msg); + break; + case MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA: + sysid = mavlink_msg_safety_set_allowed_area_get_target_system(msg); + compid = mavlink_msg_safety_set_allowed_area_get_target_component(msg); + break; + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + sysid = mavlink_msg_set_attitude_target_get_target_system(msg); + compid = mavlink_msg_set_attitude_target_get_target_component(msg); + break; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: + sysid = mavlink_msg_set_position_target_global_int_get_target_system(msg); + compid = mavlink_msg_set_position_target_global_int_get_target_component(msg); + break; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + sysid = mavlink_msg_set_position_target_local_ned_get_target_system(msg); + compid = mavlink_msg_set_position_target_local_ned_get_target_component(msg); + break; + case MAVLINK_MSG_ID_V2_EXTENSION: + sysid = mavlink_msg_v2_extension_get_target_system(msg); + compid = mavlink_msg_v2_extension_get_target_component(msg); + break; + } +} + diff --git a/libraries/GCS_MAVLink/MAVLink_routing.h b/libraries/GCS_MAVLink/MAVLink_routing.h new file mode 100644 index 0000000000..d5f960dc23 --- /dev/null +++ b/libraries/GCS_MAVLink/MAVLink_routing.h @@ -0,0 +1,56 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/// @file MAVLink_routing.h +/// @brief handle routing of MAVLink packets by ID + +#ifndef __MAVLINK_ROUTING_H +#define __MAVLINK_ROUTING_H + +#include +#include +#include + +// 5 routes should be enough for now. This will need to increase as we +// make more extensive use of MAVLink forwarding +#define MAVLINK_MAX_ROUTES 5 + +/* + object to handle MAVLink packet routing + */ +class MAVLink_routing +{ +public: + MAVLink_routing(void); + + /* + forward a MAVLink message to the right port. This also + automatically learns the route for the sender if it is not + already known. + + This returns true if the message matched a route and was + forwarded. + */ + bool check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t* msg); + +private: + // a simple linear routing table. We don't expect to have a lot of + // routes, so a scalable structure isn't worthwhile yet. + uint8_t num_routes; + struct route { + uint8_t sysid; + uint8_t compid; + mavlink_channel_t channel; + } routes[MAVLINK_MAX_ROUTES]; + + // learn new routes + void learn_route(mavlink_channel_t in_channel, const mavlink_message_t* msg); + + // extract target sysid and compid from a message + void get_targets(const mavlink_message_t* msg, int16_t &sysid, int16_t &compid); + + // special handling for heartbeat messages + void handle_heartbeat(mavlink_channel_t in_channel, const mavlink_message_t* msg); +}; + +#endif // __MAVLINK_ROUTING_H +