Tracker: add SYSID_TARGET and request 1hz pos data from vehicle

AntennaTracker/GCS_Mavlink.pde

@@ -457,7 +457,18 @@ GCS_MAVLINK::data_stream_send(void)
 */
 void mavlink_snoop(const mavlink_message_t* msg)
 {
+    // return immediately if sysid doesn't match our target sysid
+    if ((g.sysid_target != 0) && (g.sysid_target != msg->sysid)) {
+        return;
+    }
+
     switch (msg->msgid) {
+    case MAVLINK_MSG_ID_HEARTBEAT:
+    {
+        mavlink_check_target(msg);
+        break;
+    }
+
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
     {
         // decode
@@ -478,6 +489,53 @@ void mavlink_snoop(const mavlink_message_t* msg)
     }
 }
 
+// locks onto a particular target sysid and sets it's position data stream to at least 1hz
+void mavlink_check_target(const mavlink_message_t* msg)
+{
+    static bool target_set = false;
+
+    // exit immediately if the target has already been set
+    if (target_set) {
+        return;
+    }
+
+    // decode
+    mavlink_heartbeat_t packet;
+    mavlink_msg_heartbeat_decode(msg, &packet);
+
+    // exit immediately if this is not a vehicle we would track
+    if ((packet.type == MAV_TYPE_ANTENNA_TRACKER) ||
+        (packet.type == MAV_TYPE_GCS) ||
+        (packet.type == MAV_TYPE_ONBOARD_CONTROLLER) ||
+        (packet.type == MAV_TYPE_GIMBAL)) {
+        return;
+    }
+
+    // set our sysid to the target, this ensures we lock onto a single vehicle
+    if (g.sysid_target == 0) {
+        g.sysid_target = msg->sysid;
+    }
+
+    // send data stream request to target on all channels
+    //  Note: this doesn't check success for all sends meaning it's not guaranteed the vehicle's positions will be sent at 1hz
+    for (uint8_t i=0; i < num_gcs; i++) {
+        if (gcs[i].initialised) {
+            if (comm_get_txspace((mavlink_channel_t)i) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_DATA_STREAM_LEN) {
+                mavlink_msg_request_data_stream_send(
+                    (mavlink_channel_t)i,
+                    msg->sysid,
+                    msg->compid,
+                    MAV_DATA_STREAM_POSITION,
+                    1,  // 1hz
+                    1); // start streaming
+            }
+        }
+    }
+
+    // flag target has been set
+    target_set = true;
+}
+
 void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 {
     switch (msg->msgid) {

AntennaTracker/Parameters.h

@@ -90,7 +90,8 @@ public:
         k_param_pitch_trim,
         k_param_yaw_range,
         k_param_pitch_range,
-        k_param_distance_min,       // 137
+        k_param_distance_min,
+        k_param_sysid_target,       // 138
 
         //
         // 150: Telemetry control
@@ -118,6 +119,7 @@ public:
     //
     AP_Int16 sysid_this_mav;
     AP_Int16 sysid_my_gcs;
+    AP_Int16 sysid_target;
 
     AP_Int8 compass_enabled;
 

AntennaTracker/Parameters.pde

@@ -29,6 +29,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(sysid_my_gcs,           "SYSID_MYGCS",    255),
 
+    // @Param: SYSID_TARGET
+    // @DisplayName: Target vehicle's MAVLink system ID
+    // @Description: The identifier of the vehicle being tracked. This should be zero (to auto detect) or be the same as the SYSID_THISMAV parameter of the vehicle being tracked.
+    // @Range: 1 255
+    // @User: Advanced
+    GSCALAR(sysid_target,           "SYSID_TARGET",    0),
+
     // @Param: MAG_ENABLE
     // @DisplayName: Enable Compass
     // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1.