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 #include <math.h>
  27
  28 #include "platform.h"
  29
  30 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK)
  31
  32 #include "build/build_config.h"
  33 #include "build/debug.h"
  34
  35 #include "common/axis.h"
  36 #include "common/color.h"
  37 #include "common/maths.h"
  38 #include "common/utils.h"
  39 #include "common/string_light.h"
  40
  41 #include "config/feature.h"
  42
  43 #include "drivers/serial.h"
  44 #include "drivers/time.h"
  45 #include "drivers/display.h"
  46 #include "drivers/osd_symbols.h"
  47
  48 #include "fc/config.h"
  49 #include "fc/fc_core.h"
  50 #include "fc/rc_controls.h"
  51 #include "fc/rc_modes.h"
  52 #include "fc/runtime_config.h"
  53 #include "fc/settings.h"
  54
  55 #include "flight/failsafe.h"
  56 #include "flight/imu.h"
  57 #include "flight/mixer_profile.h"
  58 #include "flight/pid.h"
  59 #include "flight/servos.h"
  60
  61 #include "io/adsb.h"
  62 #include "io/gps.h"
  63 #include "io/ledstrip.h"
  64 #include "io/serial.h"
  65 #include "io/osd.h"
  66
  67 #include "navigation/navigation.h"
  68 #include "navigation/navigation_private.h"
  69
  70 #include "rx/rx.h"
  71 #include "rx/mavlink.h"
  72
  73 #include "sensors/acceleration.h"
  74 #include "sensors/barometer.h"
  75 #include "sensors/battery.h"
  76 #include "sensors/boardalignment.h"
  77 #include "sensors/gyro.h"
  78 #include "sensors/pitotmeter.h"
  79 #include "sensors/diagnostics.h"
  80 #include "sensors/sensors.h"
  81 #include "sensors/temperature.h"
  82 #include "sensors/esc_sensor.h"
  83
  84 #include "telemetry/mavlink.h"
  85 #include "telemetry/telemetry.h"
  86
  87 #include "blackbox/blackbox_io.h"
  88
  89 #include "scheduler/scheduler.h"
  90
  91 #pragma GCC diagnostic push
  92 #pragma GCC diagnostic ignored "-Wunused-function"
  93 #define MAVLINK_COMM_NUM_BUFFERS 1
  94 #include "common/mavlink.h"
  95 #pragma GCC diagnostic pop
  96
  97 #define TELEMETRY_MAVLINK_PORT_MODE     MODE_RXTX
  98 #define TELEMETRY_MAVLINK_MAXRATE       50
  99 #define TELEMETRY_MAVLINK_DELAY         ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE)
 100
 101 /**
 102  * MAVLink requires angles to be in the range -Pi..Pi.
 103  * This converts angles from a range of 0..Pi to -Pi..Pi
 104  */
 105 #define RADIANS_TO_MAVLINK_RANGE(angle) (angle > M_PIf) ? angle - (2 * M_PIf) : angle
 106
 107 /** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */
 108 typedef enum APM_PLANE_MODE
 109 {
 110    PLANE_MODE_MANUAL=0,
 111    PLANE_MODE_CIRCLE=1,
 112    PLANE_MODE_STABILIZE=2,
 113    PLANE_MODE_TRAINING=3,
 114    PLANE_MODE_ACRO=4,
 115    PLANE_MODE_FLY_BY_WIRE_A=5,
 116    PLANE_MODE_FLY_BY_WIRE_B=6,
 117    PLANE_MODE_CRUISE=7,
 118    PLANE_MODE_AUTOTUNE=8,
 119    PLANE_MODE_AUTO=10,
 120    PLANE_MODE_RTL=11,
 121    PLANE_MODE_LOITER=12,
 122    PLANE_MODE_TAKEOFF=13,
 123    PLANE_MODE_AVOID_ADSB=14,
 124    PLANE_MODE_GUIDED=15,
 125    PLANE_MODE_INITIALIZING=16,
 126    PLANE_MODE_QSTABILIZE=17,
 127    PLANE_MODE_QHOVER=18,
 128    PLANE_MODE_QLOITER=19,
 129    PLANE_MODE_QLAND=20,
 130    PLANE_MODE_QRTL=21,
 131    PLANE_MODE_QAUTOTUNE=22,
 132    PLANE_MODE_ENUM_END=23,
 133 } APM_PLANE_MODE;
 134
 135 /** @brief A mapping of copter flight modes for custom_mode field of heartbeat. */
 136 typedef enum APM_COPTER_MODE
 137 {
 138    COPTER_MODE_STABILIZE=0,
 139    COPTER_MODE_ACRO=1,
 140    COPTER_MODE_ALT_HOLD=2,
 141    COPTER_MODE_AUTO=3,
 142    COPTER_MODE_GUIDED=4,
 143    COPTER_MODE_LOITER=5,
 144    COPTER_MODE_RTL=6,
 145    COPTER_MODE_CIRCLE=7,
 146    COPTER_MODE_LAND=9,
 147    COPTER_MODE_DRIFT=11,
 148    COPTER_MODE_SPORT=13,
 149    COPTER_MODE_FLIP=14,
 150    COPTER_MODE_AUTOTUNE=15,
 151    COPTER_MODE_POSHOLD=16,
 152    COPTER_MODE_BRAKE=17,
 153    COPTER_MODE_THROW=18,
 154    COPTER_MODE_AVOID_ADSB=19,
 155    COPTER_MODE_GUIDED_NOGPS=20,
 156    COPTER_MODE_SMART_RTL=21,
 157    COPTER_MODE_ENUM_END=22,
 158 } APM_COPTER_MODE;
 159
 160 static serialPort_t *mavlinkPort = NULL;
 161 static serialPortConfig_t *portConfig;
 162
 163 static bool mavlinkTelemetryEnabled =  false;
 164 static portSharing_e mavlinkPortSharing;
 165
 166 /* MAVLink datastream rates in Hz */
 167 static uint8_t mavRates[] = {
 168     [MAV_DATA_STREAM_EXTENDED_STATUS] = 2,      // 2Hz
 169     [MAV_DATA_STREAM_RC_CHANNELS] = 5,          // 5Hz
 170     [MAV_DATA_STREAM_POSITION] = 2,             // 2Hz
 171     [MAV_DATA_STREAM_EXTRA1] = 10,              // 10Hz
 172     [MAV_DATA_STREAM_EXTRA2] = 2,               // 2Hz
 173     [MAV_DATA_STREAM_EXTRA3] = 1                // 1Hz
 174 };
 175
 176 #define MAXSTREAMS (sizeof(mavRates) / sizeof(mavRates[0]))
 177
 178 static timeUs_t lastMavlinkMessage = 0;
 179 static uint8_t mavTicks[MAXSTREAMS];
 180 static mavlink_message_t mavSendMsg;
 181 static mavlink_message_t mavRecvMsg;
 182 static mavlink_status_t mavRecvStatus;
 183
 184 static uint8_t mavSystemId = 1;
 185 static uint8_t mavComponentId = MAV_COMP_ID_SYSTEM_CONTROL;
 186
 187 static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
 188 {
 189     switch (flightMode)
 190     {
 191         case FLM_ACRO:          return COPTER_MODE_ACRO;
 192         case FLM_ACRO_AIR:      return COPTER_MODE_ACRO;
 193         case FLM_ANGLE:         return COPTER_MODE_STABILIZE;
 194         case FLM_HORIZON:       return COPTER_MODE_STABILIZE;
 195         case FLM_ANGLEHOLD:     return COPTER_MODE_STABILIZE;
 196         case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD;
 197         case FLM_POSITION_HOLD: return COPTER_MODE_POSHOLD;
 198         case FLM_RTH:           return COPTER_MODE_RTL;
 199         case FLM_MISSION:       return COPTER_MODE_AUTO;
 200         case FLM_LAUNCH:        return COPTER_MODE_THROW;
 201         case FLM_FAILSAFE:
 202             {
 203                 if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) {
 204                     return COPTER_MODE_RTL;
 205                 } else if (failsafePhase() == FAILSAFE_LANDING) {
 206                     return COPTER_MODE_LAND;
 207                 } else {
 208                     // There is no valid mapping to ArduCopter
 209                     return COPTER_MODE_ENUM_END;
 210                 }
 211             }
 212         default:                return COPTER_MODE_ENUM_END;
 213     }
 214 }
 215
 216 static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode)
 217 {
 218     switch (flightMode)
 219     {
 220         case FLM_MANUAL:        return PLANE_MODE_MANUAL;
 221         case FLM_ACRO:          return PLANE_MODE_ACRO;
 222         case FLM_ACRO_AIR:      return PLANE_MODE_ACRO;
 223         case FLM_ANGLE:         return PLANE_MODE_FLY_BY_WIRE_A;
 224         case FLM_HORIZON:       return PLANE_MODE_STABILIZE;
 225         case FLM_ANGLEHOLD:     return PLANE_MODE_STABILIZE;
 226         case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B;
 227         case FLM_POSITION_HOLD: return PLANE_MODE_LOITER;
 228         case FLM_RTH:           return PLANE_MODE_RTL;
 229         case FLM_MISSION:       return PLANE_MODE_AUTO;
 230         case FLM_CRUISE:        return PLANE_MODE_CRUISE;
 231         case FLM_LAUNCH:        return PLANE_MODE_TAKEOFF;
 232         case FLM_FAILSAFE:
 233             {
 234                 if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) {
 235                     return PLANE_MODE_RTL;
 236                 }
 237                 else if (failsafePhase() == FAILSAFE_LANDING) {
 238                     return PLANE_MODE_AUTO;
 239                 }
 240                 else {
 241                     // There is no valid mapping to ArduPlane
 242                     return PLANE_MODE_ENUM_END;
 243                 }
 244             }
 245         default:                return PLANE_MODE_ENUM_END;
 246     }
 247 }
 248
 249 static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
 250 {
 251     uint8_t rate = (uint8_t) mavRates[streamNum];
 252     if (rate == 0) {
 253         return 0;
 254     }
 255
 256     if (mavTicks[streamNum] == 0) {
 257         // we're triggering now, setup the next trigger point
 258         if (rate > TELEMETRY_MAVLINK_MAXRATE) {
 259             rate = TELEMETRY_MAVLINK_MAXRATE;
 260         }
 261
 262         mavTicks[streamNum] = (TELEMETRY_MAVLINK_MAXRATE / rate);
 263         return 1;
 264     }
 265
 266     // count down at TASK_RATE_HZ
 267     mavTicks[streamNum]--;
 268     return 0;
 269 }
 270
 271 void freeMAVLinkTelemetryPort(void)
 272 {
 273     closeSerialPort(mavlinkPort);
 274     mavlinkPort = NULL;
 275     mavlinkTelemetryEnabled = false;
 276 }
 277
 278 void initMAVLinkTelemetry(void)
 279 {
 280     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK);
 281     mavlinkPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_MAVLINK);
 282 }
 283
 284 void configureMAVLinkTelemetryPort(void)
 285 {
 286     if (!portConfig) {
 287         return;
 288     }
 289
 290     baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
 291     if (baudRateIndex == BAUD_AUTO) {
 292         // default rate for minimOSD
 293         baudRateIndex = BAUD_57600;
 294     }
 295
 296     mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_PORT_MODE, SERIAL_NOT_INVERTED);
 297
 298     if (!mavlinkPort) {
 299         return;
 300     }
 301
 302     mavlinkTelemetryEnabled = true;
 303 }
 304
 305 static void configureMAVLinkStreamRates(void)
 306 {
 307     mavRates[MAV_DATA_STREAM_EXTENDED_STATUS] = telemetryConfig()->mavlink.extended_status_rate;
 308     mavRates[MAV_DATA_STREAM_RC_CHANNELS] = telemetryConfig()->mavlink.rc_channels_rate;
 309     mavRates[MAV_DATA_STREAM_POSITION] = telemetryConfig()->mavlink.position_rate;
 310     mavRates[MAV_DATA_STREAM_EXTRA1] = telemetryConfig()->mavlink.extra1_rate;
 311     mavRates[MAV_DATA_STREAM_EXTRA2] = telemetryConfig()->mavlink.extra2_rate;
 312     mavRates[MAV_DATA_STREAM_EXTRA3] = telemetryConfig()->mavlink.extra3_rate;
 313 }
 314
 315 void checkMAVLinkTelemetryState(void)
 316 {
 317     bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing);
 318
 319     if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) {
 320         return;
 321     }
 322
 323     if (newTelemetryEnabledValue) {
 324         configureMAVLinkTelemetryPort();
 325         configureMAVLinkStreamRates();
 326     } else
 327         freeMAVLinkTelemetryPort();
 328 }
 329
 330 static void mavlinkSendMessage(void)
 331 {
 332     uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN];
 333
 334     mavlink_status_t* chan_state = mavlink_get_channel_status(MAVLINK_COMM_0);
 335     if (telemetryConfig()->mavlink.version == 1) {
 336         chan_state->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
 337     } else {
 338         chan_state->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
 339     }
 340
 341     int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg);
 342
 343     for (int i = 0; i < msgLength; i++) {
 344         serialWrite(mavlinkPort, mavBuffer[i]);
 345     }
 346 }
 347
 348 void mavlinkSendSystemStatus(void)
 349 {
 350     // Receiver is assumed to be always present
 351     uint32_t onboard_control_sensors_present    = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
 352     // GYRO and RC are assumed as minimum requirements
 353     uint32_t onboard_control_sensors_enabled    = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
 354     uint32_t onboard_control_sensors_health     = 0;
 355
 356     if (getHwGyroStatus() == HW_SENSOR_OK) {
 357         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
 358         // Missing presence will report as sensor unhealthy
 359         onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
 360     }
 361
 362     hardwareSensorStatus_e accStatus = getHwAccelerometerStatus();
 363     if (accStatus == HW_SENSOR_OK) {
 364         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
 365         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
 366         onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
 367     } else if (accStatus == HW_SENSOR_UNHEALTHY) {
 368         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
 369         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
 370     } else if (accStatus == HW_SENSOR_UNAVAILABLE) {
 371         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
 372     }
 373
 374     hardwareSensorStatus_e compassStatus = getHwCompassStatus();
 375     if (compassStatus == HW_SENSOR_OK) {
 376         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
 377         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
 378         onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
 379     } else if (compassStatus == HW_SENSOR_UNHEALTHY) {
 380         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
 381         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
 382     } else if (compassStatus == HW_SENSOR_UNAVAILABLE) {
 383         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
 384     }
 385
 386     hardwareSensorStatus_e baroStatus = getHwBarometerStatus();
 387     if (baroStatus == HW_SENSOR_OK) {
 388         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
 389         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
 390         onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
 391     } else if (baroStatus == HW_SENSOR_UNHEALTHY) {
 392         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
 393         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
 394     } else if (baroStatus == HW_SENSOR_UNAVAILABLE) {
 395         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
 396     }
 397
 398     hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus();
 399     if (pitotStatus == HW_SENSOR_OK) {
 400         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
 401         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
 402         onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
 403     } else if (pitotStatus == HW_SENSOR_UNHEALTHY) {
 404         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
 405         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
 406     } else if (pitotStatus == HW_SENSOR_UNAVAILABLE) {
 407         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
 408     }
 409
 410     hardwareSensorStatus_e gpsStatus = getHwGPSStatus();
 411     if (gpsStatus == HW_SENSOR_OK) {
 412         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
 413         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
 414         onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
 415     } else if (gpsStatus == HW_SENSOR_UNHEALTHY) {
 416         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
 417         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
 418     } else if (gpsStatus == HW_SENSOR_UNAVAILABLE) {
 419         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
 420     }
 421
 422     hardwareSensorStatus_e opFlowStatus = getHwOpticalFlowStatus();
 423     if (opFlowStatus == HW_SENSOR_OK) {
 424         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
 425         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
 426         onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
 427     } else if (opFlowStatus == HW_SENSOR_UNHEALTHY) {
 428         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
 429         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
 430     } else if (opFlowStatus == HW_SENSOR_UNAVAILABLE) {
 431         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
 432     }
 433
 434     hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus();
 435     if (rangefinderStatus == HW_SENSOR_OK) {
 436         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
 437         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
 438         onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
 439     } else if (rangefinderStatus == HW_SENSOR_UNHEALTHY) {
 440         onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
 441         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
 442     } else if (rangefinderStatus == HW_SENSOR_UNAVAILABLE) {
 443         onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
 444     }
 445
 446     if (rxIsReceivingSignal() && rxAreFlightChannelsValid()) {
 447         onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
 448     }
 449
 450 #ifdef USE_BLACKBOX
 451     // BLACKBOX is assumed enabled and present for boards with capability
 452     onboard_control_sensors_present |= MAV_SYS_STATUS_LOGGING;
 453     onboard_control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
 454     // Unhealthy only for cases with not enough space to record
 455     if (!isBlackboxDeviceFull()) {
 456         onboard_control_sensors_health |= MAV_SYS_STATUS_LOGGING;
 457     }
 458 #endif
 459
 460     mavlink_msg_sys_status_pack(mavSystemId, mavComponentId, &mavSendMsg,
 461         // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present.
 462         //Value of 0: not present. Value of 1: present. Indices according MAV_SYS_STATUS_SENSOR
 463         onboard_control_sensors_present,
 464         // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled
 465         onboard_control_sensors_enabled,
 466         // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error.
 467         onboard_control_sensors_health,
 468         // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
 469         constrain(averageSystemLoadPercent*10, 0, 1000),
 470         // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
 471         feature(FEATURE_VBAT) ? getBatteryVoltage() * 10 : 0,
 472         // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
 473         isAmperageConfigured() ? getAmperage() : -1,
 474         // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
 475         feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100,
 476         // 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)
 477         0,
 478         // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
 479         0,
 480         // errors_count1 Autopilot-specific errors
 481         0,
 482         // errors_count2 Autopilot-specific errors
 483         0,
 484         // errors_count3 Autopilot-specific errors
 485         0,
 486         // errors_count4 Autopilot-specific errors
 487         0);
 488
 489     mavlinkSendMessage();
 490 }
 491
 492 void mavlinkSendRCChannelsAndRSSI(void)
 493 {
 494 #define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0)
 495     if (telemetryConfig()->mavlink.version == 1) {
 496         mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg,
 497             // time_boot_ms Timestamp (milliseconds since system boot)
 498             millis(),
 499             // 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.
 500             0,
 501             // chan1_raw RC channel 1 value, in microseconds
 502             GET_CHANNEL_VALUE(0),
 503             // chan2_raw RC channel 2 value, in microseconds
 504             GET_CHANNEL_VALUE(1),
 505             // chan3_raw RC channel 3 value, in microseconds
 506             GET_CHANNEL_VALUE(2),
 507             // chan4_raw RC channel 4 value, in microseconds
 508             GET_CHANNEL_VALUE(3),
 509             // chan5_raw RC channel 5 value, in microseconds
 510             GET_CHANNEL_VALUE(4),
 511             // chan6_raw RC channel 6 value, in microseconds
 512             GET_CHANNEL_VALUE(5),
 513             // chan7_raw RC channel 7 value, in microseconds
 514             GET_CHANNEL_VALUE(6),
 515             // chan8_raw RC channel 8 value, in microseconds
 516             GET_CHANNEL_VALUE(7),
 517             // rssi Receive signal strength indicator, 0: 0%, 254: 100%
 518                 //https://github.com/mavlink/mavlink/issues/1027
 519             scaleRange(getRSSI(), 0, 1023, 0, 254));
 520         }
 521     else {
 522         mavlink_msg_rc_channels_pack(mavSystemId, mavComponentId, &mavSendMsg,
 523             // time_boot_ms Timestamp (milliseconds since system boot)
 524             millis(),
 525             // Total number of RC channels being received.
 526             rxRuntimeConfig.channelCount,
 527             // chan1_raw RC channel 1 value, in microseconds
 528             GET_CHANNEL_VALUE(0),
 529             // chan2_raw RC channel 2 value, in microseconds
 530             GET_CHANNEL_VALUE(1),
 531             // chan3_raw RC channel 3 value, in microseconds
 532             GET_CHANNEL_VALUE(2),
 533             // chan4_raw RC channel 4 value, in microseconds
 534             GET_CHANNEL_VALUE(3),
 535             // chan5_raw RC channel 5 value, in microseconds
 536             GET_CHANNEL_VALUE(4),
 537             // chan6_raw RC channel 6 value, in microseconds
 538             GET_CHANNEL_VALUE(5),
 539             // chan7_raw RC channel 7 value, in microseconds
 540             GET_CHANNEL_VALUE(6),
 541             // chan8_raw RC channel 8 value, in microseconds
 542             GET_CHANNEL_VALUE(7),
 543             // chan9_raw RC channel 9 value, in microseconds
 544             GET_CHANNEL_VALUE(8),
 545             // chan10_raw RC channel 10 value, in microseconds
 546             GET_CHANNEL_VALUE(9),
 547             // chan11_raw RC channel 11 value, in microseconds
 548             GET_CHANNEL_VALUE(10),
 549             // chan12_raw RC channel 12 value, in microseconds
 550             GET_CHANNEL_VALUE(11),
 551             // chan13_raw RC channel 13 value, in microseconds
 552             GET_CHANNEL_VALUE(12),
 553             // chan14_raw RC channel 14 value, in microseconds
 554             GET_CHANNEL_VALUE(13),
 555             // chan15_raw RC channel 15 value, in microseconds
 556             GET_CHANNEL_VALUE(14),
 557             // chan16_raw RC channel 16 value, in microseconds
 558             GET_CHANNEL_VALUE(15),
 559             // chan17_raw RC channel 17 value, in microseconds
 560             GET_CHANNEL_VALUE(16),
 561             // chan18_raw RC channel 18 value, in microseconds
 562             GET_CHANNEL_VALUE(17),
 563             // rssi Receive signal strength indicator, 0: 0%, 254: 100%
 564                 //https://github.com/mavlink/mavlink/issues/1027
 565             scaleRange(getRSSI(), 0, 1023, 0, 254));
 566     }
 567 #undef GET_CHANNEL_VALUE
 568
 569     mavlinkSendMessage();
 570 }
 571
 572 #if defined(USE_GPS)
 573 void mavlinkSendPosition(timeUs_t currentTimeUs)
 574 {
 575     uint8_t gpsFixType = 0;
 576
 577     if (!(sensors(SENSOR_GPS)
 578 #ifdef USE_GPS_FIX_ESTIMATION
 579             || STATE(GPS_ESTIMATED_FIX)
 580 #endif
 581         ))
 582         return;
 583
 584     if (gpsSol.fixType == GPS_NO_FIX)
 585         gpsFixType = 1;
 586     else if (gpsSol.fixType == GPS_FIX_2D)
 587             gpsFixType = 2;
 588     else if (gpsSol.fixType == GPS_FIX_3D)
 589             gpsFixType = 3;
 590
 591     mavlink_msg_gps_raw_int_pack(mavSystemId, mavComponentId, &mavSendMsg,
 592         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
 593         currentTimeUs,
 594         // 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.
 595         gpsFixType,
 596         // lat Latitude in 1E7 degrees
 597         gpsSol.llh.lat,
 598         // lon Longitude in 1E7 degrees
 599         gpsSol.llh.lon,
 600         // alt Altitude in 1E3 meters (millimeters) above MSL
 601         gpsSol.llh.alt * 10,
 602         // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 603         gpsSol.eph,
 604         // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 605         gpsSol.epv,
 606         // vel GPS ground speed (m/s * 100). If unknown, set to: 65535
 607         gpsSol.groundSpeed,
 608         // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
 609         gpsSol.groundCourse * 10,
 610         // satellites_visible Number of satellites visible. If unknown, set to 255
 611         gpsSol.numSat,
 612         // alt_ellipsoid Altitude (above WGS84, EGM96 ellipsoid). Positive for up
 613         0,
 614         // h_acc Position uncertainty in mm,
 615         gpsSol.eph * 10,
 616         // v_acc Altitude uncertainty in mm,
 617         gpsSol.epv * 10,
 618         // vel_acc Speed uncertainty in mm (??)
 619         0,
 620         // hdg_acc Heading uncertainty in degE5
 621         0,
 622         // yaw Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.
 623         0);
 624
 625     mavlinkSendMessage();
 626
 627     // Global position
 628     mavlink_msg_global_position_int_pack(mavSystemId, mavComponentId, &mavSendMsg,
 629         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
 630         currentTimeUs,
 631         // lat Latitude in 1E7 degrees
 632         gpsSol.llh.lat,
 633         // lon Longitude in 1E7 degrees
 634         gpsSol.llh.lon,
 635         // alt Altitude in 1E3 meters (millimeters) above MSL
 636         gpsSol.llh.alt * 10,
 637         // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
 638         getEstimatedActualPosition(Z) * 10,
 639         // [cm/s] Ground X Speed (Latitude, positive north)
 640         getEstimatedActualVelocity(X),
 641         // [cm/s] Ground Y Speed (Longitude, positive east)
 642         getEstimatedActualVelocity(Y),
 643         // [cm/s] Ground Z Speed (Altitude, positive down)
 644         getEstimatedActualVelocity(Z),
 645         // [cdeg] Vehicle heading (yaw angle) (0.0..359.99 degrees, 0=north)
 646         DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)
 647     );
 648
 649     mavlinkSendMessage();
 650
 651     mavlink_msg_gps_global_origin_pack(mavSystemId, mavComponentId, &mavSendMsg,
 652         // latitude Latitude (WGS84), expressed as * 1E7
 653         GPS_home.lat,
 654         // longitude Longitude (WGS84), expressed as * 1E7
 655         GPS_home.lon,
 656         // altitude Altitude(WGS84), expressed as * 1000
 657         GPS_home.alt * 10, // FIXME
 658         // time_usec Timestamp (microseconds since system boot)
 659         // Use millis() * 1000 as micros() will overflow after 1.19 hours.
 660         ((uint64_t) millis()) * 1000);
 661
 662     mavlinkSendMessage();
 663 }
 664 #endif
 665
 666 void mavlinkSendAttitude(void)
 667 {
 668     mavlink_msg_attitude_pack(mavSystemId, mavComponentId, &mavSendMsg,
 669         // time_boot_ms Timestamp (milliseconds since system boot)
 670         millis(),
 671         // roll Roll angle (rad)
 672         RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.roll)),
 673         // pitch Pitch angle (rad)
 674         RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(-attitude.values.pitch)),
 675         // yaw Yaw angle (rad)
 676         RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)),
 677         // rollspeed Roll angular speed (rad/s)
 678         gyro.gyroADCf[FD_ROLL],
 679         // pitchspeed Pitch angular speed (rad/s)
 680         gyro.gyroADCf[FD_PITCH],
 681         // yawspeed Yaw angular speed (rad/s)
 682         gyro.gyroADCf[FD_YAW]);
 683
 684     mavlinkSendMessage();
 685 }
 686
 687 void mavlinkSendHUDAndHeartbeat(void)
 688 {
 689     float mavAltitude = 0;
 690     float mavGroundSpeed = 0;
 691     float mavAirSpeed = 0;
 692     float mavClimbRate = 0;
 693
 694 #if defined(USE_GPS)
 695     // use ground speed if source available
 696     if (sensors(SENSOR_GPS)
 697 #ifdef USE_GPS_FIX_ESTIMATION
 698             || STATE(GPS_ESTIMATED_FIX)
 699 #endif
 700         ) {
 701         mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
 702     }
 703 #endif
 704
 705 #if defined(USE_PITOT)
 706     if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
 707         mavAirSpeed = getAirspeedEstimate() / 100.0f;
 708     }
 709 #endif
 710
 711     // select best source for altitude
 712     mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
 713     mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
 714
 715     int16_t thr = getThrottlePercent(osdUsingScaledThrottle());
 716     mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg,
 717         // airspeed Current airspeed in m/s
 718         mavAirSpeed,
 719         // groundspeed Current ground speed in m/s
 720         mavGroundSpeed,
 721         // heading Current heading in degrees, in compass units (0..360, 0=north)
 722         DECIDEGREES_TO_DEGREES(attitude.values.yaw),
 723         // throttle Current throttle setting in integer percent, 0 to 100
 724         thr,
 725         // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
 726         mavAltitude,
 727         // climb Current climb rate in meters/second
 728         mavClimbRate);
 729
 730     mavlinkSendMessage();
 731
 732
 733     uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 734     if (ARMING_FLAG(ARMED))
 735         mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
 736
 737     uint8_t mavSystemType;
 738     switch (mixerConfig()->platformType)
 739     {
 740         case PLATFORM_MULTIROTOR:
 741             mavSystemType = MAV_TYPE_QUADROTOR;
 742             break;
 743         case PLATFORM_TRICOPTER:
 744             mavSystemType = MAV_TYPE_TRICOPTER;
 745             break;
 746         case PLATFORM_AIRPLANE:
 747             mavSystemType = MAV_TYPE_FIXED_WING;
 748             break;
 749         case PLATFORM_ROVER:
 750             mavSystemType = MAV_TYPE_GROUND_ROVER;
 751             break;
 752         case PLATFORM_BOAT:
 753             mavSystemType = MAV_TYPE_SURFACE_BOAT;
 754             break;
 755         case PLATFORM_HELICOPTER:
 756             mavSystemType = MAV_TYPE_HELICOPTER;
 757             break;
 758         default:
 759             mavSystemType = MAV_TYPE_GENERIC;
 760             break;
 761     }
 762
 763     flightModeForTelemetry_e flm = getFlightModeForTelemetry();
 764     uint8_t mavCustomMode;
 765
 766     if (STATE(FIXED_WING_LEGACY)) {
 767         mavCustomMode = (uint8_t)inavToArduPlaneMap(flm);
 768     }
 769     else {
 770         mavCustomMode = (uint8_t)inavToArduCopterMap(flm);
 771     }
 772
 773     if (flm != FLM_MANUAL) {
 774         mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
 775     }
 776     if (flm == FLM_POSITION_HOLD || flm == FLM_RTH || flm == FLM_MISSION) {
 777         mavModes |= MAV_MODE_FLAG_GUIDED_ENABLED;
 778     }
 779
 780     uint8_t mavSystemState = 0;
 781     if (ARMING_FLAG(ARMED)) {
 782         if (failsafeIsActive()) {
 783             mavSystemState = MAV_STATE_CRITICAL;
 784         }
 785         else {
 786             mavSystemState = MAV_STATE_ACTIVE;
 787         }
 788     }
 789     else if (areSensorsCalibrating()) {
 790         mavSystemState = MAV_STATE_CALIBRATING;
 791     }
 792     else {
 793         mavSystemState = MAV_STATE_STANDBY;
 794     }
 795
 796     mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg,
 797         // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 798         mavSystemType,
 799         // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
 800         MAV_AUTOPILOT_GENERIC,
 801         // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
 802         mavModes,
 803         // custom_mode A bitfield for use for autopilot-specific flags.
 804         mavCustomMode,
 805         // system_status System status flag, see MAV_STATE ENUM
 806         mavSystemState);
 807
 808     mavlinkSendMessage();
 809 }
 810
 811 void mavlinkSendBatteryTemperatureStatusText(void)
 812 {
 813     uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN];
 814     uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN];
 815     memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages));
 816     memset(batteryVoltagesExt, 0, sizeof(batteryVoltagesExt));
 817     if (feature(FEATURE_VBAT)) {
 818         uint8_t batteryCellCount = getBatteryCellCount();
 819         if (batteryCellCount > 0) {
 820             for (int cell=0; cell < batteryCellCount && cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN + MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; cell++) {
 821                 if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) {
 822                     batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10;
 823                 } else {
 824                     batteryVoltagesExt[cell-MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN] = getBatteryAverageCellVoltage() * 10;
 825                 }
 826             }
 827         }
 828         else {
 829             batteryVoltages[0] = getBatteryVoltage() * 10;
 830         }
 831     }
 832     else {
 833         batteryVoltages[0] = 0;
 834     }
 835
 836     mavlink_msg_battery_status_pack(mavSystemId, mavComponentId, &mavSendMsg,
 837         // id Battery ID
 838         0,
 839         // battery_function Function of the battery
 840         MAV_BATTERY_FUNCTION_UNKNOWN,
 841         // type Type (chemistry) of the battery
 842         MAV_BATTERY_TYPE_UNKNOWN,
 843         // temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature
 844         INT16_MAX,
 845         // voltages Battery voltage of cells, in millivolts (1 = 1 millivolt). Cells above the valid cell count for this battery should have the UINT16_MAX value.
 846         batteryVoltages,
 847         // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
 848         isAmperageConfigured() ? getAmperage() : -1,
 849         // current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate
 850         isAmperageConfigured() ? getMAhDrawn() : -1,
 851         // energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt)  (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate
 852         isAmperageConfigured() ? getMWhDrawn()*36 : -1,
 853         // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery);
 854         feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1,
 855         // time_remaining Remaining battery time, 0: autopilot does not provide remaining battery time estimate
 856         0, // TODO this could easily be implemented
 857         // charge_state State for extent of discharge, provided by autopilot for warning or external reactions
 858         0,
 859         // voltages_ext Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead.
 860         batteryVoltagesExt,
 861         // mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode.
 862         0,
 863         // fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported).
 864         0);
 865
 866     mavlinkSendMessage();
 867
 868
 869     int16_t temperature;
 870     sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature);
 871     mavlink_msg_scaled_pressure_pack(mavSystemId, mavComponentId, &mavSendMsg,
 872         millis(),
 873         0,
 874         0,
 875         temperature * 10,
 876         0);
 877
 878     mavlinkSendMessage();
 879
 880
 881 // FIXME - Status text is limited to boards with USE_OSD
 882 #ifdef USE_OSD
 883     char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "};
 884     textAttributes_t elemAttr = osdGetSystemMessage(buff, sizeof(buff), false);
 885     if (buff[0] != SYM_BLANK) {
 886         MAV_SEVERITY severity = MAV_SEVERITY_NOTICE;
 887         if (TEXT_ATTRIBUTES_HAVE_BLINK(elemAttr)) {
 888             severity = MAV_SEVERITY_CRITICAL;
 889         } else if TEXT_ATTRIBUTES_HAVE_INVERTED(elemAttr) {
 890             severity = MAV_SEVERITY_WARNING;
 891         }
 892
 893         mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg,
 894             (uint8_t)severity,
 895             buff,
 896             0,
 897             0);
 898
 899         mavlinkSendMessage();
 900     }
 901 #endif
 902
 903
 904 }
 905
 906 void processMAVLinkTelemetry(timeUs_t currentTimeUs)
 907 {
 908     // is executed @ TELEMETRY_MAVLINK_MAXRATE rate
 909     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
 910         mavlinkSendSystemStatus();
 911     }
 912
 913     if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS)) {
 914         mavlinkSendRCChannelsAndRSSI();
 915     }
 916
 917 #ifdef USE_GPS
 918     if (mavlinkStreamTrigger(MAV_DATA_STREAM_POSITION)) {
 919         mavlinkSendPosition(currentTimeUs);
 920     }
 921 #endif
 922
 923     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA1)) {
 924         mavlinkSendAttitude();
 925     }
 926
 927     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) {
 928         mavlinkSendHUDAndHeartbeat();
 929     }
 930
 931     if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) {
 932         mavlinkSendBatteryTemperatureStatusText();
 933     }
 934
 935 }
 936
 937 static bool handleIncoming_MISSION_CLEAR_ALL(void)
 938 {
 939     mavlink_mission_clear_all_t msg;
 940     mavlink_msg_mission_clear_all_decode(&mavRecvMsg, &msg);
 941
 942     // Check if this message is for us
 943     if (msg.target_system == mavSystemId) {
 944         resetWaypointList();
 945         mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
 946         mavlinkSendMessage();
 947         return true;
 948     }
 949
 950     return false;
 951 }
 952
 953 // Static state for MISSION UPLOAD transaction (starting with MISSION_COUNT)
 954 static int incomingMissionWpCount = 0;
 955 static int incomingMissionWpSequence = 0;
 956
 957 static bool handleIncoming_MISSION_COUNT(void)
 958 {
 959     mavlink_mission_count_t msg;
 960     mavlink_msg_mission_count_decode(&mavRecvMsg, &msg);
 961
 962     // Check if this message is for us
 963     if (msg.target_system == mavSystemId) {
 964         if (msg.count <= NAV_MAX_WAYPOINTS) {
 965             incomingMissionWpCount = msg.count; // We need to know how many items to request
 966             incomingMissionWpSequence = 0;
 967             mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
 968             mavlinkSendMessage();
 969             return true;
 970         }
 971         else if (ARMING_FLAG(ARMED)) {
 972             mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION);
 973             mavlinkSendMessage();
 974             return true;
 975         }
 976         else {
 977             mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION);
 978             mavlinkSendMessage();
 979             return true;
 980         }
 981     }
 982
 983     return false;
 984 }
 985
 986 static bool handleIncoming_MISSION_ITEM(void)
 987 {
 988     mavlink_mission_item_t msg;
 989     mavlink_msg_mission_item_decode(&mavRecvMsg, &msg);
 990
 991     // Check if this message is for us
 992     if (msg.target_system == mavSystemId) {
 993         // Check supported values first
 994         if (ARMING_FLAG(ARMED)) {
 995             mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION);
 996             mavlinkSendMessage();
 997             return true;
 998         }
 999
1000         if ((msg.autocontinue == 0) || (msg.command != MAV_CMD_NAV_WAYPOINT && msg.command != MAV_CMD_NAV_RETURN_TO_LAUNCH)) {
1001             mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION);
1002             mavlinkSendMessage();
1003             return true;
1004         }
1005
1006         if ((msg.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) && !(msg.frame == MAV_FRAME_MISSION && msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH)) {
1007             mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION);
1008             mavlinkSendMessage();
1009             return true;
1010         }
1011
1012         if (msg.seq == incomingMissionWpSequence) {
1013             incomingMissionWpSequence++;
1014
1015             navWaypoint_t wp;
1016             wp.action = (msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH) ? NAV_WP_ACTION_RTH : NAV_WP_ACTION_WAYPOINT;
1017             wp.lat = (int32_t)(msg.x * 1e7f);
1018             wp.lon = (int32_t)(msg.y * 1e7f);
1019             wp.alt = msg.z * 100.0f;
1020             wp.p1 = 0;
1021             wp.p2 = 0;
1022             wp.p3 = 0;
1023             wp.flag = (incomingMissionWpSequence >= incomingMissionWpCount) ? NAV_WP_FLAG_LAST : 0;
1024
1025             setWaypoint(incomingMissionWpSequence, &wp);
1026
1027             if (incomingMissionWpSequence >= incomingMissionWpCount) {
1028                 if (isWaypointListValid()) {
1029                     mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION);
1030                     mavlinkSendMessage();
1031                 }
1032                 else {
1033                     mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION);
1034                     mavlinkSendMessage();
1035                 }
1036             }
1037             else {
1038                 mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
1039                 mavlinkSendMessage();
1040             }
1041         }
1042         else {
1043             // Wrong sequence number received
1044             mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION);
1045             mavlinkSendMessage();
1046         }
1047
1048         return true;
1049     }
1050
1051     return false;
1052 }
1053
1054 static bool handleIncoming_MISSION_REQUEST_LIST(void)
1055 {
1056     mavlink_mission_request_list_t msg;
1057     mavlink_msg_mission_request_list_decode(&mavRecvMsg, &msg);
1058
1059     // Check if this message is for us
1060     if (msg.target_system == mavSystemId) {
1061         mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION);
1062         mavlinkSendMessage();
1063         return true;
1064     }
1065
1066     return false;
1067 }
1068
1069 static bool handleIncoming_MISSION_REQUEST(void)
1070 {
1071     mavlink_mission_request_t msg;
1072     mavlink_msg_mission_request_decode(&mavRecvMsg, &msg);
1073
1074     // Check if this message is for us
1075     if (msg.target_system == mavSystemId) {
1076         int wpCount = getWaypointCount();
1077
1078         if (msg.seq < wpCount) {
1079             navWaypoint_t wp;
1080             getWaypoint(msg.seq + 1, &wp);
1081
1082             mavlink_msg_mission_item_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid,
1083                         msg.seq,
1084                         wp.action == NAV_WP_ACTION_RTH ? MAV_FRAME_MISSION : MAV_FRAME_GLOBAL_RELATIVE_ALT,
1085                         wp.action == NAV_WP_ACTION_RTH ? MAV_CMD_NAV_RETURN_TO_LAUNCH : MAV_CMD_NAV_WAYPOINT,
1086                         0,
1087                         1,
1088                         0, 0, 0, 0,
1089                         wp.lat / 1e7f,
1090                         wp.lon / 1e7f,
1091                         wp.alt / 100.0f,
1092                         MAV_MISSION_TYPE_MISSION);
1093             mavlinkSendMessage();
1094         }
1095         else {
1096             mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION);
1097             mavlinkSendMessage();
1098         }
1099
1100         return true;
1101     }
1102
1103     return false;
1104 }
1105
1106 static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) {
1107     mavlink_rc_channels_override_t msg;
1108     mavlink_msg_rc_channels_override_decode(&mavRecvMsg, &msg);
1109     // Don't check system ID because it's not configurable with systems like Crossfire
1110     mavlinkRxHandleMessage(&msg);
1111     return true;
1112 }
1113
1114 #ifdef USE_ADSB
1115 static bool handleIncoming_ADSB_VEHICLE(void) {
1116     mavlink_adsb_vehicle_t msg;
1117     mavlink_msg_adsb_vehicle_decode(&mavRecvMsg, &msg);
1118
1119     adsbVehicleValues_t* vehicle = getVehicleForFill();
1120     if(vehicle != NULL){
1121         vehicle->icao = msg.ICAO_address;
1122         vehicle->lat = msg.lat;
1123         vehicle->lon = msg.lon;
1124         vehicle->alt = (int32_t)(msg.altitude / 10);
1125         vehicle->heading = msg.heading;
1126         vehicle->flags = msg.flags;
1127         vehicle->altitudeType = msg.altitude_type;
1128         memcpy(&(vehicle->callsign), msg.callsign, sizeof(vehicle->callsign));
1129         vehicle->emitterType = msg.emitter_type;
1130         vehicle->tslc = msg.tslc;
1131
1132         adsbNewVehicle(vehicle);
1133     }
1134
1135     //debug vehicle
1136    /* if(vehicle != NULL){
1137
1138         char name[9] = "DUMMY    ";
1139
1140         vehicle->icao = 666;
1141         vehicle->lat = 492383514;
1142         vehicle->lon = 165148681;
1143         vehicle->alt = 100000;
1144         vehicle->heading = 180;
1145         vehicle->flags = ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS;
1146         vehicle->altitudeType = 0;
1147         memcpy(&(vehicle->callsign), name, sizeof(vehicle->callsign));
1148         vehicle->emitterType = 6;
1149         vehicle->tslc = 0;
1150
1151         adsbNewVehicle(vehicle);
1152     }*/
1153
1154     return true;
1155 }
1156 #endif
1157
1158 static bool processMAVLinkIncomingTelemetry(void)
1159 {
1160     while (serialRxBytesWaiting(mavlinkPort) > 0) {
1161         // Limit handling to one message per cycle
1162         char c = serialRead(mavlinkPort);
1163         uint8_t result = mavlink_parse_char(0, c, &mavRecvMsg, &mavRecvStatus);
1164         if (result == MAVLINK_FRAMING_OK) {
1165             switch (mavRecvMsg.msgid) {
1166                 case MAVLINK_MSG_ID_HEARTBEAT:
1167                     break;
1168                 case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
1169                     return handleIncoming_MISSION_CLEAR_ALL();
1170                 case MAVLINK_MSG_ID_MISSION_COUNT:
1171                     return handleIncoming_MISSION_COUNT();
1172                 case MAVLINK_MSG_ID_MISSION_ITEM:
1173                     return handleIncoming_MISSION_ITEM();
1174                 case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
1175                     return handleIncoming_MISSION_REQUEST_LIST();
1176                 case MAVLINK_MSG_ID_MISSION_REQUEST:
1177                     return handleIncoming_MISSION_REQUEST();
1178                 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
1179                     return handleIncoming_RC_CHANNELS_OVERRIDE();
1180 #ifdef USE_ADSB
1181                 case MAVLINK_MSG_ID_ADSB_VEHICLE:
1182                     return handleIncoming_ADSB_VEHICLE();
1183 #endif
1184                 default:
1185                     return false;
1186             }
1187         }
1188     }
1189
1190     return false;
1191 }
1192
1193 void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
1194 {
1195     static bool incomingRequestServed;
1196
1197     if (!mavlinkTelemetryEnabled) {
1198         return;
1199     }
1200
1201     if (!mavlinkPort) {
1202         return;
1203     }
1204
1205     // If we did serve data on incoming request - skip next scheduled messages batch to avoid link clogging
1206     if (processMAVLinkIncomingTelemetry()) {
1207         incomingRequestServed = true;
1208     }
1209
1210     if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
1211         // Only process scheduled data if we didn't serve any incoming request this cycle
1212         if (!incomingRequestServed ||
1213             (
1214                  (rxConfig()->receiverType == RX_TYPE_SERIAL) &&
1215                  (rxConfig()->serialrx_provider == SERIALRX_MAVLINK) &&
1216                  !tristateWithDefaultOnIsActive(rxConfig()->halfDuplex)
1217             )
1218         ) {
1219             processMAVLinkTelemetry(currentTimeUs);
1220         }
1221         lastMavlinkMessage = currentTimeUs;
1222         incomingRequestServed = false;
1223     }
1224 }
1225
1226 #endif