From 7e3daacda098e49a7e96fa9bc6c634d2a1098c08 Mon Sep 17 00:00:00 2001 From: Mike McCauley Date: Wed, 14 May 2014 12:11:22 +1000 Subject: [PATCH] AntennaTracker: add support for PROXY_MODE --- Tools/AntennaTracker/AntennaTracker.pde | 4 + Tools/AntennaTracker/GCS_Mavlink.pde | 173 +++++++++++++++++++++++- Tools/AntennaTracker/Parameters.h | 2 + Tools/AntennaTracker/Parameters.pde | 8 ++ Tools/AntennaTracker/system.pde | 7 +- 5 files changed, 186 insertions(+), 8 deletions(-) diff --git a/Tools/AntennaTracker/AntennaTracker.pde b/Tools/AntennaTracker/AntennaTracker.pde index 56590451dd..c9142e8b83 100644 --- a/Tools/AntennaTracker/AntennaTracker.pde +++ b/Tools/AntennaTracker/AntennaTracker.pde @@ -194,6 +194,10 @@ static RC_Channel channel_pitch(CH_2); static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; +// If proxy_mode is enabled, uartC is connected to a remote vehicle, +// not gcs[1] +static GCS_MAVLINK proxy_vehicle; + // board specific config static AP_BoardConfig BoardConfig; diff --git a/Tools/AntennaTracker/GCS_Mavlink.pde b/Tools/AntennaTracker/GCS_Mavlink.pde index 3f0895148b..4b4e464186 100644 --- a/Tools/AntennaTracker/GCS_Mavlink.pde +++ b/Tools/AntennaTracker/GCS_Mavlink.pde @@ -528,14 +528,73 @@ GCS_MAVLINK::data_stream_send(void) } } - - void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { + if (g.proxy_mode == true) + { + if (chan == proxy_vehicle.chan) + { + // From the remote vehicle. + // All messages from the remote are proxied to GCS + // We also eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and MAVLINK_MSG_ID_SCALED_PRESSUREs + + switch (msg->msgid) + { + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + { + // decode + mavlink_global_position_int_t packet; + mavlink_msg_global_position_int_decode(msg, &packet); + tracking_update_position(packet); + break; + } + + case MAVLINK_MSG_ID_SCALED_PRESSURE: + { + // decode + mavlink_scaled_pressure_t packet; + mavlink_msg_scaled_pressure_decode(msg, &packet); + tracking_update_pressure(packet); + break; + } + } + // Proxy to all the GCS stations + for (uint8_t i=0; i ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) { + _mavlink_resend_uart(out_chan, msg); + } + } + } + } + // Else its from the GCS, and it might be for the remote and.or it might be for the tracker + // So we fall through to the below + } + + switch (msg->msgid) { + // If we are currently operating as a proxy for a remote, + // alas we have to look inside each packet to see if its for us or for the remote case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { + if (g.proxy_mode == true && proxy_vehicle.initialised) + { + // See if its for the remote + mavlink_request_data_stream_t packet; + mavlink_msg_request_data_stream_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + { + // Not for us, must be for the remote + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + break; + } + // Else its for us, the tracker + } handle_request_data_stream(msg, true); break; } @@ -543,24 +602,75 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + if (g.proxy_mode == true && proxy_vehicle.initialised) + { + // See if its for the remote + mavlink_param_request_list_t packet; + mavlink_msg_param_request_list_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + { + // Not for us, must be for the remote + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + break; + } + // Else its for us, the tracker + } handle_param_request_list(msg); break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + if (g.proxy_mode == true && proxy_vehicle.initialised) + { + // See if its for the remote + mavlink_param_request_read_t packet; + mavlink_msg_param_request_read_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + { + // Not for us, must be for the remote + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + break; + } + // Else its for us, the tracker + } handle_param_request_read(msg); break; } case MAVLINK_MSG_ID_PARAM_SET: { + if (g.proxy_mode == true && proxy_vehicle.initialised) + { + // See if its for the remote + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + { + // Not for us, must be for the remote + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + break; + } + // Else its for us, the tracker + } handle_param_set(msg, NULL); break; } case MAVLINK_MSG_ID_HEARTBEAT: { + // Heartbeats are always proxied to the remote and also handled locally + if (g.proxy_mode == true && proxy_vehicle.initialised) + { + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + } if (msg->sysid != g.sysid_my_gcs) break; break; } @@ -570,7 +680,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) break; + if (mavlink_check_target(packet.target_system, packet.target_component)) { + // Its for the remote, proxy it + if (g.proxy_mode == true && proxy_vehicle.initialised) { + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + } + break; + } uint8_t result = MAV_RESULT_UNSUPPORTED; @@ -674,8 +791,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_mission_write_partial_list_t packet; mavlink_msg_mission_write_partial_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - + if (mavlink_check_target(packet.target_system,packet.target_component)) { + if (g.proxy_mode == true && proxy_vehicle.initialised) { + // Its for the remote, proxy it + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + } + break; + } if (packet.start_index == 0) { // New home at wp index 0. Ask for it @@ -696,7 +819,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) uint8_t result = MAV_MISSION_ACCEPTED; mavlink_msg_mission_item_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + if (mavlink_check_target(packet.target_system,packet.target_component)) { + if (g.proxy_mode == true && proxy_vehicle.initialised) { + // Its for the remote, proxy it + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + } + break; + } struct Location tell_command = {}; @@ -777,9 +907,15 @@ mission_failed: case MAVLINK_MSG_ID_MANUAL_CONTROL: { - if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); + + if (g.proxy_mode == true && proxy_vehicle.initialised) { + // Also proxy it to the remote + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + } + if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs tracking_manual_control(packet); break; } @@ -789,6 +925,11 @@ mission_failed: // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(msg, &packet); + if (g.proxy_mode == true && proxy_vehicle.initialised) { + // Also proxy it to the remote + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + } tracking_update_position(packet); break; } @@ -798,6 +939,11 @@ mission_failed: // decode mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(msg, &packet); + if (g.proxy_mode == true && proxy_vehicle.initialised) { + // Also proxy it to the remote + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + } tracking_update_pressure(packet); break; } @@ -807,6 +953,11 @@ mission_failed: // decode mavlink_set_mode_t packet; mavlink_msg_set_mode_decode(msg, &packet); + if (g.proxy_mode == true && proxy_vehicle.initialised) { + // Also proxy it to the remote + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + } if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) { // we ignore base_mode as there is no sane way to map @@ -833,6 +984,11 @@ mission_failed: #endif default: + // Proxy all other messages to the remote + if (g.proxy_mode && proxy_vehicle.initialised) { + if (comm_get_txspace(proxy_vehicle.chan) > ((uint16_t)msg->len) + MAVLINK_NUM_NON_PAYLOAD_BYTES) + _mavlink_resend_uart(proxy_vehicle.chan, msg); + } break; } // end switch @@ -905,6 +1061,9 @@ static void gcs_update(void) gcs[i].update(NULL); } } + // Also check for messages from the remote if we are in proxy mode + if (g.proxy_mode == true && proxy_vehicle.initialised) + proxy_vehicle.update(); } static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) diff --git a/Tools/AntennaTracker/Parameters.h b/Tools/AntennaTracker/Parameters.h index 933431ab74..54c5bc42dd 100644 --- a/Tools/AntennaTracker/Parameters.h +++ b/Tools/AntennaTracker/Parameters.h @@ -80,6 +80,7 @@ public: k_param_BoardConfig, k_param_gps, k_param_scan_speed, + k_param_proxy_mode, k_param_channel_yaw = 200, k_param_channel_pitch, @@ -116,6 +117,7 @@ public: AP_Float start_longitude; AP_Float startup_delay; + AP_Int8 proxy_mode; // Waypoints // diff --git a/Tools/AntennaTracker/Parameters.pde b/Tools/AntennaTracker/Parameters.pde index f72d2d996f..6d60416cc1 100644 --- a/Tools/AntennaTracker/Parameters.pde +++ b/Tools/AntennaTracker/Parameters.pde @@ -115,6 +115,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @Range: 0 10 GSCALAR(startup_delay, "STARTUP_DELAY", 0), + // @Param: PROXY_MODE + // @DisplayName: Also act as a MAVLink proxy for a vehicle + // @Description: If true, the tracker will act as a MAVlink proxy for a remote vehicle, and will eavesdrop vehicle position reports. + // @Units: boolean + // @Increment: 1 + // @Range: 0 1 + GSCALAR(proxy_mode, "PROXY_MODE", 0), + // barometer ground calibration. The GND_ prefix is chosen for // compatibility with previous releases of ArduPlane // @Group: GND_ diff --git a/Tools/AntennaTracker/system.pde b/Tools/AntennaTracker/system.pde index 26134318e1..f41b02beb4 100644 --- a/Tools/AntennaTracker/system.pde +++ b/Tools/AntennaTracker/system.pde @@ -35,7 +35,12 @@ static void init_tracker() check_usb_mux(); // we have a 2nd serial port for telemetry - gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE); + hal.uartC->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD), + 128, SERIAL1_BUFSIZE); + if (g.proxy_mode == true) + proxy_vehicle.init(hal.uartC); + else + gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE); mavlink_system.sysid = g.sysid_this_mav;