diff --git a/AntennaTracker/GCS_Mavlink.pde b/AntennaTracker/GCS_Mavlink.pde index 446ece3ebb..03175a94f0 100644 --- a/AntennaTracker/GCS_Mavlink.pde +++ b/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) { diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index 461edd4e2b..53698567e2 100644 --- a/AntennaTracker/Parameters.h +++ b/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; diff --git a/AntennaTracker/Parameters.pde b/AntennaTracker/Parameters.pde index cbff17d635..d403f04e18 100644 --- a/AntennaTracker/Parameters.pde +++ b/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.