AntennaTracker/GCS_Mavlink.cpp

   1 #include "GCS_Mavlink.h"
   2 #include "Tracker.h"
   3
   4 MAV_TYPE GCS_Tracker::frame_type() const
   5 {
   6     return MAV_TYPE_ANTENNA_TRACKER;
   7 }
   8
   9 MAV_MODE GCS_MAVLINK_Tracker::base_mode() const
  10 {
  11     uint8_t _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
  12     // work out the base_mode. This value is not very useful
  13     // for APM, but we calculate it as best we can so a generic
  14     // MAVLink enabled ground station can work out something about
  15     // what the MAV is up to. The actual bit values are highly
  16     // ambiguous for most of the APM flight modes. In practice, you
  17     // only get useful information from the custom_mode, which maps to
  18     // the APM flight mode and has a well defined meaning in the
  19     // ArduPlane documentation
  20     switch (tracker.mode->number()) {
  21     case Mode::Number::MANUAL:
  22         _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
  23         break;
  24
  25     case Mode::Number::STOP:
  26         break;
  27
  28     case Mode::Number::SCAN:
  29     case Mode::Number::SERVOTEST:
  30     case Mode::Number::AUTO:
  31     case Mode::Number::GUIDED:
  32         _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED |
  33             MAV_MODE_FLAG_STABILIZE_ENABLED;
  34         // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
  35         // APM does in any mode, as that is defined as "system finds its own goal
  36         // positions", which APM does not currently do
  37         break;
  38
  39     case Mode::Number::INITIALISING:
  40         break;
  41     }
  42
  43     // we are armed if safety switch is not disarmed
  44     if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED &&
  45         tracker.mode != &tracker.mode_initialising &&
  46         hal.util->get_soft_armed()) {
  47         _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
  48     }
  49
  50     return (MAV_MODE)_base_mode;
  51 }
  52
  53 uint32_t GCS_Tracker::custom_mode() const
  54 {
  55     return (uint32_t)tracker.mode->number();
  56 }
  57
  58 MAV_STATE GCS_MAVLINK_Tracker::vehicle_system_status() const
  59 {
  60     if (tracker.mode == &tracker.mode_initialising) {
  61         return MAV_STATE_CALIBRATING;
  62     }
  63     return MAV_STATE_ACTIVE;
  64 }
  65
  66 void GCS_MAVLINK_Tracker::send_nav_controller_output() const
  67 {
  68         float alt_diff = (tracker.g.alt_source == ALT_SOURCE_BARO) ? tracker.nav_status.alt_difference_baro : tracker.nav_status.alt_difference_gps;
  69
  70     mavlink_msg_nav_controller_output_send(
  71         chan,
  72         0,
  73         tracker.nav_status.pitch,
  74         tracker.nav_status.bearing,
  75         tracker.nav_status.bearing,
  76         MIN(tracker.nav_status.distance, UINT16_MAX),
  77         alt_diff,
  78         0,
  79         0);
  80 }
  81
  82 void GCS_MAVLINK_Tracker::handle_set_attitude_target(const mavlink_message_t &msg)
  83 {
  84     // decode packet
  85     mavlink_set_attitude_target_t packet;
  86     mavlink_msg_set_attitude_target_decode(&msg, &packet);
  87
  88     // exit if vehicle is not in Guided mode
  89     if (tracker.mode != &tracker.mode_guided) {
  90         return;
  91     }
  92
  93     // sanity checks:
  94     if (!is_zero(packet.body_roll_rate)) {
  95         return;
  96     }
  97     if (!(packet.type_mask & (1<<0))) {
  98         // not told to ignore body roll rate
  99         return;
 100     }
 101     if (!(packet.type_mask & (1<<6))) {
 102         // not told to ignore throttle
 103         return;
 104     }
 105     if (packet.type_mask & (1<<7)) {
 106         // told to ignore attitude (we don't allow continuous motion yet)
 107         return;
 108     }
 109     if ((packet.type_mask & (1<<3)) && (packet.type_mask&(1<<4))) {
 110         // told to ignore both pitch and yaw rates - nothing to do?!
 111         return;
 112     }
 113
 114     const bool use_yaw_rate = !(packet.type_mask & (1<<2));
 115
 116     tracker.mode_guided.set_angle(
 117         Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
 118         use_yaw_rate,
 119         packet.body_yaw_rate);
 120 }
 121
 122 /*
 123   send PID tuning message
 124  */
 125 void GCS_MAVLINK_Tracker::send_pid_tuning()
 126 {
 127     const Parameters &g = tracker.g;
 128
 129     // Pitch PID
 130     if (g.gcs_pid_mask & 1) {
 131         const AP_PIDInfo *pid_info = &g.pidPitch2Srv.get_pid_info();
 132         mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
 133                                     pid_info->target,
 134                                     pid_info->actual,
 135                                     pid_info->FF,
 136                                     pid_info->P,
 137                                     pid_info->I,
 138                                     pid_info->D,
 139                                     pid_info->slew_rate,
 140                                     pid_info->Dmod);
 141         if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
 142             return;
 143         }
 144     }
 145
 146     // Yaw PID
 147     if (g.gcs_pid_mask & 2) {
 148         const AP_PIDInfo *pid_info = &g.pidYaw2Srv.get_pid_info();
 149         mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
 150                                     pid_info->target,
 151                                     pid_info->actual,
 152                                     pid_info->FF,
 153                                     pid_info->P,
 154                                     pid_info->I,
 155                                     pid_info->D,
 156                                     pid_info->slew_rate,
 157                                     pid_info->Dmod);
 158         if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
 159             return;
 160         }
 161     }
 162 }
 163
 164 /*
 165   default stream rates to 1Hz
 166  */
 167 const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
 168     // @Param: RAW_SENS
 169     // @DisplayName: Raw sensor stream rate
 170     // @Description: Raw sensor stream rate to ground station
 171     // @Units: Hz
 172     // @Range: 0 50
 173     // @Increment: 1
 174     // @RebootRequired: True
 175     // @User: Advanced
 176     AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0],  1),
 177
 178     // @Param: EXT_STAT
 179     // @DisplayName: Extended status stream rate to ground station
 180     // @Description: Extended status stream rate to ground station
 181     // @Units: Hz
 182     // @Range: 0 50
 183     // @Increment: 1
 184     // @RebootRequired: True
 185     // @User: Advanced
 186     AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1],  1),
 187
 188     // @Param: RC_CHAN
 189     // @DisplayName: RC Channel stream rate to ground station
 190     // @Description: RC Channel stream rate to ground station
 191     // @Units: Hz
 192     // @Range: 0 50
 193     // @Increment: 1
 194     // @RebootRequired: True
 195     // @User: Advanced
 196     AP_GROUPINFO("RC_CHAN",  2, GCS_MAVLINK_Parameters, streamRates[2],  1),
 197
 198     // @Param: RAW_CTRL
 199     // @DisplayName: Raw Control stream rate to ground station
 200     // @Description: Raw Control stream rate to ground station
 201     // @Units: Hz
 202     // @Range: 0 50
 203     // @Increment: 1
 204     // @RebootRequired: True
 205     // @User: Advanced
 206     AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3],  1),
 207
 208     // @Param: POSITION
 209     // @DisplayName: Position stream rate to ground station
 210     // @Description: Position stream rate to ground station
 211     // @Units: Hz
 212     // @Range: 0 50
 213     // @Increment: 1
 214     // @RebootRequired: True
 215     // @User: Advanced
 216     AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4],  1),
 217
 218     // @Param: EXTRA1
 219     // @DisplayName: Extra data type 1 stream rate to ground station
 220     // @Description: Extra data type 1 stream rate to ground station
 221     // @Units: Hz
 222     // @Range: 0 50
 223     // @Increment: 1
 224     // @RebootRequired: True
 225     // @User: Advanced
 226     AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK_Parameters, streamRates[5],  1),
 227
 228     // @Param: EXTRA2
 229     // @DisplayName: Extra data type 2 stream rate to ground station
 230     // @Description: Extra data type 2 stream rate to ground station
 231     // @Units: Hz
 232     // @Range: 0 50
 233     // @Increment: 1
 234     // @RebootRequired: True
 235     // @User: Advanced
 236     AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK_Parameters, streamRates[6],  1),
 237
 238     // @Param: EXTRA3
 239     // @DisplayName: Extra data type 3 stream rate to ground station
 240     // @Description: Extra data type 3 stream rate to ground station
 241     // @Units: Hz
 242     // @Range: 0 50
 243     // @Increment: 1
 244     // @RebootRequired: True
 245     // @User: Advanced
 246     AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK_Parameters, streamRates[7],  1),
 247
 248     // @Param: PARAMS
 249     // @DisplayName: Parameter stream rate to ground station
 250     // @Description: Parameter stream rate to ground station
 251     // @Units: Hz
 252     // @Range: 0 50
 253     // @Increment: 1
 254     // @RebootRequired: True
 255     // @User: Advanced
 256     AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK_Parameters, streamRates[8],  10),
 257     AP_GROUPEND
 258 };
 259
 260 static const ap_message STREAM_RAW_SENSORS_msgs[] = {
 261     MSG_RAW_IMU,
 262     MSG_SCALED_IMU2,
 263     MSG_SCALED_IMU3,
 264     MSG_SCALED_PRESSURE,
 265     MSG_SCALED_PRESSURE2,
 266     MSG_SCALED_PRESSURE3,
 267 };
 268 static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
 269     MSG_SYS_STATUS,
 270     MSG_POWER_STATUS,
 271 #if HAL_WITH_MCU_MONITORING
 272     MSG_MCU_STATUS,
 273 #endif
 274     MSG_MEMINFO,
 275     MSG_NAV_CONTROLLER_OUTPUT,
 276     MSG_GPS_RAW,
 277     MSG_GPS_RTK,
 278 #if GPS_MAX_RECEIVERS > 1
 279     MSG_GPS2_RAW,
 280     MSG_GPS2_RTK,
 281 #endif
 282 };
 283 static const ap_message STREAM_POSITION_msgs[] = {
 284     MSG_LOCATION,
 285     MSG_LOCAL_POSITION
 286 };
 287 static const ap_message STREAM_RAW_CONTROLLER_msgs[] = {
 288     MSG_SERVO_OUTPUT_RAW,
 289 };
 290 static const ap_message STREAM_RC_CHANNELS_msgs[] = {
 291     MSG_RC_CHANNELS,
 292     MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
 293 };
 294 static const ap_message STREAM_EXTRA1_msgs[] = {
 295     MSG_ATTITUDE,
 296     MSG_PID_TUNING,
 297 };
 298 static const ap_message STREAM_EXTRA3_msgs[] = {
 299     MSG_AHRS,
 300 #if AP_SIM_ENABLED
 301     MSG_SIMSTATE,
 302 #endif
 303     MSG_SYSTEM_TIME,
 304     MSG_AHRS2,
 305 #if COMPASS_CAL_ENABLED
 306     MSG_MAG_CAL_REPORT,
 307     MSG_MAG_CAL_PROGRESS,
 308 #endif
 309     MSG_EKF_STATUS_REPORT,
 310     MSG_BATTERY_STATUS,
 311 };
 312 static const ap_message STREAM_PARAMS_msgs[] = {
 313     MSG_NEXT_PARAM
 314 };
 315
 316 const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
 317     MAV_STREAM_ENTRY(STREAM_RAW_SENSORS),
 318     MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS),
 319     MAV_STREAM_ENTRY(STREAM_POSITION),
 320     MAV_STREAM_ENTRY(STREAM_RAW_CONTROLLER),
 321     MAV_STREAM_ENTRY(STREAM_RC_CHANNELS),
 322     MAV_STREAM_ENTRY(STREAM_EXTRA1),
 323     MAV_STREAM_ENTRY(STREAM_EXTRA3),
 324     MAV_STREAM_ENTRY(STREAM_PARAMS),
 325     MAV_STREAM_TERMINATOR // must have this at end of stream_entries
 326 };
 327
 328 /*
 329   We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and
 330   MAVLINK_MSG_ID_SCALED_PRESSUREs
 331 */
 332 void GCS_MAVLINK_Tracker::packetReceived(const mavlink_status_t &status,
 333                                          const mavlink_message_t &msg)
 334 {
 335     // return immediately if sysid doesn't match our target sysid
 336     if ((tracker.g.sysid_target != 0) && (tracker.g.sysid_target != msg.sysid)) {
 337         GCS_MAVLINK::packetReceived(status, msg);
 338         return;
 339     }
 340
 341     switch (msg.msgid) {
 342     case MAVLINK_MSG_ID_HEARTBEAT:
 343     {
 344         mavlink_check_target(msg);
 345         break;
 346     }
 347
 348     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
 349     {
 350         // decode
 351         mavlink_global_position_int_t packet;
 352         mavlink_msg_global_position_int_decode(&msg, &packet);
 353         tracker.tracking_update_position(packet);
 354         break;
 355     }
 356
 357     case MAVLINK_MSG_ID_SCALED_PRESSURE:
 358     {
 359         // decode
 360         mavlink_scaled_pressure_t packet;
 361         mavlink_msg_scaled_pressure_decode(&msg, &packet);
 362         tracker.tracking_update_pressure(packet);
 363         break;
 364     }
 365     }
 366     GCS_MAVLINK::packetReceived(status, msg);
 367 }
 368
 369 // locks onto a particular target sysid and sets it's position data stream to at least 1hz
 370 void GCS_MAVLINK_Tracker::mavlink_check_target(const mavlink_message_t &msg)
 371 {
 372     // exit immediately if the target has already been set
 373     if (tracker.target_set) {
 374         return;
 375     }
 376
 377     // decode
 378     mavlink_heartbeat_t packet;
 379     mavlink_msg_heartbeat_decode(&msg, &packet);
 380
 381     // exit immediately if this is not a vehicle we would track
 382     if ((packet.type == MAV_TYPE_ANTENNA_TRACKER) ||
 383         (packet.type == MAV_TYPE_GCS) ||
 384         (packet.type == MAV_TYPE_ONBOARD_CONTROLLER) ||
 385         (packet.type == MAV_TYPE_GIMBAL)) {
 386         return;
 387     }
 388
 389     // set our sysid to the target, this ensures we lock onto a single vehicle
 390     if (tracker.g.sysid_target == 0) {
 391         tracker.g.sysid_target.set(msg.sysid);
 392     }
 393
 394     // send data stream request to target on all channels
 395     //  Note: this doesn't check success for all sends meaning it's not guaranteed the vehicle's positions will be sent at 1hz
 396     tracker.gcs().request_datastream_position(msg.sysid, msg.compid);
 397     tracker.gcs().request_datastream_airpressure(msg.sysid, msg.compid);
 398
 399     // flag target has been set
 400     tracker.target_set = true;
 401 }
 402
 403 uint8_t GCS_MAVLINK_Tracker::sysid_my_gcs() const
 404 {
 405     return tracker.g.sysid_my_gcs;
 406 }
 407
 408 MAV_RESULT GCS_MAVLINK_Tracker::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg)
 409 {
 410     MAV_RESULT ret = GCS_MAVLINK::_handle_command_preflight_calibration_baro(msg);
 411     if (ret == MAV_RESULT_ACCEPTED) {
 412         // zero the altitude difference on next baro update
 413         tracker.nav_status.need_altitude_calibration = true;
 414     }
 415     return ret;
 416 }
 417
 418 MAV_RESULT GCS_MAVLINK_Tracker::handle_command_component_arm_disarm(const mavlink_command_int_t &packet)
 419 {
 420     if (is_equal(packet.param1,1.0f)) {
 421         tracker.arm_servos();
 422         return MAV_RESULT_ACCEPTED;
 423     }
 424     if (is_zero(packet.param1))  {
 425         tracker.disarm_servos();
 426         return MAV_RESULT_ACCEPTED;
 427     }
 428     return MAV_RESULT_UNSUPPORTED;
 429 }
 430
 431 MAV_RESULT GCS_MAVLINK_Tracker::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
 432 {
 433     switch(packet.command) {
 434
 435     case MAV_CMD_DO_SET_SERVO:
 436         // ensure we are in servo test mode
 437         tracker.set_mode(tracker.mode_servotest, ModeReason::SERVOTEST);
 438
 439         if (!tracker.mode_servotest.set_servo(packet.param1, packet.param2)) {
 440             return MAV_RESULT_FAILED;
 441         }
 442         return MAV_RESULT_ACCEPTED;
 443
 444         // mavproxy/mavutil sends this when auto command is entered
 445     case MAV_CMD_MISSION_START:
 446         tracker.set_mode(tracker.mode_auto, ModeReason::GCS_COMMAND);
 447         return MAV_RESULT_ACCEPTED;
 448
 449     default:
 450         return GCS_MAVLINK::handle_command_int_packet(packet, msg);
 451     }
 452 }
 453
 454 void GCS_MAVLINK_Tracker::handle_message(const mavlink_message_t &msg)
 455 {
 456     switch (msg.msgid) {
 457
 458     case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
 459         handle_set_attitude_target(msg);
 460         break;
 461
 462 #if AP_TRACKER_SET_HOME_VIA_MISSION_UPLOAD_ENABLED
 463     // When mavproxy 'wp sethome'
 464     case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
 465         handle_message_mission_write_partial_list(msg);
 466         break;
 467
 468     // XXX receive a WP from GCS and store in EEPROM if it is HOME
 469     case MAVLINK_MSG_ID_MISSION_ITEM:
 470         handle_message_mission_item(msg);
 471         break;
 472 #endif
 473
 474     case MAVLINK_MSG_ID_MANUAL_CONTROL:
 475         handle_message_manual_control(msg);
 476         break;
 477
 478     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
 479         handle_message_global_position_int(msg);
 480         break;
 481
 482     case MAVLINK_MSG_ID_SCALED_PRESSURE:
 483         handle_message_scaled_pressure(msg);
 484         break;
 485     }
 486
 487     GCS_MAVLINK::handle_message(msg);
 488 }
 489
 490
 491 #if AP_TRACKER_SET_HOME_VIA_MISSION_UPLOAD_ENABLED
 492 void GCS_MAVLINK_Tracker::handle_message_mission_write_partial_list(const mavlink_message_t &msg)
 493 {
 494         // decode
 495         mavlink_mission_write_partial_list_t packet;
 496         mavlink_msg_mission_write_partial_list_decode(&msg, &packet);
 497         if (packet.start_index == 0)
 498         {
 499             // New home at wp index 0. Ask for it
 500             waypoint_receiving = true;
 501             send_message(MSG_NEXT_MISSION_REQUEST_WAYPOINTS);
 502         }
 503 }
 504
 505 void GCS_MAVLINK_Tracker::handle_message_mission_item(const mavlink_message_t &msg)
 506 {
 507         mavlink_mission_item_t packet;
 508         MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED;
 509
 510         mavlink_msg_mission_item_decode(&msg, &packet);
 511
 512         Location tell_command;
 513
 514         switch (packet.frame)
 515         {
 516         case MAV_FRAME_MISSION:
 517         case MAV_FRAME_GLOBAL:
 518         {
 519             tell_command = Location{
 520                 int32_t(1.0e7f*packet.x), // in as DD converted to * t7
 521                 int32_t(1.0e7f*packet.y), // in as DD converted to * t7
 522                 int32_t(packet.z*1.0e2f), // in as m converted to cm
 523                 Location::AltFrame::ABSOLUTE
 524             };
 525             break;
 526         }
 527
 528 #ifdef MAV_FRAME_LOCAL_NED
 529         case MAV_FRAME_LOCAL_NED:                         // local (relative to home position)
 530         {
 531             tell_command = Location{
 532                 int32_t(1.0e7f*ToDeg(packet.x/(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat),
 533                 int32_t(1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng),
 534                 int32_t(-packet.z*1.0e2f),
 535                 Location::AltFrame::ABOVE_HOME
 536             };
 537             break;
 538         }
 539 #endif
 540
 541 #ifdef MAV_FRAME_LOCAL
 542         case MAV_FRAME_LOCAL:                         // local (relative to home position)
 543         {
 544             tell_command = {
 545                 int32_t(1.0e7f*ToDeg(packet.x/(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat),
 546                 int32_t(1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng),
 547                 int32_t(packet.z*1.0e2f),
 548                 Location::AltFrame::ABOVE_HOME
 549             };
 550             break;
 551         }
 552 #endif
 553
 554         case MAV_FRAME_GLOBAL_RELATIVE_ALT:                         // absolute lat/lng, relative altitude
 555         {
 556             tell_command = {
 557                 int32_t(1.0e7f * packet.x), // in as DD converted to * t7
 558                 int32_t(1.0e7f * packet.y), // in as DD converted to * t7
 559                 int32_t(packet.z * 1.0e2f),
 560                 Location::AltFrame::ABOVE_HOME
 561             };
 562             break;
 563         }
 564
 565         default:
 566             result = MAV_MISSION_UNSUPPORTED_FRAME;
 567             break;
 568         }
 569
 570         if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
 571
 572         // Check if receiving waypoints (mission upload expected)
 573         if (!waypoint_receiving) {
 574             result = MAV_MISSION_ERROR;
 575             goto mission_failed;
 576         }
 577
 578         // check if this is the HOME wp
 579         if (packet.seq == 0) {
 580             if (!tracker.set_home(tell_command, false)) {
 581                 result = MAV_MISSION_ERROR;
 582                 goto mission_failed;
 583             }
 584             send_text(MAV_SEVERITY_INFO,"New HOME received");
 585             waypoint_receiving = false;
 586         }
 587
 588 mission_failed:
 589         // send ACK (including in success case)
 590         mavlink_msg_mission_ack_send(
 591             chan,
 592             msg.sysid,
 593             msg.compid,
 594             result,
 595             MAV_MISSION_TYPE_MISSION);
 596 }
 597 #endif
 598
 599 void GCS_MAVLINK_Tracker::handle_message_manual_control(const mavlink_message_t &msg)
 600 {
 601         mavlink_manual_control_t packet;
 602         mavlink_msg_manual_control_decode(&msg, &packet);
 603         tracker.tracking_manual_control(packet);
 604 }
 605
 606 void GCS_MAVLINK_Tracker::handle_message_global_position_int(const mavlink_message_t &msg)
 607 {
 608         // decode
 609         mavlink_global_position_int_t packet;
 610         mavlink_msg_global_position_int_decode(&msg, &packet);
 611         tracker.tracking_update_position(packet);
 612 }
 613
 614 void GCS_MAVLINK_Tracker::handle_message_scaled_pressure(const mavlink_message_t &msg)
 615 {
 616         mavlink_scaled_pressure_t packet;
 617         mavlink_msg_scaled_pressure_decode(&msg, &packet);
 618         tracker.tracking_update_pressure(packet);
 619 }
 620
 621 // send position tracker is using
 622 void GCS_MAVLINK_Tracker::send_global_position_int()
 623 {
 624     if (!tracker.stationary) {
 625         GCS_MAVLINK::send_global_position_int();
 626         return;
 627     }
 628
 629     mavlink_msg_global_position_int_send(
 630         chan,
 631         AP_HAL::millis(),
 632         tracker.current_loc.lat,  // in 1E7 degrees
 633         tracker.current_loc.lng,  // in 1E7 degrees
 634         tracker.current_loc.alt,  // millimeters above ground/sea level
 635         0,                        // millimeters above home
 636         0,                        // X speed cm/s (+ve North)
 637         0,                        // Y speed cm/s (+ve East)
 638         0,                        // Z speed cm/s (+ve Down)
 639         tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree
 640 }