Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Randy Mackay 2015-04-27 16:44:32 +09:00
parent 10b9f2ebe0
commit 94737e4df6
3 changed files with 68 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
AntennaTracker/GCS_Mavlink.pde
View File

@ -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) {

4
AntennaTracker/Parameters.h
View File

@ -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;

7
AntennaTracker/Parameters.pde
View File

@ -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.