From d84ca0b7809c2b4edc6d8994625610f9c735ddaa 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/26] 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 116c76d087a04692898efcd55a4b7644fc21fe50 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/26] 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 2e850fcc2ad8ec5781a32ed58365dbb142e40beb 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/26] 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 c8187be5b8de805436bf6e320177ad1ccbb045fb 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/26] 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 00ab05bf8944c4ebaee39b48d0a604b4ef2fecf4 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/26] 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 af0d40356afb166ea872162b61405cf0564cc779 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/26] available modes fixes + unit tests
---
src/main/telemetry/mavlink.c | 96 +++-
src/test/unit/CMakeLists.txt | 10 +
src/test/unit/mavlink_unittest.cc | 888 ++++++++++++++++++++++++++++++
3 files changed, 985 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 2cb53b64ecb..f377b24a80e 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -43,6 +43,12 @@ set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TE
set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c")
set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER)
+set_property(SOURCE mavlink_unittest.cc PROPERTY depends
+ "telemetry/mavlink.c" "common/maths.c")
+set_property(SOURCE mavlink_unittest.cc PROPERTY definitions USE_TELEMETRY USE_TELEMETRY_MAVLINK)
+set_property(SOURCE mavlink_unittest.cc PROPERTY includes
+ "${CMAKE_CURRENT_SOURCE_DIR}/../../../lib/main/MAVLink")
+
function(unit_test src)
get_filename_component(basename ${src} NAME)
string(REPLACE ".cc" "" name ${basename} )
@@ -51,6 +57,7 @@ function(unit_test src)
list(TRANSFORM headers REPLACE "\.c$" ".h")
list(APPEND deps ${headers})
get_property(defs SOURCE ${src} PROPERTY definitions)
+ get_property(extra_includes SOURCE ${src} PROPERTY includes)
set(test_definitions "UNIT_TEST")
if (defs)
list(APPEND test_definitions ${defs})
@@ -60,6 +67,9 @@ function(unit_test src)
set(gen_name ${name}_gen)
get_generated_files_dir(gen ${gen_name})
target_include_directories(${name} PRIVATE . ${MAIN_DIR} ${gen})
+ if (extra_includes)
+ target_include_directories(${name} PRIVATE ${extra_includes})
+ endif()
target_compile_definitions(${name} PRIVATE ${test_definitions})
target_compile_options(${name} PRIVATE -pthread -Wall -Wextra -Wno-extern-c-compat -ggdb3 -O0)
enable_settings(${name} ${gen_name} OUTPUTS setting_files SETTINGS_CXX g++)
diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc
new file mode 100644
index 00000000000..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 3d0082ad56c5e9eee7bbfcb1415e94dccd9de2f7 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/26] 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 029b1434f99bfb42cab5a62136a6520389b385a8 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/26] 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 ed1fc090719d90f2d3714d8d67ce526d9474ec6b 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/26] 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 202d7929c74e761a3e8cb6a40b2db5fe30887724 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/26] 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 5c5ab9c21181d600b7306c64e77e5abb36fa3f69 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/26] 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 8f46cd55433ae1170dff7d3fe94e0261fe3b0c57 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/26] 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 2aa31036ddd7a661a6eb04821181e7681676c703 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/26] 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 9708629f4c425a02904a44fce16acc484348bcfd 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/26] 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 eca94ff1e9b5f5b3db18507f66edc819448021b7 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/26] 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 e8223a5c04a7e732bfbd4ddd0c8cf46c337c97da 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/26] 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 6cad3da77d5667110acc7b790bf97d59cbd0cd28 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/26] 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 0d8f66ec025cc98359055abb2f09ebadd3257c1e 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/26] 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 6bcbce7c272c9699639873bd812fde7997475113 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/26] mode fixgit status!
---
docs/development/msp/inav_enums.json | 122 ++++++++++++-----------
docs/development/msp/inav_enums_ref.md | 130 ++++++++++++++-----------
src/main/telemetry/mavlink.c | 76 ++++++++-------
3 files changed, 181 insertions(+), 147 deletions(-)
diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json
index ffe24b3f24c..ca057033b4c 100644
--- a/docs/development/msp/inav_enums.json
+++ b/docs/development/msp/inav_enums.json
@@ -12,7 +12,8 @@
"ACC_ICM42605": "8",
"ACC_BMI270": "9",
"ACC_LSM6DXX": "10",
- "ACC_FAKE": "11",
+ "ACC_ICM45686": "11",
+ "ACC_FAKE": "12",
"ACC_MAX": "ACC_FAKE"
},
"accEvent_t": {
@@ -801,53 +802,54 @@
"DEVHW_ICM42605": "7",
"DEVHW_BMI270": "8",
"DEVHW_LSM6D": "9",
- "DEVHW_MPU9250": "10",
- "DEVHW_BMP085": "11",
- "DEVHW_BMP280": "12",
- "DEVHW_MS5611": "13",
- "DEVHW_MS5607": "14",
- "DEVHW_LPS25H": "15",
- "DEVHW_SPL06": "16",
- "DEVHW_BMP388": "17",
- "DEVHW_DPS310": "18",
- "DEVHW_B2SMPB": "19",
- "DEVHW_HMC5883": "20",
- "DEVHW_AK8963": "21",
- "DEVHW_AK8975": "22",
- "DEVHW_IST8310_0": "23",
- "DEVHW_IST8310_1": "24",
- "DEVHW_IST8308": "25",
- "DEVHW_QMC5883": "26",
- "DEVHW_QMC5883P": "27",
- "DEVHW_MAG3110": "28",
- "DEVHW_LIS3MDL": "29",
- "DEVHW_RM3100": "30",
- "DEVHW_VCM5883": "31",
- "DEVHW_MLX90393": "32",
- "DEVHW_LM75_0": "33",
- "DEVHW_LM75_1": "34",
- "DEVHW_LM75_2": "35",
- "DEVHW_LM75_3": "36",
- "DEVHW_LM75_4": "37",
- "DEVHW_LM75_5": "38",
- "DEVHW_LM75_6": "39",
- "DEVHW_LM75_7": "40",
- "DEVHW_DS2482": "41",
- "DEVHW_MAX7456": "42",
- "DEVHW_SRF10": "43",
- "DEVHW_VL53L0X": "44",
- "DEVHW_VL53L1X": "45",
- "DEVHW_US42": "46",
- "DEVHW_TOF10120_I2C": "47",
- "DEVHW_TERARANGER_EVO_I2C": "48",
- "DEVHW_MS4525": "49",
- "DEVHW_DLVR": "50",
- "DEVHW_M25P16": "51",
- "DEVHW_W25N": "52",
- "DEVHW_UG2864": "53",
- "DEVHW_SDCARD": "54",
- "DEVHW_IRLOCK": "55",
- "DEVHW_PCF8574": "56"
+ "DEVHW_ICM45686": "10",
+ "DEVHW_MPU9250": "11",
+ "DEVHW_BMP085": "12",
+ "DEVHW_BMP280": "13",
+ "DEVHW_MS5611": "14",
+ "DEVHW_MS5607": "15",
+ "DEVHW_LPS25H": "16",
+ "DEVHW_SPL06": "17",
+ "DEVHW_BMP388": "18",
+ "DEVHW_DPS310": "19",
+ "DEVHW_B2SMPB": "20",
+ "DEVHW_HMC5883": "21",
+ "DEVHW_AK8963": "22",
+ "DEVHW_AK8975": "23",
+ "DEVHW_IST8310_0": "24",
+ "DEVHW_IST8310_1": "25",
+ "DEVHW_IST8308": "26",
+ "DEVHW_QMC5883": "27",
+ "DEVHW_QMC5883P": "28",
+ "DEVHW_MAG3110": "29",
+ "DEVHW_LIS3MDL": "30",
+ "DEVHW_RM3100": "31",
+ "DEVHW_VCM5883": "32",
+ "DEVHW_MLX90393": "33",
+ "DEVHW_LM75_0": "34",
+ "DEVHW_LM75_1": "35",
+ "DEVHW_LM75_2": "36",
+ "DEVHW_LM75_3": "37",
+ "DEVHW_LM75_4": "38",
+ "DEVHW_LM75_5": "39",
+ "DEVHW_LM75_6": "40",
+ "DEVHW_LM75_7": "41",
+ "DEVHW_DS2482": "42",
+ "DEVHW_MAX7456": "43",
+ "DEVHW_SRF10": "44",
+ "DEVHW_VL53L0X": "45",
+ "DEVHW_VL53L1X": "46",
+ "DEVHW_US42": "47",
+ "DEVHW_TOF10120_I2C": "48",
+ "DEVHW_TERARANGER_EVO_I2C": "49",
+ "DEVHW_MS4525": "50",
+ "DEVHW_DLVR": "51",
+ "DEVHW_M25P16": "52",
+ "DEVHW_W25N": "53",
+ "DEVHW_UG2864": "54",
+ "DEVHW_SDCARD": "55",
+ "DEVHW_IRLOCK": "56",
+ "DEVHW_PCF8574": "57"
},
"deviceFlags_e": {
"_source": "inav/src/main/drivers/bus.h",
@@ -1528,7 +1530,8 @@
"GYRO_ICM42605": "8",
"GYRO_BMI270": "9",
"GYRO_LSM6DXX": "10",
- "GYRO_FAKE": "11"
+ "GYRO_ICM45686": "11",
+ "GYRO_FAKE": "12"
},
"HardwareMotorTypes_e": {
"_source": "inav/src/main/drivers/pwm_esc_detect.h",
@@ -1862,7 +1865,8 @@
"LED_MODE_ANGLE": "3",
"LED_MODE_MAG": "4",
"LED_MODE_BARO": "5",
- "LED_SPECIAL": "6"
+ "LED_MODE_LOITER": "6",
+ "LED_SPECIAL": "7"
},
"ledOverlayId_e": {
"_source": "inav/src/main/io/ledstrip.h",
@@ -2238,6 +2242,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",
@@ -2327,8 +2339,7 @@
"MULTI_FUNC_3": "3",
"MULTI_FUNC_4": "4",
"MULTI_FUNC_5": "5",
- "MULTI_FUNC_6": "6",
- "MULTI_FUNC_END": "7"
+ "MULTI_FUNC_END": "6"
},
"multiFunctionFlags_e": {
"_source": "inav/src/main/fc/multifunction.h",
@@ -3293,9 +3304,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",
@@ -3831,7 +3841,7 @@
"THR_HI": "(2 << (2 * THROTTLE))"
},
"systemState_e": {
- "_source": "inav/src/main/fc/fc_init.c",
+ "_source": "inav/src/main/fc/fc_init.h",
"SYSTEM_STATE_INITIALISING": "0",
"SYSTEM_STATE_CONFIG_LOADED": "(1 << 0)",
"SYSTEM_STATE_SENSORS_READY": "(1 << 1)",
diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md
index af53d1c6b9a..ca7eccab335 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)
@@ -370,7 +371,8 @@
| `ACC_ICM42605` | 8 | |
| `ACC_BMI270` | 9 | |
| `ACC_LSM6DXX` | 10 | |
-| `ACC_FAKE` | 11 | |
+| `ACC_ICM45686` | 11 | |
+| `ACC_FAKE` | 12 | |
| `ACC_MAX` | ACC_FAKE | |
---
@@ -1407,53 +1409,54 @@
| `DEVHW_ICM42605` | 7 | |
| `DEVHW_BMI270` | 8 | |
| `DEVHW_LSM6D` | 9 | |
-| `DEVHW_MPU9250` | 10 | |
-| `DEVHW_BMP085` | 11 | |
-| `DEVHW_BMP280` | 12 | |
-| `DEVHW_MS5611` | 13 | |
-| `DEVHW_MS5607` | 14 | |
-| `DEVHW_LPS25H` | 15 | |
-| `DEVHW_SPL06` | 16 | |
-| `DEVHW_BMP388` | 17 | |
-| `DEVHW_DPS310` | 18 | |
-| `DEVHW_B2SMPB` | 19 | |
-| `DEVHW_HMC5883` | 20 | |
-| `DEVHW_AK8963` | 21 | |
-| `DEVHW_AK8975` | 22 | |
-| `DEVHW_IST8310_0` | 23 | |
-| `DEVHW_IST8310_1` | 24 | |
-| `DEVHW_IST8308` | 25 | |
-| `DEVHW_QMC5883` | 26 | |
-| `DEVHW_QMC5883P` | 27 | |
-| `DEVHW_MAG3110` | 28 | |
-| `DEVHW_LIS3MDL` | 29 | |
-| `DEVHW_RM3100` | 30 | |
-| `DEVHW_VCM5883` | 31 | |
-| `DEVHW_MLX90393` | 32 | |
-| `DEVHW_LM75_0` | 33 | |
-| `DEVHW_LM75_1` | 34 | |
-| `DEVHW_LM75_2` | 35 | |
-| `DEVHW_LM75_3` | 36 | |
-| `DEVHW_LM75_4` | 37 | |
-| `DEVHW_LM75_5` | 38 | |
-| `DEVHW_LM75_6` | 39 | |
-| `DEVHW_LM75_7` | 40 | |
-| `DEVHW_DS2482` | 41 | |
-| `DEVHW_MAX7456` | 42 | |
-| `DEVHW_SRF10` | 43 | |
-| `DEVHW_VL53L0X` | 44 | |
-| `DEVHW_VL53L1X` | 45 | |
-| `DEVHW_US42` | 46 | |
-| `DEVHW_TOF10120_I2C` | 47 | |
-| `DEVHW_TERARANGER_EVO_I2C` | 48 | |
-| `DEVHW_MS4525` | 49 | |
-| `DEVHW_DLVR` | 50 | |
-| `DEVHW_M25P16` | 51 | |
-| `DEVHW_W25N` | 52 | |
-| `DEVHW_UG2864` | 53 | |
-| `DEVHW_SDCARD` | 54 | |
-| `DEVHW_IRLOCK` | 55 | |
-| `DEVHW_PCF8574` | 56 | |
+| `DEVHW_ICM45686` | 10 | |
+| `DEVHW_MPU9250` | 11 | |
+| `DEVHW_BMP085` | 12 | |
+| `DEVHW_BMP280` | 13 | |
+| `DEVHW_MS5611` | 14 | |
+| `DEVHW_MS5607` | 15 | |
+| `DEVHW_LPS25H` | 16 | |
+| `DEVHW_SPL06` | 17 | |
+| `DEVHW_BMP388` | 18 | |
+| `DEVHW_DPS310` | 19 | |
+| `DEVHW_B2SMPB` | 20 | |
+| `DEVHW_HMC5883` | 21 | |
+| `DEVHW_AK8963` | 22 | |
+| `DEVHW_AK8975` | 23 | |
+| `DEVHW_IST8310_0` | 24 | |
+| `DEVHW_IST8310_1` | 25 | |
+| `DEVHW_IST8308` | 26 | |
+| `DEVHW_QMC5883` | 27 | |
+| `DEVHW_QMC5883P` | 28 | |
+| `DEVHW_MAG3110` | 29 | |
+| `DEVHW_LIS3MDL` | 30 | |
+| `DEVHW_RM3100` | 31 | |
+| `DEVHW_VCM5883` | 32 | |
+| `DEVHW_MLX90393` | 33 | |
+| `DEVHW_LM75_0` | 34 | |
+| `DEVHW_LM75_1` | 35 | |
+| `DEVHW_LM75_2` | 36 | |
+| `DEVHW_LM75_3` | 37 | |
+| `DEVHW_LM75_4` | 38 | |
+| `DEVHW_LM75_5` | 39 | |
+| `DEVHW_LM75_6` | 40 | |
+| `DEVHW_LM75_7` | 41 | |
+| `DEVHW_DS2482` | 42 | |
+| `DEVHW_MAX7456` | 43 | |
+| `DEVHW_SRF10` | 44 | |
+| `DEVHW_VL53L0X` | 45 | |
+| `DEVHW_VL53L1X` | 46 | |
+| `DEVHW_US42` | 47 | |
+| `DEVHW_TOF10120_I2C` | 48 | |
+| `DEVHW_TERARANGER_EVO_I2C` | 49 | |
+| `DEVHW_MS4525` | 50 | |
+| `DEVHW_DLVR` | 51 | |
+| `DEVHW_M25P16` | 52 | |
+| `DEVHW_W25N` | 53 | |
+| `DEVHW_UG2864` | 54 | |
+| `DEVHW_SDCARD` | 55 | |
+| `DEVHW_IRLOCK` | 56 | |
+| `DEVHW_PCF8574` | 57 | |
---
## `deviceFlags_e`
@@ -2489,7 +2492,8 @@
| `GYRO_ICM42605` | 8 | |
| `GYRO_BMI270` | 9 | |
| `GYRO_LSM6DXX` | 10 | |
-| `GYRO_FAKE` | 11 | |
+| `GYRO_ICM45686` | 11 | |
+| `GYRO_FAKE` | 12 | |
---
## `HardwareMotorTypes_e`
@@ -2925,7 +2929,8 @@
| `LED_MODE_ANGLE` | 3 | |
| `LED_MODE_MAG` | 4 | |
| `LED_MODE_BARO` | 5 | |
-| `LED_SPECIAL` | 6 | |
+| `LED_MODE_LOITER` | 6 | |
+| `LED_SPECIAL` | 7 | |
---
## `ledOverlayId_e`
@@ -3374,6 +3379,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`
@@ -3533,8 +3551,7 @@
| `MULTI_FUNC_3` | 3 | |
| `MULTI_FUNC_4` | 4 | |
| `MULTI_FUNC_5` | 5 | |
-| `MULTI_FUNC_6` | 6 | |
-| `MULTI_FUNC_END` | 7 | |
+| `MULTI_FUNC_END` | 6 | |
---
## `multiFunctionFlags_e`
@@ -4821,9 +4838,8 @@
|---|---:|---|
| `SD_3016` | 0 | |
| `HD_5018` | 1 | |
-| `HD_3016` | 2 | |
-| `HD_6022` | 3 | |
-| `HD_5320` | 4 | |
+| `HD_6022` | 2 | |
+| `HD_5320` | 3 | |
---
## `resourceOwner_e`
@@ -5629,7 +5645,7 @@
---
## `systemState_e`
-> Source: ../../../src/main/fc/fc_init.h
+> Source: ../../../src/main/fc/fc_init.c
| Enumerator | Value | Condition |
|---|---:|---|
@@ -5643,7 +5659,7 @@
---
## `systemState_e`
-> Source: ../../../src/main/fc/fc_init.c
+> Source: ../../../src/main/fc/fc_init.h
| Enumerator | Value | Condition |
|---|---:|---|
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 8fb71b626cb4f0a5c9191e9a89aeefd86bf4efeb 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/26] 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 aa03cbc936450ec364bd1226029351c5b51ce245 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Tue, 3 Mar 2026 16:43:43 -0500
Subject: [PATCH 21/26] docgen
---
dm.txt | 0
docs/development/msp/README.md | 2 +-
docs/development/msp/msp_messages.checksum | 2 +-
docs/development/msp/rev | 2 +-
4 files changed, 3 insertions(+), 3 deletions(-)
delete mode 100644 dm.txt
diff --git a/dm.txt b/dm.txt
deleted file mode 100644
index e69de29bb2d..00000000000
diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md
index e3fe2dc5b4d..b0f871fa65a 100644
--- a/docs/development/msp/README.md
+++ b/docs/development/msp/README.md
@@ -9,7 +9,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
-**JSON file rev: 4**
+**JSON file rev: 0**
**Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty**
diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum
index ae689c4b8eb..14e143e04b9 100644
--- a/docs/development/msp/msp_messages.checksum
+++ b/docs/development/msp/msp_messages.checksum
@@ -1 +1 @@
-6e8861395a4275489a12012e6985d733
+7146802c1d6c89bce6aeda0d5e95c137
diff --git a/docs/development/msp/rev b/docs/development/msp/rev
index bf0d87ab1b2..c227083464f 100644
--- a/docs/development/msp/rev
+++ b/docs/development/msp/rev
@@ -1 +1 @@
-4
\ No newline at end of file
+0
\ No newline at end of file
From 493efd5a49d2227572b9574bd7898256c37932ed 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 22/26] 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 25a7196f03069c79e944187a7a3538f8c643d9bf 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 23/26] 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 a3752233149..1b6d2fc5f99 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1328,9 +1328,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) {
resetAltitudeController(navTerrainFollowingRequested());
setupAltitudeController();
- setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
}
+ // POSHOLD is a 3D hold mode: always capture current altitude setpoint when entering.
+ setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
+
// Prepare position controller if idle or current Mode NOT active in position hold state
if (previousState != NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_LOITER_ABOVE_HOME &&
previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND &&
@@ -3896,7 +3898,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
// Only valid when armed and in poshold mode
else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) && isGCSValid()) {
// Convert to local coordinates
- geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
+ geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, waypointMissionAltConvMode(wpData->p3));
navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 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 5b103ef4adb6781115992f6197c02c03446d5af3 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 24/26] 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 53b59f5d490..5db845fdce1 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -4225,7 +4225,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 d0e36f5219899328893160b5060cd7a10b245d12 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 25/26] 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 1c905dd539cc5ee7f1e4b9adb27d8aade65a37f5 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 26/26] 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;