src/main/telemetry/mavlink.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 /*
  19  * telemetry_mavlink.c
  20  *
  21  * Author: Konstantin Sharlaimov
  22  */
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <string.h>
  26
  27 #include "platform.h"
  28
  29 #if defined(TELEMETRY) && defined(TELEMETRY_MAVLINK)
  30
  31 #include "common/maths.h"
  32 #include "common/axis.h"
  33 #include "common/color.h"
  34
  35 #include "config/feature.h"
  36 #include "config/parameter_group.h"
  37 #include "config/parameter_group_ids.h"
  38
  39 #include "drivers/system.h"
  40 #include "drivers/sensor.h"
  41 #include "drivers/accgyro.h"
  42
  43 #include "fc/config.h"
  44 #include "fc/rc_controls.h"
  45 #include "fc/runtime_config.h"
  46
  47 #include "flight/mixer.h"
  48 #include "flight/pid.h"
  49 #include "flight/imu.h"
  50 #include "flight/failsafe.h"
  51 #include "flight/navigation.h"
  52 #include "flight/altitude.h"
  53
  54 #include "io/serial.h"
  55 #include "io/gimbal.h"
  56 #include "io/gps.h"
  57 #include "io/ledstrip.h"
  58 #include "io/motors.h"
  59
  60 #include "rx/rx.h"
  61
  62 #include "sensors/sensors.h"
  63 #include "sensors/acceleration.h"
  64 #include "sensors/gyro.h"
  65 #include "sensors/barometer.h"
  66 #include "sensors/boardalignment.h"
  67 #include "sensors/battery.h"
  68
  69 #include "telemetry/telemetry.h"
  70 #include "telemetry/mavlink.h"
  71
  72 // mavlink library uses unnames unions that's causes GCC to complain if -Wpedantic is used
  73 // until this is resolved in mavlink library - ignore -Wpedantic for mavlink code
  74 #pragma GCC diagnostic push
  75 #pragma GCC diagnostic ignored "-Wpedantic"
  76 #include "common/mavlink.h"
  77 #pragma GCC diagnostic pop
  78
  79 #define TELEMETRY_MAVLINK_INITIAL_PORT_MODE MODE_TX
  80 #define TELEMETRY_MAVLINK_MAXRATE 50
  81 #define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE)
  82
  83 extern uint16_t rssi; // FIXME dependency on mw.c
  84
  85 static serialPort_t *mavlinkPort = NULL;
  86 static serialPortConfig_t *portConfig;
  87
  88 static bool mavlinkTelemetryEnabled =  false;
  89 static portSharing_e mavlinkPortSharing;
  90
  91 /* MAVLink datastream rates in Hz */
  92 static const uint8_t mavRates[] = {
  93     [MAV_DATA_STREAM_EXTENDED_STATUS] = 2, //2Hz
  94     [MAV_DATA_STREAM_RC_CHANNELS] = 5, //5Hz
  95     [MAV_DATA_STREAM_POSITION] = 2, //2Hz
  96     [MAV_DATA_STREAM_EXTRA1] = 10, //10Hz
  97     [MAV_DATA_STREAM_EXTRA2] = 10 //2Hz
  98 };
  99
 100 #define MAXSTREAMS (sizeof(mavRates) / sizeof(mavRates[0]))
 101
 102 static uint8_t mavTicks[MAXSTREAMS];
 103 static mavlink_message_t mavMsg;
 104 static uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN];
 105 static uint32_t lastMavlinkMessage = 0;
 106
 107 static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
 108 {
 109     uint8_t rate = (uint8_t) mavRates[streamNum];
 110     if (rate == 0) {
 111         return 0;
 112     }
 113
 114     if (mavTicks[streamNum] == 0) {
 115         // we're triggering now, setup the next trigger point
 116         if (rate > TELEMETRY_MAVLINK_MAXRATE) {
 117             rate = TELEMETRY_MAVLINK_MAXRATE;
 118         }
 119
 120         mavTicks[streamNum] = (TELEMETRY_MAVLINK_MAXRATE / rate);
 121         return 1;
 122     }
 123
 124     // count down at TASK_RATE_HZ
 125     mavTicks[streamNum]--;
 126     return 0;
 127 }
 128
 129
 130 static void mavlinkSerialWrite(uint8_t * buf, uint16_t length)
 131 {
 132     for (int i = 0; i < length; i++)
 133         serialWrite(mavlinkPort, buf[i]);
 134 }
 135
 136 void freeMAVLinkTelemetryPort(void)
 137 {
 138     closeSerialPort(mavlinkPort);
 139     mavlinkPort = NULL;
 140     mavlinkTelemetryEnabled = false;
 141 }
 142
 143 void initMAVLinkTelemetry(void)
 144 {
 145     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK);
 146     mavlinkPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_MAVLINK);
 147 }
 148
 149 void configureMAVLinkTelemetryPort(void)
 150 {
 151     if (!portConfig) {
 152         return;
 153     }
 154
 155     baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
 156     if (baudRateIndex == BAUD_AUTO) {
 157         // default rate for minimOSD
 158         baudRateIndex = BAUD_57600;
 159     }
 160
 161     mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
 162
 163     if (!mavlinkPort) {
 164         return;
 165     }
 166
 167     mavlinkTelemetryEnabled = true;
 168 }
 169
 170 void checkMAVLinkTelemetryState(void)
 171 {
 172     if (portConfig && telemetryCheckRxPortShared(portConfig)) {
 173         if (!mavlinkTelemetryEnabled && telemetrySharedPort != NULL) {
 174             mavlinkPort = telemetrySharedPort;
 175             mavlinkTelemetryEnabled = true;
 176         }
 177     } else {
 178         bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing);
 179
 180         if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) {
 181             return;
 182         }
 183
 184         if (newTelemetryEnabledValue)
 185             configureMAVLinkTelemetryPort();
 186         else
 187             freeMAVLinkTelemetryPort();
 188     }
 189 }
 190
 191 void mavlinkSendSystemStatus(void)
 192 {
 193     uint16_t msgLength;
 194
 195     uint32_t onboardControlAndSensors = 35843;
 196
 197     /*
 198     onboard_control_sensors_present Bitmask
 199     fedcba9876543210
 200     1000110000000011    For all   = 35843
 201     0001000000000100    With Mag  = 4100
 202     0010000000001000    With Baro = 8200
 203     0100000000100000    With GPS  = 16416
 204     0000001111111111
 205     */
 206
 207     if (sensors(SENSOR_MAG))  onboardControlAndSensors |=  4100;
 208     if (sensors(SENSOR_BARO)) onboardControlAndSensors |=  8200;
 209     if (sensors(SENSOR_GPS))  onboardControlAndSensors |= 16416;
 210
 211     mavlink_msg_sys_status_pack(0, 200, &mavMsg,
 212         // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present.
 213         //Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure,
 214         // 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position,
 215         // 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization,
 216         // 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 217         onboardControlAndSensors,
 218         // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled
 219         onboardControlAndSensors,
 220         // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error.
 221         onboardControlAndSensors & 1023,
 222         // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
 223         0,
 224         // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
 225         (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) ? getBatteryVoltage() * 100 : 0,
 226         // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
 227         (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) ? getAmperage() : -1,
 228         // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
 229         (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) ? calculateBatteryPercentageRemaining() : 100,
 230         // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
 231         0,
 232         // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
 233         0,
 234         // errors_count1 Autopilot-specific errors
 235         0,
 236         // errors_count2 Autopilot-specific errors
 237         0,
 238         // errors_count3 Autopilot-specific errors
 239         0,
 240         // errors_count4 Autopilot-specific errors
 241         0);
 242     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 243     mavlinkSerialWrite(mavBuffer, msgLength);
 244 }
 245
 246 void mavlinkSendRCChannelsAndRSSI(void)
 247 {
 248     uint16_t msgLength;
 249     mavlink_msg_rc_channels_raw_pack(0, 200, &mavMsg,
 250         // time_boot_ms Timestamp (milliseconds since system boot)
 251         millis(),
 252         // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
 253         0,
 254         // chan1_raw RC channel 1 value, in microseconds
 255         (rxRuntimeConfig.channelCount >= 1) ? rcData[0] : 0,
 256         // chan2_raw RC channel 2 value, in microseconds
 257         (rxRuntimeConfig.channelCount >= 2) ? rcData[1] : 0,
 258         // chan3_raw RC channel 3 value, in microseconds
 259         (rxRuntimeConfig.channelCount >= 3) ? rcData[2] : 0,
 260         // chan4_raw RC channel 4 value, in microseconds
 261         (rxRuntimeConfig.channelCount >= 4) ? rcData[3] : 0,
 262         // chan5_raw RC channel 5 value, in microseconds
 263         (rxRuntimeConfig.channelCount >= 5) ? rcData[4] : 0,
 264         // chan6_raw RC channel 6 value, in microseconds
 265         (rxRuntimeConfig.channelCount >= 6) ? rcData[5] : 0,
 266         // chan7_raw RC channel 7 value, in microseconds
 267         (rxRuntimeConfig.channelCount >= 7) ? rcData[6] : 0,
 268         // chan8_raw RC channel 8 value, in microseconds
 269         (rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0,
 270         // rssi Receive signal strength indicator, 0: 0%, 255: 100%
 271         scaleRange(rssi, 0, 1023, 0, 255));
 272     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 273     mavlinkSerialWrite(mavBuffer, msgLength);
 274 }
 275
 276 #if defined(GPS)
 277 void mavlinkSendPosition(void)
 278 {
 279     uint16_t msgLength;
 280     uint8_t gpsFixType = 0;
 281
 282     if (!sensors(SENSOR_GPS))
 283         return;
 284
 285     if (!STATE(GPS_FIX)) {
 286         gpsFixType = 1;
 287     }
 288     else {
 289         if (GPS_numSat < 5) {
 290             gpsFixType = 2;
 291         }
 292         else {
 293             gpsFixType = 3;
 294         }
 295     }
 296
 297     mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg,
 298         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
 299         micros(),
 300         // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
 301         gpsFixType,
 302         // lat Latitude in 1E7 degrees
 303         GPS_coord[LAT],
 304         // lon Longitude in 1E7 degrees
 305         GPS_coord[LON],
 306         // alt Altitude in 1E3 meters (millimeters) above MSL
 307         GPS_altitude * 1000,
 308         // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 309         65535,
 310         // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 311         65535,
 312         // vel GPS ground speed (m/s * 100). If unknown, set to: 65535
 313         GPS_speed,
 314         // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
 315         GPS_ground_course * 10,
 316         // satellites_visible Number of satellites visible. If unknown, set to 255
 317         GPS_numSat);
 318     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 319     mavlinkSerialWrite(mavBuffer, msgLength);
 320
 321     // Global position
 322     mavlink_msg_global_position_int_pack(0, 200, &mavMsg,
 323         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
 324         micros(),
 325         // lat Latitude in 1E7 degrees
 326         GPS_coord[LAT],
 327         // lon Longitude in 1E7 degrees
 328         GPS_coord[LON],
 329         // alt Altitude in 1E3 meters (millimeters) above MSL
 330         GPS_altitude * 1000,
 331         // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
 332 #if defined(BARO) || defined(SONAR)
 333         (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : GPS_altitude * 1000,
 334 #else
 335         GPS_altitude * 1000,
 336 #endif
 337         // Ground X Speed (Latitude), expressed as m/s * 100
 338         0,
 339         // Ground Y Speed (Longitude), expressed as m/s * 100
 340         0,
 341         // Ground Z Speed (Altitude), expressed as m/s * 100
 342         0,
 343         // heading Current heading in degrees, in compass units (0..360, 0=north)
 344         DECIDEGREES_TO_DEGREES(attitude.values.yaw)
 345     );
 346     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 347     mavlinkSerialWrite(mavBuffer, msgLength);
 348
 349     mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg,
 350         // latitude Latitude (WGS84), expressed as * 1E7
 351         GPS_home[LAT],
 352         // longitude Longitude (WGS84), expressed as * 1E7
 353         GPS_home[LON],
 354         // altitude Altitude(WGS84), expressed as * 1000
 355         0);
 356     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 357     mavlinkSerialWrite(mavBuffer, msgLength);
 358 }
 359 #endif
 360
 361 void mavlinkSendAttitude(void)
 362 {
 363     uint16_t msgLength;
 364     mavlink_msg_attitude_pack(0, 200, &mavMsg,
 365         // time_boot_ms Timestamp (milliseconds since system boot)
 366         millis(),
 367         // roll Roll angle (rad)
 368         DECIDEGREES_TO_RADIANS(attitude.values.roll),
 369         // pitch Pitch angle (rad)
 370         DECIDEGREES_TO_RADIANS(-attitude.values.pitch),
 371         // yaw Yaw angle (rad)
 372         DECIDEGREES_TO_RADIANS(attitude.values.yaw),
 373         // rollspeed Roll angular speed (rad/s)
 374         0,
 375         // pitchspeed Pitch angular speed (rad/s)
 376         0,
 377         // yawspeed Yaw angular speed (rad/s)
 378         0);
 379     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 380     mavlinkSerialWrite(mavBuffer, msgLength);
 381 }
 382
 383 void mavlinkSendHUDAndHeartbeat(void)
 384 {
 385     uint16_t msgLength;
 386     float mavAltitude = 0;
 387     float mavGroundSpeed = 0;
 388     float mavAirSpeed = 0;
 389     float mavClimbRate = 0;
 390
 391 #if defined(GPS)
 392     // use ground speed if source available
 393     if (sensors(SENSOR_GPS)) {
 394         mavGroundSpeed = GPS_speed / 100.0f;
 395     }
 396 #endif
 397
 398     // select best source for altitude
 399 #if defined(BARO) || defined(SONAR)
 400     if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
 401         // Baro or sonar generally is a better estimate of altitude than GPS MSL altitude
 402         mavAltitude = getEstimatedAltitude() / 100.0;
 403     }
 404 #if defined(GPS)
 405     else if (sensors(SENSOR_GPS)) {
 406         // No sonar or baro, just display altitude above MLS
 407         mavAltitude = GPS_altitude;
 408     }
 409 #endif
 410 #elif defined(GPS)
 411     if (sensors(SENSOR_GPS)) {
 412         // No sonar or baro, just display altitude above MLS
 413         mavAltitude = GPS_altitude;
 414     }
 415 #endif
 416
 417     mavlink_msg_vfr_hud_pack(0, 200, &mavMsg,
 418         // airspeed Current airspeed in m/s
 419         mavAirSpeed,
 420         // groundspeed Current ground speed in m/s
 421         mavGroundSpeed,
 422         // heading Current heading in degrees, in compass units (0..360, 0=north)
 423         DECIDEGREES_TO_DEGREES(attitude.values.yaw),
 424         // throttle Current throttle setting in integer percent, 0 to 100
 425         scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100),
 426         // alt Current altitude (MSL), in meters, if we have sonar or baro use them, otherwise use GPS (less accurate)
 427         mavAltitude,
 428         // climb Current climb rate in meters/second
 429         mavClimbRate);
 430     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 431     mavlinkSerialWrite(mavBuffer, msgLength);
 432
 433
 434     uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 435     if (ARMING_FLAG(ARMED))
 436         mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
 437
 438     uint8_t mavSystemType;
 439     switch(mixerConfig()->mixerMode)
 440     {
 441         case MIXER_TRI:
 442             mavSystemType = MAV_TYPE_TRICOPTER;
 443             break;
 444         case MIXER_QUADP:
 445         case MIXER_QUADX:
 446         case MIXER_Y4:
 447         case MIXER_VTAIL4:
 448             mavSystemType = MAV_TYPE_QUADROTOR;
 449             break;
 450         case MIXER_Y6:
 451         case MIXER_HEX6:
 452         case MIXER_HEX6X:
 453             mavSystemType = MAV_TYPE_HEXAROTOR;
 454             break;
 455         case MIXER_OCTOX8:
 456         case MIXER_OCTOFLATP:
 457         case MIXER_OCTOFLATX:
 458             mavSystemType = MAV_TYPE_OCTOROTOR;
 459             break;
 460         case MIXER_FLYING_WING:
 461         case MIXER_AIRPLANE:
 462         case MIXER_CUSTOM_AIRPLANE:
 463             mavSystemType = MAV_TYPE_FIXED_WING;
 464             break;
 465         case MIXER_HELI_120_CCPM:
 466         case MIXER_HELI_90_DEG:
 467             mavSystemType = MAV_TYPE_HELICOPTER;
 468             break;
 469         default:
 470             mavSystemType = MAV_TYPE_GENERIC;
 471             break;
 472     }
 473
 474     // Custom mode for compatibility with APM OSDs
 475     uint8_t mavCustomMode = 1;  // Acro by default
 476
 477     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 478         mavCustomMode = 0;      //Stabilize
 479         mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
 480     }
 481     if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE))
 482         mavCustomMode = 2;      //Alt Hold
 483     if (FLIGHT_MODE(GPS_HOME_MODE))
 484         mavCustomMode = 6;      //Return to Launch
 485     if (FLIGHT_MODE(GPS_HOLD_MODE))
 486         mavCustomMode = 16;     //Position Hold (Earlier called Hybrid)
 487
 488     uint8_t mavSystemState = 0;
 489     if (ARMING_FLAG(ARMED)) {
 490         if (failsafeIsActive()) {
 491             mavSystemState = MAV_STATE_CRITICAL;
 492         }
 493         else {
 494             mavSystemState = MAV_STATE_ACTIVE;
 495         }
 496     }
 497     else {
 498         mavSystemState = MAV_STATE_STANDBY;
 499     }
 500
 501     mavlink_msg_heartbeat_pack(0, 200, &mavMsg,
 502         // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 503         mavSystemType,
 504         // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
 505         MAV_AUTOPILOT_GENERIC,
 506         // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
 507         mavModes,
 508         // custom_mode A bitfield for use for autopilot-specific flags.
 509         mavCustomMode,
 510         // system_status System status flag, see MAV_STATE ENUM
 511         mavSystemState);
 512     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 513     mavlinkSerialWrite(mavBuffer, msgLength);
 514 }
 515
 516 void processMAVLinkTelemetry(void)
 517 {
 518     // is executed @ TELEMETRY_MAVLINK_MAXRATE rate
 519     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
 520         mavlinkSendSystemStatus();
 521     }
 522
 523     if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS)) {
 524         mavlinkSendRCChannelsAndRSSI();
 525     }
 526
 527 #ifdef GPS
 528     if (mavlinkStreamTrigger(MAV_DATA_STREAM_POSITION)) {
 529         mavlinkSendPosition();
 530     }
 531 #endif
 532
 533     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA1)) {
 534         mavlinkSendAttitude();
 535     }
 536
 537     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) {
 538         mavlinkSendHUDAndHeartbeat();
 539     }
 540 }
 541
 542 void handleMAVLinkTelemetry(void)
 543 {
 544     if (!mavlinkTelemetryEnabled) {
 545         return;
 546     }
 547
 548     if (!mavlinkPort) {
 549         return;
 550     }
 551
 552     uint32_t now = micros();
 553     if ((now - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
 554         processMAVLinkTelemetry();
 555         lastMavlinkMessage = now;
 556     }
 557 }
 558
 559 #endif