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