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_MAVLINK)
  33
  34 #include "common/maths.h"
  35 #include "common/axis.h"
  36 #include "common/color.h"
  37 #include "common/utils.h"
  38
  39 #include "config/feature.h"
  40 #include "pg/pg.h"
  41 #include "pg/pg_ids.h"
  42 #include "pg/rx.h"
  43
  44 #include "drivers/accgyro/accgyro.h"
  45 #include "drivers/sensor.h"
  46 #include "drivers/time.h"
  47
  48 #include "config/config.h"
  49 #include "fc/rc_controls.h"
  50 #include "fc/runtime_config.h"
  51
  52 #include "flight/mixer.h"
  53 #include "flight/pid.h"
  54 #include "flight/imu.h"
  55 #include "flight/failsafe.h"
  56 #include "flight/position.h"
  57
  58 #include "io/serial.h"
  59 #include "io/gimbal.h"
  60 #include "io/gps.h"
  61 #include "io/ledstrip.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 const 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 ARRAYLEN(mavRates)
 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 static void mavlinkSerialWrite(uint8_t * buf, uint16_t length)
 133 {
 134     for (int i = 0; i < length; i++)
 135         serialWrite(mavlinkPort, buf[i]);
 136 }
 137
 138 static int16_t headingOrScaledMilliAmpereHoursDrawn(void)
 139 {
 140     if (isAmperageConfigured() && telemetryConfig()->mavlink_mah_as_heading_divisor > 0) {
 141         // In the Connex Prosight OSD, this goes between 0 and 999, so it will need to be scaled in that range.
 142         return getMAhDrawn() / telemetryConfig()->mavlink_mah_as_heading_divisor;
 143     }
 144     // heading Current heading in degrees, in compass units (0..360, 0=north)
 145     return DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 146 }
 147
 148 void freeMAVLinkTelemetryPort(void)
 149 {
 150     closeSerialPort(mavlinkPort);
 151     mavlinkPort = NULL;
 152     mavlinkTelemetryEnabled = false;
 153 }
 154
 155 void initMAVLinkTelemetry(void)
 156 {
 157     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK);
 158     mavlinkPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_MAVLINK);
 159 }
 160
 161 void configureMAVLinkTelemetryPort(void)
 162 {
 163     if (!portConfig) {
 164         return;
 165     }
 166
 167     baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
 168     if (baudRateIndex == BAUD_AUTO) {
 169         // default rate for minimOSD
 170         baudRateIndex = BAUD_57600;
 171     }
 172
 173     mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
 174
 175     if (!mavlinkPort) {
 176         return;
 177     }
 178
 179     mavlinkTelemetryEnabled = true;
 180 }
 181
 182 void checkMAVLinkTelemetryState(void)
 183 {
 184     if (portConfig && telemetryCheckRxPortShared(portConfig, rxRuntimeState.serialrxProvider)) {
 185         if (!mavlinkTelemetryEnabled && telemetrySharedPort != NULL) {
 186             mavlinkPort = telemetrySharedPort;
 187             mavlinkTelemetryEnabled = true;
 188         }
 189     } else {
 190         bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing);
 191
 192         if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) {
 193             return;
 194         }
 195
 196         if (newTelemetryEnabledValue)
 197             configureMAVLinkTelemetryPort();
 198         else
 199             freeMAVLinkTelemetryPort();
 200     }
 201 }
 202
 203 void mavlinkSendSystemStatus(void)
 204 {
 205     uint16_t msgLength;
 206
 207     uint32_t onboardControlAndSensors = 35843;
 208
 209     /*
 210     onboard_control_sensors_present Bitmask
 211     fedcba9876543210
 212     1000110000000011    For all   = 35843
 213     0001000000000100    With Mag  = 4100
 214     0010000000001000    With Baro = 8200
 215     0100000000100000    With GPS  = 16416
 216     0000001111111111
 217     */
 218
 219     if (sensors(SENSOR_MAG))  onboardControlAndSensors |=  4100;
 220     if (sensors(SENSOR_BARO)) onboardControlAndSensors |=  8200;
 221     if (sensors(SENSOR_GPS))  onboardControlAndSensors |= 16416;
 222
 223     uint16_t batteryVoltage = 0;
 224     int16_t batteryAmperage = -1;
 225     int8_t batteryRemaining = 100;
 226
 227     if (getBatteryState() < BATTERY_NOT_PRESENT) {
 228         batteryVoltage = isBatteryVoltageConfigured() ? getBatteryVoltage() * 10 : batteryVoltage;
 229         batteryAmperage = isAmperageConfigured() ? getAmperage() : batteryAmperage;
 230         batteryRemaining = isBatteryVoltageConfigured() ? calculateBatteryPercentageRemaining() : batteryRemaining;
 231     }
 232
 233     mavlink_msg_sys_status_pack(0, 200, &mavMsg,
 234         // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present.
 235         //Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure,
 236         // 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position,
 237         // 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization,
 238         // 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 239         onboardControlAndSensors,
 240         // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled
 241         onboardControlAndSensors,
 242         // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error.
 243         onboardControlAndSensors & 1023,
 244         // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
 245         0,
 246         // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
 247         batteryVoltage,
 248         // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
 249         batteryAmperage,
 250         // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
 251         batteryRemaining,
 252         // 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)
 253         0,
 254         // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
 255         0,
 256         // errors_count1 Autopilot-specific errors
 257         0,
 258         // errors_count2 Autopilot-specific errors
 259         0,
 260         // errors_count3 Autopilot-specific errors
 261         0,
 262         // errors_count4 Autopilot-specific errors
 263         0);
 264     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 265     mavlinkSerialWrite(mavBuffer, msgLength);
 266 }
 267
 268 void mavlinkSendRCChannelsAndRSSI(void)
 269 {
 270     uint16_t msgLength;
 271     mavlink_msg_rc_channels_raw_pack(0, 200, &mavMsg,
 272         // time_boot_ms Timestamp (milliseconds since system boot)
 273         millis(),
 274         // 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.
 275         0,
 276         // chan1_raw RC channel 1 value, in microseconds
 277         (rxRuntimeState.channelCount >= 1) ? rcData[0] : 0,
 278         // chan2_raw RC channel 2 value, in microseconds
 279         (rxRuntimeState.channelCount >= 2) ? rcData[1] : 0,
 280         // chan3_raw RC channel 3 value, in microseconds
 281         (rxRuntimeState.channelCount >= 3) ? rcData[2] : 0,
 282         // chan4_raw RC channel 4 value, in microseconds
 283         (rxRuntimeState.channelCount >= 4) ? rcData[3] : 0,
 284         // chan5_raw RC channel 5 value, in microseconds
 285         (rxRuntimeState.channelCount >= 5) ? rcData[4] : 0,
 286         // chan6_raw RC channel 6 value, in microseconds
 287         (rxRuntimeState.channelCount >= 6) ? rcData[5] : 0,
 288         // chan7_raw RC channel 7 value, in microseconds
 289         (rxRuntimeState.channelCount >= 7) ? rcData[6] : 0,
 290         // chan8_raw RC channel 8 value, in microseconds
 291         (rxRuntimeState.channelCount >= 8) ? rcData[7] : 0,
 292         // rssi Receive signal strength indicator, 0: 0%, 254: 100%
 293         scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 254));
 294     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 295     mavlinkSerialWrite(mavBuffer, msgLength);
 296 }
 297
 298 #if defined(USE_GPS)
 299 void mavlinkSendPosition(void)
 300 {
 301     uint16_t msgLength;
 302     uint8_t gpsFixType = 0;
 303
 304     if (!sensors(SENSOR_GPS))
 305         return;
 306
 307     if (!STATE(GPS_FIX)) {
 308         gpsFixType = 1;
 309     }
 310     else {
 311         if (gpsSol.numSat < GPS_MIN_SAT_COUNT) {
 312             gpsFixType = 2;
 313         }
 314         else {
 315             gpsFixType = 3;
 316         }
 317     }
 318
 319     mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg,
 320         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
 321         micros(),
 322         // 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.
 323         gpsFixType,
 324         // lat Latitude in 1E7 degrees
 325         gpsSol.llh.lat,
 326         // lon Longitude in 1E7 degrees
 327         gpsSol.llh.lon,
 328         // alt Altitude in 1E3 meters (millimeters) above MSL
 329         gpsSol.llh.altCm * 10,
 330         // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 331         65535,
 332         // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 333         65535,
 334         // vel GPS ground speed (m/s * 100). If unknown, set to: 65535
 335         gpsSol.groundSpeed,
 336         // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
 337         gpsSol.groundCourse * 10,
 338         // satellites_visible Number of satellites visible. If unknown, set to 255
 339         gpsSol.numSat);
 340     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 341     mavlinkSerialWrite(mavBuffer, msgLength);
 342
 343     // Global position
 344     mavlink_msg_global_position_int_pack(0, 200, &mavMsg,
 345         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
 346         micros(),
 347         // lat Latitude in 1E7 degrees
 348         gpsSol.llh.lat,
 349         // lon Longitude in 1E7 degrees
 350         gpsSol.llh.lon,
 351         // alt Altitude in 1E3 meters (millimeters) above MSL
 352         gpsSol.llh.altCm * 10,
 353         // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
 354         getEstimatedAltitudeCm() * 10,
 355         // Ground X Speed (Latitude), expressed as m/s * 100
 356         0,
 357         // Ground Y Speed (Longitude), expressed as m/s * 100
 358         0,
 359         // Ground Z Speed (Altitude), expressed as m/s * 100
 360         0,
 361         // heading Current heading in degrees, in compass units (0..360, 0=north)
 362         headingOrScaledMilliAmpereHoursDrawn()
 363     );
 364     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 365     mavlinkSerialWrite(mavBuffer, msgLength);
 366
 367     mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg,
 368         // latitude Latitude (WGS84), expressed as * 1E7
 369         GPS_home_llh.lat,
 370         // longitude Longitude (WGS84), expressed as * 1E7
 371         GPS_home_llh.lon,
 372         // altitude Altitude(WGS84), expressed as * 1000
 373         0);
 374     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 375     mavlinkSerialWrite(mavBuffer, msgLength);
 376 }
 377 #endif
 378
 379 void mavlinkSendAttitude(void)
 380 {
 381     uint16_t msgLength;
 382     mavlink_msg_attitude_pack(0, 200, &mavMsg,
 383         // time_boot_ms Timestamp (milliseconds since system boot)
 384         millis(),
 385         // roll Roll angle (rad)
 386         DECIDEGREES_TO_RADIANS(attitude.values.roll),
 387         // pitch Pitch angle (rad)
 388         DECIDEGREES_TO_RADIANS(-attitude.values.pitch),
 389         // yaw Yaw angle (rad)
 390         DECIDEGREES_TO_RADIANS(attitude.values.yaw),
 391         // rollspeed Roll angular speed (rad/s)
 392         0,
 393         // pitchspeed Pitch angular speed (rad/s)
 394         0,
 395         // yawspeed Yaw angular speed (rad/s)
 396         0);
 397     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 398     mavlinkSerialWrite(mavBuffer, msgLength);
 399 }
 400
 401 void mavlinkSendHUDAndHeartbeat(void)
 402 {
 403     uint16_t msgLength;
 404     float mavAltitude = 0;
 405     float mavGroundSpeed = 0;
 406     float mavAirSpeed = 0;
 407     float mavClimbRate = 0;
 408
 409 #if defined(USE_GPS)
 410     // use ground speed if source available
 411     if (sensors(SENSOR_GPS)) {
 412         mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
 413     }
 414 #endif
 415
 416     mavAltitude = getEstimatedAltitudeCm() / 100.0;
 417
 418     mavlink_msg_vfr_hud_pack(0, 200, &mavMsg,
 419         // airspeed Current airspeed in m/s
 420         mavAirSpeed,
 421         // groundspeed Current ground speed in m/s
 422         mavGroundSpeed,
 423         // heading Current heading in degrees, in compass units (0..360, 0=north)
 424         headingOrScaledMilliAmpereHoursDrawn(),
 425         // throttle Current throttle setting in integer percent, 0 to 100
 426         scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100),
 427         // alt Current altitude (MSL), in meters, if we have sonar or baro use them, otherwise use GPS (less accurate)
 428         mavAltitude,
 429         // climb Current climb rate in meters/second
 430         mavClimbRate);
 431     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 432     mavlinkSerialWrite(mavBuffer, msgLength);
 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_OCTOX8P:
 457         case MIXER_OCTOFLATP:
 458         case MIXER_OCTOFLATX:
 459             mavSystemType = MAV_TYPE_OCTOROTOR;
 460             break;
 461         case MIXER_FLYING_WING:
 462         case MIXER_AIRPLANE:
 463         case MIXER_CUSTOM_AIRPLANE:
 464             mavSystemType = MAV_TYPE_FIXED_WING;
 465             break;
 466         case MIXER_HELI_120_CCPM:
 467         case MIXER_HELI_90_DEG:
 468             mavSystemType = MAV_TYPE_HELICOPTER;
 469             break;
 470         default:
 471             mavSystemType = MAV_TYPE_GENERIC;
 472             break;
 473     }
 474
 475     // Custom mode for compatibility with APM OSDs
 476     uint8_t mavCustomMode = 1;  // Acro by default
 477
 478     if (FLIGHT_MODE(ANGLE_MODE | HORIZON_MODE | ALT_HOLD_MODE | POS_HOLD_MODE)) {
 479         mavCustomMode = 0;      //Stabilize
 480         mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
 481     }
 482
 483     uint8_t mavSystemState = 0;
 484     if (ARMING_FLAG(ARMED)) {
 485         if (failsafeIsActive()) {
 486             mavSystemState = MAV_STATE_CRITICAL;
 487         }
 488         else {
 489             mavSystemState = MAV_STATE_ACTIVE;
 490         }
 491     }
 492     else {
 493         mavSystemState = MAV_STATE_STANDBY;
 494     }
 495
 496     mavlink_msg_heartbeat_pack(0, 200, &mavMsg,
 497         // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 498         mavSystemType,
 499         // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
 500         MAV_AUTOPILOT_GENERIC,
 501         // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
 502         mavModes,
 503         // custom_mode A bitfield for use for autopilot-specific flags.
 504         mavCustomMode,
 505         // system_status System status flag, see MAV_STATE ENUM
 506         mavSystemState);
 507     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
 508     mavlinkSerialWrite(mavBuffer, msgLength);
 509 }
 510
 511 void processMAVLinkTelemetry(void)
 512 {
 513     // is executed @ TELEMETRY_MAVLINK_MAXRATE rate
 514     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
 515         mavlinkSendSystemStatus();
 516     }
 517
 518     if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS)) {
 519         mavlinkSendRCChannelsAndRSSI();
 520     }
 521
 522 #ifdef USE_GPS
 523     if (mavlinkStreamTrigger(MAV_DATA_STREAM_POSITION)) {
 524         mavlinkSendPosition();
 525     }
 526 #endif
 527
 528     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA1)) {
 529         mavlinkSendAttitude();
 530     }
 531
 532     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) {
 533         mavlinkSendHUDAndHeartbeat();
 534     }
 535 }
 536
 537 void handleMAVLinkTelemetry(void)
 538 {
 539     if (!mavlinkTelemetryEnabled) {
 540         return;
 541     }
 542
 543     if (!mavlinkPort) {
 544         return;
 545     }
 546
 547     uint32_t now = micros();
 548     if ((now - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
 549         processMAVLinkTelemetry();
 550         lastMavlinkMessage = now;
 551     }
 552 }
 553
 554 #endif