AntennaTracker: add support for PROXY_MODE

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;
 

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<num_gcs; i++) {
+                if (gcs[i].initialised) {
+                    mavlink_channel_t out_chan = (mavlink_channel_t)i;
+                    // only forward if it would fit in the transmit buffer
+                    if (comm_get_txspace(out_chan) > ((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)

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
     //

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_

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;