src/main/telemetry/mavlink.c

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