From d6ee8e757ea87e57758021f71f6823d9cea73f5d Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Sat, 13 Dec 2025 03:27:41 -0500
Subject: [PATCH 01/27] mavlink compatibility fixes
---
src/main/telemetry/mavlink.c | 644 ++++++++++++++++++++++++++++++-----
1 file changed, 558 insertions(+), 86 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 60f6aed67c3..20a4b780257 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -97,6 +97,15 @@
#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)
+
+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 +138,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 +167,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,12 +192,14 @@ 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 mavRatesConfigured[MAXSTREAMS];
static uint8_t mavTicks[MAXSTREAMS];
static mavlink_message_t mavSendMsg;
static mavlink_message_t mavRecvMsg;
@@ -286,6 +308,22 @@ static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
return 0;
}
+static void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate)
+{
+ mavRates[streamNum] = rate;
+ mavTicks[streamNum] = 0;
+}
+
+static int32_t mavlinkStreamIntervalUs(uint8_t streamNum)
+{
+ uint8_t rate = mavRates[streamNum];
+ if (rate == 0) {
+ return -1;
+ }
+
+ return 1000000 / rate;
+}
+
void freeMAVLinkTelemetryPort(void)
{
closeSerialPort(mavlinkPort);
@@ -297,6 +335,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 +363,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 +406,178 @@ static void mavlinkSendMessage(void)
}
}
+static void __attribute__((unused)) mavlinkSendAutopilotVersion(void)
+{
+ if (telemetryConfig()->mavlink.version == 1) return;
+
+ // will need to add real capabilities according to ifdef: https://mavlink.io/en/messages/common.html#MAV_PROTOCOL_CAPABILITY
+ uint64_t capabilities = 0;
+ capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
+ capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT; // i assume
+ capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT;
+ // MAV_PROTOCOL_CAPABILITY_MISSION_FENCE geofence
+ 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
+ 0, // flight_sw_version
+ 0, // middleware_sw_version
+ 0, // 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};
+
+ mavlink_msg_protocol_version_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ MAVLINK_VERSION,
+ MAVLINK_VERSION,
+ MAVLINK_VERSION,
+ 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;
+ }
+}
+
+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 void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom)
+{
+ for (uint8_t i = 0; i < count; i++) {
+ const uint8_t modeIndex = i + 1;
+ const uint8_t stdMode = MAV_STANDARD_MODE_NON_STANDARD;
+ const uint32_t properties = 0;
+
+ mavlink_msg_available_modes_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ count,
+ 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 +803,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;
@@ -783,7 +1019,7 @@ void mavlinkSendHUDAndHeartbeat(void)
flightModeForTelemetry_e flm = getFlightModeForTelemetry();
uint8_t mavCustomMode;
- if (STATE(FIXED_WING_LEGACY)) {
+ if (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE) {
mavCustomMode = (uint8_t)inavToArduPlaneMap(flm);
}
else {
@@ -904,6 +1140,7 @@ void mavlinkSendBatteryTemperatureStatusText(void)
mavlinkSendMessage();
+ mavlinkSendExtendedSysState();
// FIXME - Status text is limited to boards with USE_OSD
#ifdef USE_OSD
@@ -955,6 +1192,10 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs)
mavlinkSendHUDAndHeartbeat();
}
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_SYS_STATE)) {
+ mavlinkSendExtendedSysState();
+ }
+
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) {
mavlinkSendBatteryTemperatureStatusText();
}
@@ -1157,70 +1398,104 @@ static bool handleIncoming_MISSION_REQUEST(void)
return false;
}
-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;
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.p2 = 0;
wp.p3 = 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 {
+ return true;
#ifdef USE_BARO
- if (msg.command == MAV_CMD_DO_CHANGE_ALTITUDE) {
- const float altitudeMeters = msg.param1;
- const uint8_t frame = (uint8_t)msg.frame;
-
+ case MAV_CMD_DO_CHANGE_ALTITUDE:
+ {
geoAltitudeDatumFlag_e datum;
+
switch (frame) {
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_INT:
@@ -1231,42 +1506,239 @@ static bool handleIncoming_COMMAND_INT(void)
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();
+ 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();
+ {
+ const int32_t targetAltitudeCm = (int32_t)lrintf(altitudeMeters * 100.0f);
+ const bool accepted = navigationSetAltitudeTargetWithDatum(datum, targetAltitudeCm);
+ mavlinkSendCommandAck(command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent);
+ }
return true;
}
#endif
- mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
- msg.command,
- MAV_RESULT_UNSUPPORTED,
- 0,
- 0,
- mavRecvMsg.sysid,
- mavRecvMsg.compid);
+ 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;
+ }
+
+ 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:
+ mavlinkSendProtocolVersion();
+ mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
+ return true;
+ case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
+ 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:
+ mavlinkSendHUDAndHeartbeat();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
+ mavlinkSendAutopilotVersion();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_PROTOCOL_VERSION:
+ 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:
+ mavlinkSendHUDAndHeartbeat();
+ sent = true;
+ break;
+ case MAVLINK_MSG_ID_AVAILABLE_MODES:
+ {
+ flightModeForTelemetry_e flm = getFlightModeForTelemetry();
+ uint8_t currentCustom;
+ if (mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY)) {
+ currentCustom = (uint8_t)inavToArduPlaneMap(flm);
+ mavlinkSendAvailableModes(planeModes, ARRAYLEN(planeModes), currentCustom);
+ } else {
+ currentCustom = (uint8_t)inavToArduCopterMap(flm);
+ mavlinkSendAvailableModes(copterModes, ARRAYLEN(copterModes), currentCustom);
+ }
+ sent = true;
+ }
+ break;
+ case MAVLINK_MSG_ID_CURRENT_MODE:
+ {
+ flightModeForTelemetry_e flm = getFlightModeForTelemetry();
+ uint8_t currentCustom;
+ if (mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY)) {
+ currentCustom = (uint8_t)inavToArduPlaneMap(flm);
+ } else {
+ currentCustom = (uint8_t)inavToArduCopterMap(flm);
+ }
+ mavlink_msg_current_mode_pack(
+ mavSystemId,
+ mavComponentId,
+ &mavSendMsg,
+ MAV_STANDARD_MODE_NON_STANDARD,
+ currentCustom,
+ currentCustom);
+ 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
+ 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);
+
+ return handleIncoming_COMMAND(msg.target_system, mavRecvMsg.sysid, mavRecvMsg.compid, msg.command, msg.confirmation, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7);
+}
+
+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;
+ }
+
+ uint8_t frame = msg.coordinate_frame;
+ if (!mavlinkFrameIsSupported(frame,
+ MAV_FRAME_SUPPORTED_GLOBAL_INT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) {
return true;
}
- return false;
+
+ if (isGCSValid()) {
+ navWaypoint_t wp;
+ wp.action = NAV_WP_ACTION_WAYPOINT;
+ wp.lat = msg.lat_int;
+ wp.lon = msg.lon_int;
+ wp.alt = (int32_t)(msg.alt * 100.0f);
+ wp.p1 = 0;
+ wp.p2 = 0;
+ wp.p3 = 0;
+ wp.flag = 0;
+
+ setWaypoint(255, &wp);
+ }
+
+ return true;
}
@@ -1391,10 +1863,8 @@ static bool processMAVLinkIncomingTelemetry(void)
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:
@@ -1403,6 +1873,8 @@ static bool processMAVLinkIncomingTelemetry(void)
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_GLOBAL_INT:
+ return handleIncoming_SET_POSITION_TARGET_GLOBAL_INT();
#ifdef USE_ADSB
case MAVLINK_MSG_ID_ADSB_VEHICLE:
return handleIncoming_ADSB_VEHICLE();
From 9b3553fb1e3f5ca1b89b98a7f1afaa44da2fa7a6 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Sat, 13 Dec 2025 13:50:31 -0500
Subject: [PATCH 02/27] compatibility + messages
---
dm.txt | 0
src/main/telemetry/mavlink.c | 377 ++++++++++++++++++++++-------------
2 files changed, 241 insertions(+), 136 deletions(-)
create mode 100644 dm.txt
diff --git a/dm.txt b/dm.txt
new file mode 100644
index 00000000000..e69de29bb2d
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 20a4b780257..2c9ee1142a8 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"
@@ -98,6 +99,9 @@
#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 3
typedef enum {
MAV_FRAME_SUPPORTED_NONE = 0,
@@ -210,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)
@@ -406,16 +431,16 @@ static void mavlinkSendMessage(void)
}
}
-static void __attribute__((unused)) mavlinkSendAutopilotVersion(void)
+static void mavlinkSendAutopilotVersion(void)
{
if (telemetryConfig()->mavlink.version == 1) return;
- // will need to add real capabilities according to ifdef: https://mavlink.io/en/messages/common.html#MAV_PROTOCOL_CAPABILITY
+ // Capabilities aligned with what we actually support.
uint64_t capabilities = 0;
capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
- capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT; // i assume
+ capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
+ capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT;
capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT;
- // MAV_PROTOCOL_CAPABILITY_MISSION_FENCE geofence
capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT;
// Bare minimum: caps + IDs. Everything else 0 is fine.
@@ -882,8 +907,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
@@ -990,31 +1015,7 @@ void mavlinkSendHUDAndHeartbeat(void)
if (ARMING_FLAG(ARMED))
mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
- 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;
- }
+ uint8_t mavSystemType = mavlinkGetVehicleType();
flightModeForTelemetry_e flm = getFlightModeForTelemetry();
uint8_t mavCustomMode;
@@ -1222,6 +1223,67 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void)
static int incomingMissionWpCount = 0;
static int incomingMissionWpSequence = 0;
+static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, uint16_t command, uint8_t autocontinue, uint16_t seq, int32_t lat, int32_t lon, float altMeters)
+{
+ if ((autocontinue == 0) || (command != MAV_CMD_NAV_WAYPOINT && 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 (!mavlinkFrameIsSupported(frame,
+ MAV_FRAME_SUPPORTED_GLOBAL |
+ MAV_FRAME_SUPPORTED_GLOBAL_INT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT |
+ MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT) &&
+ !(frame == MAV_FRAME_MISSION && 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);
+ mavlinkSendMessage();
+ return true;
+ }
+
+ if (seq == incomingMissionWpSequence) {
+ incomingMissionWpSequence++;
+
+ navWaypoint_t wp;
+ wp.action = (command == MAV_CMD_NAV_RETURN_TO_LAUNCH) ? NAV_WP_ACTION_RTH : 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 = 0;
+ wp.flag = (incomingMissionWpSequence >= incomingMissionWpCount) ? NAV_WP_FLAG_LAST : 0;
+
+ 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();
+ }
+ }
+ else {
+ 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;
+}
+
static bool handleIncoming_MISSION_COUNT(void)
{
mavlink_mission_count_t msg;
@@ -1232,7 +1294,7 @@ static bool handleIncoming_MISSION_COUNT(void)
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);
+ mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
mavlinkSendMessage();
return true;
}
@@ -1256,94 +1318,44 @@ static bool handleIncoming_MISSION_ITEM(void)
mavlink_mission_item_t msg;
mavlink_msg_mission_item_decode(&mavRecvMsg, &msg);
- // 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);
- 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.p2 = 0;
- wp.p3 = 0;
- setWaypoint(255, &wp);
+ if (msg.target_system != mavSystemId) {
+ return false;
+ }
+ if (ARMING_FLAG(ARMED)) {
+ // Legacy Mission Planner 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_ACCEPTED, 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);
+ 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 ((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);
- mavlinkSendMessage();
- return true;
- }
-
- if (msg.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.action = 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.alt = (int32_t)(msg.z * 100.0f);
wp.p1 = 0;
wp.p2 = 0;
wp.p3 = 0;
- wp.flag = (incomingMissionWpSequence >= incomingMissionWpCount) ? NAV_WP_FLAG_LAST : 0;
-
- setWaypoint(incomingMissionWpSequence, &wp);
+ setWaypoint(255, &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();
- }
- }
- else {
- mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION);
- mavlinkSendMessage();
- }
- }
- else {
- // Wrong sequence number received
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0);
+ mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
+ mavRecvMsg.sysid, mavRecvMsg.compid,
+ MAV_MISSION_ACCEPTED, 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);
mavlinkSendMessage();
+ return true;
}
-
- return true;
}
- return false;
+ return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.autocontinue, msg.seq, (int32_t)(msg.x * 1e7f), (int32_t)(msg.y * 1e7f), msg.z);
}
static bool handleIncoming_MISSION_REQUEST_LIST(void)
@@ -1366,36 +1378,35 @@ 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;
+ }
- if (msg.seq < wpCount) {
- navWaypoint_t wp;
- getWaypoint(msg.seq + 1, &wp);
-
- 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,
- 0,
- 1,
- 0, 0, 0, 0,
- wp.lat / 1e7f,
- wp.lon / 1e7f,
- wp.alt / 100.0f,
- 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();
- }
+ int wpCount = getWaypointCount();
- return true;
+ if (msg.seq < wpCount) {
+ navWaypoint_t wp;
+ getWaypoint(msg.seq + 1, &wp);
+
+ 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,
+ 0,
+ 1,
+ 0, 0, 0, 0,
+ wp.lat / 1e7f,
+ wp.lon / 1e7f,
+ wp.alt / 100.0f,
+ 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 false;
+ return true;
}
static bool mavlinkMessageToStream(uint16_t messageId, uint8_t *streamNum)
@@ -1568,12 +1579,20 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
return true;
}
case MAV_CMD_REQUEST_PROTOCOL_VERSION:
- mavlinkSendProtocolVersion();
- mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
+ 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:
- mavlinkSendAutopilotVersion();
- mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent);
+ 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:
{
@@ -1586,12 +1605,16 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
sent = true;
break;
case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
- mavlinkSendAutopilotVersion();
- sent = true;
+ if (telemetryConfig()->mavlink.version != 1) {
+ mavlinkSendAutopilotVersion();
+ sent = true;
+ }
break;
case MAVLINK_MSG_ID_PROTOCOL_VERSION:
- mavlinkSendProtocolVersion();
- sent = true;
+ if (telemetryConfig()->mavlink.version != 1) {
+ mavlinkSendProtocolVersion();
+ sent = true;
+ }
break;
case MAVLINK_MSG_ID_SYS_STATUS:
mavlinkSendSystemStatus();
@@ -1708,6 +1731,82 @@ static bool handleIncoming_COMMAND_LONG(void)
return handleIncoming_COMMAND(msg.target_system, mavRecvMsg.sysid, mavRecvMsg.compid, msg.command, msg.confirmation, 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)) {
+ 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.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);
+
+ mavlink_msg_mission_item_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid,
+ msg.seq,
+ wp.action == NAV_WP_ACTION_RTH ? MAV_FRAME_MISSION : MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+ wp.action == NAV_WP_ACTION_RTH ? MAV_CMD_NAV_RETURN_TO_LAUNCH : MAV_CMD_NAV_WAYPOINT,
+ 0,
+ 1,
+ 0, 0, 0, 0,
+ wp.lat,
+ wp.lon,
+ wp.alt / 100.0f,
+ 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;
+}
+
+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;
+ }
+
+ if (msg.start_stop == 0) {
+ mavlinkSetStreamRate(msg.req_stream_id, 0);
+ return true;
+ }
+
+ uint8_t rate = (uint8_t)msg.req_message_rate;
+ if (rate > TELEMETRY_MAVLINK_MAXRATE) {
+ rate = TELEMETRY_MAVLINK_MAXRATE;
+ }
+ 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;
@@ -1860,6 +1959,8 @@ 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();
@@ -1869,6 +1970,10 @@ static bool processMAVLinkIncomingTelemetry(void)
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
From 579f9184511a688a4aaf8e960c40b80acbc5516f Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Sat, 13 Dec 2025 15:04:54 -0500
Subject: [PATCH 03/27] qgc mission errors fix + frame handling
---
src/main/telemetry/mavlink.c | 381 +++++++++++++++++++++++++++++------
1 file changed, 322 insertions(+), 59 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 2c9ee1142a8..92cb358225a 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -449,7 +449,7 @@ static void mavlinkSendAutopilotVersion(void)
mavComponentId,
&mavSendMsg,
capabilities, // capabilities
- 0, // flight_sw_version
+ 0, // flight_sw_version. Setting this to actual Ardupilot version makes QGC not display modes anymore but "Unknown", except Guided is "GUIDED". Why?
0, // middleware_sw_version
0, // os_sw_version
0, // board_version
@@ -500,6 +500,31 @@ static bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowed
}
}
+static bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame)
+{
+ return frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_INT;
+}
+
+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;
@@ -965,7 +990,7 @@ void mavlinkSendAttitude(void)
mavlinkSendMessage();
}
-void mavlinkSendHUDAndHeartbeat(void)
+void mavlinkSendVfrHud(void)
{
float mavAltitude = 0;
float mavGroundSpeed = 0;
@@ -1009,11 +1034,11 @@ void mavlinkSendHUDAndHeartbeat(void)
mavClimbRate);
mavlinkSendMessage();
+}
-
- 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;
+void mavlinkSendHeartbeat(void)
+{
+ uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t mavSystemType = mavlinkGetVehicleType();
@@ -1027,12 +1052,22 @@ void mavlinkSendHUDAndHeartbeat(void)
mavCustomMode = (uint8_t)inavToArduCopterMap(flm);
}
- 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) {
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)) {
@@ -1190,7 +1225,8 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs)
}
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) {
- mavlinkSendHUDAndHeartbeat();
+ mavlinkSendVfrHud();
+ mavlinkSendHeartbeat();
}
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_SYS_STATE)) {
@@ -1223,36 +1259,169 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void)
static int incomingMissionWpCount = 0;
static int incomingMissionWpSequence = 0;
-static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, uint16_t command, uint8_t autocontinue, uint16_t seq, int32_t lat, int32_t lon, float altMeters)
+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)
{
- if ((autocontinue == 0) || (command != MAV_CMD_NAV_WAYPOINT && command != MAV_CMD_NAV_RETURN_TO_LAUNCH)) {
+ 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;
}
- 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) &&
- !(frame == MAV_FRAME_MISSION && 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);
- mavlinkSendMessage();
- return true;
+ UNUSED(param3);
+
+ navWaypoint_t wp;
+ memset(&wp, 0, sizeof(wp));
+
+ 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.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.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ break;
+
+ case MAV_CMD_NAV_TAKEOFF:
+ 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;
+ }
+ // INAV has no dedicated TAKEOFF mission action; treat it as a normal waypoint.
+ wp.action = NAV_WP_ACTION_WAYPOINT;
+ wp.lat = lat;
+ wp.lon = lon;
+ wp.alt = (int32_t)(altMeters * 100.0f);
+ 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;
+ }
+ wp.action = NAV_WP_ACTION_RTH;
+ wp.lat = coordinateFrame ? lat : 0;
+ wp.lon = coordinateFrame ? lon : 0;
+ wp.alt = coordinateFrame ? (int32_t)(altMeters * 100.0f) : 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ break;
+ }
+
+ 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;
+ }
+ wp.action = NAV_WP_ACTION_LAND;
+ wp.lat = lat;
+ wp.lon = lon;
+ wp.alt = (int32_t)(altMeters * 100.0f);
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ 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 (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.action = NAV_WP_ACTION_JUMP;
+ wp.p1 = (int16_t)lrintf(param1 + 1.0f);
+ wp.p2 = (int16_t)lrintf(param2);
+ 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.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.action = NAV_WP_ACTION_SET_HEAD;
+ wp.p1 = (int16_t)lrintf(param1);
+ break;
+
+ 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 (seq == incomingMissionWpSequence) {
incomingMissionWpSequence++;
- navWaypoint_t wp;
- wp.action = (command == MAV_CMD_NAV_RETURN_TO_LAUNCH) ? NAV_WP_ACTION_RTH : 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 = 0;
wp.flag = (incomingMissionWpSequence >= incomingMissionWpCount) ? NAV_WP_FLAG_LAST : 0;
setWaypoint(incomingMissionWpSequence, &wp);
@@ -1277,8 +1446,18 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
}
}
else {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0);
- mavlinkSendMessage();
+ // 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;
@@ -1355,7 +1534,7 @@ static bool handleIncoming_MISSION_ITEM(void)
}
}
- return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.autocontinue, msg.seq, (int32_t)(msg.x * 1e7f), (int32_t)(msg.y * 1e7f), msg.z);
+ 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)
@@ -1373,6 +1552,78 @@ 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;
@@ -1388,18 +1639,24 @@ static bool handleIncoming_MISSION_REQUEST(void)
navWaypoint_t wp;
getWaypoint(msg.seq + 1, &wp);
- 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,
- 0,
- 1,
- 0, 0, 0, 0,
- wp.lat / 1e7f,
- wp.lon / 1e7f,
- wp.alt / 100.0f,
- MAV_MISSION_TYPE_MISSION);
- mavlinkSendMessage();
+ mavlinkMissionItemData_t item;
+ if (fillMavlinkMissionItemFromWaypoint(&wp, false, &item)) {
+ mavlink_msg_mission_item_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 / 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_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);
@@ -1601,7 +1858,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
switch (messageId) {
case MAVLINK_MSG_ID_HEARTBEAT:
- mavlinkSendHUDAndHeartbeat();
+ mavlinkSendHeartbeat();
sent = true;
break;
case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
@@ -1625,7 +1882,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
sent = true;
break;
case MAVLINK_MSG_ID_VFR_HUD:
- mavlinkSendHUDAndHeartbeat();
+ mavlinkSendVfrHud();
sent = true;
break;
case MAVLINK_MSG_ID_AVAILABLE_MODES:
@@ -1746,7 +2003,7 @@ static bool handleIncoming_MISSION_ITEM_INT(void)
return true;
}
- return mavlinkHandleMissionItemCommon(true, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.x, msg.y, msg.z);
+ 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)
@@ -1764,18 +2021,24 @@ static bool handleIncoming_MISSION_REQUEST_INT(void)
navWaypoint_t wp;
getWaypoint(msg.seq + 1, &wp);
- mavlink_msg_mission_item_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid,
- msg.seq,
- wp.action == NAV_WP_ACTION_RTH ? MAV_FRAME_MISSION : MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
- wp.action == NAV_WP_ACTION_RTH ? MAV_CMD_NAV_RETURN_TO_LAUNCH : MAV_CMD_NAV_WAYPOINT,
- 0,
- 1,
- 0, 0, 0, 0,
- wp.lat,
- wp.lon,
- wp.alt / 100.0f,
- MAV_MISSION_TYPE_MISSION);
- mavlinkSendMessage();
+ 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);
From 4682d1c7f7ee89551520ea99cc46dfff9ea33494 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Sat, 13 Dec 2025 15:19:47 -0500
Subject: [PATCH 04/27] no takeoff
---
src/main/telemetry/mavlink.c | 18 ------------------
1 file changed, 18 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 92cb358225a..4d6a6b3d446 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1308,24 +1308,6 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
break;
- case MAV_CMD_NAV_TAKEOFF:
- 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;
- }
- // INAV has no dedicated TAKEOFF mission action; treat it as a normal waypoint.
- wp.action = NAV_WP_ACTION_WAYPOINT;
- wp.lat = lat;
- wp.lon = lon;
- wp.alt = (int32_t)(altMeters * 100.0f);
- wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
- break;
-
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
{
const bool coordinateFrame = mavlinkFrameIsSupported(frame,
From 6b7bdba648adbecd51b4767af6138ba1b2a6b54c Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Sun, 14 Dec 2025 12:40:05 -0500
Subject: [PATCH 05/27] defaults in wps
---
src/main/telemetry/mavlink.c | 24 +++++++++++++++++++++---
1 file changed, 21 insertions(+), 3 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 4d6a6b3d446..c8303c8544c 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1287,6 +1287,8 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
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;
@@ -1305,6 +1307,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
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;
@@ -1322,9 +1325,11 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
return true;
}
wp.action = NAV_WP_ACTION_RTH;
- wp.lat = coordinateFrame ? lat : 0;
- wp.lon = coordinateFrame ? lon : 0;
+ 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 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
break;
}
@@ -1343,7 +1348,9 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
wp.lat = lat;
wp.lon = lon;
wp.alt = (int32_t)(altMeters * 100.0f);
- wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ 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:
@@ -1357,9 +1364,13 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
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:
@@ -1377,6 +1388,8 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
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;
@@ -1391,8 +1404,13 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
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;
default:
From bd521f2b9842721732496120eadfd3257db47e14 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Sat, 17 Jan 2026 01:03:27 -0500
Subject: [PATCH 06/27] available modes fixes + unit tests
---
src/main/telemetry/mavlink.c | 96 +++-
src/test/unit/CMakeLists.txt | 7 +
src/test/unit/mavlink_unittest.cc | 888 ++++++++++++++++++++++++++++++
3 files changed, 982 insertions(+), 9 deletions(-)
create mode 100644 src/test/unit/mavlink_unittest.cc
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index c8303c8544c..adc50d33262 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -335,6 +335,9 @@ static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
static void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate)
{
+ if (streamNum >= MAXSTREAMS) {
+ return;
+ }
mavRates[streamNum] = rate;
mavTicks[streamNum] = 0;
}
@@ -560,10 +563,84 @@ static const mavlinkModeDescriptor_t copterModes[] = {
{ COPTER_MODE_DRIFT, "DRIFT" },
};
-static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom)
+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);
+ case PLANE_MODE_AUTO:
+ return isModeActivationConditionPresent(BOXNAVWP);
+ case PLANE_MODE_RTL:
+ return isModeActivationConditionPresent(BOXNAVRTH);
+ case PLANE_MODE_LOITER:
+ return isModeActivationConditionPresent(BOXNAVPOSHOLD);
+ case PLANE_MODE_GUIDED:
+ return isModeActivationConditionPresent(BOXNAVPOSHOLD) &&
+ isModeActivationConditionPresent(BOXGCSNAV);
+ 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_POSHOLD:
+ case COPTER_MODE_GUIDED:
+ return isModeActivationConditionPresent(BOXNAVPOSHOLD);
+ case COPTER_MODE_RTL:
+ return isModeActivationConditionPresent(BOXNAVRTH);
+ case COPTER_MODE_AUTO:
+ return isModeActivationConditionPresent(BOXNAVWP);
+ case COPTER_MODE_THROW:
+ return isModeActivationConditionPresent(BOXNAVLAUNCH);
+ 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++) {
- const uint8_t modeIndex = i + 1;
+ if (!isModeConfigured(modes[i].customMode)) {
+ continue;
+ }
+
+ modeIndex++;
const uint8_t stdMode = MAV_STANDARD_MODE_NON_STANDARD;
const uint32_t properties = 0;
@@ -571,7 +648,7 @@ static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint
mavSystemId,
mavComponentId,
&mavSendMsg,
- count,
+ availableCount,
modeIndex,
stdMode,
modes[i].customMode,
@@ -981,11 +1058,11 @@ 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();
}
@@ -1891,10 +1968,10 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
uint8_t currentCustom;
if (mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY)) {
currentCustom = (uint8_t)inavToArduPlaneMap(flm);
- mavlinkSendAvailableModes(planeModes, ARRAYLEN(planeModes), currentCustom);
+ mavlinkSendAvailableModes(planeModes, ARRAYLEN(planeModes), currentCustom, mavlinkPlaneModeIsConfigured);
} else {
currentCustom = (uint8_t)inavToArduCopterMap(flm);
- mavlinkSendAvailableModes(copterModes, ARRAYLEN(copterModes), currentCustom);
+ mavlinkSendAvailableModes(copterModes, ARRAYLEN(copterModes), currentCustom, mavlinkCopterModeIsConfigured);
}
sent = true;
}
@@ -1985,7 +2062,8 @@ static bool handleIncoming_COMMAND_LONG(void)
mavlink_command_long_t msg;
mavlink_msg_command_long_decode(&mavRecvMsg, &msg);
- return handleIncoming_COMMAND(msg.target_system, mavRecvMsg.sysid, mavRecvMsg.compid, msg.command, msg.confirmation, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7);
+ // 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)
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 6347d9052f0..5c0e1636c61 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -66,6 +66,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 extra_includes
+ "../../lib/main/MAVLink")
+
function(unit_test src)
get_filename_component(basename ${src} NAME)
string(REPLACE ".cc" "" name ${basename} )
@@ -74,6 +80,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})
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
new file mode 100644
index 00000000000..306e15e8bf4
--- /dev/null
+++ b/src/test/unit/mavlink_unittest.cc
@@ -0,0 +1,888 @@
+/*
+ * 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);
+
+ 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.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.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, 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, 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, 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));
+}
+
+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, 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;
+}
+
+}
From 92aaaf39ed9b5379342dfb33b20f85f661e4004d Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Tue, 27 Jan 2026 23:54:00 -0500
Subject: [PATCH 07/27] fix wrong mav version call
---
src/main/telemetry/mavlink.c | 7 ++++---
1 file changed, 4 insertions(+), 3 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index adc50d33262..34bbebc3050 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -473,14 +473,15 @@ static void mavlinkSendProtocolVersion(void)
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,
- MAVLINK_VERSION,
- MAVLINK_VERSION,
- MAVLINK_VERSION,
+ protocolVersion,
+ protocolVersion,
+ protocolVersion,
specHash,
libHash);
From d48e0cd113bc7c6a6ecdb2dbec8a171cd66ae790 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Tue, 27 Jan 2026 23:56:16 -0500
Subject: [PATCH 08/27] honor altitude frames for guided
---
src/main/telemetry/mavlink.c | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 34bbebc3050..51348bdfef2 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1597,7 +1597,7 @@ static bool handleIncoming_MISSION_ITEM(void)
wp.alt = (int32_t)(msg.z * 100.0f);
wp.p1 = 0;
wp.p2 = 0;
- wp.p3 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(msg.frame) ? NAV_WP_ALTMODE : 0;
setWaypoint(255, &wp);
mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
@@ -1827,7 +1827,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
wp.p1 = 0;
}
wp.p2 = 0;
- wp.p3 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
wp.flag = 0;
setWaypoint(255, &wp);
@@ -2173,7 +2173,7 @@ static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void)
wp.alt = (int32_t)(msg.alt * 100.0f);
wp.p1 = 0;
wp.p2 = 0;
- wp.p3 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
wp.flag = 0;
setWaypoint(255, &wp);
From 8c983b72584d7d269dd9f8e772d63b48a4894c98 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Tue, 27 Jan 2026 23:57:08 -0500
Subject: [PATCH 09/27] remove duplicate extended sys state
---
src/main/telemetry/mavlink.c | 2 --
1 file changed, 2 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 51348bdfef2..5aaefe89e7b 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1254,8 +1254,6 @@ void mavlinkSendBatteryTemperatureStatusText(void)
mavlinkSendMessage();
- mavlinkSendExtendedSysState();
-
// FIXME - Status text is limited to boards with USE_OSD
#ifdef USE_OSD
char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "};
From c71550c174dc4c23812635711b3d68e0611a99eb Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Tue, 27 Jan 2026 23:57:32 -0500
Subject: [PATCH 10/27] align heartbeat type for fixed wing
---
src/main/telemetry/mavlink.c | 8 +++++---
1 file changed, 5 insertions(+), 3 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 5aaefe89e7b..6bef484bb43 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1118,16 +1118,18 @@ void mavlinkSendHeartbeat(void)
{
uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
- uint8_t mavSystemType = mavlinkGetVehicleType();
-
flightModeForTelemetry_e flm = getFlightModeForTelemetry();
uint8_t mavCustomMode;
+ uint8_t mavSystemType;
- if (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE) {
+ const bool isPlane = (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE);
+ if (isPlane) {
mavCustomMode = (uint8_t)inavToArduPlaneMap(flm);
+ mavSystemType = MAV_TYPE_FIXED_WING;
}
else {
mavCustomMode = (uint8_t)inavToArduCopterMap(flm);
+ mavSystemType = mavlinkGetVehicleType();
}
const bool manualInputAllowed = !(flm == FLM_MISSION || flm == FLM_RTH || flm == FLM_FAILSAFE);
From 91450ea04ce7bc407f0365ced3ec8c37ba8dd95f Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Tue, 27 Jan 2026 23:59:43 -0500
Subject: [PATCH 11/27] add mavlink protocol and guided tests
---
src/test/unit/mavlink_unittest.cc | 95 +++++++++++++++++++++++++++++++
1 file changed, 95 insertions(+)
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
index 306e15e8bf4..177e593ec93 100644
--- a/src/test/unit/mavlink_unittest.cc
+++ b/src/test/unit/mavlink_unittest.cc
@@ -72,6 +72,7 @@ extern "C" {
#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);
@@ -214,6 +215,7 @@ TEST(MavlinkTelemetryTest, CommandLongRepositionUsesGlobalFrameAndParams)
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);
}
@@ -276,6 +278,7 @@ TEST(MavlinkTelemetryTest, CommandIntRepositionScalesCoordinates)
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);
}
@@ -425,6 +428,28 @@ TEST(MavlinkTelemetryTest, MissionRequestSendsWaypoint)
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();
@@ -467,6 +492,26 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntSetsWaypoint)
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)
@@ -531,6 +576,56 @@ TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream)
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();
From d942755029956fdd73367aacc504cb676f433447 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Wed, 28 Jan 2026 00:03:36 -0500
Subject: [PATCH 12/27] treat guided global altitude as relative
---
src/main/telemetry/mavlink.c | 6 +++---
src/test/unit/mavlink_unittest.cc | 10 +++++-----
2 files changed, 8 insertions(+), 8 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 6bef484bb43..8f55a2f1114 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1408,7 +1408,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
wp.alt = coordinateFrame ? (int32_t)(altMeters * 100.0f) : 0;
wp.p1 = 0; // Land if non-zero
wp.p2 = 0;
- wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ wp.p3 = 0;
break;
}
@@ -1597,7 +1597,7 @@ static bool handleIncoming_MISSION_ITEM(void)
wp.alt = (int32_t)(msg.z * 100.0f);
wp.p1 = 0;
wp.p2 = 0;
- wp.p3 = mavlinkFrameUsesAbsoluteAltitude(msg.frame) ? NAV_WP_ALTMODE : 0;
+ wp.p3 = 0;
setWaypoint(255, &wp);
mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
@@ -2173,7 +2173,7 @@ static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void)
wp.alt = (int32_t)(msg.alt * 100.0f);
wp.p1 = 0;
wp.p2 = 0;
- wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
+ wp.p3 = 0;
wp.flag = 0;
setWaypoint(255, &wp);
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
index 177e593ec93..e4cf902a390 100644
--- a/src/test/unit/mavlink_unittest.cc
+++ b/src/test/unit/mavlink_unittest.cc
@@ -215,7 +215,7 @@ TEST(MavlinkTelemetryTest, CommandLongRepositionUsesGlobalFrameAndParams)
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.p3, 0);
EXPECT_EQ(lastWaypoint.p1, 123);
}
@@ -428,7 +428,7 @@ TEST(MavlinkTelemetryTest, MissionRequestSendsWaypoint)
EXPECT_NEAR(item.z, 12.34f, 1e-4f);
}
-TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesAbsoluteAltitude)
+TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesRelativeAltitude)
{
initMavlinkTestState();
ENABLE_ARMING_FLAG(ARMED);
@@ -447,7 +447,7 @@ TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesAbsoluteAltitude)
handleMAVLinkTelemetry(1000);
EXPECT_EQ(setWaypointCalls, 1);
- EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE);
+ EXPECT_EQ(lastWaypoint.p3, 0);
}
TEST(MavlinkTelemetryTest, ParamRequestListRespondsWithEmptyParam)
@@ -495,7 +495,7 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntSetsWaypoint)
EXPECT_EQ(lastWaypoint.p3, 0);
}
-TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude)
+TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesRelativeAltitude)
{
initMavlinkTestState();
@@ -511,7 +511,7 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude)
handleMAVLinkTelemetry(1000);
EXPECT_EQ(setWaypointCalls, 1);
- EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE);
+ EXPECT_EQ(lastWaypoint.p3, 0);
}
TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream)
From 69655ada0a8016051d42c4d6b32b07d2dc4e65c1 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Wed, 28 Jan 2026 00:04:21 -0500
Subject: [PATCH 13/27] reject mission upload while armed
---
src/main/telemetry/mavlink.c | 13 +++++------
src/test/unit/mavlink_unittest.cc | 37 +++++++++++++++++++++++++++++++
2 files changed, 43 insertions(+), 7 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 8f55a2f1114..8c2fae8f119 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1548,19 +1548,18 @@ static bool handleIncoming_MISSION_COUNT(void)
// 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 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 {
+ } 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;
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
index e4cf902a390..0e7ee34a168 100644
--- a/src/test/unit/mavlink_unittest.cc
+++ b/src/test/unit/mavlink_unittest.cc
@@ -330,6 +330,43 @@ TEST(MavlinkTelemetryTest, MissionCountRequestsFirstItem)
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();
From 49b97f6a6a7d31afa52144aa48be8dca775a0def Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Wed, 28 Jan 2026 00:07:20 -0500
Subject: [PATCH 14/27] restore guided absolute altitude handling
---
src/main/telemetry/mavlink.c | 6 +++---
src/test/unit/mavlink_unittest.cc | 10 +++++-----
2 files changed, 8 insertions(+), 8 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 8c2fae8f119..c65f4722a4b 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1408,7 +1408,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
wp.alt = coordinateFrame ? (int32_t)(altMeters * 100.0f) : 0;
wp.p1 = 0; // Land if non-zero
wp.p2 = 0;
- wp.p3 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
break;
}
@@ -1448,7 +1448,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
wp.action = NAV_WP_ACTION_JUMP;
wp.p1 = (int16_t)lrintf(param1 + 1.0f);
wp.p2 = (int16_t)lrintf(param2);
- wp.p3 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(msg.frame) ? NAV_WP_ALTMODE : 0;
break;
case MAV_CMD_DO_SET_ROI:
@@ -2172,7 +2172,7 @@ static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void)
wp.alt = (int32_t)(msg.alt * 100.0f);
wp.p1 = 0;
wp.p2 = 0;
- wp.p3 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
wp.flag = 0;
setWaypoint(255, &wp);
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
index 0e7ee34a168..56522cfee18 100644
--- a/src/test/unit/mavlink_unittest.cc
+++ b/src/test/unit/mavlink_unittest.cc
@@ -215,7 +215,7 @@ TEST(MavlinkTelemetryTest, CommandLongRepositionUsesGlobalFrameAndParams)
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, 0);
+ EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE);
EXPECT_EQ(lastWaypoint.p1, 123);
}
@@ -465,7 +465,7 @@ TEST(MavlinkTelemetryTest, MissionRequestSendsWaypoint)
EXPECT_NEAR(item.z, 12.34f, 1e-4f);
}
-TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesRelativeAltitude)
+TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesAbsoluteAltitude)
{
initMavlinkTestState();
ENABLE_ARMING_FLAG(ARMED);
@@ -484,7 +484,7 @@ TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesRelativeAltitude)
handleMAVLinkTelemetry(1000);
EXPECT_EQ(setWaypointCalls, 1);
- EXPECT_EQ(lastWaypoint.p3, 0);
+ EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE);
}
TEST(MavlinkTelemetryTest, ParamRequestListRespondsWithEmptyParam)
@@ -532,7 +532,7 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntSetsWaypoint)
EXPECT_EQ(lastWaypoint.p3, 0);
}
-TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesRelativeAltitude)
+TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude)
{
initMavlinkTestState();
@@ -548,7 +548,7 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesRelativeAltitude)
handleMAVLinkTelemetry(1000);
EXPECT_EQ(setWaypointCalls, 1);
- EXPECT_EQ(lastWaypoint.p3, 0);
+ EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE);
}
TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream)
From 9f61e65092e8890af4cb2bd2b7fb5fbbef0b55e5 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Wed, 28 Jan 2026 00:10:58 -0500
Subject: [PATCH 15/27] support guided mission_item_int
---
src/main/telemetry/mavlink.c | 32 ++++++++++++++++++++++++++++---
src/test/unit/mavlink_unittest.cc | 25 ++++++++++++++++++++++++
2 files changed, 54 insertions(+), 3 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index c65f4722a4b..5adda674fc2 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -2076,9 +2076,35 @@ static bool handleIncoming_MISSION_ITEM_INT(void)
}
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 (isGCSValid() && (msg.command == MAV_CMD_NAV_WAYPOINT) && (msg.current == 2)) {
+ if (!(msg.frame == MAV_FRAME_GLOBAL_INT || msg.frame == MAV_FRAME_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;
+ }
+
+ navWaypoint_t wp;
+ wp.action = NAV_WP_ACTION_WAYPOINT;
+ wp.lat = msg.x;
+ wp.lon = msg.y;
+ wp.alt = (int32_t)(msg.z * 100.0f);
+ wp.p1 = 0;
+ wp.p2 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(msg.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;
+ } else {
+ 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);
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
index 56522cfee18..6e09307247f 100644
--- a/src/test/unit/mavlink_unittest.cc
+++ b/src/test/unit/mavlink_unittest.cc
@@ -408,6 +408,31 @@ TEST(MavlinkTelemetryTest, MissionItemIntSingleItemAcksAccepted)
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();
From 592246f34b08e87ab6827c19c1b6935a7ad0323c Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Wed, 28 Jan 2026 00:17:06 -0500
Subject: [PATCH 16/27] map mavlink modes to box config
---
src/main/telemetry/mavlink.c | 10 ++++++++--
1 file changed, 8 insertions(+), 2 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 5adda674fc2..c9f22da0800 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -579,7 +579,9 @@ static bool mavlinkPlaneModeIsConfigured(uint8_t customMode)
case PLANE_MODE_FLY_BY_WIRE_B:
return isModeActivationConditionPresent(BOXNAVALTHOLD);
case PLANE_MODE_CRUISE:
- return isModeActivationConditionPresent(BOXNAVCRUISE);
+ return isModeActivationConditionPresent(BOXNAVCRUISE) ||
+ (isModeActivationConditionPresent(BOXNAVCOURSEHOLD) &&
+ isModeActivationConditionPresent(BOXNAVALTHOLD));
case PLANE_MODE_AUTO:
return isModeActivationConditionPresent(BOXNAVWP);
case PLANE_MODE_RTL:
@@ -608,14 +610,18 @@ static bool mavlinkCopterModeIsConfigured(uint8_t customMode)
case COPTER_MODE_ALT_HOLD:
return isModeActivationConditionPresent(BOXNAVALTHOLD);
case COPTER_MODE_POSHOLD:
- case COPTER_MODE_GUIDED:
return isModeActivationConditionPresent(BOXNAVPOSHOLD);
+ case COPTER_MODE_GUIDED:
+ return isModeActivationConditionPresent(BOXNAVPOSHOLD) &&
+ isModeActivationConditionPresent(BOXGCSNAV);
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;
}
From b0e4fcee6f5eb8b1daa22324989581ee7db9bc18 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Wed, 28 Jan 2026 00:25:57 -0500
Subject: [PATCH 17/27] fix do_jump waypoint p3
---
src/main/telemetry/mavlink.c | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index c9f22da0800..a9d72d2db70 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1454,7 +1454,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u
wp.action = NAV_WP_ACTION_JUMP;
wp.p1 = (int16_t)lrintf(param1 + 1.0f);
wp.p2 = (int16_t)lrintf(param2);
- wp.p3 = mavlinkFrameUsesAbsoluteAltitude(msg.frame) ? NAV_WP_ALTMODE : 0;
+ wp.p3 = 0;
break;
case MAV_CMD_DO_SET_ROI:
From 5ee9fbd32dec5c98df4e9f9194e2c2d66a8d7924 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Wed, 28 Jan 2026 00:27:07 -0500
Subject: [PATCH 18/27] set guided mission_item altitude mode
---
src/main/telemetry/mavlink.c | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index a9d72d2db70..2ff8f8fd0dc 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1602,7 +1602,7 @@ static bool handleIncoming_MISSION_ITEM(void)
wp.alt = (int32_t)(msg.z * 100.0f);
wp.p1 = 0;
wp.p2 = 0;
- wp.p3 = 0;
+ wp.p3 = mavlinkFrameUsesAbsoluteAltitude(msg.frame) ? NAV_WP_ALTMODE : 0;
setWaypoint(255, &wp);
mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg,
From 1521edb51e7530cf4d131c7862a7d467c7db5d6f Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Mon, 23 Feb 2026 22:13:00 -0500
Subject: [PATCH 19/27] mode fixgit status!
---
docs/development/msp/inav_enums_ref.md | 14 +++++
src/main/telemetry/mavlink.c | 76 ++++++++++++++------------
2 files changed, 56 insertions(+), 34 deletions(-)
diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md
index 618620b234f..36a2295b411 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)
@@ -3387,6 +3388,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`
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 2ff8f8fd0dc..9af070c8f87 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -263,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;
}
}
@@ -294,23 +293,41 @@ 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;
}
}
+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 int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
{
uint8_t rate = (uint8_t) mavRates[streamNum];
@@ -586,11 +603,11 @@ static bool mavlinkPlaneModeIsConfigured(uint8_t customMode)
return isModeActivationConditionPresent(BOXNAVWP);
case PLANE_MODE_RTL:
return isModeActivationConditionPresent(BOXNAVRTH);
- case PLANE_MODE_LOITER:
- return isModeActivationConditionPresent(BOXNAVPOSHOLD);
case PLANE_MODE_GUIDED:
return isModeActivationConditionPresent(BOXNAVPOSHOLD) &&
isModeActivationConditionPresent(BOXGCSNAV);
+ case PLANE_MODE_LOITER:
+ return isModeActivationConditionPresent(BOXNAVPOSHOLD);
case PLANE_MODE_TAKEOFF:
return isModeActivationConditionPresent(BOXNAVLAUNCH);
default:
@@ -609,11 +626,11 @@ static bool mavlinkCopterModeIsConfigured(uint8_t customMode)
isModeActivationConditionPresent(BOXANGLEHOLD);
case COPTER_MODE_ALT_HOLD:
return isModeActivationConditionPresent(BOXNAVALTHOLD);
- case COPTER_MODE_POSHOLD:
- return isModeActivationConditionPresent(BOXNAVPOSHOLD);
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:
@@ -1124,17 +1141,15 @@ void mavlinkSendHeartbeat(void)
{
uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
- flightModeForTelemetry_e flm = getFlightModeForTelemetry();
- uint8_t mavCustomMode;
- uint8_t mavSystemType;
-
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;
if (isPlane) {
- mavCustomMode = (uint8_t)inavToArduPlaneMap(flm);
mavSystemType = MAV_TYPE_FIXED_WING;
}
else {
- mavCustomMode = (uint8_t)inavToArduCopterMap(flm);
mavSystemType = mavlinkGetVehicleType();
}
@@ -1970,34 +1985,27 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
break;
case MAVLINK_MSG_ID_AVAILABLE_MODES:
{
- flightModeForTelemetry_e flm = getFlightModeForTelemetry();
- uint8_t currentCustom;
- if (mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY)) {
- currentCustom = (uint8_t)inavToArduPlaneMap(flm);
- mavlinkSendAvailableModes(planeModes, ARRAYLEN(planeModes), currentCustom, mavlinkPlaneModeIsConfigured);
+ 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 {
- currentCustom = (uint8_t)inavToArduCopterMap(flm);
- mavlinkSendAvailableModes(copterModes, ARRAYLEN(copterModes), currentCustom, mavlinkCopterModeIsConfigured);
+ mavlinkSendAvailableModes(copterModes, ARRAYLEN(copterModes), modeSelection.customMode, mavlinkCopterModeIsConfigured);
}
sent = true;
}
break;
case MAVLINK_MSG_ID_CURRENT_MODE:
{
- flightModeForTelemetry_e flm = getFlightModeForTelemetry();
- uint8_t currentCustom;
- if (mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY)) {
- currentCustom = (uint8_t)inavToArduPlaneMap(flm);
- } else {
- currentCustom = (uint8_t)inavToArduCopterMap(flm);
- }
+ 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,
- currentCustom,
- currentCustom);
+ modeSelection.customMode,
+ modeSelection.customMode);
mavlinkSendMessage();
sent = true;
}
From 870d50b14028cbce7a5efa0a979a801449c13568 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Mon, 23 Feb 2026 22:27:25 -0500
Subject: [PATCH 20/27] add mavlink doc
---
docs/Mavlink.md | 130 ++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 130 insertions(+)
create mode 100644 docs/Mavlink.md
diff --git a/docs/Mavlink.md b/docs/Mavlink.md
new file mode 100644
index 00000000000..8fdd440d3c1
--- /dev/null
+++ b/docs/Mavlink.md
@@ -0,0 +1,130 @@
+# 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.
+- `SET_POSITION_TARGET_GLOBAL_INT`: writes the GCS-guided waypoint when the frame is supported.
+- `RC_CHANNELS_OVERRIDE` passes channel values to the MAVLink serial receiver backend.
+- `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_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`.
From 2ac7b28ef919ad05ad443441e3a6805aa19cf22b Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Tue, 3 Mar 2026 19:53:49 -0500
Subject: [PATCH 21/27] qodo fixes + set_pos_tgt_global alt change + docs
---
docs/Mavlink.md | 7 +--
src/main/telemetry/mavlink.c | 100 ++++++++++++++++++++++++-----------
2 files changed, 73 insertions(+), 34 deletions(-)
diff --git a/docs/Mavlink.md b/docs/Mavlink.md
index 8fdd440d3c1..b29dcdf8f89 100644
--- a/docs/Mavlink.md
+++ b/docs/Mavlink.md
@@ -46,9 +46,9 @@ Messages are organized into MAVLink datastream groups. Each group sends **one me
- `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.
-- `SET_POSITION_TARGET_GLOBAL_INT`: writes the GCS-guided waypoint when the frame is supported.
-- `RC_CHANNELS_OVERRIDE` passes channel values to the MAVLink serial receiver backend.
+- `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).
@@ -59,6 +59,7 @@ Messages are organized into MAVLink datastream groups. Each group sends **one me
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`.
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 9af070c8f87..8626ae0b489 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -526,6 +526,33 @@ 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 uint8_t navWaypointFrame(const navWaypoint_t *wp, bool useIntMessages)
{
switch (wp->action) {
@@ -1857,33 +1884,12 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent);
}
return true;
-#ifdef USE_BARO
case MAV_CMD_DO_CHANGE_ALTITUDE:
{
- 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:
- 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);
- mavlinkSendCommandAck(command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent);
- }
+ const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame(frame, param1);
+ mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent);
return true;
}
-#endif
case MAV_CMD_SET_MESSAGE_INTERVAL:
{
uint8_t stream;
@@ -2175,15 +2181,25 @@ static bool handleIncoming_REQUEST_DATA_STREAM(void)
return false;
}
- if (msg.start_stop == 0) {
- mavlinkSetStreamRate(msg.req_stream_id, 0);
- return true;
+ 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;
+ }
}
- uint8_t 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;
}
+
mavlinkSetStreamRate(msg.req_stream_id, rate);
return true;
}
@@ -2196,20 +2212,40 @@ static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void)
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) {
+ mavlinkSetAltitudeTargetFromFrame(frame, msg.alt);
+ return true;
+ }
+
+ if (xIgnored || yIgnored) {
+ return true;
+ }
+
if (isGCSValid()) {
navWaypoint_t wp;
wp.action = NAV_WP_ACTION_WAYPOINT;
wp.lat = msg.lat_int;
wp.lon = msg.lon_int;
- wp.alt = (int32_t)(msg.alt * 100.0f);
+ wp.alt = zIgnored ? 0 : (int32_t)(msg.alt * 100.0f);
wp.p1 = 0;
wp.p2 = 0;
wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0;
@@ -2225,7 +2261,9 @@ static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void)
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) {
+ return false;
+ }
mavlinkRxHandleMessage(&msg);
return true;
}
From 2a1b71fc0ef9889d2e59e7890caaf2b54c81a464 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Thu, 5 Mar 2026 21:24:03 -0500
Subject: [PATCH 22/27] fixed stream rate runaway on radio_status, realtime
stream rate cap, reverted strict rc_override sysid check (caused fs),
SET_POSITION_TARGET_LOCAL_NED for qgc change alt
---
src/main/navigation/navigation.c | 6 +-
src/main/telemetry/mavlink.c | 249 +++++++++++++++++++------------
2 files changed, 161 insertions(+), 94 deletions(-)
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 8f60155b68c..21a130bef7b 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1333,9 +1333,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 &&
@@ -3964,7 +3966,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 8626ae0b489..e63c35d3177 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -101,7 +101,7 @@
#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 3
+#define ARDUPILOT_VERSION_PATCH 0
typedef enum {
MAV_FRAME_SUPPORTED_NONE = 0,
@@ -204,7 +204,7 @@ static uint8_t mavRates[] = {
static timeUs_t lastMavlinkMessage = 0;
static uint8_t mavRatesConfigured[MAXSTREAMS];
-static uint8_t mavTicks[MAXSTREAMS];
+static timeUs_t mavStreamNextDue[MAXSTREAMS];
static mavlink_message_t mavSendMsg;
static mavlink_message_t mavRecvMsg;
static mavlink_status_t mavRecvStatus;
@@ -328,25 +328,28 @@ static mavlinkModeSelection_t selectMavlinkMode(bool isPlane)
return modeSelection;
}
-static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
+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;
}
@@ -355,13 +358,13 @@ static void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate)
if (streamNum >= MAXSTREAMS) {
return;
}
- mavRates[streamNum] = rate;
- mavTicks[streamNum] = 0;
+ mavRates[streamNum] = mavlinkClampStreamRate(rate);
+ mavStreamNextDue[streamNum] = 0;
}
static int32_t mavlinkStreamIntervalUs(uint8_t streamNum)
{
- uint8_t rate = mavRates[streamNum];
+ uint8_t rate = mavlinkClampStreamRate(mavRates[streamNum]);
if (rate == 0) {
return -1;
}
@@ -461,6 +464,7 @@ static void mavlinkSendAutopilotVersion(void)
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.
@@ -469,9 +473,9 @@ static void mavlinkSendAutopilotVersion(void)
mavComponentId,
&mavSendMsg,
capabilities, // capabilities
- 0, // flight_sw_version. Setting this to actual Ardupilot version makes QGC not display modes anymore but "Unknown", except Guided is "GUIDED". Why?
- 0, // middleware_sw_version
- 0, // os_sw_version
+ 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
@@ -553,6 +557,69 @@ static MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitud
#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) {
@@ -1075,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)
);
@@ -1184,7 +1251,7 @@ void mavlinkSendHeartbeat(void)
if (manualInputAllowed) {
mavModes |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
- if (flm == FLM_POSITION_HOLD) {
+ if (flm == FLM_POSITION_HOLD && isGCSValid()) {
mavModes |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
else if (flm == FLM_MISSION || flm == FLM_RTH ) {
@@ -1331,35 +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)) {
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2, currentTimeUs)) {
mavlinkSendVfrHud();
mavlinkSendHeartbeat();
}
- if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_SYS_STATE)) {
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_SYS_STATE, currentTimeUs)) {
mavlinkSendExtendedSysState();
}
- if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) {
+ if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3, currentTimeUs)) {
mavlinkSendBatteryTemperatureStatusText();
}
@@ -1627,36 +1693,16 @@ static bool handleIncoming_MISSION_ITEM(void)
}
if (ARMING_FLAG(ARMED)) {
- // Legacy Mission Planner 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);
- 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.p2 = 0;
- wp.p3 = mavlinkFrameUsesAbsoluteAltitude(msg.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;
- } else {
- mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0);
- mavlinkSendMessage();
- return true;
+ // 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);
@@ -1863,7 +1909,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
}
if (isGCSValid()) {
- navWaypoint_t wp;
+ navWaypoint_t wp = {0};
wp.action = NAV_WP_ACTION_WAYPOINT;
wp.lat = (int32_t)(latitudeDeg * 1e7f);
wp.lon = (int32_t)(longitudeDeg * 1e7f);
@@ -1886,7 +1932,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem
return true;
case MAV_CMD_DO_CHANGE_ALTITUDE:
{
- const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame(frame, param1);
+ const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame((uint8_t)lrintf(param2), param1);
mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent);
return true;
}
@@ -2096,35 +2142,15 @@ static bool handleIncoming_MISSION_ITEM_INT(void)
}
if (ARMING_FLAG(ARMED)) {
- if (isGCSValid() && (msg.command == MAV_CMD_NAV_WAYPOINT) && (msg.current == 2)) {
- if (!(msg.frame == MAV_FRAME_GLOBAL_INT || msg.frame == MAV_FRAME_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;
- }
-
- navWaypoint_t wp;
- wp.action = NAV_WP_ACTION_WAYPOINT;
- wp.lat = msg.x;
- wp.lon = msg.y;
- wp.alt = (int32_t)(msg.z * 100.0f);
- wp.p1 = 0;
- wp.p2 = 0;
- wp.p3 = mavlinkFrameUsesAbsoluteAltitude(msg.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;
- } else {
- 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.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);
@@ -2232,7 +2258,9 @@ static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void)
// Altitude-only SET_POSITION_TARGET_GLOBAL_INT mirrors MAV_CMD_DO_CHANGE_ALTITUDE semantics.
if (xIgnored && yIgnored && !zIgnored) {
- mavlinkSetAltitudeTargetFromFrame(frame, msg.alt);
+ if (isGCSValid()) {
+ mavlinkSetAltitudeTargetFromFrame(frame, msg.alt);
+ }
return true;
}
@@ -2241,7 +2269,7 @@ static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void)
}
if (isGCSValid()) {
- navWaypoint_t wp;
+ navWaypoint_t wp = {0};
wp.action = NAV_WP_ACTION_WAYPOINT;
wp.lat = msg.lat_int;
wp.lon = msg.lon_int;
@@ -2257,13 +2285,48 @@ static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void)
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);
- if (msg.target_system != mavSystemId) {
+ /*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;
}
@@ -2397,6 +2460,8 @@ static bool processMAVLinkIncomingTelemetry(void)
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
From 8d2084a1fa6ce934314e700a026dca958d269849 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Thu, 5 Mar 2026 22:15:53 -0500
Subject: [PATCH 23/27] clean up msp alt decode
---
src/main/fc/fc_msp.c | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index bef7c54de40..0bb7107b707 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -4385,7 +4385,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
break;
}
- if (navigationSetAltitudeTargetWithDatum((geoAltitudeDatumFlag_e)sbufReadU8(src), (int32_t)sbufReadU32(src))) {
+ uint8_t setAltDatum = (geoAltitudeDatumFlag_e)sbufReadU8(src);
+ int32_t setNewAlt = sbufReadU32(src);
+ if (navigationSetAltitudeTargetWithDatum(setAltDatum, setNewAlt)) {
*ret = MSP_RESULT_ACK;
break;
}
From da7e20eabb87f33e5390bbeddae1be2ec93613b2 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Thu, 5 Mar 2026 22:47:21 -0500
Subject: [PATCH 24/27] fixed version + autocontinue
---
src/main/telemetry/mavlink.c | 15 +++++++++++----
1 file changed, 11 insertions(+), 4 deletions(-)
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index e63c35d3177..d62761dbdf4 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -467,15 +467,20 @@ static void mavlinkSendAutopilotVersion(void)
capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT;
+ const uint32_t flightSwVersion =
+ ((uint32_t)ARDUPILOT_VERSION_MAJOR << 24) |
+ ((uint32_t)ARDUPILOT_VERSION_MINOR << 16) |
+ ((uint32_t)ARDUPILOT_VERSION_PATCH << 8);
+
// 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
+ flightSwVersion, // flight_sw_version
+ 0, // middleware_sw_version
+ 0, // os_sw_version
0, // board_version
0ULL, // flight_custom_version
0ULL, // middleware_custom_version
@@ -1453,7 +1458,9 @@ static int incomingMissionWpSequence = 0;
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)
{
- if (autocontinue == 0) {
+ const bool lastMissionItem = incomingMissionWpCount > 0 && ((int)seq + 1 >= incomingMissionWpCount);
+
+ if (autocontinue == 0 && !lastMissionItem) {
mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0);
mavlinkSendMessage();
return true;
From 0a23646d091cf9cefd03f85f10a7998860856532 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Sun, 29 Mar 2026 14:03:28 -0400
Subject: [PATCH 25/27] fixed unit test
---
src/test/unit/mavlink_unittest.cc | 78 +++++++++++++++++++++++++++++--
1 file changed, 74 insertions(+), 4 deletions(-)
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
index 6e09307247f..19acf755e08 100644
--- a/src/test/unit/mavlink_unittest.cc
+++ b/src/test/unit/mavlink_unittest.cc
@@ -100,9 +100,14 @@ static navWaypoint_t lastWaypoint;
static int setWaypointCalls;
static int resetWaypointCalls;
static int mavlinkRxHandleCalls;
+static int altitudeTargetCalls;
+static geoAltitudeDatumFlag_e lastAltitudeDatum;
+static int32_t lastAltitudeTargetCm;
+static bool altitudeTargetAccepted;
static bool gcsValid;
static int waypointCount;
static navWaypoint_t waypointStore[4];
+static float estimatedPosition[3];
static void resetSerialBuffers(void)
{
@@ -137,9 +142,14 @@ static void initMavlinkTestState(void)
setWaypointCalls = 0;
resetWaypointCalls = 0;
mavlinkRxHandleCalls = 0;
+ altitudeTargetCalls = 0;
+ lastAltitudeDatum = NAV_WP_TAKEOFF_DATUM;
+ lastAltitudeTargetCm = 0;
+ altitudeTargetAccepted = true;
gcsValid = true;
waypointCount = 0;
memset(waypointStore, 0, sizeof(waypointStore));
+ memset(estimatedPosition, 0, sizeof(estimatedPosition));
memset(&rxLinkStatistics, 0, sizeof(rxLinkStatistics));
telemetryConfigMutable()->mavlink.sysid = 1;
@@ -542,7 +552,7 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntSetsWaypoint)
mavlink_message_t msg;
mavlink_msg_set_position_target_global_int_pack(
42, 200, &msg,
- 0, 1, MAV_COMP_ID_MISSIONPLANNER,
+ 0, 1, 0,
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0,
375000000, -1222500000, 12.3f,
0, 0, 0, 0, 0, 0, 0, 0);
@@ -564,7 +574,7 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude)
mavlink_message_t msg;
mavlink_msg_set_position_target_global_int_pack(
42, 200, &msg,
- 0, 1, MAV_COMP_ID_MISSIONPLANNER,
+ 0, 1, 0,
MAV_FRAME_GLOBAL_INT, 0,
375000000, -1222500000, 12.3f,
0, 0, 0, 0, 0, 0, 0, 0);
@@ -576,6 +586,59 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude)
EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE);
}
+TEST(MavlinkTelemetryTest, CommandLongChangeAltitudeUsesRequestedDatum)
+{
+ initMavlinkTestState();
+
+ mavlink_message_t msg;
+ mavlink_msg_command_long_pack(
+ 42, 200, &msg,
+ 1, MAV_COMP_ID_MISSIONPLANNER,
+ MAV_CMD_DO_CHANGE_ALTITUDE,
+ 0,
+ 12.3f, (float)MAV_FRAME_GLOBAL_RELATIVE_ALT, 0, 0, 0, 0, 0);
+
+ pushRxMessage(&msg);
+ 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_CHANGE_ALTITUDE);
+ EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED);
+ EXPECT_EQ(altitudeTargetCalls, 1);
+ EXPECT_EQ(lastAltitudeDatum, NAV_WP_TAKEOFF_DATUM);
+ EXPECT_EQ(lastAltitudeTargetCm, 1230);
+}
+
+TEST(MavlinkTelemetryTest, SetPositionTargetLocalNedUpdatesAltitudeOnly)
+{
+ initMavlinkTestState();
+ estimatedPosition[Z] = 1000.0f;
+
+ mavlink_message_t msg;
+ mavlink_msg_set_position_target_local_ned_pack(
+ 42, 200, &msg,
+ 0, 1, 0,
+ MAV_FRAME_LOCAL_OFFSET_NED,
+ POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE,
+ 0.0f, 0.0f, -2.5f,
+ 0.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f);
+
+ pushRxMessage(&msg);
+ handleMAVLinkTelemetry(1000);
+
+ EXPECT_EQ(altitudeTargetCalls, 1);
+ EXPECT_EQ(lastAltitudeDatum, NAV_WP_TAKEOFF_DATUM);
+ EXPECT_EQ(lastAltitudeTargetCm, 1250);
+}
+
TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream)
{
initMavlinkTestState();
@@ -881,8 +944,7 @@ bool osdUsingScaledThrottle(void)
float getEstimatedActualPosition(int axis)
{
- UNUSED(axis);
- return 0.0f;
+ return estimatedPosition[axis];
}
float getEstimatedActualVelocity(int axis)
@@ -970,6 +1032,14 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t *wp)
setWaypointCalls++;
}
+bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm)
+{
+ lastAltitudeDatum = datumFlag;
+ lastAltitudeTargetCm = targetAltitudeCm;
+ altitudeTargetCalls++;
+ return altitudeTargetAccepted;
+}
+
int getWaypointCount(void)
{
return waypointCount;
From 83d35eec4b87ecb14b9ef69ad409da8134504b6f Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Tue, 19 May 2026 22:15:25 -0400
Subject: [PATCH 26/27] Regenerate MSP docs for mav_fix2 rebase
---
docs/development/msp/README.md | 16 ++++-
docs/development/msp/inav_enums.json | 92 +++++++++++++++++---------
docs/development/msp/inav_enums_ref.md | 88 +++++++++++++++---------
3 files changed, 133 insertions(+), 63 deletions(-)
diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md
index a09b622b651..1615a6db304 100644
--- a/docs/development/msp/README.md
+++ b/docs/development/msp/README.md
@@ -450,6 +450,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)
[8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index)
[8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading)
+[8752 - MSP2_INAV_SET_AUX_RC](#msp2_inav_set_aux_rc)
[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind)
[12289 - MSP2_RX_BIND](#msp2_rx_bind)
@@ -2311,7 +2312,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
| `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) |
| `eph` | `uint16_t` | 2 | cm | Estimated Horizontal Position Accuracy (`gpsSol.eph`) |
| `epv` | `uint16_t` | 2 | cm | Estimated Vertical Position Accuracy (`gpsSol.epv`) |
-| `hwVersion` | `uint32_t` | 4 | Version code | GPS hardware version (`gpsState.hwVersion`). Values: 500=UBLOX5, 600=UBLOX6, 700=UBLOX7, 800=UBLOX8, 900=UBLOX9, 1000=UBLOX10, 0=UNKNOWN |
+| `hwVersion` | `uint8_t` | 1 | - | GPS hardware version bit-field: bits[7:6]=series (0b01=u-blox Neo/M), bits[5:0]=generation. E.g. 0x48=M8, 0x49=M9, 0x4A=M10, 0=unknown. |
**Notes:** Requires `USE_GPS`.
@@ -4710,6 +4711,19 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
**Notes:** Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.
+## `MSP2_INAV_SET_AUX_RC (8752 / 0x2230)`
+**Description:** Bandwidth-efficient auxiliary RC channel update. Sets CH13-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough.
+
+**Request Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `definitionByte` | `uint8_t` | 1 | - | Packed start channel and resolution. Bits 7-3: start channel index (valid range 12-31 for CH13-CH32; 0-11 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error). |
+| `channelData` | `uint8_t[]` | array | PWM (encoded) | Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM, clamped to 750-2250us. |
+
+**Reply Payload:** **None**
+
+**Notes:** CH1-CH12 (index 0-11) are protected and will return `MSP_RESULT_ERROR`. Payload size must be 2-49 bytes. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. When MSP is the primary RX provider, channels covered by `MSP_SET_RAW_RC` are automatically skipped. Channels in the `mspOverrideChannels` bitmask are skipped when MSP RC Override mode is active. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes and values are clamped to 750-2250us.
+
## `MSP2_BETAFLIGHT_BIND (12288 / 0x3000)`
**Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2).
diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json
index 023699a11ab..01f43b0a8b6 100644
--- a/docs/development/msp/inav_enums.json
+++ b/docs/development/msp/inav_enums.json
@@ -1,9 +1,9 @@
{
"build": {
"fc_version": {
- "major": 10,
+ "major": 9,
"minor": 0,
- "patch": 0
+ "patch": 1
}
},
"enums": {
@@ -373,7 +373,8 @@
"BARO_B2SMPB": "10",
"BARO_MSP": "11",
"BARO_FAKE": "12",
- "BARO_MAX": "BARO_FAKE"
+ "BARO_CRSF": "13",
+ "BARO_MAX": "BARO_CRSF"
},
"batCapacityUnit_e": {
"_source": "inav/src/main/sensors/battery_config_structs.h",
@@ -744,7 +745,7 @@
"CRSF_FRAMETYPE_GPS": "2",
"CRSF_FRAMETYPE_VARIO_SENSOR": "7",
"CRSF_FRAMETYPE_BATTERY_SENSOR": "8",
- "CRSF_FRAMETYPE_BAROMETER_ALTITUDE": "9",
+ "CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR": "9",
"CRSF_FRAMETYPE_AIRSPEED_SENSOR": "10",
"CRSF_FRAMETYPE_RPM": "12",
"CRSF_FRAMETYPE_TEMP": "13",
@@ -770,8 +771,7 @@
"CRSF_FRAME_BATTERY_SENSOR_INDEX": "",
"CRSF_FRAME_FLIGHT_MODE_INDEX": "",
"CRSF_FRAME_GPS_INDEX": "",
- "CRSF_FRAME_VARIO_SENSOR_INDEX": "",
- "CRSF_FRAME_BAROMETER_ALTITUDE_INDEX": "",
+ "CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX": "",
"CRSF_FRAME_TEMP_INDEX": "",
"CRSF_FRAME_RPM_INDEX": "",
"CRSF_FRAME_AIRSPEED_INDEX": "",
@@ -802,7 +802,9 @@
"CURRENT_SENSOR_FAKE": "3",
"CURRENT_SENSOR_ESC": "4",
"CURRENT_SENSOR_SMARTPORT": "5",
- "CURRENT_SENSOR_MAX": "CURRENT_SENSOR_SMARTPORT"
+ "CURRENT_SENSOR_CRSF": "6",
+ "CURRENT_SENSOR_CAN": "7",
+ "CURRENT_SENSOR_MAX": "CURRENT_SENSOR_CAN"
},
"devHardwareType_e": {
"_source": "inav/src/main/drivers/bus.h",
@@ -938,6 +940,14 @@
"DJI_OSD_CN_ADJUSTEMNTS": "6",
"DJI_OSD_CN_MAX_ELEMENTS": "7"
},
+ "dronecanBitrate_e": {
+ "_source": "inav/src/main/drivers/dronecan/dronecan.h",
+ "DRONECAN_BITRATE_125KBPS": "0",
+ "DRONECAN_BITRATE_250KBPS": "1",
+ "DRONECAN_BITRATE_500KBPS": "2",
+ "DRONECAN_BITRATE_1000KBPS": "3",
+ "DRONECAN_BITRATE_COUNT": "4"
+ },
"dshotCommands_e": {
"_source": "inav/src/main/drivers/pwm_output.h",
"DSHOT_CMD_SPIN_DIRECTION_NORMAL": "20",
@@ -1506,8 +1516,10 @@
"_source": "inav/src/main/io/gps.h",
"GPS_UBLOX": "0",
"GPS_MSP": "1",
- "GPS_FAKE": "2",
- "GPS_PROVIDER_COUNT": "3"
+ "GPS_CRSF": "2",
+ "GPS_FAKE": "3",
+ "GPS_DRONECAN": "4",
+ "GPS_PROVIDER_COUNT": "5"
},
"gpsState_e": {
"_source": "inav/src/main/io/gps_private.h",
@@ -1844,13 +1856,6 @@
"ITERM_RELAX_RP": "1",
"ITERM_RELAX_RPY": "2"
},
- "led_pin_pwm_mode_e": {
- "_source": "inav/src/main/drivers/light_ws2811strip.h",
- "LED_PIN_PWM_MODE_SHARED_LOW": "0",
- "LED_PIN_PWM_MODE_SHARED_HIGH": "1",
- "LED_PIN_PWM_MODE_LOW": "2",
- "LED_PIN_PWM_MODE_HIGH": "3"
- },
"ledBaseFunctionId_e": {
"_source": "inav/src/main/io/ledstrip.h",
"LED_FUNCTION_COLOR": "0",
@@ -2067,7 +2072,7 @@
"LOGIC_CONDITION_TIMER": "49",
"LOGIC_CONDITION_DELTA": "50",
"LOGIC_CONDITION_APPROX_EQUAL": "51",
- "LOGIC_CONDITION_LED_PIN_PWM": "52",
+ "LOGIC_CONDITION_PINIO_PWM": "52",
"LOGIC_CONDITION_DISABLE_GPS_FIX": "53",
"LOGIC_CONDITION_RESET_MAG_CALIBRATION": "54",
"LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY": "55",
@@ -2108,7 +2113,8 @@
"LOG_TOPIC_POS_ESTIMATOR": "8",
"LOG_TOPIC_VTX": "9",
"LOG_TOPIC_OSD": "10",
- "LOG_TOPIC_COUNT": "11"
+ "LOG_TOPIC_CAN": "11",
+ "LOG_TOPIC_COUNT": "12"
},
"lsm6dxxConfigMasks_e": {
"_source": "inav/src/main/drivers/accgyro/accgyro_lsm6dxx.h",
@@ -2259,6 +2265,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",
@@ -2914,7 +2928,8 @@
"OSD_NAV_FW_ALT_CONTROL_RESPONSE": "166",
"OSD_NAV_MIN_GROUND_SPEED": "167",
"OSD_THROTTLE_GAUGE": "168",
- "OSD_ITEM_COUNT": "169"
+ "OSD_GPS_EXTRA_STATS": "169",
+ "OSD_ITEM_COUNT": "170"
},
"osd_sidebar_arrow_e": {
"_source": "inav/src/main/io/osd_grid.c",
@@ -3011,7 +3026,8 @@
"OUTPUT_MODE_AUTO": "0",
"OUTPUT_MODE_MOTORS": "1",
"OUTPUT_MODE_SERVOS": "2",
- "OUTPUT_MODE_LED": "3"
+ "OUTPUT_MODE_LED": "3",
+ "OUTPUT_MODE_PINIO": "4"
},
"pageId_e": {
"_source": "inav/src/main/io/dashboard.h",
@@ -3065,7 +3081,8 @@
"pinLabel_e": {
"_source": "inav/src/main/drivers/pwm_mapping.h",
"PIN_LABEL_NONE": "0",
- "PIN_LABEL_LED": "1"
+ "PIN_LABEL_LED": "1",
+ "PIN_LABEL_PINIO_BASE": "2"
},
"pitotSensor_e": {
"_source": "inav/src/main/sensors/pitotmeter.h",
@@ -3315,9 +3332,8 @@
"_source": "inav/src/main/io/displayport_msp_osd.c",
"SD_3016": "0",
"HD_5018": "1",
- "HD_3016": "2",
- "HD_6022": "3",
- "HD_5320": "4"
+ "HD_6022": "2",
+ "HD_5320": "3"
},
"resourceOwner_e": {
"_source": "inav/src/main/drivers/resource.h",
@@ -3355,7 +3371,8 @@
"OWNER_OLED_DISPLAY": "31",
"OWNER_PINIO": "32",
"OWNER_IRLOCK": "33",
- "OWNER_TOTAL_COUNT": "34"
+ "OWNER_DRONECAN": "34",
+ "OWNER_TOTAL_COUNT": "35"
},
"resourceType_e": {
"_source": "inav/src/main/drivers/resource.h",
@@ -3372,7 +3389,8 @@
"RESOURCE_QUADSPI_BK2IO3": "10",
"RESOURCE_ADC_CH1": "11",
"RESOURCE_RX_CE": "12",
- "RESOURCE_TOTAL_COUNT": "13"
+ "RESOURCE_CAN_TX": "13",
+ "RESOURCE_TOTAL_COUNT": "14"
},
"reversibleMotorsThrottleState_e": {
"_source": "inav/src/main/flight/mixer.h",
@@ -3577,7 +3595,7 @@
"FUNCTION_DJI_HD_OSD": "(1 << 21)",
"FUNCTION_SERVO_SERIAL": "(1 << 22)",
"FUNCTION_TELEMETRY_SMARTPORT_MASTER": "(1 << 23)",
- "FUNCTION_UNUSED_2": "(1 << 24)",
+ "FUNCTION_CRSF_SENSOR": "(1 << 24)",
"FUNCTION_MSP_OSD": "(1 << 25)",
"FUNCTION_GIMBAL": "(1 << 26)",
"FUNCTION_GIMBAL_HEADTRACKER": "(1 << 27)"
@@ -3708,6 +3726,11 @@
"HITL_GPS_TIMEOUT": "(1 << 8)",
"HITL_PITOT_FAILURE": "(1 << 9)"
},
+ "sitlCANMode_e": {
+ "_source": "inav/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c",
+ "SITL_CAN_MODE_STUB": "0",
+ "SITL_CAN_MODE_SOCKETCAN": "1"
+ },
"smartAudioVersion_e": {
"_source": "inav/src/main/io/vtx_smartaudio.h",
"SA_UNKNOWN": "0",
@@ -3865,7 +3888,8 @@
"_source": "inav/src/main/drivers/timer.h",
"TCH_DMA_IDLE": "0",
"TCH_DMA_READY": "1",
- "TCH_DMA_ACTIVE": "2"
+ "TCH_DMA_ACTIVE": "2",
+ "TCH_DMA_CIRCULAR": "3"
},
"tempSensorType_e": {
"_source": "inav/src/main/sensors/temperature.h",
@@ -3897,7 +3921,8 @@
"TIM_USE_SERVO": "(1 << 3)",
"TIM_USE_MC_CHNFW": "(1 << 4)",
"TIM_USE_LED": "(1 << 24)",
- "TIM_USE_BEEPER": "(1 << 25)"
+ "TIM_USE_BEEPER": "(1 << 25)",
+ "TIM_USE_PINIO": "(1 << 26)"
},
"timId_e": {
"_source": "inav/src/main/io/ledstrip.c",
@@ -4018,7 +4043,8 @@
"MSG_CFG_SBAS": "22",
"MSG_CFG_GNSS": "62",
"MSG_MON_GNSS": "40",
- "MSG_NAV_SIG": "67"
+ "MSG_NAV_SIG": "67",
+ "MSG_MON_RF": "56"
},
"vcselPeriodType_e": {
"_source": "inav/src/main/drivers/rangefinder/rangefinder_vl53l0x.c",
@@ -4044,7 +4070,9 @@
"VOLTAGE_SENSOR_ESC": "2",
"VOLTAGE_SENSOR_FAKE": "3",
"VOLTAGE_SENSOR_SMARTPORT": "4",
- "VOLTAGE_SENSOR_MAX": "VOLTAGE_SENSOR_SMARTPORT"
+ "VOLTAGE_SENSOR_CRSF": "5",
+ "VOLTAGE_SENSOR_CAN": "6",
+ "VOLTAGE_SENSOR_MAX": "VOLTAGE_SENSOR_CAN"
},
"vs600Band_e": {
"_source": "inav/src/main/io/smartport_master.h",
@@ -4150,4 +4178,4 @@
"ZERO_CALIBRATION_FAIL": "3"
}
}
-}
+}
\ No newline at end of file
diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md
index 36a2295b411..0fcdf6f6d9c 100644
--- a/docs/development/msp/inav_enums_ref.md
+++ b/docs/development/msp/inav_enums_ref.md
@@ -73,6 +73,7 @@
- [displayTransactionOption_e](#enum-displaytransactionoption_e)
- [displayWidgetType_e](#enum-displaywidgettype_e)
- [DjiCraftNameElements_t](#enum-djicraftnameelements_t)
+- [dronecanBitrate_e](#enum-dronecanbitrate_e)
- [dshotCommands_e](#enum-dshotcommands_e)
- [dumpFlags_e](#enum-dumpflags_e)
- [dynamicGyroNotchMode_e](#enum-dynamicgyronotchmode_e)
@@ -152,7 +153,6 @@
- [ibusSensorValue_e](#enum-ibussensorvalue_e)
- [inputSource_e](#enum-inputsource_e)
- [itermRelax_e](#enum-itermrelax_e)
-- [led_pin_pwm_mode_e](#enum-led_pin_pwm_mode_e)
- [ledBaseFunctionId_e](#enum-ledbasefunctionid_e)
- [ledDirectionId_e](#enum-leddirectionid_e)
- [ledModeIndex_e](#enum-ledmodeindex_e)
@@ -301,6 +301,7 @@
- [simTransmissionState_e](#enum-simtransmissionstate_e)
- [simTxFlags_e](#enum-simtxflags_e)
- [simulatorFlags_t](#enum-simulatorflags_t)
+- [sitlCANMode_e](#enum-sitlcanmode_e)
- [smartAudioVersion_e](#enum-smartaudioversion_e)
- [smartportFuelUnit_e](#enum-smartportfuelunit_e)
- [softSerialPortIndex_e](#enum-softserialportindex_e)
@@ -836,7 +837,8 @@
| `BARO_B2SMPB` | 10 | |
| `BARO_MSP` | 11 | |
| `BARO_FAKE` | 12 | |
-| `BARO_MAX` | BARO_FAKE | |
+| `BARO_CRSF` | 13 | |
+| `BARO_MAX` | BARO_CRSF | |
---
## `batCapacityUnit_e`
@@ -1318,7 +1320,7 @@
| `CRSF_FRAMETYPE_GPS` | 2 | |
| `CRSF_FRAMETYPE_VARIO_SENSOR` | 7 | |
| `CRSF_FRAMETYPE_BATTERY_SENSOR` | 8 | |
-| `CRSF_FRAMETYPE_BAROMETER_ALTITUDE` | 9 | |
+| `CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR` | 9 | |
| `CRSF_FRAMETYPE_AIRSPEED_SENSOR` | 10 | |
| `CRSF_FRAMETYPE_RPM` | 12 | |
| `CRSF_FRAMETYPE_TEMP` | 13 | |
@@ -1349,8 +1351,7 @@
| `CRSF_FRAME_BATTERY_SENSOR_INDEX` | | |
| `CRSF_FRAME_FLIGHT_MODE_INDEX` | | |
| `CRSF_FRAME_GPS_INDEX` | | |
-| `CRSF_FRAME_VARIO_SENSOR_INDEX` | | |
-| `CRSF_FRAME_BAROMETER_ALTITUDE_INDEX` | | |
+| `CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX` | | |
| `CRSF_FRAME_TEMP_INDEX` | | |
| `CRSF_FRAME_RPM_INDEX` | | |
| `CRSF_FRAME_AIRSPEED_INDEX` | | |
@@ -1396,7 +1397,9 @@
| `CURRENT_SENSOR_FAKE` | 3 | |
| `CURRENT_SENSOR_ESC` | 4 | |
| `CURRENT_SENSOR_SMARTPORT` | 5 | |
-| `CURRENT_SENSOR_MAX` | CURRENT_SENSOR_SMARTPORT | |
+| `CURRENT_SENSOR_CRSF` | 6 | |
+| `CURRENT_SENSOR_CAN` | 7 | |
+| `CURRENT_SENSOR_MAX` | CURRENT_SENSOR_CAN | |
---
## `devHardwareType_e`
@@ -1582,6 +1585,19 @@
| `DJI_OSD_CN_ADJUSTEMNTS` | 6 | |
| `DJI_OSD_CN_MAX_ELEMENTS` | 7 | |
+---
+## `dronecanBitrate_e`
+
+> Source: ../../../src/main/drivers/dronecan/dronecan.h
+
+| Enumerator | Value | Condition |
+|---|---:|---|
+| `DRONECAN_BITRATE_125KBPS` | 0 | |
+| `DRONECAN_BITRATE_250KBPS` | 1 | |
+| `DRONECAN_BITRATE_500KBPS` | 2 | |
+| `DRONECAN_BITRATE_1000KBPS` | 3 | |
+| `DRONECAN_BITRATE_COUNT` | 4 | |
+
---
## `dshotCommands_e`
@@ -2440,8 +2456,10 @@
|---|---:|---|
| `GPS_UBLOX` | 0 | |
| `GPS_MSP` | 1 | |
-| `GPS_FAKE` | 2 | |
-| `GPS_PROVIDER_COUNT` | 3 | |
+| `GPS_CRSF` | 2 | |
+| `GPS_FAKE` | 3 | |
+| `GPS_DRONECAN` | 4 | |
+| `GPS_PROVIDER_COUNT` | 5 | |
---
## `gpsState_e`
@@ -2880,18 +2898,6 @@
| `ITERM_RELAX_RP` | 1 | |
| `ITERM_RELAX_RPY` | 2 | |
----
-## `led_pin_pwm_mode_e`
-
-> Source: ../../../src/main/drivers/light_ws2811strip.h
-
-| Enumerator | Value | Condition |
-|---|---:|---|
-| `LED_PIN_PWM_MODE_SHARED_LOW` | 0 | |
-| `LED_PIN_PWM_MODE_SHARED_HIGH` | 1 | |
-| `LED_PIN_PWM_MODE_LOW` | 2 | |
-| `LED_PIN_PWM_MODE_HIGH` | 3 | |
-
---
## `ledBaseFunctionId_e`
@@ -3160,7 +3166,7 @@
| `LOGIC_CONDITION_TIMER` | 49 | |
| `LOGIC_CONDITION_DELTA` | 50 | |
| `LOGIC_CONDITION_APPROX_EQUAL` | 51 | |
-| `LOGIC_CONDITION_LED_PIN_PWM` | 52 | |
+| `LOGIC_CONDITION_PINIO_PWM` | 52 | |
| `LOGIC_CONDITION_DISABLE_GPS_FIX` | 53 | |
| `LOGIC_CONDITION_RESET_MAG_CALIBRATION` | 54 | |
| `LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY` | 55 | |
@@ -3211,7 +3217,8 @@
| `LOG_TOPIC_POS_ESTIMATOR` | 8 | |
| `LOG_TOPIC_VTX` | 9 | |
| `LOG_TOPIC_OSD` | 10 | |
-| `LOG_TOPIC_COUNT` | 11 | |
+| `LOG_TOPIC_CAN` | 11 | |
+| `LOG_TOPIC_COUNT` | 12 | |
---
## `lsm6dxxConfigMasks_e`
@@ -3560,7 +3567,8 @@
| `MULTI_FUNC_3` | 3 | |
| `MULTI_FUNC_4` | 4 | |
| `MULTI_FUNC_5` | 5 | |
-| `MULTI_FUNC_END` | 6 | |
+| `MULTI_FUNC_6` | 6 | |
+| `MULTI_FUNC_END` | 7 | |
---
## `multiFunctionFlags_e`
@@ -4305,7 +4313,8 @@
| `OSD_NAV_FW_ALT_CONTROL_RESPONSE` | 166 | |
| `OSD_NAV_MIN_GROUND_SPEED` | 167 | |
| `OSD_THROTTLE_GAUGE` | 168 | |
-| `OSD_ITEM_COUNT` | 169 | |
+| `OSD_GPS_EXTRA_STATS` | 169 | |
+| `OSD_ITEM_COUNT` | 170 | |
---
## `osd_sidebar_arrow_e`
@@ -4458,6 +4467,7 @@
| `OUTPUT_MODE_MOTORS` | 1 | |
| `OUTPUT_MODE_SERVOS` | 2 | |
| `OUTPUT_MODE_LED` | 3 | |
+| `OUTPUT_MODE_PINIO` | 4 | |
---
## `pageId_e`
@@ -4547,6 +4557,7 @@
|---|---:|---|
| `PIN_LABEL_NONE` | 0 | |
| `PIN_LABEL_LED` | 1 | |
+| `PIN_LABEL_PINIO_BASE` | 2 | |
---
## `pitotSensor_e`
@@ -4892,7 +4903,8 @@
| `OWNER_OLED_DISPLAY` | 31 | |
| `OWNER_PINIO` | 32 | |
| `OWNER_IRLOCK` | 33 | |
-| `OWNER_TOTAL_COUNT` | 34 | |
+| `OWNER_DRONECAN` | 34 | |
+| `OWNER_TOTAL_COUNT` | 35 | |
---
## `resourceType_e`
@@ -4914,7 +4926,8 @@
| `RESOURCE_QUADSPI_BK2IO3` | 10 | |
| `RESOURCE_ADC_CH1` | 11 | |
| `RESOURCE_RX_CE` | 12 | |
-| `RESOURCE_TOTAL_COUNT` | 13 | |
+| `RESOURCE_CAN_TX` | 13 | |
+| `RESOURCE_TOTAL_COUNT` | 14 | |
---
## `reversibleMotorsThrottleState_e`
@@ -5240,7 +5253,7 @@
| `FUNCTION_DJI_HD_OSD` | (1 << 21) | |
| `FUNCTION_SERVO_SERIAL` | (1 << 22) | |
| `FUNCTION_TELEMETRY_SMARTPORT_MASTER` | (1 << 23) | |
-| `FUNCTION_UNUSED_2` | (1 << 24) | |
+| `FUNCTION_CRSF_SENSOR` | (1 << 24) | |
| `FUNCTION_MSP_OSD` | (1 << 25) | |
| `FUNCTION_GIMBAL` | (1 << 26) | |
| `FUNCTION_GIMBAL_HEADTRACKER` | (1 << 27) | |
@@ -5441,6 +5454,16 @@
| `HITL_GPS_TIMEOUT` | (1 << 8) | |
| `HITL_PITOT_FAILURE` | (1 << 9) | |
+---
+## `sitlCANMode_e`
+
+> Source: ../../../src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
+
+| Enumerator | Value | Condition |
+|---|---:|---|
+| `SITL_CAN_MODE_STUB` | 0 | |
+| `SITL_CAN_MODE_SOCKETCAN` | 1 | |
+
---
## `smartAudioVersion_e`
@@ -5655,7 +5678,7 @@
---
## `systemState_e`
-> Source: ../../../src/main/fc/fc_init.c
+> Source: ../../../src/main/fc/fc_init.h
| Enumerator | Value | Condition |
|---|---:|---|
@@ -5669,7 +5692,7 @@
---
## `systemState_e`
-> Source: ../../../src/main/fc/fc_init.h
+> Source: ../../../src/main/fc/fc_init.c
| Enumerator | Value | Condition |
|---|---:|---|
@@ -5690,6 +5713,7 @@
| `TCH_DMA_IDLE` | 0 | |
| `TCH_DMA_READY` | 1 | |
| `TCH_DMA_ACTIVE` | 2 | |
+| `TCH_DMA_CIRCULAR` | 3 | |
---
## `tempSensorType_e`
@@ -5747,6 +5771,7 @@
| `TIM_USE_MC_CHNFW` | (1 << 4) | |
| `TIM_USE_LED` | (1 << 24) | |
| `TIM_USE_BEEPER` | (1 << 25) | |
+| `TIM_USE_PINIO` | (1 << 26) | |
---
## `timId_e`
@@ -5917,6 +5942,7 @@
| `MSG_CFG_GNSS` | 62 | |
| `MSG_MON_GNSS` | 40 | |
| `MSG_NAV_SIG` | 67 | |
+| `MSG_MON_RF` | 56 | |
---
## `vcselPeriodType_e`
@@ -5957,7 +5983,9 @@
| `VOLTAGE_SENSOR_ESC` | 2 | |
| `VOLTAGE_SENSOR_FAKE` | 3 | |
| `VOLTAGE_SENSOR_SMARTPORT` | 4 | |
-| `VOLTAGE_SENSOR_MAX` | VOLTAGE_SENSOR_SMARTPORT | |
+| `VOLTAGE_SENSOR_CRSF` | 5 | |
+| `VOLTAGE_SENSOR_CAN` | 6 | |
+| `VOLTAGE_SENSOR_MAX` | VOLTAGE_SENSOR_CAN | |
---
## `vs600Band_e`
From 4bc54198a2c75120e5fed6cdb70f163ac898fee4 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Tue, 19 May 2026 22:21:45 -0400
Subject: [PATCH 27/27] Remove accidental empty dm file
---
dm.txt | 0
1 file changed, 0 insertions(+), 0 deletions(-)
delete mode 100644 dm.txt
diff --git a/dm.txt b/dm.txt
deleted file mode 100644
index e69de29bb2d..00000000000