diff --git a/docs/Mavlink.md b/docs/Mavlink.md
new file mode 100644
index 00000000000..b29dcdf8f89
--- /dev/null
+++ b/docs/Mavlink.md
@@ -0,0 +1,131 @@
+# MAVLink INAV Implementation
+
+INAV has a partial implementation of MAVLink that is intended primarily for simple telemetry and operation. It supports RC, missions, telemetry and some features such as Guided mode; but it is very different from a compliant MAVLink spec vehicle such as Pixhawk or Ardupilot and important differences exist, as such it is not 100% compatible and cannot be expected to work the same way. The standard MAVLink header library is used in compilation.
+
+## Fundamental differences from ArduPilot/PX4
+
+- **No MAVLink parameter API**: INAV sends a single stub parameter and otherwise ignores parameter traffic. Configure the aircraft through the INAV Configurator or CLI instead.
+- **Limited command support**: only a subset of commands is implemented; unsupported commands are `COMMAND_ACK`ed as `UNSUPPORTED`.
+- **Mission handling**: uploads are rejected while armed (except legacy guided waypoint writes). Mission frames are validated per command and unsupported frames are rejected.
+- **Mode reporting**: `custom_mode` approximates ArduPilot modes and may not match all INAV states.
+- **Flow control expectations**: INAV honours `RADIO_STATUS.txbuf` to avoid overrunning radios; without it, packets are simply paced at 20 ms intervals.
+- **Half‑duplex etiquette**: when half‑duplex is enabled, INAV waits one telemetry tick after any received frame before transmitting to reduce collisions.
+
+### Relevant CLI options
+
+- `mavlink_sysid` – system ID used in every outbound packet (default 1); most inbound handlers only act on packets targeted to this system ID.
+- `mavlink_autopilot_type` – heartbeat autopilot ID (`GENERIC` or `ARDUPILOT`).
+- `mavlink_version` – force MAVLink v1 when set to 1.
+- Stream rates (Hz): `mavlink_ext_status_rate`, `mavlink_rc_chan_rate`, `mavlink_pos_rate`, `mavlink_extra1_rate`, `mavlink_extra2_rate`, `mavlink_extra3_rate`. Each group is polled up to 50 Hz; a rate of 0 disables the group.
+- `mavlink_min_txbuffer` – minimum remote TX buffer level before sending when `RADIO_STATUS` provides flow control.
+- `mavlink_radio_type` – scales `RADIO_STATUS` RSSI/SNR for **generic**, **ELRS**, or **SiK** links.
+
+## Supported Outgoing Messages
+
+Messages are organized into MAVLink datastream groups. Each group sends **one message per trigger** at the configured rate:
+
+- `SYS_STATUS`: Advertises detected sensors (gyro/accel/compass, baro, pitot, GPS, optical flow, rangefinder, RC, blackbox) and whether they are healthy. Includes main loop load, battery voltage/current/percentage, and logging capability.
+- `RC_CHANNELS_RAW`: for v1, `RC_CHANNELS`: for v2. Up to 18 input channels plus RSSI mapped to MAVLink units.
+- `GPS_RAW_INT`: for GNSS fix quality, HDOP/VDOP, velocity, and satellite count when a fix (or estimated fix) exists.
+- `GLOBAL_POSITION_INT`: couples GPS position with INAV's altitude and velocity estimates.
+- `GPS_GLOBAL_ORIGIN`: broadcasts the current home position.
+- `ATTITUDE`: Roll, pitch, yaw, and angular rates.
+- `VFR_HUD`: with airspeed (if a healthy pitot is available), ground speed, throttle, altitude, and climb rate.
+- `HEARTBEAT`: encodes arming state and maps INAV flight modes onto ArduPilot-style `custom_mode`: values (see mappings below).
+- `EXTENDED_SYS_STATE`: publishes landed state.
+- `BATTERY_STATUS`: with per-cell voltages (cells 11‑14 in `voltages_ext`), current draw, consumed mAh/Wh, and remaining percentage when available.
+- `SCALED_PRESSURE`: for IMU/baro temperature.
+- `STATUSTEXT`: when the OSD has a pending system message; severity follows OSD attributes (notice/warning/critical).
+- On-demand (command-triggered) messages: `AUTOPILOT_VERSION`, `PROTOCOL_VERSION`, `MESSAGE_INTERVAL`, `HOME_POSITION`, `AVAILABLE_MODES`, and `CURRENT_MODE`.
+
+## Supported Incoming Messages
+
+- `HEARTBEAT`: used to detect ADS‑B participants when `type` is `MAV_TYPE_ADSB`.
+- `MISSION_COUNT`: starts an upload transaction (capped at `NAV_MAX_WAYPOINTS`).
+- `MISSION_ITEM` / `MISSION_ITEM_INT`: stores mission waypoints; rejects unsupported frames/sequence errors. Upload while armed is rejected except legacy guided waypoint writes.
+- `MISSION_REQUEST_LIST`, `MISSION_REQUEST`, `MISSION_REQUEST_INT`: downloads active mission items; returns `MISSION_ACK` on bad sequence.
+- `MISSION_CLEAR_ALL`: clears stored mission.
+- `COMMAND_LONG` / `COMMAND_INT`: command transport for supported `MAV_CMD_*` handlers.
+- `REQUEST_DATA_STREAM`: legacy stream-rate control per stream group. `MAV_DATA_STREAM_ALL` (0) applies to all INAV-scheduled groups (`EXTENDED_STATUS`, `RC_CHANNELS`, `POSITION`, `EXTRA1`, `EXTRA2`, `EXTRA3`, `EXTENDED_SYS_STATE`); `start_stop==0` stops the addressed stream(s).
+- `SET_POSITION_TARGET_GLOBAL_INT`: requires matching `target_system` and `target_component` (`0` or local component), validates frame/type-mask semantics, updates guided WP255 for XY targets, and supports altitude-only control (`X/Y ignore`, `Z set`) using the same datum logic as `MAV_CMD_DO_CHANGE_ALTITUDE`.
+- `RC_CHANNELS_OVERRIDE` passes channel values to the MAVLink serial receiver backend when targeted to the local system ID.
+- `RADIO_STATUS` updates remote TX buffer level and scales RSSI/SNR according to `mavlink_radio_type` (also feeds link stats for MAVLink RX receivers).
+- `ADSB_VEHICLE` populates the internal traffic list when ADS‑B is enabled.
+- `PARAM_REQUEST_LIST` elicits a stub `PARAM_VALUE` response so ground stations stop requesting parameters (INAV does not expose parameters over MAVLink).
+
+
+## Supported Commands
+
+Limited implementation of the Command protocol.
+
+- `MAV_CMD_DO_REPOSITION`: sets the Follow Me/GCS Nav waypoint when GCS NAV is valid. Accepts `MAV_FRAME_GLOBAL`, `MAV_FRAME_GLOBAL_INT`, `MAV_FRAME_GLOBAL_RELATIVE_ALT`, `MAV_FRAME_GLOBAL_RELATIVE_ALT_INT`; otherwise `UNSUPPORTED`.
+- `MAV_CMD_DO_CHANGE_ALTITUDE`: supported when barometer support is compiled (`USE_BARO`); accepts global/global-int (MSL datum) and global-relative/global-relative-int (takeoff-relative datum), then calls navigation altitude-target update.
+- `MAV_CMD_SET_MESSAGE_INTERVAL` / `MAV_CMD_GET_MESSAGE_INTERVAL`: adjust or query telemetry stream output for supported message IDs (streamed messages only; intervals slower than 1 Hz are not accepted).
+- `MAV_CMD_GET_HOME_POSITION`: replies with `HOME_POSITION` when home fix exists.
+- `MAV_CMD_REQUEST_MESSAGE`: emits one selected message (`HEARTBEAT`, `SYS_STATUS`, `ATTITUDE`, `VFR_HUD`, `AVAILABLE_MODES`, `CURRENT_MODE`, `EXTENDED_SYS_STATE`, RC channels, GPS/global/origin, battery/pressure, and `HOME_POSITION` when available) or `UNSUPPORTED`.
+- `MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES`: returns stub `AUTOPILOT_VERSION` (v2 only; v1 returns `UNSUPPORTED`).
+- `MAV_CMD_REQUEST_PROTOCOL_VERSION`: returns stub `PROTOCOL_VERSION` (v2 only; v1 returns `UNSUPPORTED`).
+
+## Mode mappings (INAV → MAVLink/ArduPilot)
+
+`custom_mode` is derived from active INAV telemetry flight mode (`getFlightModeForTelemetry()`), then mapped per vehicle type.
+
+- **Multirotor profiles**
+ - ACRO / ACRO AIR → **ACRO**
+ - ANGLE / HORIZON / ANGLE HOLD → **STABILIZE**
+ - ALT HOLD → **ALT_HOLD**
+ - POS HOLD → **GUIDED** (if GCS valid), otherwise **POSHOLD**
+ - RTH → **RTL**
+ - MISSION → **AUTO**
+ - LAUNCH → **THROW**
+ - FAILSAFE → **RTL** (RTH/other phases) or **LAND** (landing phase)
+ - Any other unmapped mode falls back to **STABILIZE**
+- **Fixed-wing profiles**
+ - MANUAL → **MANUAL**
+ - ACRO / ACRO AIR → **ACRO**
+ - ANGLE → **FBWA**
+ - HORIZON / ANGLE HOLD → **STABILIZE**
+ - ALT HOLD → **FBWB**
+ - POS HOLD → **GUIDED** (if GCS valid), otherwise **LOITER**
+ - RTH → **RTL**
+ - MISSION → **AUTO**
+ - CRUISE → **CRUISE**
+ - LAUNCH → **TAKEOFF**
+ - FAILSAFE → **RTL** (RTH/other phases) or **AUTOLAND** (landing phase)
+ - Any other unmapped mode falls back to **MANUAL**
+
+## Datastream groups and defaults
+
+Default rates (Hz) are shown; adjust with the CLI keys above.
+
+| Datastream group | Messages | Default rate |
+| --- | --- | --- |
+| `EXTENDED_STATUS` | `SYS_STATUS` | 2 Hz |
+| `RC_CHANNELS` | `RC_CHANNELS_RAW` (v1) / `RC_CHANNELS` (v2) | 1 Hz |
+| `POSITION` | `GPS_RAW_INT`, `GLOBAL_POSITION_INT`, `GPS_GLOBAL_ORIGIN` | 2 Hz |
+| `EXTRA1` | `ATTITUDE` | 3 Hz |
+| `EXTRA2` | `VFR_HUD`, `HEARTBEAT` | 2 Hz |
+| `EXT_SYS_STATE` | `EXTENDED_SYS_STATE` | 1 Hz (defaults to `mavlink_extra3_rate`) |
+| `EXTRA3` | `BATTERY_STATUS`, `SCALED_PRESSURE`, `STATUSTEXT` (when present) | 1 Hz |
+
+## Operating tips
+
+- Set `mavlink_radio_type` to **ELRS** or **SiK** if you use those links to get accurate link quality scaling in `RADIO_STATUS`.
+- If you rely on RC override via MAVLink, ensure the serial receiver type is set to `SERIALRX_MAVLINK` and consider enabling `telemetry_halfduplex` when RX shares the port.
+- To reduce bandwidth, lower the stream rates for groups you do not need, or disable them entirely by setting the rate to 0.
+
+
+## MAVLink Missions
+
+Partial compatibility with MAVLink mission planners such as QGC is implemented, however the differences between the two protocols means INAV cannot be programmed to it's full potential using only the MAVLINK mission protocol; only simple missions are possible. It is recommended to use MultiWii Planner or the INAV Configurator to program missions.
+
+## MSP mission parity gaps (MAV ↔ MSP)
+
+- WAYPOINT: MSP→MAV sends lat/lon/alt but drops leg speed `p1` and all user-action bits in `p3` (only alt-mode bit drives frame). MAV→MSP stores lat/lon/alt but forces `p1=0`, `p2=0`, keeps only alt-mode bit in `p3`; leg speed and user bits are lost.
+- POSHOLD_TIME / LOITER_TIME: loiter time `p1` OK; leg speed `p2` and user-action bits in `p3` are discarded both directions.
+- LAND: lat/lon/alt OK; leg speed `p1`, ground elevation `p2`, and user-action bits in `p3` are cleared in both directions (only alt-mode bit is retained from frame on upload).
+- RTH: RTH land-if-nonzero flag in `p1` is ignored both ways (always zeroed); user-action bits dropped; alt is sent only if the MAVLink frame is coordinate and returns with alt-mode bit set on upload.
+- JUMP: target and repeat count OK
+- SET_POI: lat/lon/alt OK; `param1` is fixed to `MAV_ROI_LOCATION`; user-action bits in `p3` are dropped (alt-mode bit respected on upload).
+- SET_HEAD: heading `p1` OK; user-action bits in `p3` are not represented.
+- Net effect: actions and positions OK, but MSP-specific fields (leg speed, LAND elevation adjustment, RTH land flag, user-action bits in `p3`) are lost, so MAVLink missions cannot fully conform to `MSP_navigation_messages.md`.
diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md
index 2ca0fff6675..c4b6d06b58d 100644
--- a/docs/development/msp/README.md
+++ b/docs/development/msp/README.md
@@ -9,7 +9,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
-**JSON file rev: 4**
+**JSON file rev: 0**
**Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty**
diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json
index ffe24b3f24c..3844e59cee9 100644
--- a/docs/development/msp/inav_enums.json
+++ b/docs/development/msp/inav_enums.json
@@ -2238,6 +2238,14 @@
"MAG_FAKE": "16",
"MAG_MAX": "MAG_FAKE"
},
+ "mavFrameSupportMask_e": {
+ "_source": "inav/src/main/telemetry/mavlink.c",
+ "MAV_FRAME_SUPPORTED_NONE": "0",
+ "MAV_FRAME_SUPPORTED_GLOBAL": "(1 << 0)",
+ "MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT": "(1 << 1)",
+ "MAV_FRAME_SUPPORTED_GLOBAL_INT": "(1 << 2)",
+ "MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT": "(1 << 3)"
+ },
"mavlinkAutopilotType_e": {
"_source": "inav/src/main/telemetry/telemetry.h",
"MAVLINK_AUTOPILOT_GENERIC": "0",
@@ -3831,7 +3839,7 @@
"THR_HI": "(2 << (2 * THROTTLE))"
},
"systemState_e": {
- "_source": "inav/src/main/fc/fc_init.c",
+ "_source": "inav/src/main/fc/fc_init.h",
"SYSTEM_STATE_INITIALISING": "0",
"SYSTEM_STATE_CONFIG_LOADED": "(1 << 0)",
"SYSTEM_STATE_SENSORS_READY": "(1 << 1)",
diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md
index af53d1c6b9a..164ebd9bc14 100644
--- a/docs/development/msp/inav_enums_ref.md
+++ b/docs/development/msp/inav_enums_ref.md
@@ -173,6 +173,7 @@
- [ltm_modes_e](#enum-ltm_modes_e)
- [ltmUpdateRate_e](#enum-ltmupdaterate_e)
- [magSensor_e](#enum-magsensor_e)
+- [mavFrameSupportMask_e](#enum-mavframesupportmask_e)
- [mavlinkAutopilotType_e](#enum-mavlinkautopilottype_e)
- [mavlinkRadio_e](#enum-mavlinkradio_e)
- [measurementSteps_e](#enum-measurementsteps_e)
@@ -3374,6 +3375,19 @@
| `MAG_FAKE` | 16 | |
| `MAG_MAX` | MAG_FAKE | |
+---
+## `mavFrameSupportMask_e`
+
+> Source: ../../../src/main/telemetry/mavlink.c
+
+| Enumerator | Value | Condition |
+|---|---:|---|
+| `MAV_FRAME_SUPPORTED_NONE` | 0 | |
+| `MAV_FRAME_SUPPORTED_GLOBAL` | (1 << 0) | |
+| `MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT` | (1 << 1) | |
+| `MAV_FRAME_SUPPORTED_GLOBAL_INT` | (1 << 2) | |
+| `MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT` | (1 << 3) | |
+
---
## `mavlinkAutopilotType_e`
@@ -5629,7 +5643,7 @@
---
## `systemState_e`
-> Source: ../../../src/main/fc/fc_init.h
+> Source: ../../../src/main/fc/fc_init.c
| Enumerator | Value | Condition |
|---|---:|---|
@@ -5643,7 +5657,7 @@
---
## `systemState_e`
-> Source: ../../../src/main/fc/fc_init.c
+> Source: ../../../src/main/fc/fc_init.h
| Enumerator | Value | Condition |
|---|---:|---|
diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum
index ae689c4b8eb..14e143e04b9 100644
--- a/docs/development/msp/msp_messages.checksum
+++ b/docs/development/msp/msp_messages.checksum
@@ -1 +1 @@
-6e8861395a4275489a12012e6985d733
+7146802c1d6c89bce6aeda0d5e95c137
diff --git a/docs/development/msp/rev b/docs/development/msp/rev
index bf0d87ab1b2..c227083464f 100644
--- a/docs/development/msp/rev
+++ b/docs/development/msp/rev
@@ -1 +1 @@
-4
\ No newline at end of file
+0
\ No newline at end of file
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index a3752233149..1b6d2fc5f99 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1328,9 +1328,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
resetAltitudeController(navTerrainFollowingRequested());
setupAltitudeController();
- setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
}
+ // POSHOLD is a 3D hold mode: always capture current altitude setpoint when entering.
+ setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
+
// Prepare position controller if idle or current Mode NOT active in position hold state
if (previousState != NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_LOITER_ABOVE_HOME &&
previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND &&
@@ -3896,7 +3898,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
// Only valid when armed and in poshold mode
else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) && isGCSValid()) {
// Convert to local coordinates
- geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
+ geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, waypointMissionAltConvMode(wpData->p3));
navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 60f6aed67c3..d9d6931eba6 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -31,6 +31,7 @@
#include "build/build_config.h"
#include "build/debug.h"
+#include "build/version.h"
#include "common/axis.h"
#include "common/color.h"
@@ -97,6 +98,18 @@
#define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX
#define TELEMETRY_MAVLINK_MAXRATE 50
#define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE)
+#define MAV_DATA_STREAM_EXTENDED_SYS_STATE (MAV_DATA_STREAM_EXTRA3 + 1)
+#define ARDUPILOT_VERSION_MAJOR 4
+#define ARDUPILOT_VERSION_MINOR 6
+#define ARDUPILOT_VERSION_PATCH 0
+
+typedef enum {
+ MAV_FRAME_SUPPORTED_NONE = 0,
+ MAV_FRAME_SUPPORTED_GLOBAL = (1 << 0),
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT = (1 << 1),
+ MAV_FRAME_SUPPORTED_GLOBAL_INT = (1 << 2),
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT = (1 << 3),
+} mavFrameSupportMask_e;
/**
* MAVLink requires angles to be in the range -Pi..Pi.
@@ -129,7 +142,11 @@ typedef enum APM_PLANE_MODE
PLANE_MODE_QLAND=20,
PLANE_MODE_QRTL=21,
PLANE_MODE_QAUTOTUNE=22,
- PLANE_MODE_ENUM_END=23,
+ PLANE_MODE_QACRO=23,
+ PLANE_MODE_THERMAL=24,
+ PLANE_MODE_LOITER_ALT_QLAND=25,
+ PLANE_MODE_AUTOLAND=26,
+ PLANE_MODE_ENUM_END=27,
} APM_PLANE_MODE;
/** @brief A mapping of copter flight modes for custom_mode field of heartbeat. */
@@ -154,7 +171,14 @@ typedef enum APM_COPTER_MODE
COPTER_MODE_AVOID_ADSB=19,
COPTER_MODE_GUIDED_NOGPS=20,
COPTER_MODE_SMART_RTL=21,
- COPTER_MODE_ENUM_END=22,
+ COPTER_MODE_FLOWHOLD=22,
+ COPTER_MODE_FOLLOW=23,
+ COPTER_MODE_ZIGZAG=24,
+ COPTER_MODE_SYSTEMID=25,
+ COPTER_MODE_AUTOROTATE=26,
+ COPTER_MODE_AUTO_RTL=27,
+ COPTER_MODE_TURTLE=28,
+ COPTER_MODE_ENUM_END=29,
} APM_COPTER_MODE;
static serialPort_t *mavlinkPort = NULL;
@@ -172,13 +196,15 @@ static uint8_t mavRates[] = {
[MAV_DATA_STREAM_POSITION] = 2, // 2Hz
[MAV_DATA_STREAM_EXTRA1] = 3, // 3Hz
[MAV_DATA_STREAM_EXTRA2] = 2, // 2Hz, HEARTBEATs are important
- [MAV_DATA_STREAM_EXTRA3] = 1 // 1Hz
+ [MAV_DATA_STREAM_EXTRA3] = 1, // 1Hz
+ [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = 1 // 1Hz
};
#define MAXSTREAMS (sizeof(mavRates) / sizeof(mavRates[0]))
static timeUs_t lastMavlinkMessage = 0;
-static uint8_t mavTicks[MAXSTREAMS];
+static uint8_t mavRatesConfigured[MAXSTREAMS];
+static timeUs_t mavStreamNextDue[MAXSTREAMS];
static mavlink_message_t mavSendMsg;
static mavlink_message_t mavRecvMsg;
static mavlink_status_t mavRecvStatus;
@@ -188,6 +214,27 @@ static uint8_t mavSystemId = 1;
static uint8_t mavAutopilotType;
static uint8_t mavComponentId = MAV_COMP_ID_AUTOPILOT1;
+static uint8_t mavlinkGetVehicleType(void)
+{
+ switch (mixerConfig()->platformType)
+ {
+ case PLATFORM_MULTIROTOR:
+ return MAV_TYPE_QUADROTOR;
+ case PLATFORM_TRICOPTER:
+ return MAV_TYPE_TRICOPTER;
+ case PLATFORM_AIRPLANE:
+ return MAV_TYPE_FIXED_WING;
+ case PLATFORM_ROVER:
+ return MAV_TYPE_GROUND_ROVER;
+ case PLATFORM_BOAT:
+ return MAV_TYPE_SURFACE_BOAT;
+ case PLATFORM_HELICOPTER:
+ return MAV_TYPE_HELICOPTER;
+ default:
+ return MAV_TYPE_GENERIC;
+ }
+}
+
static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
{
switch (flightMode)
@@ -216,11 +263,10 @@ static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
} else if (failsafePhase() == FAILSAFE_LANDING) {
return COPTER_MODE_LAND;
} else {
- // There is no valid mapping to ArduCopter
- return COPTER_MODE_ENUM_END;
+ return COPTER_MODE_RTL;
}
}
- default: return COPTER_MODE_ENUM_END;
+ default: return COPTER_MODE_STABILIZE;
}
}
@@ -247,45 +293,85 @@ static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode)
case FLM_MISSION: return PLANE_MODE_AUTO;
case FLM_CRUISE: return PLANE_MODE_CRUISE;
case FLM_LAUNCH: return PLANE_MODE_TAKEOFF;
- case FLM_FAILSAFE:
+ case FLM_FAILSAFE: //failsafePhase_e
{
if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) {
return PLANE_MODE_RTL;
}
else if (failsafePhase() == FAILSAFE_LANDING) {
- return PLANE_MODE_AUTO;
+ return PLANE_MODE_AUTOLAND;
}
else {
- // There is no valid mapping to ArduPlane
- return PLANE_MODE_ENUM_END;
+ return PLANE_MODE_RTL;
}
}
- default: return PLANE_MODE_ENUM_END;
+ default: return PLANE_MODE_MANUAL;
}
}
-static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
+typedef struct mavlinkModeSelection_s {
+ flightModeForTelemetry_e flightMode;
+ uint8_t customMode;
+} mavlinkModeSelection_t;
+
+static mavlinkModeSelection_t selectMavlinkMode(bool isPlane)
+{
+ mavlinkModeSelection_t modeSelection;
+ modeSelection.flightMode = getFlightModeForTelemetry();
+
+ if (isPlane) {
+ modeSelection.customMode = (uint8_t)inavToArduPlaneMap(modeSelection.flightMode);
+ } else {
+ modeSelection.customMode = (uint8_t)inavToArduCopterMap(modeSelection.flightMode);
+ }
+
+ return modeSelection;
+}
+
+static uint8_t mavlinkClampStreamRate(uint8_t rate)
{
- uint8_t rate = (uint8_t) mavRates[streamNum];
+ if (rate > TELEMETRY_MAVLINK_MAXRATE) {
+ return TELEMETRY_MAVLINK_MAXRATE;
+ }
+
+ return rate;
+}
+
+static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum, timeUs_t currentTimeUs)
+{
+ const uint8_t rate = mavlinkClampStreamRate(mavRates[streamNum]);
if (rate == 0) {
return 0;
}
- if (mavTicks[streamNum] == 0) {
- // we're triggering now, setup the next trigger point
- if (rate > TELEMETRY_MAVLINK_MAXRATE) {
- rate = TELEMETRY_MAVLINK_MAXRATE;
- }
-
- mavTicks[streamNum] = (TELEMETRY_MAVLINK_MAXRATE / rate);
+ const timeUs_t intervalUs = 1000000UL / rate;
+ if ((mavStreamNextDue[streamNum] == 0) || (cmpTimeUs(currentTimeUs, mavStreamNextDue[streamNum]) >= 0)) {
+ mavStreamNextDue[streamNum] = currentTimeUs + intervalUs;
return 1;
}
- // count down at TASK_RATE_HZ
- mavTicks[streamNum]--;
return 0;
}
+static void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate)
+{
+ if (streamNum >= MAXSTREAMS) {
+ return;
+ }
+ mavRates[streamNum] = mavlinkClampStreamRate(rate);
+ mavStreamNextDue[streamNum] = 0;
+}
+
+static int32_t mavlinkStreamIntervalUs(uint8_t streamNum)
+{
+ uint8_t rate = mavlinkClampStreamRate(mavRates[streamNum]);
+ if (rate == 0) {
+ return -1;
+ }
+
+ return 1000000 / rate;
+}
+
void freeMAVLinkTelemetryPort(void)
{
closeSerialPort(mavlinkPort);
@@ -297,6 +383,7 @@ void initMAVLinkTelemetry(void)
{
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK);
mavlinkPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_MAVLINK);
+ memcpy(mavRatesConfigured, mavRates, sizeof(mavRatesConfigured));
}
void configureMAVLinkTelemetryPort(void)
@@ -324,12 +411,14 @@ void configureMAVLinkTelemetryPort(void)
static void configureMAVLinkStreamRates(void)
{
- mavRates[MAV_DATA_STREAM_EXTENDED_STATUS] = telemetryConfig()->mavlink.extended_status_rate;
- mavRates[MAV_DATA_STREAM_RC_CHANNELS] = telemetryConfig()->mavlink.rc_channels_rate;
- mavRates[MAV_DATA_STREAM_POSITION] = telemetryConfig()->mavlink.position_rate;
- mavRates[MAV_DATA_STREAM_EXTRA1] = telemetryConfig()->mavlink.extra1_rate;
- mavRates[MAV_DATA_STREAM_EXTRA2] = telemetryConfig()->mavlink.extra2_rate;
- mavRates[MAV_DATA_STREAM_EXTRA3] = telemetryConfig()->mavlink.extra3_rate;
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_STATUS, telemetryConfig()->mavlink.extended_status_rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_RC_CHANNELS, telemetryConfig()->mavlink.rc_channels_rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_POSITION, telemetryConfig()->mavlink.position_rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA1, telemetryConfig()->mavlink.extra1_rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA2, telemetryConfig()->mavlink.extra2_rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA3, telemetryConfig()->mavlink.extra3_rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_SYS_STATE, telemetryConfig()->mavlink.extra3_rate);
+ memcpy(mavRatesConfigured, mavRates, sizeof(mavRates));
}
void checkMAVLinkTelemetryState(void)
@@ -365,6 +454,375 @@ static void mavlinkSendMessage(void)
}
}
+static void mavlinkSendAutopilotVersion(void)
+{
+ if (telemetryConfig()->mavlink.version == 1) return;
+
+ // Capabilities aligned with what we actually support.
+ uint64_t capabilities = 0;
+ capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
+ capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
+ capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT;
+ capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT;
+ capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
+ capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT;
+
+ // Bare minimum: caps + IDs. Everything else 0 is fine.
+ mavlink_msg_autopilot_version_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ capabilities, // capabilities
+ ARDUPILOT_VERSION_MAJOR, // flight_sw_version
+ ARDUPILOT_VERSION_MINOR, // middleware_sw_version
+ ARDUPILOT_VERSION_PATCH, // os_sw_version
+ 0, // board_version
+ 0ULL, // flight_custom_version
+ 0ULL, // middleware_custom_version
+ 0ULL, // os_custom_version
+ 0ULL, // vendor_id
+ 0ULL, // product_id
+ (uint64_t)mavSystemId, // uid (any stable nonzero is fine)
+ 0ULL // uid2
+ );
+ mavlinkSendMessage();
+}
+
+static void mavlinkSendProtocolVersion(void)
+{
+ if (telemetryConfig()->mavlink.version == 1) return;
+
+ uint8_t specHash[8] = {0};
+ uint8_t libHash[8] = {0};
+ const uint16_t protocolVersion = (uint16_t)telemetryConfig()->mavlink.version * 100;
+
+ mavlink_msg_protocol_version_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ protocolVersion,
+ protocolVersion,
+ protocolVersion,
+ specHash,
+ libHash);
+
+ mavlinkSendMessage();
+}
+
+static bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask)
+{
+ switch (frame) {
+ case MAV_FRAME_GLOBAL:
+ return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL;
+ case MAV_FRAME_GLOBAL_INT:
+ return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_INT;
+ case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+ return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT;
+ case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
+ return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT;
+ default:
+ return false;
+ }
+}
+
+static bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame)
+{
+ return frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_INT;
+}
+
+static MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters)
+{
+#ifdef USE_BARO
+ geoAltitudeDatumFlag_e datum;
+
+ switch (frame) {
+ case MAV_FRAME_GLOBAL:
+ case MAV_FRAME_GLOBAL_INT:
+ datum = NAV_WP_MSL_DATUM;
+ break;
+ case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+ case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
+ datum = NAV_WP_TAKEOFF_DATUM;
+ break;
+ default:
+ return MAV_RESULT_UNSUPPORTED;
+ }
+
+ const int32_t targetAltitudeCm = (int32_t)lrintf(altitudeMeters * 100.0f);
+ return navigationSetAltitudeTargetWithDatum(datum, targetAltitudeCm) ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED;
+#else
+ UNUSED(frame);
+ UNUSED(altitudeMeters);
+ return MAV_RESULT_UNSUPPORTED;
+#endif
+}
+
+static MAV_MISSION_RESULT mavlinkMissionResultFromCommandResult(MAV_RESULT result)
+{
+ switch (result) {
+ case MAV_RESULT_ACCEPTED:
+ return MAV_MISSION_ACCEPTED;
+ case MAV_RESULT_UNSUPPORTED:
+ return MAV_MISSION_UNSUPPORTED;
+ default:
+ return MAV_MISSION_ERROR;
+ }
+}
+
+static bool mavlinkHandleArmedGuidedMissionItem(uint8_t current, uint8_t frame, mavFrameSupportMask_e allowedFrames, int32_t latitudeE7, int32_t longitudeE7, float altitudeMeters)
+{
+ if (!isGCSValid()) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
+ mavRecvMsg.sysid, mavRecvMsg.compid,
+ MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+
+ if (!mavlinkFrameIsSupported(frame, allowedFrames)) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
+ mavRecvMsg.sysid, mavRecvMsg.compid,
+ MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+
+ if (current == 2) {
+ navWaypoint_t wp = {0};
+ wp.action = NAV_WP_ACTION_WAYPOINT;
+ wp.lat = latitudeE7;
+ wp.lon = longitudeE7;
+ wp.alt = (int32_t)lrintf(altitudeMeters * 100.0f);
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+
+ setWaypoint(255, &wp);
+
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
+ mavRecvMsg.sysid, mavRecvMsg.compid,
+ MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+
+ if (current == 3) {
+ const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame(frame, altitudeMeters);
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
+ mavRecvMsg.sysid, mavRecvMsg.compid,
+ mavlinkMissionResultFromCommandResult(result), MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
+ mavRecvMsg.sysid, mavRecvMsg.compid,
+ MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+}
+
+static uint8_t navWaypointFrame(const navWaypoint_t *wp, bool useIntMessages)
+{
+ switch (wp->action) {
+ case NAV_WP_ACTION_RTH:
+ case NAV_WP_ACTION_JUMP:
+ case NAV_WP_ACTION_SET_HEAD:
+ return MAV_FRAME_MISSION;
+ default:
+ break;
+ }
+
+ const bool absoluteAltitude = (wp->p3 & NAV_WP_ALTMODE) == NAV_WP_ALTMODE;
+
+ if (absoluteAltitude) {
+ return useIntMessages ? MAV_FRAME_GLOBAL_INT : MAV_FRAME_GLOBAL;
+ }
+
+ return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT;
+}
+
+typedef struct {
+ uint8_t customMode;
+ const char *name;
+} mavlinkModeDescriptor_t;
+
+static const mavlinkModeDescriptor_t planeModes[] = {
+ { PLANE_MODE_MANUAL, "MANUAL" },
+ { PLANE_MODE_ACRO, "ACRO" },
+ { PLANE_MODE_STABILIZE, "STABILIZE" },
+ { PLANE_MODE_FLY_BY_WIRE_A,"FBWA" },
+ { PLANE_MODE_FLY_BY_WIRE_B,"FBWB" },
+ { PLANE_MODE_CRUISE, "CRUISE" },
+ { PLANE_MODE_AUTO, "AUTO" },
+ { PLANE_MODE_RTL, "RTL" },
+ { PLANE_MODE_LOITER, "LOITER" },
+ { PLANE_MODE_TAKEOFF, "TAKEOFF" },
+ { PLANE_MODE_GUIDED, "GUIDED" },
+};
+
+static const mavlinkModeDescriptor_t copterModes[] = {
+ { COPTER_MODE_ACRO, "ACRO" },
+ { COPTER_MODE_STABILIZE, "STABILIZE" },
+ { COPTER_MODE_ALT_HOLD, "ALT_HOLD" },
+ { COPTER_MODE_POSHOLD, "POSHOLD" },
+ { COPTER_MODE_LOITER, "LOITER" },
+ { COPTER_MODE_AUTO, "AUTO" },
+ { COPTER_MODE_GUIDED, "GUIDED" },
+ { COPTER_MODE_RTL, "RTL" },
+ { COPTER_MODE_LAND, "LAND" },
+ { COPTER_MODE_BRAKE, "BRAKE" },
+ { COPTER_MODE_THROW, "THROW" },
+ { COPTER_MODE_SMART_RTL, "SMART_RTL" },
+ { COPTER_MODE_DRIFT, "DRIFT" },
+};
+
+static bool mavlinkPlaneModeIsConfigured(uint8_t customMode)
+{
+ switch ((APM_PLANE_MODE)customMode) {
+ case PLANE_MODE_MANUAL:
+ return isModeActivationConditionPresent(BOXMANUAL);
+ case PLANE_MODE_ACRO:
+ return true;
+ case PLANE_MODE_STABILIZE:
+ return isModeActivationConditionPresent(BOXHORIZON) ||
+ isModeActivationConditionPresent(BOXANGLEHOLD);
+ case PLANE_MODE_FLY_BY_WIRE_A:
+ return isModeActivationConditionPresent(BOXANGLE);
+ case PLANE_MODE_FLY_BY_WIRE_B:
+ return isModeActivationConditionPresent(BOXNAVALTHOLD);
+ case PLANE_MODE_CRUISE:
+ return isModeActivationConditionPresent(BOXNAVCRUISE) ||
+ (isModeActivationConditionPresent(BOXNAVCOURSEHOLD) &&
+ isModeActivationConditionPresent(BOXNAVALTHOLD));
+ case PLANE_MODE_AUTO:
+ return isModeActivationConditionPresent(BOXNAVWP);
+ case PLANE_MODE_RTL:
+ return isModeActivationConditionPresent(BOXNAVRTH);
+ case PLANE_MODE_GUIDED:
+ return isModeActivationConditionPresent(BOXNAVPOSHOLD) &&
+ isModeActivationConditionPresent(BOXGCSNAV);
+ case PLANE_MODE_LOITER:
+ return isModeActivationConditionPresent(BOXNAVPOSHOLD);
+ case PLANE_MODE_TAKEOFF:
+ return isModeActivationConditionPresent(BOXNAVLAUNCH);
+ default:
+ return false;
+ }
+}
+
+static bool mavlinkCopterModeIsConfigured(uint8_t customMode)
+{
+ switch ((APM_COPTER_MODE)customMode) {
+ case COPTER_MODE_ACRO:
+ return true;
+ case COPTER_MODE_STABILIZE:
+ return isModeActivationConditionPresent(BOXANGLE) ||
+ isModeActivationConditionPresent(BOXHORIZON) ||
+ isModeActivationConditionPresent(BOXANGLEHOLD);
+ case COPTER_MODE_ALT_HOLD:
+ return isModeActivationConditionPresent(BOXNAVALTHOLD);
+ case COPTER_MODE_GUIDED:
+ return isModeActivationConditionPresent(BOXNAVPOSHOLD) &&
+ isModeActivationConditionPresent(BOXGCSNAV);
+ case COPTER_MODE_POSHOLD:
+ return isModeActivationConditionPresent(BOXNAVPOSHOLD);
+ case COPTER_MODE_RTL:
+ return isModeActivationConditionPresent(BOXNAVRTH);
+ case COPTER_MODE_AUTO:
+ return isModeActivationConditionPresent(BOXNAVWP);
+ case COPTER_MODE_THROW:
+ return isModeActivationConditionPresent(BOXNAVLAUNCH);
+ case COPTER_MODE_BRAKE:
+ return isModeActivationConditionPresent(BOXBRAKING);
+ default:
+ return false;
+ }
+}
+
+static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom,
+ bool (*isModeConfigured)(uint8_t customMode))
+{
+ uint8_t availableCount = 0;
+ for (uint8_t i = 0; i < count; i++) {
+ if (isModeConfigured(modes[i].customMode)) {
+ availableCount++;
+ }
+ }
+
+ if (availableCount == 0) {
+ return;
+ }
+
+ uint8_t modeIndex = 0;
+ for (uint8_t i = 0; i < count; i++) {
+ if (!isModeConfigured(modes[i].customMode)) {
+ continue;
+ }
+
+ modeIndex++;
+ const uint8_t stdMode = MAV_STANDARD_MODE_NON_STANDARD;
+ const uint32_t properties = 0;
+
+ mavlink_msg_available_modes_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ availableCount,
+ modeIndex,
+ stdMode,
+ modes[i].customMode,
+ properties,
+ modes[i].name);
+
+ mavlinkSendMessage();
+
+ if (modes[i].customMode == currentCustom) {
+ mavlink_msg_current_mode_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ stdMode,
+ currentCustom,
+ currentCustom);
+ mavlinkSendMessage();
+ }
+ }
+}
+
+static void mavlinkSendExtendedSysState(void)
+{
+ uint8_t vtolState = MAV_VTOL_STATE_UNDEFINED;
+ uint8_t landedState;
+
+ switch (NAV_Status.state) {
+ case MW_NAV_STATE_LAND_START:
+ case MW_NAV_STATE_LAND_IN_PROGRESS:
+ case MW_NAV_STATE_LAND_SETTLE:
+ case MW_NAV_STATE_LAND_START_DESCENT:
+ case MW_NAV_STATE_EMERGENCY_LANDING:
+ landedState = MAV_LANDED_STATE_LANDING;
+ break;
+ case MW_NAV_STATE_LANDED:
+ landedState = MAV_LANDED_STATE_ON_GROUND;
+ break;
+ default:
+ if (!ARMING_FLAG(ARMED) || STATE(LANDING_DETECTED)) {
+ landedState = MAV_LANDED_STATE_ON_GROUND;
+ } else {
+ landedState = MAV_LANDED_STATE_IN_AIR;
+ }
+ break;
+ }
+
+ mavlink_msg_extended_sys_state_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ vtolState,
+ landedState);
+
+ mavlinkSendMessage();
+}
+
void mavlinkSendSystemStatus(void)
{
// Receiver is assumed to be always present
@@ -590,6 +1048,29 @@ void mavlinkSendRCChannelsAndRSSI(void)
}
#if defined(USE_GPS)
+static void mavlinkSendHomePosition(void)
+{
+ float q[4] = { 1.0f, 0.0f, 0.0f, 0.0f };
+
+ mavlink_msg_home_position_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ GPS_home.lat,
+ GPS_home.lon,
+ GPS_home.alt * 10,
+ 0.0f,
+ 0.0f,
+ 0.0f,
+ q,
+ 0.0f,
+ 0.0f,
+ 0.0f,
+ ((uint64_t) millis()) * 1000);
+
+ mavlinkSendMessage();
+}
+
void mavlinkSendPosition(timeUs_t currentTimeUs)
{
uint8_t gpsFixType = 0;
@@ -646,8 +1127,8 @@ void mavlinkSendPosition(timeUs_t currentTimeUs)
// Global position
mavlink_msg_global_position_int_pack(mavSystemId, mavComponentId, &mavSendMsg,
- // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
- currentTimeUs,
+ // time_boot_ms Timestamp (milliseconds since system boot)
+ currentTimeUs / 1000,
// lat Latitude in 1E7 degrees
gpsSol.llh.lat,
// lon Longitude in 1E7 degrees
@@ -661,7 +1142,7 @@ void mavlinkSendPosition(timeUs_t currentTimeUs)
// [cm/s] Ground Y Speed (Longitude, positive east)
getEstimatedActualVelocity(Y),
// [cm/s] Ground Z Speed (Altitude, positive down)
- getEstimatedActualVelocity(Z),
+ -getEstimatedActualVelocity(Z),
// [cdeg] Vehicle heading (yaw angle) (0.0..359.99 degrees, 0=north)
DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)
);
@@ -695,16 +1176,16 @@ void mavlinkSendAttitude(void)
// yaw Yaw angle (rad)
RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)),
// rollspeed Roll angular speed (rad/s)
- gyro.gyroADCf[FD_ROLL],
+ DEGREES_TO_RADIANS(gyro.gyroADCf[FD_ROLL]),
// pitchspeed Pitch angular speed (rad/s)
- gyro.gyroADCf[FD_PITCH],
+ DEGREES_TO_RADIANS(gyro.gyroADCf[FD_PITCH]),
// yawspeed Yaw angular speed (rad/s)
- gyro.gyroADCf[FD_YAW]);
+ DEGREES_TO_RADIANS(gyro.gyroADCf[FD_YAW]));
mavlinkSendMessage();
}
-void mavlinkSendHUDAndHeartbeat(void)
+void mavlinkSendVfrHud(void)
{
float mavAltitude = 0;
float mavGroundSpeed = 0;
@@ -748,54 +1229,40 @@ void mavlinkSendHUDAndHeartbeat(void)
mavClimbRate);
mavlinkSendMessage();
+}
+void mavlinkSendHeartbeat(void)
+{
+ uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
- uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
- if (ARMING_FLAG(ARMED))
- mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
-
+ const bool isPlane = (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE);
+ const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane);
+ flightModeForTelemetry_e flm = modeSelection.flightMode;
+ uint8_t mavCustomMode = modeSelection.customMode;
uint8_t mavSystemType;
- switch (mixerConfig()->platformType)
- {
- case PLATFORM_MULTIROTOR:
- mavSystemType = MAV_TYPE_QUADROTOR;
- break;
- case PLATFORM_TRICOPTER:
- mavSystemType = MAV_TYPE_TRICOPTER;
- break;
- case PLATFORM_AIRPLANE:
- mavSystemType = MAV_TYPE_FIXED_WING;
- break;
- case PLATFORM_ROVER:
- mavSystemType = MAV_TYPE_GROUND_ROVER;
- break;
- case PLATFORM_BOAT:
- mavSystemType = MAV_TYPE_SURFACE_BOAT;
- break;
- case PLATFORM_HELICOPTER:
- mavSystemType = MAV_TYPE_HELICOPTER;
- break;
- default:
- mavSystemType = MAV_TYPE_GENERIC;
- break;
- }
-
- flightModeForTelemetry_e flm = getFlightModeForTelemetry();
- uint8_t mavCustomMode;
-
- if (STATE(FIXED_WING_LEGACY)) {
- mavCustomMode = (uint8_t)inavToArduPlaneMap(flm);
+ if (isPlane) {
+ mavSystemType = MAV_TYPE_FIXED_WING;
}
else {
- mavCustomMode = (uint8_t)inavToArduCopterMap(flm);
+ mavSystemType = mavlinkGetVehicleType();
}
- if (flm != FLM_MANUAL) {
- mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+ const bool manualInputAllowed = !(flm == FLM_MISSION || flm == FLM_RTH || flm == FLM_FAILSAFE);
+ if (manualInputAllowed) {
+ mavModes |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
- if (flm == FLM_POSITION_HOLD || flm == FLM_RTH || flm == FLM_MISSION) {
+ if (flm == FLM_POSITION_HOLD && isGCSValid()) {
mavModes |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
+ else if (flm == FLM_MISSION || flm == FLM_RTH ) {
+ mavModes |= MAV_MODE_FLAG_AUTO_ENABLED;
+ }
+ else if (flm != FLM_MANUAL && flm!= FLM_ACRO && flm!=FLM_ACRO_AIR) {
+ mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+ }
+
+ if (ARMING_FLAG(ARMED))
+ mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
uint8_t mavSystemState = 0;
if (ARMING_FLAG(ARMED)) {
@@ -904,7 +1371,6 @@ void mavlinkSendBatteryTemperatureStatusText(void)
mavlinkSendMessage();
-
// FIXME - Status text is limited to boards with USE_OSD
#ifdef USE_OSD
char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "};
@@ -932,30 +1398,34 @@ void mavlinkSendBatteryTemperatureStatusText(void)
void processMAVLinkTelemetry(timeUs_t currentTimeUs)
{
- // is executed @ TELEMETRY_MAVLINK_MAXRATE rate
- if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS, currentTimeUs)) {
mavlinkSendSystemStatus();
}
- if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS)) {
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS, currentTimeUs)) {
mavlinkSendRCChannelsAndRSSI();
}
#ifdef USE_GPS
- if (mavlinkStreamTrigger(MAV_DATA_STREAM_POSITION)) {
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_POSITION, currentTimeUs)) {
mavlinkSendPosition(currentTimeUs);
}
#endif
- if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA1)) {
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA1, currentTimeUs)) {
mavlinkSendAttitude();
}
- if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) {
- mavlinkSendHUDAndHeartbeat();
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2, currentTimeUs)) {
+ mavlinkSendVfrHud();
+ mavlinkSendHeartbeat();
}
- if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) {
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_SYS_STATE, currentTimeUs)) {
+ mavlinkSendExtendedSysState();
+ }
+
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3, currentTimeUs)) {
mavlinkSendBatteryTemperatureStatusText();
}
@@ -981,130 +1451,263 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void)
static int incomingMissionWpCount = 0;
static int incomingMissionWpSequence = 0;
-static bool handleIncoming_MISSION_COUNT(void)
+static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, uint16_t command, uint8_t autocontinue, uint16_t seq, float param1, float param2, float param3, float param4, int32_t lat, int32_t lon, float altMeters)
{
- mavlink_mission_count_t msg;
- mavlink_msg_mission_count_decode(&mavRecvMsg, &msg);
-
- // Check if this message is for us
- if (msg.target_system == mavSystemId) {
- if (msg.count <= NAV_MAX_WAYPOINTS) {
- incomingMissionWpCount = msg.count; // We need to know how many items to request
- incomingMissionWpSequence = 0;
- mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
- mavlinkSendMessage();
- return true;
- }
- else if (ARMING_FLAG(ARMED)) {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
- mavlinkSendMessage();
- return true;
- }
- else {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0);
- mavlinkSendMessage();
- return true;
- }
+ if (autocontinue == 0) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
}
- return false;
-}
+ UNUSED(param3);
-static bool handleIncoming_MISSION_ITEM(void)
-{
- mavlink_mission_item_t msg;
- mavlink_msg_mission_item_decode(&mavRecvMsg, &msg);
+ navWaypoint_t wp;
+ memset(&wp, 0, sizeof(wp));
- // Check if this message is for us
- if (msg.target_system == mavSystemId) {
- // Check supported values first
- if (ARMING_FLAG(ARMED)) {
- // Legacy Mission Planner BS for GUIDED
- if (isGCSValid() && (msg.command == MAV_CMD_NAV_WAYPOINT) && (msg.current == 2)) {
- if (!(msg.frame == MAV_FRAME_GLOBAL)) {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
- mavRecvMsg.sysid, mavRecvMsg.compid,
- MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0);
+ switch (command) {
+ case MAV_CMD_NAV_WAYPOINT:
+ if (!mavlinkFrameIsSupported(frame,
+ MAV_FRAME_SUPPORTED_GLOBAL |
+ MAV_FRAME_SUPPORTED_GLOBAL_INT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+ wp.action = NAV_WP_ACTION_WAYPOINT;
+ wp.lat = lat;
+ wp.lon = lon;
+ wp.alt = (int32_t)(altMeters * 100.0f);
+ wp.p1 = 0;
+ wp.p2 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ break;
+
+ case MAV_CMD_NAV_LOITER_TIME:
+ if (!mavlinkFrameIsSupported(frame,
+ MAV_FRAME_SUPPORTED_GLOBAL |
+ MAV_FRAME_SUPPORTED_GLOBAL_INT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+ wp.action = NAV_WP_ACTION_HOLD_TIME;
+ wp.lat = lat;
+ wp.lon = lon;
+ wp.alt = (int32_t)(altMeters * 100.0f);
+ wp.p1 = (int16_t)lrintf(param1);
+ wp.p2 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ break;
+
+ case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+ {
+ const bool coordinateFrame = mavlinkFrameIsSupported(frame,
+ MAV_FRAME_SUPPORTED_GLOBAL |
+ MAV_FRAME_SUPPORTED_GLOBAL_INT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT);
+
+ if (!coordinateFrame && frame != MAV_FRAME_MISSION) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0);
mavlinkSendMessage();
return true;
}
-
- navWaypoint_t wp;
- wp.action = NAV_WP_ACTION_WAYPOINT;
- wp.lat = (int32_t)(msg.x * 1e7f);
- wp.lon = (int32_t)(msg.y * 1e7f);
- wp.alt = (int32_t)(msg.z * 100.0f);
- wp.p1 = 0;
+ wp.action = NAV_WP_ACTION_RTH;
+ wp.lat = 0;
+ wp.lon = 0;
+ wp.alt = coordinateFrame ? (int32_t)(altMeters * 100.0f) : 0;
+ wp.p1 = 0; // Land if non-zero
wp.p2 = 0;
- wp.p3 = 0;
- setWaypoint(255, &wp);
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ break;
+ }
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
- mavRecvMsg.sysid, mavRecvMsg.compid,
- MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0);
+ case MAV_CMD_NAV_LAND:
+ if (!mavlinkFrameIsSupported(frame,
+ MAV_FRAME_SUPPORTED_GLOBAL |
+ MAV_FRAME_SUPPORTED_GLOBAL_INT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0);
mavlinkSendMessage();
return true;
- } else {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
+ }
+ wp.action = NAV_WP_ACTION_LAND;
+ wp.lat = lat;
+ wp.lon = lon;
+ wp.alt = (int32_t)(altMeters * 100.0f);
+ wp.p1 = 0; // Speed (cm/s)
+ wp.p2 = 0; // Elevation Adjustment (m): P2 defines the ground elevation (in metres) for the LAND WP. If the altitude mode is absolute, this is also absolute; for relative altitude, then it is the difference between the assumed home location and the LAND WP.
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; // Altitude Mode & Actions, P3 defines the altitude mode. 0 (default, legacy) = Relative to Home, 1 = Absolute (AMSL).
+ break;
+
+ case MAV_CMD_DO_JUMP:
+ if (frame != MAV_FRAME_MISSION) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0);
mavlinkSendMessage();
return true;
}
- }
-
- if ((msg.autocontinue == 0) || (msg.command != MAV_CMD_NAV_WAYPOINT && msg.command != MAV_CMD_NAV_RETURN_TO_LAUNCH)) {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0);
- mavlinkSendMessage();
- return true;
- }
+ if (param1 < 0.0f) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+ wp.lat = 0;
+ wp.lon = 0;
+ wp.alt = 0;
+ wp.action = NAV_WP_ACTION_JUMP;
+ wp.p1 = (int16_t)lrintf(param1 + 1.0f);
+ wp.p2 = (int16_t)lrintf(param2);
+ wp.p3 = 0;
+ break;
+
+ case MAV_CMD_DO_SET_ROI:
+ if (param1 != MAV_ROI_LOCATION ||
+ !mavlinkFrameIsSupported(frame,
+ MAV_FRAME_SUPPORTED_GLOBAL |
+ MAV_FRAME_SUPPORTED_GLOBAL_INT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+ wp.action = NAV_WP_ACTION_SET_POI;
+ wp.lat = lat;
+ wp.lon = lon;
+ wp.alt = (int32_t)(altMeters * 100.0f);
+ wp.p1 = 0;
+ wp.p2 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ break;
+
+ case MAV_CMD_CONDITION_YAW:
+ if (frame != MAV_FRAME_MISSION) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+ if (param4 != 0.0f) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+ wp.lat = 0;
+ wp.lon = 0;
+ wp.alt = 0;
+ wp.action = NAV_WP_ACTION_SET_HEAD;
+ wp.p1 = (int16_t)lrintf(param1);
+ wp.p2 = 0;
+ wp.p3 = 0;
+ break;
- if ((msg.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) && !(msg.frame == MAV_FRAME_MISSION && msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH)) {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0);
+ default:
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0);
mavlinkSendMessage();
return true;
- }
+ }
- if (msg.seq == incomingMissionWpSequence) {
- incomingMissionWpSequence++;
+ if (seq == incomingMissionWpSequence) {
+ incomingMissionWpSequence++;
- navWaypoint_t wp;
- wp.action = (msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH) ? NAV_WP_ACTION_RTH : NAV_WP_ACTION_WAYPOINT;
- wp.lat = (int32_t)(msg.x * 1e7f);
- wp.lon = (int32_t)(msg.y * 1e7f);
- wp.alt = msg.z * 100.0f;
- wp.p1 = 0;
- wp.p2 = 0;
- wp.p3 = 0;
- wp.flag = (incomingMissionWpSequence >= incomingMissionWpCount) ? NAV_WP_FLAG_LAST : 0;
+ wp.flag = (incomingMissionWpSequence >= incomingMissionWpCount) ? NAV_WP_FLAG_LAST : 0;
- setWaypoint(incomingMissionWpSequence, &wp);
+ setWaypoint(incomingMissionWpSequence, &wp);
- if (incomingMissionWpSequence >= incomingMissionWpCount) {
- if (isWaypointListValid()) {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0);
- mavlinkSendMessage();
- }
- else {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION, 0);
- mavlinkSendMessage();
- }
+ if (incomingMissionWpSequence >= incomingMissionWpCount) {
+ if (isWaypointListValid()) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
}
else {
- mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION, 0);
mavlinkSendMessage();
}
}
else {
- // Wrong sequence number received
+ if (useIntMessages) {
+ mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
+ } else {
+ mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
+ }
+ mavlinkSendMessage();
+ }
+ }
+ else {
+ // If we get a duplicate of the last accepted item, re-request the next one instead of aborting.
+ if (seq + 1 == incomingMissionWpSequence) {
+ if (useIntMessages) {
+ mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
+ } else {
+ mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
+ }
+ mavlinkSendMessage();
+ } else {
mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0);
mavlinkSendMessage();
}
+ }
- return true;
+ return true;
+}
+
+static bool handleIncoming_MISSION_COUNT(void)
+{
+ mavlink_mission_count_t msg;
+ mavlink_msg_mission_count_decode(&mavRecvMsg, &msg);
+
+ // Check if this message is for us
+ if (msg.target_system == mavSystemId) {
+ if (ARMING_FLAG(ARMED)) {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+ if (msg.count <= NAV_MAX_WAYPOINTS) {
+ incomingMissionWpCount = msg.count; // We need to know how many items to request
+ incomingMissionWpSequence = 0;
+ mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
+ mavlinkSendMessage();
+ return true;
+ } else {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
}
return false;
}
+static bool handleIncoming_MISSION_ITEM(void)
+{
+ mavlink_mission_item_t msg;
+ mavlink_msg_mission_item_decode(&mavRecvMsg, &msg);
+
+ if (msg.target_system != mavSystemId) {
+ return false;
+ }
+
+ if (ARMING_FLAG(ARMED)) {
+ // Legacy Mission Planner/QGC guided item handling.
+ if (msg.command == MAV_CMD_NAV_WAYPOINT) {
+ return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame,
+ MAV_FRAME_SUPPORTED_GLOBAL | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT,
+ (int32_t)lrintf(msg.x * 1e7f), (int32_t)lrintf(msg.y * 1e7f), msg.z);
+ }
+
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+
+ return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, (int32_t)(msg.x * 1e7f), (int32_t)(msg.y * 1e7f), msg.z);
+}
+
static bool handleIncoming_MISSION_REQUEST_LIST(void)
{
mavlink_mission_request_list_t msg;
@@ -1120,160 +1723,611 @@ static bool handleIncoming_MISSION_REQUEST_LIST(void)
return false;
}
+typedef struct {
+ uint8_t frame;
+ uint16_t command;
+ float param1;
+ float param2;
+ float param3;
+ float param4;
+ int32_t lat;
+ int32_t lon;
+ float alt;
+} mavlinkMissionItemData_t;
+
+static bool fillMavlinkMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item)
+{
+ mavlinkMissionItemData_t data = {0};
+
+ data.frame = navWaypointFrame(wp, useIntMessages);
+
+ switch (wp->action) {
+ case NAV_WP_ACTION_WAYPOINT:
+ data.command = MAV_CMD_NAV_WAYPOINT;
+ data.lat = wp->lat;
+ data.lon = wp->lon;
+ data.alt = wp->alt / 100.0f;
+ break;
+
+ case NAV_WP_ACTION_HOLD_TIME:
+ data.command = MAV_CMD_NAV_LOITER_TIME;
+ data.param1 = wp->p1;
+ data.lat = wp->lat;
+ data.lon = wp->lon;
+ data.alt = wp->alt / 100.0f;
+ break;
+
+ case NAV_WP_ACTION_RTH:
+ data.command = MAV_CMD_NAV_RETURN_TO_LAUNCH;
+ break;
+
+ case NAV_WP_ACTION_LAND:
+ data.command = MAV_CMD_NAV_LAND;
+ data.lat = wp->lat;
+ data.lon = wp->lon;
+ data.alt = wp->alt / 100.0f;
+ break;
+
+ case NAV_WP_ACTION_JUMP:
+ data.command = MAV_CMD_DO_JUMP;
+ data.param1 = (wp->p1 > 0) ? (float)(wp->p1 - 1) : 0.0f;
+ data.param2 = wp->p2;
+ break;
+
+ case NAV_WP_ACTION_SET_POI:
+ data.command = MAV_CMD_DO_SET_ROI;
+ data.param1 = MAV_ROI_LOCATION;
+ data.lat = wp->lat;
+ data.lon = wp->lon;
+ data.alt = wp->alt / 100.0f;
+ break;
+
+ case NAV_WP_ACTION_SET_HEAD:
+ data.command = MAV_CMD_CONDITION_YAW;
+ data.param1 = wp->p1;
+ break;
+
+ default:
+ return false;
+ }
+
+ *item = data;
+ return true;
+}
+
static bool handleIncoming_MISSION_REQUEST(void)
{
mavlink_mission_request_t msg;
mavlink_msg_mission_request_decode(&mavRecvMsg, &msg);
- // Check if this message is for us
- if (msg.target_system == mavSystemId) {
- int wpCount = getWaypointCount();
+ if (msg.target_system != mavSystemId) {
+ return false;
+ }
+
+ int wpCount = getWaypointCount();
- if (msg.seq < wpCount) {
- navWaypoint_t wp;
- getWaypoint(msg.seq + 1, &wp);
+ if (msg.seq < wpCount) {
+ navWaypoint_t wp;
+ getWaypoint(msg.seq + 1, &wp);
+ mavlinkMissionItemData_t item;
+ if (fillMavlinkMissionItemFromWaypoint(&wp, false, &item)) {
mavlink_msg_mission_item_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid,
msg.seq,
- wp.action == NAV_WP_ACTION_RTH ? MAV_FRAME_MISSION : MAV_FRAME_GLOBAL_RELATIVE_ALT,
- wp.action == NAV_WP_ACTION_RTH ? MAV_CMD_NAV_RETURN_TO_LAUNCH : MAV_CMD_NAV_WAYPOINT,
+ item.frame,
+ item.command,
0,
1,
- 0, 0, 0, 0,
- wp.lat / 1e7f,
- wp.lon / 1e7f,
- wp.alt / 100.0f,
+ item.param1, item.param2, item.param3, item.param4,
+ item.lat / 1e7f,
+ item.lon / 1e7f,
+ item.alt,
MAV_MISSION_TYPE_MISSION);
mavlinkSendMessage();
- }
- else {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0);
+ } else {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
mavlinkSendMessage();
}
-
- return true;
+ }
+ else {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
}
- return false;
+ return true;
}
-static bool handleIncoming_COMMAND_INT(void)
+static bool mavlinkMessageToStream(uint16_t messageId, uint8_t *streamNum)
{
- mavlink_command_int_t msg;
- mavlink_msg_command_int_decode(&mavRecvMsg, &msg);
+ switch (messageId) {
+ case MAVLINK_MSG_ID_HEARTBEAT:
+ case MAVLINK_MSG_ID_VFR_HUD:
+ *streamNum = MAV_DATA_STREAM_EXTRA2;
+ return true;
+ case MAVLINK_MSG_ID_ATTITUDE:
+ *streamNum = MAV_DATA_STREAM_EXTRA1;
+ return true;
+ case MAVLINK_MSG_ID_SYS_STATUS:
+ *streamNum = MAV_DATA_STREAM_EXTENDED_STATUS;
+ return true;
+ case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
+ *streamNum = MAV_DATA_STREAM_EXTENDED_SYS_STATE;
+ return true;
+ case MAVLINK_MSG_ID_RC_CHANNELS:
+ case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
+ case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
+ *streamNum = MAV_DATA_STREAM_RC_CHANNELS;
+ return true;
+ case MAVLINK_MSG_ID_GPS_RAW_INT:
+ case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
+ case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN:
+ *streamNum = MAV_DATA_STREAM_POSITION;
+ return true;
+ case MAVLINK_MSG_ID_BATTERY_STATUS:
+ case MAVLINK_MSG_ID_SCALED_PRESSURE:
+ *streamNum = MAV_DATA_STREAM_EXTRA3;
+ return true;
+ default:
+ return false;
+ }
+}
- if (msg.target_system == mavSystemId) {
- if (msg.command == MAV_CMD_DO_REPOSITION) {
-
- if (!(msg.frame == MAV_FRAME_GLOBAL)) { //|| msg.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || msg.frame == MAV_FRAME_GLOBAL_TERRAIN_ALT)) {
-
- mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
- msg.command,
- MAV_RESULT_UNSUPPORTED,
- 0, // progress
- 0, // result_param2
- mavRecvMsg.sysid,
- mavRecvMsg.compid);
- mavlinkSendMessage();
- return true;
- }
+static void mavlinkSendCommandAck(uint16_t command, MAV_RESULT result, uint8_t ackTargetSystem, uint8_t ackTargetComponent)
+{
+ mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
+ command,
+ result,
+ 0,
+ 0,
+ ackTargetSystem,
+ ackTargetComponent);
+ mavlinkSendMessage();
+}
+
+static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem, uint8_t ackTargetComponent, uint16_t command, uint8_t frame, float param1, float param2, float param3, float param4, float latitudeDeg, float longitudeDeg, float altitudeMeters)
+{
+ if (targetSystem != mavSystemId) {
+ return false;
+ }
+ UNUSED(param3);
+
+ switch (command) {
+ case MAV_CMD_DO_REPOSITION:
+ if (!mavlinkFrameIsSupported(frame,
+ MAV_FRAME_SUPPORTED_GLOBAL |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+ MAV_FRAME_SUPPORTED_GLOBAL_INT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+ mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
+ return true;
+ }
+
+ if (isnan(latitudeDeg) || isnan(longitudeDeg)) {
+ mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent);
+ return true;
+ }
if (isGCSValid()) {
- navWaypoint_t wp;
+ navWaypoint_t wp = {0};
wp.action = NAV_WP_ACTION_WAYPOINT;
- wp.lat = (int32_t)msg.x;
- wp.lon = (int32_t)msg.y;
- wp.alt = msg.z * 100.0f;
- if (!isnan(msg.param4) && msg.param4 >= 0.0f && msg.param4 < 360.0f) {
- wp.p1 = (int16_t)msg.param4;
+ wp.lat = (int32_t)(latitudeDeg * 1e7f);
+ wp.lon = (int32_t)(longitudeDeg * 1e7f);
+ wp.alt = (int32_t)(altitudeMeters * 100.0f);
+ if (!isnan(param4) && param4 >= 0.0f && param4 < 360.0f) {
+ wp.p1 = (int16_t)param4;
} else {
wp.p1 = 0;
}
- wp.p2 = 0; // TODO: Alt modes
- wp.p3 = 0;
+ wp.p2 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
wp.flag = 0;
setWaypoint(255, &wp);
- mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
- msg.command,
- MAV_RESULT_ACCEPTED,
- 0, // progress
- 0, // result_param2
- mavRecvMsg.sysid,
- mavRecvMsg.compid);
- mavlinkSendMessage();
+ mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
} else {
- mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
- msg.command,
- MAV_RESULT_DENIED,
- 0,
- 0,
- mavRecvMsg.sysid,
- mavRecvMsg.compid);
- mavlinkSendMessage();
+ mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent);
}
- } else {
-#ifdef USE_BARO
- if (msg.command == MAV_CMD_DO_CHANGE_ALTITUDE) {
- const float altitudeMeters = msg.param1;
- const uint8_t frame = (uint8_t)msg.frame;
-
- geoAltitudeDatumFlag_e datum;
- switch (frame) {
- case MAV_FRAME_GLOBAL:
- case MAV_FRAME_GLOBAL_INT:
- datum = NAV_WP_MSL_DATUM;
- break;
- case MAV_FRAME_GLOBAL_RELATIVE_ALT:
- case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
- datum = NAV_WP_TAKEOFF_DATUM;
- break;
- default:
- mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
- msg.command,
- MAV_RESULT_UNSUPPORTED,
- 0,
- 0,
- mavRecvMsg.sysid,
- mavRecvMsg.compid);
- mavlinkSendMessage();
+ return true;
+ case MAV_CMD_DO_CHANGE_ALTITUDE:
+ {
+ const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame((uint8_t)lrintf(param2), param1);
+ mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent);
+ return true;
+ }
+ case MAV_CMD_SET_MESSAGE_INTERVAL:
+ {
+ uint8_t stream;
+ MAV_RESULT result = MAV_RESULT_UNSUPPORTED;
+
+ if (mavlinkMessageToStream((uint16_t)param1, &stream)) {
+ if (param2 == 0.0f) {
+ mavlinkSetStreamRate(stream, mavRatesConfigured[stream]);
+ result = MAV_RESULT_ACCEPTED;
+ } else if (param2 < 0.0f) {
+ mavlinkSetStreamRate(stream, 0);
+ result = MAV_RESULT_ACCEPTED;
+ } else {
+ uint32_t intervalUs = (uint32_t)param2;
+ if (intervalUs > 0) {
+ uint32_t rate = 1000000UL / intervalUs;
+ if (rate > 0) {
+ if (rate > TELEMETRY_MAVLINK_MAXRATE) {
+ rate = TELEMETRY_MAVLINK_MAXRATE;
+ }
+ mavlinkSetStreamRate(stream, rate);
+ result = MAV_RESULT_ACCEPTED;
+ }
+ }
+ }
+ }
+
+ mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent);
+ return true;
+ }
+ case MAV_CMD_GET_MESSAGE_INTERVAL:
+ {
+ uint8_t stream;
+ if (!mavlinkMessageToStream((uint16_t)param1, &stream)) {
+ mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
return true;
}
- const int32_t targetAltitudeCm = (int32_t)lrintf(altitudeMeters * 100.0f);
- const bool accepted = navigationSetAltitudeTargetWithDatum(datum, targetAltitudeCm);
- mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
- msg.command,
- accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED,
- 0,
- 0,
- mavRecvMsg.sysid,
- mavRecvMsg.compid);
- mavlinkSendMessage();
+ mavlink_msg_message_interval_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ (uint16_t)param1,
+ mavlinkStreamIntervalUs(stream));
+ mavlinkSendMessage();
+
+ mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
return true;
}
+ case MAV_CMD_REQUEST_PROTOCOL_VERSION:
+ if (telemetryConfig()->mavlink.version == 1) {
+ mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
+ } else {
+ mavlinkSendProtocolVersion();
+ mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
+ }
+ return true;
+ case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
+ if (telemetryConfig()->mavlink.version == 1) {
+ mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
+ } else {
+ mavlinkSendAutopilotVersion();
+ mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
+ }
+ return true;
+ case MAV_CMD_REQUEST_MESSAGE:
+ {
+ bool sent = false;
+ uint16_t messageId = (uint16_t)param1;
+
+ switch (messageId) {
+ case MAVLINK_MSG_ID_HEARTBEAT:
+ mavlinkSendHeartbeat();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
+ if (telemetryConfig()->mavlink.version != 1) {
+ mavlinkSendAutopilotVersion();
+ sent = true;
+ }
+ break;
+ case MAVLINK_MSG_ID_PROTOCOL_VERSION:
+ if (telemetryConfig()->mavlink.version != 1) {
+ mavlinkSendProtocolVersion();
+ sent = true;
+ }
+ break;
+ case MAVLINK_MSG_ID_SYS_STATUS:
+ mavlinkSendSystemStatus();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_ATTITUDE:
+ mavlinkSendAttitude();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_VFR_HUD:
+ mavlinkSendVfrHud();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_AVAILABLE_MODES:
+ {
+ const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY);
+ const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane);
+ if (isPlane) {
+ mavlinkSendAvailableModes(planeModes, ARRAYLEN(planeModes), modeSelection.customMode, mavlinkPlaneModeIsConfigured);
+ } else {
+ mavlinkSendAvailableModes(copterModes, ARRAYLEN(copterModes), modeSelection.customMode, mavlinkCopterModeIsConfigured);
+ }
+ sent = true;
+ }
+ break;
+ case MAVLINK_MSG_ID_CURRENT_MODE:
+ {
+ const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY);
+ const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane);
+ mavlink_msg_current_mode_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ MAV_STANDARD_MODE_NON_STANDARD,
+ modeSelection.customMode,
+ modeSelection.customMode);
+ mavlinkSendMessage();
+ sent = true;
+ }
+ break;
+ case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
+ mavlinkSendExtendedSysState();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_RC_CHANNELS:
+ case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
+ case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
+ mavlinkSendRCChannelsAndRSSI();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_GPS_RAW_INT:
+ case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
+ case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN:
+ #ifdef USE_GPS
+ mavlinkSendPosition(micros());
+ sent = true;
+ #endif
+ break;
+ case MAVLINK_MSG_ID_BATTERY_STATUS:
+ case MAVLINK_MSG_ID_SCALED_PRESSURE:
+ mavlinkSendBatteryTemperatureStatusText();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_HOME_POSITION:
+ #ifdef USE_GPS
+ if (STATE(GPS_FIX_HOME)) {
+ mavlinkSendHomePosition();
+ sent = true;
+ }
+ #endif
+ break;
+ default:
+ break;
+ }
+
+ mavlinkSendCommandAck(command, sent ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
+ return true;
+ }
+#ifdef USE_GPS
+ case MAV_CMD_GET_HOME_POSITION:
+ if (STATE(GPS_FIX_HOME)) {
+ mavlinkSendHomePosition();
+ mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
+ } else {
+ mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent);
+ }
+ return true;
#endif
- mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
- msg.command,
- MAV_RESULT_UNSUPPORTED,
- 0,
- 0,
- mavRecvMsg.sysid,
- mavRecvMsg.compid);
+ default:
+ mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent);
+ return true;
+ }
+}
+
+static bool handleIncoming_COMMAND_INT(void)
+{
+ mavlink_command_int_t msg;
+ mavlink_msg_command_int_decode(&mavRecvMsg, &msg);
+
+ return handleIncoming_COMMAND(msg.target_system, mavRecvMsg.sysid, mavRecvMsg.compid, msg.command, msg.frame, msg.param1, msg.param2, msg.param3, msg.param4, (float)msg.x / 1e7f, (float)msg.y / 1e7f, msg.z);
+}
+
+static bool handleIncoming_COMMAND_LONG(void)
+{
+ mavlink_command_long_t msg;
+ mavlink_msg_command_long_decode(&mavRecvMsg, &msg);
+
+ // COMMAND_LONG has no frame field; location commands are WGS84 global by definition.
+ return handleIncoming_COMMAND(msg.target_system, mavRecvMsg.sysid, mavRecvMsg.compid, msg.command, MAV_FRAME_GLOBAL, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7);
+}
+
+static bool handleIncoming_MISSION_ITEM_INT(void)
+{
+ mavlink_mission_item_int_t msg;
+ mavlink_msg_mission_item_int_decode(&mavRecvMsg, &msg);
+
+ if (msg.target_system != mavSystemId) {
+ return false;
+ }
+
+ if (ARMING_FLAG(ARMED)) {
+ if (msg.command == MAV_CMD_NAV_WAYPOINT) {
+ return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame,
+ MAV_FRAME_SUPPORTED_GLOBAL_INT | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT,
+ msg.x, msg.y, msg.z);
+ }
+
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ return true;
+ }
+
+ return mavlinkHandleMissionItemCommon(true, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, msg.x, msg.y, msg.z);
+}
+
+static bool handleIncoming_MISSION_REQUEST_INT(void)
+{
+ mavlink_mission_request_int_t msg;
+ mavlink_msg_mission_request_int_decode(&mavRecvMsg, &msg);
+
+ if (msg.target_system != mavSystemId) {
+ return false;
+ }
+
+ int wpCount = getWaypointCount();
+
+ if (msg.seq < wpCount) {
+ navWaypoint_t wp;
+ getWaypoint(msg.seq + 1, &wp);
+
+ mavlinkMissionItemData_t item;
+ if (fillMavlinkMissionItemFromWaypoint(&wp, true, &item)) {
+ mavlink_msg_mission_item_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid,
+ msg.seq,
+ item.frame,
+ item.command,
+ 0,
+ 1,
+ item.param1, item.param2, item.param3, item.param4,
+ item.lat,
+ item.lon,
+ item.alt,
+ MAV_MISSION_TYPE_MISSION);
mavlinkSendMessage();
+ } else {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ }
+ }
+ else {
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0);
+ mavlinkSendMessage();
+ }
+
+ return true;
+}
+
+static bool handleIncoming_REQUEST_DATA_STREAM(void)
+{
+ mavlink_request_data_stream_t msg;
+ mavlink_msg_request_data_stream_decode(&mavRecvMsg, &msg);
+
+ if (msg.target_system != mavSystemId) {
+ return false;
+ }
+
+ uint8_t rate = 0;
+ if (msg.start_stop != 0) {
+ rate = (uint8_t)msg.req_message_rate;
+ if (rate > TELEMETRY_MAVLINK_MAXRATE) {
+ rate = TELEMETRY_MAVLINK_MAXRATE;
}
+ }
+
+ if (msg.req_stream_id == MAV_DATA_STREAM_ALL) {
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_STATUS, rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_RC_CHANNELS, rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_POSITION, rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA1, rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA2, rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA3, rate);
+ mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_SYS_STATE, rate);
return true;
}
- return false;
+
+ mavlinkSetStreamRate(msg.req_stream_id, rate);
+ return true;
+}
+
+static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void)
+{
+ mavlink_set_position_target_global_int_t msg;
+ mavlink_msg_set_position_target_global_int_decode(&mavRecvMsg, &msg);
+
+ if (msg.target_system != mavSystemId) {
+ return false;
+ }
+ if (msg.target_component != 0 && msg.target_component != mavComponentId) {
+ return false;
+ }
+
+ uint8_t frame = msg.coordinate_frame;
+ if (!mavlinkFrameIsSupported(frame,
+ MAV_FRAME_SUPPORTED_GLOBAL |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+ MAV_FRAME_SUPPORTED_GLOBAL_INT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
+ return true;
+ }
+
+ const uint16_t typeMask = msg.type_mask;
+ const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0;
+ const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0;
+ const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0;
+
+ // Altitude-only SET_POSITION_TARGET_GLOBAL_INT mirrors MAV_CMD_DO_CHANGE_ALTITUDE semantics.
+ if (xIgnored && yIgnored && !zIgnored) {
+ if (isGCSValid()) {
+ mavlinkSetAltitudeTargetFromFrame(frame, msg.alt);
+ }
+ return true;
+ }
+
+ if (xIgnored || yIgnored) {
+ return true;
+ }
+
+ if (isGCSValid()) {
+ navWaypoint_t wp = {0};
+ wp.action = NAV_WP_ACTION_WAYPOINT;
+ wp.lat = msg.lat_int;
+ wp.lon = msg.lon_int;
+ wp.alt = zIgnored ? 0 : (int32_t)(msg.alt * 100.0f);
+ wp.p1 = 0;
+ wp.p2 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ wp.flag = 0;
+
+ setWaypoint(255, &wp);
+ }
+
+ return true;
+}
+
+static bool handleIncoming_SET_POSITION_TARGET_LOCAL_NED(void)
+{
+ mavlink_set_position_target_local_ned_t msg;
+ mavlink_msg_set_position_target_local_ned_decode(&mavRecvMsg, &msg);
+
+ if (msg.target_system != mavSystemId) {
+ return false;
+ }
+ if (msg.target_component != 0 && msg.target_component != mavComponentId) {
+ return false;
+ }
+
+ if (msg.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) {
+ return true;
+ }
+
+ const uint16_t typeMask = msg.type_mask;
+ const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0;
+ const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0;
+ const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0;
+
+ if (!isGCSValid() || zIgnored) {
+ return true;
+ }
+
+ if ((!xIgnored && fabsf(msg.x) > 0.01f) || (!yIgnored && fabsf(msg.y) > 0.01f)) {
+ return true;
+ }
+
+ const int32_t targetAltitudeCm = (int32_t)lrintf((float)getEstimatedActualPosition(Z) - (msg.z * 100.0f));
+ navigationSetAltitudeTargetWithDatum(NAV_WP_TAKEOFF_DATUM, targetAltitudeCm);
+
+ return true;
}
static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) {
mavlink_rc_channels_override_t msg;
mavlink_msg_rc_channels_override_decode(&mavRecvMsg, &msg);
- // Don't check system ID because it's not configurable with systems like Crossfire
+ /*if (msg.target_system != mavSystemId) { // This caused a lot of headache, when enforced, it causes an rx failsafe when mavproxy turned on
+ return false;
+ }*/
mavlinkRxHandleMessage(&msg);
return true;
}
@@ -1388,21 +2442,29 @@ static bool processMAVLinkIncomingTelemetry(void)
return handleIncoming_MISSION_COUNT();
case MAVLINK_MSG_ID_MISSION_ITEM:
return handleIncoming_MISSION_ITEM();
+ case MAVLINK_MSG_ID_MISSION_ITEM_INT:
+ return handleIncoming_MISSION_ITEM_INT();
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
return handleIncoming_MISSION_REQUEST_LIST();
- //TODO:
- //case MAVLINK_MSG_ID_COMMAND_LONG; //up to 7 float parameters
- //return handleIncoming_COMMAND_LONG();
-
+ case MAVLINK_MSG_ID_COMMAND_LONG:
+ return handleIncoming_COMMAND_LONG();
case MAVLINK_MSG_ID_COMMAND_INT: //7 parameters: parameters 1-4, 7 are floats, and parameters 5,6 are scaled integers
return handleIncoming_COMMAND_INT();
case MAVLINK_MSG_ID_MISSION_REQUEST:
return handleIncoming_MISSION_REQUEST();
+ case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
+ return handleIncoming_MISSION_REQUEST_INT();
+ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+ return handleIncoming_REQUEST_DATA_STREAM();
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
handleIncoming_RC_CHANNELS_OVERRIDE();
// Don't set that we handled a message, otherwise RC channel packets will block telemetry messages
return false;
+ case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
+ return handleIncoming_SET_POSITION_TARGET_LOCAL_NED();
+ case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
+ return handleIncoming_SET_POSITION_TARGET_GLOBAL_INT();
#ifdef USE_ADSB
case MAVLINK_MSG_ID_ADSB_VEHICLE:
return handleIncoming_ADSB_VEHICLE();
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 2cb53b64ecb..f377b24a80e 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -43,6 +43,12 @@ set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TE
set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c")
set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER)
+set_property(SOURCE mavlink_unittest.cc PROPERTY depends
+ "telemetry/mavlink.c" "common/maths.c")
+set_property(SOURCE mavlink_unittest.cc PROPERTY definitions USE_TELEMETRY USE_TELEMETRY_MAVLINK)
+set_property(SOURCE mavlink_unittest.cc PROPERTY includes
+ "${CMAKE_CURRENT_SOURCE_DIR}/../../../lib/main/MAVLink")
+
function(unit_test src)
get_filename_component(basename ${src} NAME)
string(REPLACE ".cc" "" name ${basename} )
@@ -51,6 +57,7 @@ function(unit_test src)
list(TRANSFORM headers REPLACE "\.c$" ".h")
list(APPEND deps ${headers})
get_property(defs SOURCE ${src} PROPERTY definitions)
+ get_property(extra_includes SOURCE ${src} PROPERTY includes)
set(test_definitions "UNIT_TEST")
if (defs)
list(APPEND test_definitions ${defs})
@@ -60,6 +67,9 @@ function(unit_test src)
set(gen_name ${name}_gen)
get_generated_files_dir(gen ${gen_name})
target_include_directories(${name} PRIVATE . ${MAIN_DIR} ${gen})
+ if (extra_includes)
+ target_include_directories(${name} PRIVATE ${extra_includes})
+ endif()
target_compile_definitions(${name} PRIVATE ${test_definitions})
target_compile_options(${name} PRIVATE -pthread -Wall -Wextra -Wno-extern-c-compat -ggdb3 -O0)
enable_settings(${name} ${gen_name} OUTPUTS setting_files SETTINGS_CXX g++)
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
new file mode 100644
index 00000000000..6e09307247f
--- /dev/null
+++ b/src/test/unit/mavlink_unittest.cc
@@ -0,0 +1,1045 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+#include
+
+#include
+
+extern "C" {
+ #include "platform.h"
+
+ #include "build/debug.h"
+
+ #include "common/axis.h"
+ #include "common/maths.h"
+ #include "common/time.h"
+
+ #include "config/parameter_group.h"
+ #include "config/parameter_group_ids.h"
+
+ #include "drivers/display.h"
+ #include "drivers/osd_symbols.h"
+ #include "drivers/serial.h"
+
+ #include "fc/config.h"
+ #include "fc/rc_modes.h"
+ #include "fc/runtime_config.h"
+ #include "fc/settings.h"
+
+ #include "flight/failsafe.h"
+ #include "flight/imu.h"
+ #include "flight/mixer.h"
+ #include "flight/mixer_profile.h"
+
+ #include "io/adsb.h"
+ #include "io/gps.h"
+ #include "io/osd.h"
+
+ #include "navigation/navigation.h"
+
+ #include "rx/rx.h"
+
+ #include "sensors/barometer.h"
+ #include "sensors/battery.h"
+ #include "sensors/diagnostics.h"
+ #include "sensors/gyro.h"
+ #include "sensors/pitotmeter.h"
+ #include "sensors/temperature.h"
+
+ #include "telemetry/mavlink.h"
+ #include "telemetry/telemetry.h"
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-function"
+ #define MAVLINK_COMM_NUM_BUFFERS 1
+ #include "common/mavlink.h"
+#pragma GCC diagnostic pop
+
+ void mavlinkSendAttitude(void);
+ void mavlinkSendBatteryTemperatureStatusText(void);
+
+ PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
+ PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
+ PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
+ PG_REGISTER_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 0);
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+static serialPort_t testSerialPort;
+static serialPortConfig_t testPortConfig;
+static uint8_t serialRxBuffer[2048];
+static uint8_t serialTxBuffer[2048];
+static size_t serialRxLen;
+static size_t serialRxPos;
+static size_t serialTxLen;
+
+const uint32_t baudRates[] = {
+ 0, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 250000,
+ 460800, 921600, 1000000, 1500000, 2000000, 2470000
+};
+
+static navWaypoint_t lastWaypoint;
+static int setWaypointCalls;
+static int resetWaypointCalls;
+static int mavlinkRxHandleCalls;
+static bool gcsValid;
+static int waypointCount;
+static navWaypoint_t waypointStore[4];
+
+static void resetSerialBuffers(void)
+{
+ serialRxLen = 0;
+ serialRxPos = 0;
+ serialTxLen = 0;
+}
+
+static void pushRxMessage(const mavlink_message_t *msg)
+{
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+ int length = mavlink_msg_to_send_buffer(buffer, msg);
+ memcpy(&serialRxBuffer[serialRxLen], buffer, (size_t)length);
+ serialRxLen += (size_t)length;
+}
+
+static bool popTxMessage(mavlink_message_t *msg)
+{
+ mavlink_status_t status;
+ memset(&status, 0, sizeof(status));
+ for (size_t i = 0; i < serialTxLen; i++) {
+ if (mavlink_parse_char(0, serialTxBuffer[i], msg, &status) == MAVLINK_FRAMING_OK) {
+ return true;
+ }
+ }
+ return false;
+}
+
+static void initMavlinkTestState(void)
+{
+ resetSerialBuffers();
+ setWaypointCalls = 0;
+ resetWaypointCalls = 0;
+ mavlinkRxHandleCalls = 0;
+ gcsValid = true;
+ waypointCount = 0;
+ memset(waypointStore, 0, sizeof(waypointStore));
+ memset(&rxLinkStatistics, 0, sizeof(rxLinkStatistics));
+
+ telemetryConfigMutable()->mavlink.sysid = 1;
+ telemetryConfigMutable()->mavlink.autopilot_type = MAVLINK_AUTOPILOT_ARDUPILOT;
+ telemetryConfigMutable()->mavlink.version = 2;
+ telemetryConfigMutable()->mavlink.min_txbuff = 0;
+ telemetryConfigMutable()->halfDuplex = 0;
+
+ rxConfigMutable()->receiverType = RX_TYPE_NONE;
+ rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
+ rxConfigMutable()->halfDuplex = 0;
+
+ systemConfigMutable()->current_mixer_profile_index = 0;
+ mixerProfilesMutable(0)->mixer_config.platformType = PLATFORM_AIRPLANE;
+
+ rxRuntimeConfig.channelCount = 8;
+
+ initMAVLinkTelemetry();
+ configureMAVLinkTelemetryPort();
+}
+
+TEST(MavlinkTelemetryTest, AttitudeUsesRadiansPerSecond)
+{
+ initMavlinkTestState();
+
+ attitude.values.roll = 100;
+ attitude.values.pitch = -200;
+ attitude.values.yaw = 450;
+ gyro.gyroADCf[FD_ROLL] = 90.0f;
+ gyro.gyroADCf[FD_PITCH] = -45.0f;
+ gyro.gyroADCf[FD_YAW] = 180.0f;
+
+ mavlinkSendAttitude();
+
+ mavlink_message_t msg;
+ ASSERT_TRUE(popTxMessage(&msg));
+ ASSERT_EQ(msg.msgid, MAVLINK_MSG_ID_ATTITUDE);
+
+ mavlink_attitude_t att;
+ mavlink_msg_attitude_decode(&msg, &att);
+
+ EXPECT_NEAR(att.rollspeed, DEGREES_TO_RADIANS(gyro.gyroADCf[FD_ROLL]), 1e-6f);
+ EXPECT_NEAR(att.pitchspeed, DEGREES_TO_RADIANS(gyro.gyroADCf[FD_PITCH]), 1e-6f);
+ EXPECT_NEAR(att.yawspeed, DEGREES_TO_RADIANS(gyro.gyroADCf[FD_YAW]), 1e-6f);
+}
+
+TEST(MavlinkTelemetryTest, CommandLongRepositionUsesGlobalFrameAndParams)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t cmd;
+ mavlink_msg_command_long_pack(
+ 42, 200, &cmd,
+ 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_CMD_DO_REPOSITION,
+ 0,
+ 0, 0, 0, 123.4f,
+ 37.5f, -122.25f, 12.3f);
+
+ pushRxMessage(&cmd);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t ackMsg;
+ ASSERT_TRUE(popTxMessage(&ackMsg));
+ ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_COMMAND_ACK);
+
+ mavlink_command_ack_t ack;
+ mavlink_msg_command_ack_decode(&ackMsg, &ack);
+
+ EXPECT_EQ(ack.command, MAV_CMD_DO_REPOSITION);
+ EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED);
+ EXPECT_EQ(setWaypointCalls, 1);
+ EXPECT_EQ(lastWaypoint.lat, (int32_t)(37.5f * 1e7f));
+ EXPECT_EQ(lastWaypoint.lon, (int32_t)(-122.25f * 1e7f));
+ EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f));
+ EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE);
+ EXPECT_EQ(lastWaypoint.p1, 123);
+}
+
+TEST(MavlinkTelemetryTest, CommandIntUnsupportedFrameIsRejected)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t cmd;
+ mavlink_msg_command_int_pack(
+ 42, 200, &cmd,
+ 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_FRAME_BODY_NED,
+ MAV_CMD_DO_REPOSITION,
+ 0, 0,
+ 0, 0, 0, 0,
+ 100000000, 200000000, 10.0f);
+
+ pushRxMessage(&cmd);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t ackMsg;
+ ASSERT_TRUE(popTxMessage(&ackMsg));
+ ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_COMMAND_ACK);
+
+ mavlink_command_ack_t ack;
+ mavlink_msg_command_ack_decode(&ackMsg, &ack);
+
+ EXPECT_EQ(ack.command, MAV_CMD_DO_REPOSITION);
+ EXPECT_EQ(ack.result, MAV_RESULT_UNSUPPORTED);
+ EXPECT_EQ(setWaypointCalls, 0);
+}
+
+TEST(MavlinkTelemetryTest, CommandIntRepositionScalesCoordinates)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t cmd;
+ mavlink_msg_command_int_pack(
+ 42, 200, &cmd,
+ 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+ MAV_CMD_DO_REPOSITION,
+ 0, 0,
+ 0, 0, 0, 45.6f,
+ 375000000, -1222500000, 12.3f);
+
+ pushRxMessage(&cmd);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t ackMsg;
+ ASSERT_TRUE(popTxMessage(&ackMsg));
+ ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_COMMAND_ACK);
+
+ mavlink_command_ack_t ack;
+ mavlink_msg_command_ack_decode(&ackMsg, &ack);
+
+ EXPECT_EQ(ack.command, MAV_CMD_DO_REPOSITION);
+ EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED);
+ EXPECT_EQ(setWaypointCalls, 1);
+ EXPECT_EQ(lastWaypoint.lat, 375000000);
+ EXPECT_NEAR((double)lastWaypoint.lon, -1222500000.0, 100.0);
+ EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f));
+ EXPECT_EQ(lastWaypoint.p3, 0);
+ EXPECT_EQ(lastWaypoint.p1, 45);
+}
+
+TEST(MavlinkTelemetryTest, MissionClearAllAcksAndResets)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t msg;
+ mavlink_msg_mission_clear_all_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER, MAV_MISSION_TYPE_MISSION);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ EXPECT_EQ(resetWaypointCalls, 1);
+
+ mavlink_message_t ackMsg;
+ ASSERT_TRUE(popTxMessage(&ackMsg));
+ ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
+
+ mavlink_mission_ack_t ack;
+ mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+
+ EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED);
+ EXPECT_EQ(ack.mission_type, MAV_MISSION_TYPE_MISSION);
+}
+
+TEST(MavlinkTelemetryTest, MissionCountRequestsFirstItem)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t msg;
+ mavlink_msg_mission_count_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER, 1, MAV_MISSION_TYPE_MISSION, 0);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t reqMsg;
+ ASSERT_TRUE(popTxMessage(&reqMsg));
+ ASSERT_EQ(reqMsg.msgid, MAVLINK_MSG_ID_MISSION_REQUEST_INT);
+
+ mavlink_mission_request_int_t req;
+ mavlink_msg_mission_request_int_decode(&reqMsg, &req);
+
+ EXPECT_EQ(req.seq, 0);
+ EXPECT_EQ(req.mission_type, MAV_MISSION_TYPE_MISSION);
+}
+
+TEST(MavlinkTelemetryTest, MissionCountWhileArmedIsRejected)
+{
+ initMavlinkTestState();
+ ENABLE_ARMING_FLAG(ARMED);
+
+ mavlink_message_t msg;
+ mavlink_msg_mission_count_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER, 1, MAV_MISSION_TYPE_MISSION, 0);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_status_t status;
+ memset(&status, 0, sizeof(status));
+ mavlink_message_t outMsg;
+ bool sawAck = false;
+ bool sawRequest = false;
+
+ for (size_t i = 0; i < serialTxLen; i++) {
+ if (mavlink_parse_char(0, serialTxBuffer[i], &outMsg, &status) == MAVLINK_FRAMING_OK) {
+ if (outMsg.msgid == MAVLINK_MSG_ID_MISSION_ACK) {
+ mavlink_mission_ack_t ack;
+ mavlink_msg_mission_ack_decode(&outMsg, &ack);
+ EXPECT_EQ(ack.type, MAV_MISSION_ERROR);
+ sawAck = true;
+ }
+ if (outMsg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_INT) {
+ sawRequest = true;
+ }
+ }
+ }
+
+ EXPECT_TRUE(sawAck);
+ EXPECT_FALSE(sawRequest);
+}
+
+TEST(MavlinkTelemetryTest, MissionItemIntSingleItemAcksAccepted)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t countMsg;
+ mavlink_msg_mission_count_pack(
+ 42, 200, &countMsg,
+ 1, MAV_COMP_ID_MISSIONPLANNER, 1, MAV_MISSION_TYPE_MISSION, 0);
+
+ pushRxMessage(&countMsg);
+ handleMAVLinkTelemetry(1000);
+
+ serialTxLen = 0;
+
+ mavlink_message_t itemMsg;
+ mavlink_msg_mission_item_int_pack(
+ 42, 200, &itemMsg,
+ 1, MAV_COMP_ID_MISSIONPLANNER, 0,
+ MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+ MAV_CMD_NAV_WAYPOINT, 0, 1,
+ 0, 0, 0, 0,
+ 375000000, -1222500000, 12.3f,
+ MAV_MISSION_TYPE_MISSION);
+
+ pushRxMessage(&itemMsg);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t ackMsg;
+ ASSERT_TRUE(popTxMessage(&ackMsg));
+ ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK);
+
+ mavlink_mission_ack_t ack;
+ mavlink_msg_mission_ack_decode(&ackMsg, &ack);
+
+ EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED);
+ EXPECT_EQ(setWaypointCalls, 1);
+ EXPECT_EQ(lastWaypoint.lat, 375000000);
+ EXPECT_EQ(lastWaypoint.lon, -1222500000);
+ EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f));
+}
+
+TEST(MavlinkTelemetryTest, MissionItemIntGuidedWhileArmedUpdatesWaypoint)
+{
+ initMavlinkTestState();
+ ENABLE_ARMING_FLAG(ARMED);
+
+ mavlink_message_t msg;
+ mavlink_msg_mission_item_int_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER, 0,
+ MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+ MAV_CMD_NAV_WAYPOINT, 2, 1,
+ 0, 0, 0, 0,
+ 375000000, -1222500000, 12.3f,
+ MAV_MISSION_TYPE_MISSION);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ EXPECT_EQ(setWaypointCalls, 1);
+ EXPECT_EQ(lastWaypoint.lat, 375000000);
+ EXPECT_EQ(lastWaypoint.lon, -1222500000);
+ EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f));
+ EXPECT_EQ(lastWaypoint.p3, 0);
+}
+
+TEST(MavlinkTelemetryTest, MissionRequestListSendsCount)
+{
+ initMavlinkTestState();
+ waypointCount = 2;
+
+ mavlink_message_t msg;
+ mavlink_msg_mission_request_list_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER, MAV_MISSION_TYPE_MISSION);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t countMsg;
+ ASSERT_TRUE(popTxMessage(&countMsg));
+ ASSERT_EQ(countMsg.msgid, MAVLINK_MSG_ID_MISSION_COUNT);
+
+ mavlink_mission_count_t count;
+ mavlink_msg_mission_count_decode(&countMsg, &count);
+
+ EXPECT_EQ(count.count, 2);
+ EXPECT_EQ(count.mission_type, MAV_MISSION_TYPE_MISSION);
+}
+
+TEST(MavlinkTelemetryTest, MissionRequestSendsWaypoint)
+{
+ initMavlinkTestState();
+ waypointCount = 1;
+ waypointStore[0].action = NAV_WP_ACTION_WAYPOINT;
+ waypointStore[0].lat = 375000000;
+ waypointStore[0].lon = -1222500000;
+ waypointStore[0].alt = 1234;
+ waypointStore[0].p3 = 0;
+
+ mavlink_message_t msg;
+ mavlink_msg_mission_request_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER, 0, MAV_MISSION_TYPE_MISSION);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t itemMsg;
+ ASSERT_TRUE(popTxMessage(&itemMsg));
+ ASSERT_EQ(itemMsg.msgid, MAVLINK_MSG_ID_MISSION_ITEM);
+
+ mavlink_mission_item_t item;
+ mavlink_msg_mission_item_decode(&itemMsg, &item);
+
+ EXPECT_EQ(item.seq, 0);
+ EXPECT_EQ(item.command, MAV_CMD_NAV_WAYPOINT);
+ EXPECT_EQ(item.frame, MAV_FRAME_GLOBAL_RELATIVE_ALT);
+ EXPECT_NEAR(item.x, 37.5f, 1e-4f);
+ EXPECT_NEAR(item.y, -122.25f, 1e-4f);
+ EXPECT_NEAR(item.z, 12.34f, 1e-4f);
+}
+
+TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesAbsoluteAltitude)
+{
+ initMavlinkTestState();
+ ENABLE_ARMING_FLAG(ARMED);
+
+ mavlink_message_t msg;
+ mavlink_msg_mission_item_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER, 0,
+ MAV_FRAME_GLOBAL,
+ MAV_CMD_NAV_WAYPOINT, 2, 1,
+ 0, 0, 0, 0,
+ 37.5f, -122.25f, 12.3f,
+ MAV_MISSION_TYPE_MISSION);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ EXPECT_EQ(setWaypointCalls, 1);
+ EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE);
+}
+
+TEST(MavlinkTelemetryTest, ParamRequestListRespondsWithEmptyParam)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t msg;
+ mavlink_msg_param_request_list_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t paramMsg;
+ ASSERT_TRUE(popTxMessage(¶mMsg));
+ ASSERT_EQ(paramMsg.msgid, MAVLINK_MSG_ID_PARAM_VALUE);
+
+ mavlink_param_value_t param;
+ mavlink_msg_param_value_decode(¶mMsg, ¶m);
+
+ EXPECT_EQ(param.param_count, 0);
+ EXPECT_EQ(param.param_index, 0);
+}
+
+TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntSetsWaypoint)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t msg;
+ mavlink_msg_set_position_target_global_int_pack(
+ 42, 200, &msg,
+ 0, 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0,
+ 375000000, -1222500000, 12.3f,
+ 0, 0, 0, 0, 0, 0, 0, 0);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ EXPECT_EQ(setWaypointCalls, 1);
+ EXPECT_EQ(lastWaypoint.lat, 375000000);
+ EXPECT_EQ(lastWaypoint.lon, -1222500000);
+ EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f));
+ EXPECT_EQ(lastWaypoint.p3, 0);
+}
+
+TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t msg;
+ mavlink_msg_set_position_target_global_int_pack(
+ 42, 200, &msg,
+ 0, 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_FRAME_GLOBAL_INT, 0,
+ 375000000, -1222500000, 12.3f,
+ 0, 0, 0, 0, 0, 0, 0, 0);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ EXPECT_EQ(setWaypointCalls, 1);
+ EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE);
+}
+
+TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t setMsg;
+ mavlink_msg_request_data_stream_pack(
+ 42, 200, &setMsg,
+ 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_DATA_STREAM_RC_CHANNELS, 10, 1);
+
+ pushRxMessage(&setMsg);
+ handleMAVLinkTelemetry(1000);
+
+ serialTxLen = 0;
+
+ mavlink_message_t getMsg;
+ mavlink_msg_command_long_pack(
+ 42, 200, &getMsg,
+ 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_CMD_GET_MESSAGE_INTERVAL,
+ 0,
+ (float)MAVLINK_MSG_ID_RC_CHANNELS,
+ 0, 0, 0, 0, 0, 0);
+
+ pushRxMessage(&getMsg);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t intervalMsg;
+ ASSERT_TRUE(popTxMessage(&intervalMsg));
+ ASSERT_EQ(intervalMsg.msgid, MAVLINK_MSG_ID_MESSAGE_INTERVAL);
+
+ mavlink_message_interval_t interval;
+ mavlink_msg_message_interval_decode(&intervalMsg, &interval);
+
+ EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_RC_CHANNELS);
+ EXPECT_EQ(interval.interval_us, 100000);
+
+ serialTxLen = 0;
+
+ mavlink_message_t stopMsg;
+ mavlink_msg_request_data_stream_pack(
+ 42, 200, &stopMsg,
+ 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_DATA_STREAM_RC_CHANNELS, 0, 0);
+
+ pushRxMessage(&stopMsg);
+ handleMAVLinkTelemetry(1000);
+
+ serialTxLen = 0;
+
+ pushRxMessage(&getMsg);
+ handleMAVLinkTelemetry(1000);
+
+ ASSERT_TRUE(popTxMessage(&intervalMsg));
+ ASSERT_EQ(intervalMsg.msgid, MAVLINK_MSG_ID_MESSAGE_INTERVAL);
+
+ mavlink_msg_message_interval_decode(&intervalMsg, &interval);
+
+ EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_RC_CHANNELS);
+ EXPECT_EQ(interval.interval_us, -1);
+}
+
+TEST(MavlinkTelemetryTest, RequestProtocolVersionUsesConfiguredVersion)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t msg;
+ mavlink_msg_command_long_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_CMD_REQUEST_PROTOCOL_VERSION,
+ 0,
+ 0, 0, 0, 0, 0, 0, 0);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ mavlink_message_t versionMsg;
+ ASSERT_TRUE(popTxMessage(&versionMsg));
+ ASSERT_EQ(versionMsg.msgid, MAVLINK_MSG_ID_PROTOCOL_VERSION);
+
+ mavlink_protocol_version_t version;
+ mavlink_msg_protocol_version_decode(&versionMsg, &version);
+
+ EXPECT_EQ(version.version, 200);
+ EXPECT_EQ(version.min_version, 200);
+ EXPECT_EQ(version.max_version, 200);
+}
+
+TEST(MavlinkTelemetryTest, BatteryStatusDoesNotSendExtendedSysState)
+{
+ initMavlinkTestState();
+
+ mavlinkSendBatteryTemperatureStatusText();
+
+ mavlink_status_t status;
+ memset(&status, 0, sizeof(status));
+ mavlink_message_t msg;
+ bool sawExtSysState = false;
+
+ for (size_t i = 0; i < serialTxLen; i++) {
+ if (mavlink_parse_char(0, serialTxBuffer[i], &msg, &status) == MAVLINK_FRAMING_OK) {
+ if (msg.msgid == MAVLINK_MSG_ID_EXTENDED_SYS_STATE) {
+ sawExtSysState = true;
+ break;
+ }
+ }
+ }
+
+ EXPECT_FALSE(sawExtSysState);
+}
+
+TEST(MavlinkTelemetryTest, RadioStatusUpdatesRxLinkStats)
+{
+ initMavlinkTestState();
+ rxConfigMutable()->receiverType = RX_TYPE_SERIAL;
+ rxConfigMutable()->serialrx_provider = SERIALRX_MAVLINK;
+ telemetryConfigMutable()->mavlink.radio_type = MAVLINK_RADIO_ELRS;
+
+ mavlink_message_t msg;
+ mavlink_msg_radio_status_pack(
+ 42, 200, &msg,
+ 200, 150, 255, 7, 3, 0, 0);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ EXPECT_EQ(rxLinkStatistics.uplinkRSSI, -150);
+ EXPECT_EQ(rxLinkStatistics.uplinkSNR, 7);
+ EXPECT_EQ(rxLinkStatistics.uplinkLQ, scaleRange(200, 0, 255, 0, 100));
+}
+
+TEST(MavlinkTelemetryTest, RcChannelsOverrideIsForwarded)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t msg;
+ mavlink_msg_rc_channels_override_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER,
+ 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ EXPECT_EQ(mavlinkRxHandleCalls, 1);
+}
+
+extern "C" {
+
+int32_t debug[DEBUG32_VALUE_COUNT];
+
+uint32_t armingFlags;
+uint32_t stateFlags;
+
+attitudeEulerAngles_t attitude;
+gyro_t gyro;
+gpsSolutionData_t gpsSol;
+gpsLocation_t GPS_home;
+navSystemStatus_t NAV_Status;
+rxRuntimeConfig_t rxRuntimeConfig;
+rxLinkStatistics_t rxLinkStatistics;
+uint16_t averageSystemLoadPercent;
+
+uint32_t micros(void)
+{
+ return 0;
+}
+
+uint32_t millis(void)
+{
+ return 0;
+}
+
+serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
+{
+ UNUSED(function);
+ testPortConfig.identifier = SERIAL_PORT_USART1;
+ testPortConfig.telemetry_baudrateIndex = BAUD_115200;
+ return &testPortConfig;
+}
+
+portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function)
+{
+ UNUSED(portConfig);
+ UNUSED(function);
+ return PORTSHARING_NOT_SHARED;
+}
+
+serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function,
+ serialReceiveCallbackPtr rxCallback, void *rxCallbackData,
+ uint32_t baudRate, portMode_t mode, portOptions_t options)
+{
+ UNUSED(identifier);
+ UNUSED(function);
+ UNUSED(rxCallback);
+ UNUSED(rxCallbackData);
+ UNUSED(baudRate);
+ UNUSED(mode);
+ UNUSED(options);
+ return &testSerialPort;
+}
+
+void closeSerialPort(serialPort_t *serialPort)
+{
+ UNUSED(serialPort);
+}
+
+uint32_t serialRxBytesWaiting(const serialPort_t *instance)
+{
+ UNUSED(instance);
+ return (uint32_t)(serialRxLen - serialRxPos);
+}
+
+uint32_t serialTxBytesFree(const serialPort_t *instance)
+{
+ UNUSED(instance);
+ return 1024;
+}
+
+uint8_t serialRead(serialPort_t *instance)
+{
+ UNUSED(instance);
+ return serialRxBuffer[serialRxPos++];
+}
+
+void serialWrite(serialPort_t *instance, uint8_t ch)
+{
+ UNUSED(instance);
+ serialTxBuffer[serialTxLen++] = ch;
+}
+
+void serialSetMode(serialPort_t *instance, portMode_t mode)
+{
+ UNUSED(instance);
+ UNUSED(mode);
+}
+
+bool telemetryDetermineEnabledState(portSharing_e portSharing)
+{
+ UNUSED(portSharing);
+ return true;
+}
+
+bool sensors(uint32_t mask)
+{
+ UNUSED(mask);
+ return false;
+}
+
+bool isAmperageConfigured(void)
+{
+ return false;
+}
+
+bool feature(uint32_t mask)
+{
+ UNUSED(mask);
+ return false;
+}
+
+int16_t getAmperage(void)
+{
+ return 0;
+}
+
+int32_t getMAhDrawn(void)
+{
+ return 0;
+}
+
+int32_t getMWhDrawn(void)
+{
+ return 0;
+}
+
+uint8_t getBatteryCellCount(void)
+{
+ return 0;
+}
+
+uint16_t getBatteryAverageCellVoltage(void)
+{
+ return 0;
+}
+
+uint16_t getBatteryVoltage(void)
+{
+ return 0;
+}
+
+int16_t getThrottlePercent(bool scaled)
+{
+ UNUSED(scaled);
+ return 0;
+}
+
+bool osdUsingScaledThrottle(void)
+{
+ return false;
+}
+
+float getEstimatedActualPosition(int axis)
+{
+ UNUSED(axis);
+ return 0.0f;
+}
+
+float getEstimatedActualVelocity(int axis)
+{
+ UNUSED(axis);
+ return 0.0f;
+}
+
+float getAirspeedEstimate(void)
+{
+ return 0.0f;
+}
+
+bool pitotIsHealthy(void)
+{
+ return false;
+}
+
+bool rxIsReceivingSignal(void)
+{
+ return false;
+}
+
+bool rxAreFlightChannelsValid(void)
+{
+ return false;
+}
+
+uint16_t getRSSI(void)
+{
+ return 0;
+}
+
+int16_t rxGetChannelValue(unsigned channel)
+{
+ UNUSED(channel);
+ return 1500;
+}
+
+hardwareSensorStatus_e getHwGyroStatus(void) { return HW_SENSOR_NONE; }
+hardwareSensorStatus_e getHwAccelerometerStatus(void) { return HW_SENSOR_NONE; }
+hardwareSensorStatus_e getHwCompassStatus(void) { return HW_SENSOR_NONE; }
+hardwareSensorStatus_e getHwBarometerStatus(void) { return HW_SENSOR_NONE; }
+hardwareSensorStatus_e getHwGPSStatus(void) { return HW_SENSOR_NONE; }
+hardwareSensorStatus_e getHwRangefinderStatus(void) { return HW_SENSOR_NONE; }
+hardwareSensorStatus_e getHwPitotmeterStatus(void) { return HW_SENSOR_NONE; }
+hardwareSensorStatus_e getHwOpticalFlowStatus(void) { return HW_SENSOR_NONE; }
+
+bool getBaroTemperature(int16_t *temperature)
+{
+ *temperature = 0;
+ return false;
+}
+
+bool getIMUTemperature(int16_t *temperature)
+{
+ *temperature = 0;
+ return false;
+}
+
+bool areSensorsCalibrating(void)
+{
+ return false;
+}
+
+bool failsafeIsActive(void)
+{
+ return false;
+}
+
+failsafePhase_e failsafePhase(void)
+{
+ return FAILSAFE_IDLE;
+}
+
+int isGCSValid(void)
+{
+ return gcsValid;
+}
+
+void setWaypoint(uint8_t wpNumber, const navWaypoint_t *wp)
+{
+ UNUSED(wpNumber);
+ lastWaypoint = *wp;
+ setWaypointCalls++;
+}
+
+int getWaypointCount(void)
+{
+ return waypointCount;
+}
+
+void getWaypoint(uint8_t wpNumber, navWaypoint_t *wp)
+{
+ if (wpNumber == 0 || wpNumber > ARRAYLEN(waypointStore)) {
+ memset(wp, 0, sizeof(*wp));
+ return;
+ }
+ *wp = waypointStore[wpNumber - 1];
+}
+
+bool isWaypointListValid(void)
+{
+ return true;
+}
+
+void resetWaypointList(void)
+{
+ resetWaypointCalls++;
+ waypointCount = 0;
+ memset(waypointStore, 0, sizeof(waypointStore));
+}
+
+flightModeForTelemetry_e getFlightModeForTelemetry(void)
+{
+ return FLM_MANUAL;
+}
+
+bool isModeActivationConditionPresent(boxId_e modeId)
+{
+ UNUSED(modeId);
+ return false;
+}
+
+textAttributes_t osdGetSystemMessage(char *message, size_t length, bool remove)
+{
+ UNUSED(length);
+ UNUSED(remove);
+ message[0] = 0x20;
+ message[1] = '\0';
+ return 0;
+}
+
+void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg)
+{
+ UNUSED(msg);
+ mavlinkRxHandleCalls++;
+}
+
+adsbVehicleValues_t* getVehicleForFill(void)
+{
+ return NULL;
+}
+
+void adsbNewVehicle(adsbVehicleValues_t *vehicle)
+{
+ UNUSED(vehicle);
+}
+
+bool adsbHeartbeat(void)
+{
+ return false;
+}
+
+uint8_t calculateBatteryPercentage(void)
+{
+ return 0;
+}
+
+}