From 1fb5cb53e9f6601171f08caa5782ea3b59a82d46 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/42] mavlink compatibility fixes --- src/main/telemetry/mavlink.c | 613 +++++++++++++++++++++++++++++++---- 1 file changed, 549 insertions(+), 64 deletions(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index d5084b32a62..50d59ec5909 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,77 +1398,321 @@ static bool handleIncoming_MISSION_REQUEST(void) return false; } +static bool mavlinkMessageToStream(uint16_t messageId, uint8_t *streamNum) +{ + 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; + } +} -static bool handleIncoming_COMMAND_INT(void) + +static void mavlinkSendCommandAck(uint16_t command, MAV_RESULT result, uint8_t ackTargetSystem, uint8_t ackTargetComponent) { - mavlink_command_int_t msg; - mavlink_msg_command_int_decode(&mavRecvMsg, &msg); + mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + command, + result, + 0, + 0, + ackTargetSystem, + ackTargetComponent); + mavlinkSendMessage(); +} - if (msg.target_system == mavSystemId) { +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 (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; - } + 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); + } + return true; + case MAV_CMD_SET_MESSAGE_INTERVAL: + { + uint8_t stream; + MAV_RESULT result = MAV_RESULT_UNSUPPORTED; + + if (mavlinkMessageToStream((uint16_t)param1, &stream)) { + if (param2 == 0.0f) { + mavlinkSetStreamRate(stream, mavRatesConfigured[stream]); + result = MAV_RESULT_ACCEPTED; + } else if (param2 < 0.0f) { + mavlinkSetStreamRate(stream, 0); + result = MAV_RESULT_ACCEPTED; + } else { + uint32_t intervalUs = (uint32_t)param2; + if (intervalUs > 0) { + uint32_t rate = 1000000UL / intervalUs; + if (rate > 0) { + if (rate > TELEMETRY_MAVLINK_MAXRATE) { + rate = TELEMETRY_MAVLINK_MAXRATE; + } + mavlinkSetStreamRate(stream, rate); + result = MAV_RESULT_ACCEPTED; + } + } + } + } + + mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); + return true; } - } else { - mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - msg.command, - MAV_RESULT_UNSUPPORTED, - 0, - 0, - mavRecvMsg.sysid, - mavRecvMsg.compid); + 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; } @@ -1352,10 +1837,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: @@ -1364,6 +1847,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 b89b8e1376175d91dd7bcab3c872f211acd50c7e 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/42] 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 50d59ec5909..fddcff582c7 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) @@ -1541,12 +1552,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: { @@ -1559,12 +1578,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(); @@ -1682,6 +1705,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; @@ -1834,6 +1933,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(); @@ -1843,6 +1944,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 7201a83abf086a8b674bba866ade6cfbcb8f96d4 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/42] 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 fddcff582c7..402d81d4718 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); @@ -1574,7 +1831,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: @@ -1598,7 +1855,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: @@ -1720,7 +1977,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) @@ -1738,18 +1995,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 59b7ea887bf92da7b4a629c608860b990474e5dc 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/42] 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 402d81d4718..6d3c73f469d 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 8fe997640258391c00cde71fe32c2633938a9bc1 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/42] 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 6d3c73f469d..e5ec36baa05 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 a3526add0b837116b5f1e384ee1786b7da58beb4 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/42] 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 e5ec36baa05..ace1a4061e3 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(); } @@ -1864,10 +1941,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; } @@ -1959,7 +2036,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 19fd708bfa351e48568f6bc8ded88a1d3803310b 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/42] 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 ace1a4061e3..67f539f643f 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 11d378a83146ec4e549ac9f0dc31207fbb794089 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/42] 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 67f539f643f..659cbad0953 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); @@ -2147,7 +2147,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 3594e2441542636bb58200170ea531a8c9450cd5 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/42] 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 659cbad0953..85b9083ff0a 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 ab0c7589a4ec738b054367b9f3622288f9bbd6e9 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/42] 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 85b9083ff0a..7564f687e87 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 3d4011c79eeb29e2ef361e8b56f81955494516bc 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/42] 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 e50a8780151e877189d829e4e8b881d612cb39c4 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/42] 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 7564f687e87..72f5cae72b8 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, @@ -2147,7 +2147,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 eceab90881ad6c8aa34ab6729a3a098175772e41 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/42] 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 72f5cae72b8..908ba37e7d3 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 f9d27105dc6639fbd3633c9bf2fef8bd09f9c433 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/42] 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 908ba37e7d3..a1edd8a2ce3 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: @@ -2146,7 +2146,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 ecf96f043ffe9ae4eab3b6340ea95f19d90eb175 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/42] 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 a1edd8a2ce3..6b9df606ef9 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -2050,9 +2050,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 4e2ca11209fd8ade0f44311e1d5820af31a750d8 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/42] 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 6b9df606ef9..85964597ea1 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 2b2bfb9bfa8013b37fdd8e834d7e7587c385c2bd 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/42] 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 85964597ea1..e23efca244c 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 6bc963bb8f6396bede1f5e7567b6b5a979add696 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/42] 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 e23efca244c..fe0aebe26b7 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 e2656b8e2ecb6cea52f44949fe3f09455be50b17 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/42] mode fixgit status! --- docs/development/msp/inav_enums.json | 3822 ++++++++++++++++++++++++ docs/development/msp/inav_enums_ref.md | 14 + src/main/telemetry/mavlink.c | 76 +- 3 files changed, 3878 insertions(+), 34 deletions(-) create mode 100644 docs/development/msp/inav_enums.json diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json new file mode 100644 index 00000000000..6262cc2ed47 --- /dev/null +++ b/docs/development/msp/inav_enums.json @@ -0,0 +1,3822 @@ +{ + "accelerationSensor_e": { + "_source": "../../../src/main/sensors/acceleration.h", + "ACC_NONE": "0", + "ACC_AUTODETECT": "1", + "ACC_MPU6000": "2", + "ACC_MPU6500": "3", + "ACC_MPU9250": "4", + "ACC_BMI160": "5", + "ACC_ICM20689": "6", + "ACC_BMI088": "7", + "ACC_ICM42605": "8", + "ACC_BMI270": "9", + "ACC_LSM6DXX": "10", + "ACC_FAKE": "11", + "ACC_MAX": "ACC_FAKE" + }, + "accEvent_t": { + "_source": "../../../src/main/telemetry/sim.c", + "ACC_EVENT_NONE": "0", + "ACC_EVENT_HIGH": "1", + "ACC_EVENT_LOW": "2", + "ACC_EVENT_NEG_X": "3" + }, + "adcChannel_e": { + "_source": "../../../src/main/drivers/adc.h", + "ADC_CHN_NONE": "0", + "ADC_CHN_1": "1", + "ADC_CHN_2": "2", + "ADC_CHN_3": "3", + "ADC_CHN_4": "4", + "ADC_CHN_5": "5", + "ADC_CHN_6": "6", + "ADC_CHN_MAX": "ADC_CHN_6", + "ADC_CHN_COUNT": "" + }, + "adcFunction_e": { + "_source": "../../../src/main/drivers/adc.h", + "ADC_BATTERY": "0", + "ADC_RSSI": "1", + "ADC_CURRENT": "2", + "ADC_AIRSPEED": "3", + "ADC_FUNCTION_COUNT": "4" + }, + "adjustmentFunction_e": { + "_source": "../../../src/main/fc/rc_adjustments.h", + "ADJUSTMENT_NONE": "0", + "ADJUSTMENT_RC_RATE": "1", + "ADJUSTMENT_RC_EXPO": "2", + "ADJUSTMENT_THROTTLE_EXPO": "3", + "ADJUSTMENT_PITCH_ROLL_RATE": "4", + "ADJUSTMENT_YAW_RATE": "5", + "ADJUSTMENT_PITCH_ROLL_P": "6", + "ADJUSTMENT_PITCH_ROLL_I": "7", + "ADJUSTMENT_PITCH_ROLL_D": "8", + "ADJUSTMENT_PITCH_ROLL_FF": "9", + "ADJUSTMENT_PITCH_P": "10", + "ADJUSTMENT_PITCH_I": "11", + "ADJUSTMENT_PITCH_D": "12", + "ADJUSTMENT_PITCH_FF": "13", + "ADJUSTMENT_ROLL_P": "14", + "ADJUSTMENT_ROLL_I": "15", + "ADJUSTMENT_ROLL_D": "16", + "ADJUSTMENT_ROLL_FF": "17", + "ADJUSTMENT_YAW_P": "18", + "ADJUSTMENT_YAW_I": "19", + "ADJUSTMENT_YAW_D": "20", + "ADJUSTMENT_YAW_FF": "21", + "ADJUSTMENT_RATE_PROFILE": "22", + "ADJUSTMENT_PITCH_RATE": "23", + "ADJUSTMENT_ROLL_RATE": "24", + "ADJUSTMENT_RC_YAW_EXPO": "25", + "ADJUSTMENT_MANUAL_RC_EXPO": "26", + "ADJUSTMENT_MANUAL_RC_YAW_EXPO": "27", + "ADJUSTMENT_MANUAL_PITCH_ROLL_RATE": "28", + "ADJUSTMENT_MANUAL_ROLL_RATE": "29", + "ADJUSTMENT_MANUAL_PITCH_RATE": "30", + "ADJUSTMENT_MANUAL_YAW_RATE": "31", + "ADJUSTMENT_NAV_FW_CRUISE_THR": "32", + "ADJUSTMENT_NAV_FW_PITCH2THR": "33", + "ADJUSTMENT_ROLL_BOARD_ALIGNMENT": "34", + "ADJUSTMENT_PITCH_BOARD_ALIGNMENT": "35", + "ADJUSTMENT_LEVEL_P": "36", + "ADJUSTMENT_LEVEL_I": "37", + "ADJUSTMENT_LEVEL_D": "38", + "ADJUSTMENT_POS_XY_P": "39", + "ADJUSTMENT_POS_XY_I": "40", + "ADJUSTMENT_POS_XY_D": "41", + "ADJUSTMENT_POS_Z_P": "42", + "ADJUSTMENT_POS_Z_I": "43", + "ADJUSTMENT_POS_Z_D": "44", + "ADJUSTMENT_HEADING_P": "45", + "ADJUSTMENT_VEL_XY_P": "46", + "ADJUSTMENT_VEL_XY_I": "47", + "ADJUSTMENT_VEL_XY_D": "48", + "ADJUSTMENT_VEL_Z_P": "49", + "ADJUSTMENT_VEL_Z_I": "50", + "ADJUSTMENT_VEL_Z_D": "51", + "ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE": "52", + "ADJUSTMENT_VTX_POWER_LEVEL": "53", + "ADJUSTMENT_TPA": "54", + "ADJUSTMENT_TPA_BREAKPOINT": "55", + "ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS": "56", + "ADJUSTMENT_FW_TPA_TIME_CONSTANT": "57", + "ADJUSTMENT_FW_LEVEL_TRIM": "58", + "ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX": "59", + "ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE": "60", + "ADJUSTMENT_FUNCTION_COUNT": "61" + }, + "adjustmentMode_e": { + "_source": "../../../src/main/fc/rc_adjustments.h", + "ADJUSTMENT_MODE_STEP": "0", + "ADJUSTMENT_MODE_SELECT": "1" + }, + "afatfsAppendFreeClusterPhase_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_APPEND_FREE_CLUSTER_PHASE_INITIAL": "0", + "AFATFS_APPEND_FREE_CLUSTER_PHASE_FIND_FREESPACE": "0", + "AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT1": "1", + "AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT2": "2", + "AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FILE_DIRECTORY": "3", + "AFATFS_APPEND_FREE_CLUSTER_PHASE_COMPLETE": "4", + "AFATFS_APPEND_FREE_CLUSTER_PHASE_FAILURE": "5" + }, + "afatfsAppendSuperclusterPhase_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_APPEND_SUPERCLUSTER_PHASE_INIT": "0", + "AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FREEFILE_DIRECTORY": "1", + "AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FAT": "2", + "AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FILE_DIRECTORY": "3" + }, + "afatfsCacheBlockState_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_CACHE_STATE_EMPTY": "0", + "AFATFS_CACHE_STATE_IN_SYNC": "1", + "AFATFS_CACHE_STATE_READING": "2", + "AFATFS_CACHE_STATE_WRITING": "3", + "AFATFS_CACHE_STATE_DIRTY": "4" + }, + "afatfsClusterSearchCondition_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "CLUSTER_SEARCH_FREE_AT_BEGINNING_OF_FAT_SECTOR": "0", + "CLUSTER_SEARCH_FREE": "1", + "CLUSTER_SEARCH_OCCUPIED": "2" + }, + "afatfsDeleteFilePhase_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_DELETE_FILE_DELETE_DIRECTORY_ENTRY": "0", + "AFATFS_DELETE_FILE_DEALLOCATE_CLUSTERS": "1" + }, + "afatfsError_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.h", + "AFATFS_ERROR_NONE": "0", + "AFATFS_ERROR_GENERIC": "1", + "AFATFS_ERROR_BAD_MBR": "2", + "AFATFS_ERROR_BAD_FILESYSTEM_HEADER": "3" + }, + "afatfsExtendSubdirectoryPhase_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_EXTEND_SUBDIRECTORY_PHASE_INITIAL": "0", + "AFATFS_EXTEND_SUBDIRECTORY_PHASE_ADD_FREE_CLUSTER": "0", + "AFATFS_EXTEND_SUBDIRECTORY_PHASE_WRITE_SECTORS": "1", + "AFATFS_EXTEND_SUBDIRECTORY_PHASE_SUCCESS": "2", + "AFATFS_EXTEND_SUBDIRECTORY_PHASE_FAILURE": "3" + }, + "afatfsFATPattern_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_FAT_PATTERN_UNTERMINATED_CHAIN": "0", + "AFATFS_FAT_PATTERN_TERMINATED_CHAIN": "1", + "AFATFS_FAT_PATTERN_FREE": "2" + }, + "afatfsFileOperation_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_FILE_OPERATION_NONE": "0", + "AFATFS_FILE_OPERATION_CREATE_FILE": "1", + "AFATFS_FILE_OPERATION_SEEK": "2", + "AFATFS_FILE_OPERATION_CLOSE": "3", + "AFATFS_FILE_OPERATION_TRUNCATE": "4", + "AFATFS_FILE_OPERATION_UNLINK": "5", + "AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER": [ + "(6)", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_FILE_OPERATION_LOCKED": [ + "(7)", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER": "8", + "AFATFS_FILE_OPERATION_EXTEND_SUBDIRECTORY": "9" + }, + "afatfsFilesystemState_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.h", + "AFATFS_FILESYSTEM_STATE_UNKNOWN": "0", + "AFATFS_FILESYSTEM_STATE_FATAL": "1", + "AFATFS_FILESYSTEM_STATE_INITIALIZATION": "2", + "AFATFS_FILESYSTEM_STATE_READY": "3" + }, + "afatfsFileType_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_FILE_TYPE_NONE": "0", + "AFATFS_FILE_TYPE_NORMAL": "1", + "AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY": "2", + "AFATFS_FILE_TYPE_DIRECTORY": "3" + }, + "afatfsFindClusterStatus_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_FIND_CLUSTER_IN_PROGRESS": "0", + "AFATFS_FIND_CLUSTER_FOUND": "1", + "AFATFS_FIND_CLUSTER_FATAL": "2", + "AFATFS_FIND_CLUSTER_NOT_FOUND": "3" + }, + "afatfsFreeSpaceSearchPhase_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_FREE_SPACE_SEARCH_PHASE_FIND_HOLE": "0", + "AFATFS_FREE_SPACE_SEARCH_PHASE_GROW_HOLE": "1" + }, + "afatfsInitializationPhase_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_INITIALIZATION_READ_MBR": "0", + "AFATFS_INITIALIZATION_READ_VOLUME_ID": "1", + "AFATFS_INITIALIZATION_FREEFILE_CREATE": [ + "(2)", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_INITIALIZATION_FREEFILE_CREATING": [ + "(3)", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_INITIALIZATION_FREEFILE_FAT_SEARCH": [ + "(4)", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_INITIALIZATION_FREEFILE_UPDATE_FAT": [ + "(5)", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY": [ + "(6)", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_INITIALIZATION_FREEFILE_LAST": [ + "AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_INITIALIZATION_DONE": "" + }, + "afatfsOperationStatus_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.h", + "AFATFS_OPERATION_IN_PROGRESS": "0", + "AFATFS_OPERATION_SUCCESS": "1", + "AFATFS_OPERATION_FAILURE": "2" + }, + "afatfsSaveDirectoryEntryMode_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_SAVE_DIRECTORY_NORMAL": "0", + "AFATFS_SAVE_DIRECTORY_FOR_CLOSE": "1", + "AFATFS_SAVE_DIRECTORY_DELETED": "2" + }, + "afatfsSeek_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.h", + "AFATFS_SEEK_SET": "0", + "AFATFS_SEEK_CUR": "1", + "AFATFS_SEEK_END": "2" + }, + "afatfsTruncateFilePhase_e": { + "_source": "../../../src/main/io/asyncfatfs/asyncfatfs.c", + "AFATFS_TRUNCATE_FILE_INITIAL": "0", + "AFATFS_TRUNCATE_FILE_UPDATE_DIRECTORY": "0", + "AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_NORMAL": "1", + "AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_CONTIGUOUS": [ + "(2)", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_TRUNCATE_FILE_PREPEND_TO_FREEFILE": [ + "(3)", + "AFATFS_USE_FREEFILE" + ], + "AFATFS_TRUNCATE_FILE_SUCCESS": "4" + }, + "airmodeHandlingType_e": { + "_source": "../../../src/main/fc/rc_controls.h", + "STICK_CENTER": "0", + "THROTTLE_THRESHOLD": "1", + "STICK_CENTER_ONCE": "2" + }, + "angle_index_t": { + "_source": "../../../src/main/common/axis.h", + "AI_ROLL": "0", + "AI_PITCH": "1" + }, + "armingFlag_e": { + "_source": "../../../src/main/fc/runtime_config.h", + "ARMED": "(1 << 2)", + "WAS_EVER_ARMED": "(1 << 3)", + "SIMULATOR_MODE_HITL": "(1 << 4)", + "SIMULATOR_MODE_SITL": "(1 << 5)", + "ARMING_DISABLED_GEOZONE": "(1 << 6)", + "ARMING_DISABLED_FAILSAFE_SYSTEM": "(1 << 7)", + "ARMING_DISABLED_NOT_LEVEL": "(1 << 8)", + "ARMING_DISABLED_SENSORS_CALIBRATING": "(1 << 9)", + "ARMING_DISABLED_SYSTEM_OVERLOADED": "(1 << 10)", + "ARMING_DISABLED_NAVIGATION_UNSAFE": "(1 << 11)", + "ARMING_DISABLED_COMPASS_NOT_CALIBRATED": "(1 << 12)", + "ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED": "(1 << 13)", + "ARMING_DISABLED_ARM_SWITCH": "(1 << 14)", + "ARMING_DISABLED_HARDWARE_FAILURE": "(1 << 15)", + "ARMING_DISABLED_BOXFAILSAFE": "(1 << 16)", + "ARMING_DISABLED_RC_LINK": "(1 << 18)", + "ARMING_DISABLED_THROTTLE": "(1 << 19)", + "ARMING_DISABLED_CLI": "(1 << 20)", + "ARMING_DISABLED_CMS_MENU": "(1 << 21)", + "ARMING_DISABLED_OSD_MENU": "(1 << 22)", + "ARMING_DISABLED_ROLLPITCH_NOT_CENTERED": "(1 << 23)", + "ARMING_DISABLED_SERVO_AUTOTRIM": "(1 << 24)", + "ARMING_DISABLED_OOM": "(1 << 25)", + "ARMING_DISABLED_INVALID_SETTING": "(1 << 26)", + "ARMING_DISABLED_PWM_OUTPUT_ERROR": "(1 << 27)", + "ARMING_DISABLED_NO_PREARM": "(1 << 28)", + "ARMING_DISABLED_DSHOT_BEEPER": "(1 << 29)", + "ARMING_DISABLED_LANDING_DETECTED": "(1 << 30)", + "ARMING_DISABLED_ALL_FLAGS": "(ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING | ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER | ARMING_DISABLED_LANDING_DETECTED)" + }, + "axis_e": { + "_source": "../../../src/main/common/axis.h", + "X": "0", + "Y": "1", + "Z": "2" + }, + "barometerState_e": { + "_source": "../../../src/main/sensors/barometer.c", + "BAROMETER_NEEDS_SAMPLES": "0", + "BAROMETER_NEEDS_CALCULATION": "1" + }, + "baroSensor_e": { + "_source": "../../../src/main/sensors/barometer.h", + "BARO_NONE": "0", + "BARO_AUTODETECT": "1", + "BARO_BMP085": "2", + "BARO_MS5611": "3", + "BARO_BMP280": "4", + "BARO_MS5607": "5", + "BARO_LPS25H": "6", + "BARO_SPL06": "7", + "BARO_BMP388": "8", + "BARO_DPS310": "9", + "BARO_B2SMPB": "10", + "BARO_MSP": "11", + "BARO_FAKE": "12", + "BARO_MAX": "BARO_FAKE" + }, + "batCapacityUnit_e": { + "_source": "../../../src/main/sensors/battery_config_structs.h", + "BAT_CAPACITY_UNIT_MAH": "0", + "BAT_CAPACITY_UNIT_MWH": "1" + }, + "batteryState_e": { + "_source": "../../../src/main/sensors/battery.h", + "BATTERY_OK": "0", + "BATTERY_WARNING": "1", + "BATTERY_CRITICAL": "2", + "BATTERY_NOT_PRESENT": "3" + }, + "batVoltageSource_e": { + "_source": "../../../src/main/sensors/battery_config_structs.h", + "BAT_VOLTAGE_RAW": "0", + "BAT_VOLTAGE_SAG_COMP": "1" + }, + "baudRate_e": { + "_source": "../../../src/main/io/serial.h", + "BAUD_AUTO": "0", + "BAUD_1200": "1", + "BAUD_2400": "2", + "BAUD_4800": "3", + "BAUD_9600": "4", + "BAUD_19200": "5", + "BAUD_38400": "6", + "BAUD_57600": "7", + "BAUD_115200": "8", + "BAUD_230400": "9", + "BAUD_250000": "10", + "BAUD_460800": "11", + "BAUD_921600": "12", + "BAUD_1000000": "13", + "BAUD_1500000": "14", + "BAUD_2000000": "15", + "BAUD_2470000": "16", + "BAUD_MIN": "BAUD_AUTO", + "BAUD_MAX": "BAUD_2470000" + }, + "beeperMode_e": { + "_source": "../../../src/main/io/beeper.h", + "BEEPER_SILENCE": "0", + "BEEPER_RUNTIME_CALIBRATION_DONE": "1", + "BEEPER_HARDWARE_FAILURE": "2", + "BEEPER_RX_LOST": "3", + "BEEPER_RX_LOST_LANDING": "4", + "BEEPER_DISARMING": "5", + "BEEPER_ARMING": "6", + "BEEPER_ARMING_GPS_FIX": "7", + "BEEPER_BAT_CRIT_LOW": "8", + "BEEPER_BAT_LOW": "9", + "BEEPER_GPS_STATUS": "10", + "BEEPER_RX_SET": "11", + "BEEPER_ACTION_SUCCESS": "12", + "BEEPER_ACTION_FAIL": "13", + "BEEPER_READY_BEEP": "14", + "BEEPER_MULTI_BEEPS": "15", + "BEEPER_DISARM_REPEAT": "16", + "BEEPER_ARMED": "17", + "BEEPER_SYSTEM_INIT": "18", + "BEEPER_USB": "19", + "BEEPER_LAUNCH_MODE_ENABLED": "20", + "BEEPER_LAUNCH_MODE_LOW_THROTTLE": "21", + "BEEPER_LAUNCH_MODE_IDLE_START": "22", + "BEEPER_CAM_CONNECTION_OPEN": "23", + "BEEPER_CAM_CONNECTION_CLOSE": "24", + "BEEPER_ALL": "25", + "BEEPER_PREFERENCE": "26" + }, + "biquadFilterType_e": { + "_source": "../../../src/main/common/filter.h", + "FILTER_LPF": "0", + "FILTER_NOTCH": "1" + }, + "blackboxBufferReserveStatus_e": { + "_source": "../../../src/main/blackbox/blackbox_io.h", + "BLACKBOX_RESERVE_SUCCESS": "0", + "BLACKBOX_RESERVE_TEMPORARY_FAILURE": "1", + "BLACKBOX_RESERVE_PERMANENT_FAILURE": "2" + }, + "blackboxFeatureMask_e": { + "_source": "../../../src/main/blackbox/blackbox.h", + "BLACKBOX_FEATURE_NAV_ACC": "1 << 0", + "BLACKBOX_FEATURE_NAV_POS": "1 << 1", + "BLACKBOX_FEATURE_NAV_PID": "1 << 2", + "BLACKBOX_FEATURE_MAG": "1 << 3", + "BLACKBOX_FEATURE_ACC": "1 << 4", + "BLACKBOX_FEATURE_ATTITUDE": "1 << 5", + "BLACKBOX_FEATURE_RC_DATA": "1 << 6", + "BLACKBOX_FEATURE_RC_COMMAND": "1 << 7", + "BLACKBOX_FEATURE_MOTORS": "1 << 8", + "BLACKBOX_FEATURE_GYRO_RAW": "1 << 9", + "BLACKBOX_FEATURE_GYRO_PEAKS_ROLL": "1 << 10", + "BLACKBOX_FEATURE_GYRO_PEAKS_PITCH": "1 << 11", + "BLACKBOX_FEATURE_GYRO_PEAKS_YAW": "1 << 12", + "BLACKBOX_FEATURE_SERVOS": "1 << 13" + }, + "bmi270Register_e": { + "_source": "../../../src/main/drivers/accgyro/accgyro_bmi270.c", + "BMI270_REG_CHIP_ID": "0", + "BMI270_REG_ERR_REG": "2", + "BMI270_REG_STATUS": "3", + "BMI270_REG_ACC_DATA_X_LSB": "12", + "BMI270_REG_GYR_DATA_X_LSB": "18", + "BMI270_REG_SENSORTIME_0": "24", + "BMI270_REG_SENSORTIME_1": "25", + "BMI270_REG_SENSORTIME_2": "26", + "BMI270_REG_EVENT": "27", + "BMI270_REG_INT_STATUS_0": "28", + "BMI270_REG_INT_STATUS_1": "29", + "BMI270_REG_INTERNAL_STATUS": "33", + "BMI270_REG_TEMPERATURE_LSB": "34", + "BMI270_REG_TEMPERATURE_MSB": "35", + "BMI270_REG_FIFO_LENGTH_LSB": "36", + "BMI270_REG_FIFO_LENGTH_MSB": "37", + "BMI270_REG_FIFO_DATA": "38", + "BMI270_REG_ACC_CONF": "64", + "BMI270_REG_ACC_RANGE": "65", + "BMI270_REG_GYRO_CONF": "66", + "BMI270_REG_GYRO_RANGE": "67", + "BMI270_REG_AUX_CONF": "68", + "BMI270_REG_FIFO_DOWNS": "69", + "BMI270_REG_FIFO_WTM_0": "70", + "BMI270_REG_FIFO_WTM_1": "71", + "BMI270_REG_FIFO_CONFIG_0": "72", + "BMI270_REG_FIFO_CONFIG_1": "73", + "BMI270_REG_SATURATION": "74", + "BMI270_REG_INT1_IO_CTRL": "83", + "BMI270_REG_INT2_IO_CTRL": "84", + "BMI270_REG_INT_LATCH": "85", + "BMI270_REG_INT1_MAP_FEAT": "86", + "BMI270_REG_INT2_MAP_FEAT": "87", + "BMI270_REG_INT_MAP_DATA": "88", + "BMI270_REG_INIT_CTRL": "89", + "BMI270_REG_INIT_DATA": "94", + "BMI270_REG_ACC_SELF_TEST": "109", + "BMI270_REG_GYR_SELF_TEST_AXES": "110", + "BMI270_REG_PWR_CONF": "124", + "BMI270_REG_PWR_CTRL": "125", + "BMI270_REG_CMD": "126" + }, + "bootLogEventCode_e": { + "_source": "../../../src/main/drivers/logging_codes.h", + "BOOT_EVENT_CONFIG_LOADED": "0", + "BOOT_EVENT_SYSTEM_INIT_DONE": "1", + "BOOT_EVENT_PWM_INIT_DONE": "2", + "BOOT_EVENT_EXTRA_BOOT_DELAY": "3", + "BOOT_EVENT_SENSOR_INIT_DONE": "4", + "BOOT_EVENT_GPS_INIT_DONE": "5", + "BOOT_EVENT_LEDSTRIP_INIT_DONE": "6", + "BOOT_EVENT_TELEMETRY_INIT_DONE": "7", + "BOOT_EVENT_SYSTEM_READY": "8", + "BOOT_EVENT_GYRO_DETECTION": "9", + "BOOT_EVENT_ACC_DETECTION": "10", + "BOOT_EVENT_BARO_DETECTION": "11", + "BOOT_EVENT_MAG_DETECTION": "12", + "BOOT_EVENT_RANGEFINDER_DETECTION": "13", + "BOOT_EVENT_MAG_INIT_FAILED": "14", + "BOOT_EVENT_HMC5883L_READ_OK_COUNT": "15", + "BOOT_EVENT_HMC5883L_READ_FAILED": "16", + "BOOT_EVENT_HMC5883L_SATURATION": "17", + "BOOT_EVENT_TIMER_CH_SKIPPED": "18", + "BOOT_EVENT_TIMER_CH_MAPPED": "19", + "BOOT_EVENT_PITOT_DETECTION": "20", + "BOOT_EVENT_TEMP_SENSOR_DETECTION": "21", + "BOOT_EVENT_1WIRE_DETECTION": "22", + "BOOT_EVENT_HARDWARE_IO_CONFLICT": "23", + "BOOT_EVENT_OPFLOW_DETECTION": "24", + "BOOT_EVENT_CODE_COUNT": "25" + }, + "bootLogFlags_e": { + "_source": "../../../src/main/drivers/logging_codes.h", + "BOOT_EVENT_FLAGS_NONE": "0", + "BOOT_EVENT_FLAGS_WARNING": "1 << 0", + "BOOT_EVENT_FLAGS_ERROR": "1 << 1", + "BOOT_EVENT_FLAGS_PARAM16": "1 << 14", + "BOOT_EVENT_FLAGS_PARAM32": "1 << 15" + }, + "boxId_e": { + "_source": "../../../src/main/fc/rc_modes.h", + "BOXARM": "0", + "BOXANGLE": "1", + "BOXHORIZON": "2", + "BOXNAVALTHOLD": "3", + "BOXHEADINGHOLD": "4", + "BOXHEADFREE": "5", + "BOXHEADADJ": "6", + "BOXCAMSTAB": "7", + "BOXNAVRTH": "8", + "BOXNAVPOSHOLD": "9", + "BOXMANUAL": "10", + "BOXBEEPERON": "11", + "BOXLEDLOW": "12", + "BOXLIGHTS": "13", + "BOXNAVLAUNCH": "14", + "BOXOSD": "15", + "BOXTELEMETRY": "16", + "BOXBLACKBOX": "17", + "BOXFAILSAFE": "18", + "BOXNAVWP": "19", + "BOXAIRMODE": "20", + "BOXHOMERESET": "21", + "BOXGCSNAV": "22", + "BOXSURFACE": "24", + "BOXFLAPERON": "25", + "BOXTURNASSIST": "26", + "BOXAUTOTRIM": "27", + "BOXAUTOTUNE": "28", + "BOXCAMERA1": "29", + "BOXCAMERA2": "30", + "BOXCAMERA3": "31", + "BOXOSDALT1": "32", + "BOXOSDALT2": "33", + "BOXOSDALT3": "34", + "BOXNAVCOURSEHOLD": "35", + "BOXBRAKING": "36", + "BOXUSER1": "37", + "BOXUSER2": "38", + "BOXFPVANGLEMIX": "39", + "BOXLOITERDIRCHN": "40", + "BOXMSPRCOVERRIDE": "41", + "BOXPREARM": "42", + "BOXTURTLE": "43", + "BOXNAVCRUISE": "44", + "BOXAUTOLEVEL": "45", + "BOXPLANWPMISSION": "46", + "BOXSOARING": "47", + "BOXUSER3": "48", + "BOXUSER4": "49", + "BOXCHANGEMISSION": "50", + "BOXBEEPERMUTE": "51", + "BOXMULTIFUNCTION": "52", + "BOXMIXERPROFILE": "53", + "BOXMIXERTRANSITION": "54", + "BOXANGLEHOLD": "55", + "BOXGIMBALTLOCK": "56", + "BOXGIMBALRLOCK": "57", + "BOXGIMBALCENTER": "58", + "BOXGIMBALHTRK": "59", + "CHECKBOX_ITEM_COUNT": "60" + }, + "busIndex_e": { + "_source": "../../../src/main/drivers/bus.h", + "BUSINDEX_1": "0", + "BUSINDEX_2": "1", + "BUSINDEX_3": "2", + "BUSINDEX_4": "3" + }, + "busSpeed_e": { + "_source": "../../../src/main/drivers/bus.h", + "BUS_SPEED_INITIALIZATION": "0", + "BUS_SPEED_SLOW": "1", + "BUS_SPEED_STANDARD": "2", + "BUS_SPEED_FAST": "3", + "BUS_SPEED_ULTRAFAST": "4" + }, + "busType_e": { + "_source": "../../../src/main/drivers/bus.h", + "BUSTYPE_ANY": "0", + "BUSTYPE_NONE": "0", + "BUSTYPE_I2C": "1", + "BUSTYPE_SPI": "2", + "BUSTYPE_SDIO": "3" + }, + "channelType_t": { + "_source": "../../../src/main/drivers/timer.h", + "TYPE_FREE": "0", + "TYPE_PWMINPUT": "1", + "TYPE_PPMINPUT": "2", + "TYPE_PWMOUTPUT_MOTOR": "3", + "TYPE_PWMOUTPUT_FAST": "4", + "TYPE_PWMOUTPUT_SERVO": "5", + "TYPE_SOFTSERIAL_RX": "6", + "TYPE_SOFTSERIAL_TX": "7", + "TYPE_SOFTSERIAL_RXTX": "8", + "TYPE_SOFTSERIAL_AUXTIMER": "9", + "TYPE_ADC": "10", + "TYPE_SERIAL_RX": "11", + "TYPE_SERIAL_TX": "12", + "TYPE_SERIAL_RXTX": "13", + "TYPE_TIMER": "14" + }, + "climbRateToAltitudeControllerMode_e": { + "_source": "../../../src/main/navigation/navigation_private.h", + "ROC_TO_ALT_CURRENT": "0", + "ROC_TO_ALT_CONSTANT": "1", + "ROC_TO_ALT_TARGET": "2" + }, + "colorComponent_e": { + "_source": "../../../src/main/common/color.h", + "RGB_RED": "0", + "RGB_GREEN": "1", + "RGB_BLUE": "2" + }, + "colorId_e": { + "_source": "../../../src/main/io/ledstrip.h", + "COLOR_BLACK": "0", + "COLOR_WHITE": "1", + "COLOR_RED": "2", + "COLOR_ORANGE": "3", + "COLOR_YELLOW": "4", + "COLOR_LIME_GREEN": "5", + "COLOR_GREEN": "6", + "COLOR_MINT_GREEN": "7", + "COLOR_CYAN": "8", + "COLOR_LIGHT_BLUE": "9", + "COLOR_BLUE": "10", + "COLOR_DARK_VIOLET": "11", + "COLOR_MAGENTA": "12", + "COLOR_DEEP_PINK": "13" + }, + "crsfActiveAntenna_e": { + "_source": "../../../src/main/telemetry/crsf.c", + "CRSF_ACTIVE_ANTENNA1": "0", + "CRSF_ACTIVE_ANTENNA2": "1" + }, + "crsfAddress_e": { + "_source": "../../../src/main/rx/crsf.h", + "CRSF_ADDRESS_BROADCAST": "0", + "CRSF_ADDRESS_USB": "16", + "CRSF_ADDRESS_TBS_CORE_PNP_PRO": "128", + "CRSF_ADDRESS_RESERVED1": "138", + "CRSF_ADDRESS_CURRENT_SENSOR": "192", + "CRSF_ADDRESS_GPS": "194", + "CRSF_ADDRESS_TBS_BLACKBOX": "196", + "CRSF_ADDRESS_FLIGHT_CONTROLLER": "200", + "CRSF_ADDRESS_RESERVED2": "202", + "CRSF_ADDRESS_RACE_TAG": "204", + "CRSF_ADDRESS_RADIO_TRANSMITTER": "234", + "CRSF_ADDRESS_CRSF_RECEIVER": "236", + "CRSF_ADDRESS_CRSF_TRANSMITTER": "238" + }, + "crsfFrameType_e": { + "_source": "../../../src/main/rx/crsf.h", + "CRSF_FRAMETYPE_GPS": "2", + "CRSF_FRAMETYPE_VARIO_SENSOR": "7", + "CRSF_FRAMETYPE_BATTERY_SENSOR": "8", + "CRSF_FRAMETYPE_BAROMETER_ALTITUDE": "9", + "CRSF_FRAMETYPE_LINK_STATISTICS": "20", + "CRSF_FRAMETYPE_RC_CHANNELS_PACKED": "22", + "CRSF_FRAMETYPE_ATTITUDE": "30", + "CRSF_FRAMETYPE_FLIGHT_MODE": "33", + "CRSF_FRAMETYPE_DEVICE_PING": "40", + "CRSF_FRAMETYPE_DEVICE_INFO": "41", + "CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY": "43", + "CRSF_FRAMETYPE_PARAMETER_READ": "44", + "CRSF_FRAMETYPE_PARAMETER_WRITE": "45", + "CRSF_FRAMETYPE_COMMAND": "50", + "CRSF_FRAMETYPE_MSP_REQ": "122", + "CRSF_FRAMETYPE_MSP_RESP": "123", + "CRSF_FRAMETYPE_MSP_WRITE": "124", + "CRSF_FRAMETYPE_DISPLAYPORT_CMD": "125" + }, + "crsfFrameTypeIndex_e": { + "_source": "../../../src/main/telemetry/crsf.c", + "CRSF_FRAME_START_INDEX": "0", + "CRSF_FRAME_ATTITUDE_INDEX": "CRSF_FRAME_START_INDEX", + "CRSF_FRAME_BATTERY_SENSOR_INDEX": "", + "CRSF_FRAME_FLIGHT_MODE_INDEX": "", + "CRSF_FRAME_GPS_INDEX": "", + "CRSF_FRAME_VARIO_SENSOR_INDEX": "", + "CRSF_FRAME_BAROMETER_ALTITUDE_INDEX": "", + "CRSF_SCHEDULE_COUNT_MAX": "" + }, + "crsrRfMode_e": { + "_source": "../../../src/main/telemetry/crsf.c", + "CRSF_RF_MODE_4_HZ": "0", + "CRSF_RF_MODE_50_HZ": "1", + "CRSF_RF_MODE_150_HZ": "2" + }, + "crsrRfPower_e": { + "_source": "../../../src/main/telemetry/crsf.c", + "CRSF_RF_POWER_0_mW": "0", + "CRSF_RF_POWER_10_mW": "1", + "CRSF_RF_POWER_25_mW": "2", + "CRSF_RF_POWER_100_mW": "3", + "CRSF_RF_POWER_500_mW": "4", + "CRSF_RF_POWER_1000_mW": "5", + "CRSF_RF_POWER_2000_mW": "6", + "CRSF_RF_POWER_250_mW": "7" + }, + "currentSensor_e": { + "_source": "../../../src/main/sensors/battery_config_structs.h", + "CURRENT_SENSOR_NONE": "0", + "CURRENT_SENSOR_ADC": "1", + "CURRENT_SENSOR_VIRTUAL": "2", + "CURRENT_SENSOR_FAKE": "3", + "CURRENT_SENSOR_ESC": "4", + "CURRENT_SENSOR_SMARTPORT": "5", + "CURRENT_SENSOR_MAX": "CURRENT_SENSOR_SMARTPORT" + }, + "devHardwareType_e": { + "_source": "../../../src/main/drivers/bus.h", + "DEVHW_NONE": "0", + "DEVHW_MPU6000": "1", + "DEVHW_MPU6500": "2", + "DEVHW_BMI160": "3", + "DEVHW_BMI088_GYRO": "4", + "DEVHW_BMI088_ACC": "5", + "DEVHW_ICM20689": "6", + "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" + }, + "deviceFlags_e": { + "_source": "../../../src/main/drivers/bus.h", + "DEVFLAGS_NONE": "0", + "DEVFLAGS_USE_RAW_REGISTERS": "(1 << 0)", + "DEVFLAGS_USE_MANUAL_DEVICE_SELECT": "(1 << 1)", + "DEVFLAGS_SPI_MODE_0": "(1 << 2)" + }, + "displayCanvasBitmapOption_t": { + "_source": "../../../src/main/drivers/display_canvas.h", + "DISPLAY_CANVAS_BITMAP_OPT_INVERT_COLORS": "1 << 0", + "DISPLAY_CANVAS_BITMAP_OPT_SOLID_BACKGROUND": "1 << 1", + "DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT": "1 << 2" + }, + "displayCanvasColor_e": { + "_source": "../../../src/main/drivers/display_canvas.h", + "DISPLAY_CANVAS_COLOR_BLACK": "0", + "DISPLAY_CANVAS_COLOR_TRANSPARENT": "1", + "DISPLAY_CANVAS_COLOR_WHITE": "2", + "DISPLAY_CANVAS_COLOR_GRAY": "3" + }, + "displayCanvasOutlineType_e": { + "_source": "../../../src/main/drivers/display_canvas.h", + "DISPLAY_CANVAS_OUTLINE_TYPE_NONE": "0", + "DISPLAY_CANVAS_OUTLINE_TYPE_TOP": "1 << 0", + "DISPLAY_CANVAS_OUTLINE_TYPE_RIGHT": "1 << 1", + "DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM": "1 << 2", + "DISPLAY_CANVAS_OUTLINE_TYPE_LEFT": "1 << 3" + }, + "displayportMspCommand_e": { + "_source": "../../../src/main/io/displayport_msp.h", + "MSP_DP_HEARTBEAT": "0", + "MSP_DP_RELEASE": "1", + "MSP_DP_CLEAR_SCREEN": "2", + "MSP_DP_WRITE_STRING": "3", + "MSP_DP_DRAW_SCREEN": "4", + "MSP_DP_OPTIONS": "5", + "MSP_DP_SYS": "6", + "MSP_DP_COUNT": "7" + }, + "displayTransactionOption_e": { + "_source": "../../../src/main/drivers/display.h", + "DISPLAY_TRANSACTION_OPT_NONE": "0", + "DISPLAY_TRANSACTION_OPT_PROFILED": "1 << 0", + "DISPLAY_TRANSACTION_OPT_RESET_DRAWING": "1 << 1" + }, + "displayWidgetType_e": { + "_source": "../../../src/main/drivers/display_widgets.h", + "DISPLAY_WIDGET_TYPE_AHI": "0", + "DISPLAY_WIDGET_TYPE_SIDEBAR": "1" + }, + "DjiCraftNameElements_t": { + "_source": "../../../src/main/io/osd_dji_hd.c", + "DJI_OSD_CN_MESSAGES": "0", + "DJI_OSD_CN_THROTTLE": "1", + "DJI_OSD_CN_THROTTLE_AUTO_THR": "2", + "DJI_OSD_CN_AIR_SPEED": "3", + "DJI_OSD_CN_EFFICIENCY": "4", + "DJI_OSD_CN_DISTANCE": "5", + "DJI_OSD_CN_ADJUSTEMNTS": "6", + "DJI_OSD_CN_MAX_ELEMENTS": "7" + }, + "dshotCommands_e": { + "_source": "../../../src/main/drivers/pwm_output.h", + "DSHOT_CMD_SPIN_DIRECTION_NORMAL": "20", + "DSHOT_CMD_SPIN_DIRECTION_REVERSED": "21" + }, + "dumpFlags_e": { + "_source": "../../../src/main/fc/cli.c", + "DUMP_MASTER": "(1 << 0)", + "DUMP_CONTROL_PROFILE": "(1 << 1)", + "DUMP_BATTERY_PROFILE": "(1 << 2)", + "DUMP_MIXER_PROFILE": "(1 << 3)", + "DUMP_ALL": "(1 << 4)", + "DO_DIFF": "(1 << 5)", + "SHOW_DEFAULTS": "(1 << 6)", + "HIDE_UNUSED": "(1 << 7)" + }, + "dynamicGyroNotchMode_e": { + "_source": "../../../src/main/sensors/gyro.h", + "DYNAMIC_NOTCH_MODE_2D": "0", + "DYNAMIC_NOTCH_MODE_3D": "1" + }, + "emergLandState_e": { + "_source": "../../../src/main/flight/failsafe.h", + "EMERG_LAND_IDLE": "0", + "EMERG_LAND_IN_PROGRESS": "1", + "EMERG_LAND_HAS_LANDED": "2" + }, + "escSensorFrameStatus_t": { + "_source": "../../../src/main/sensors/esc_sensor.c", + "ESC_SENSOR_FRAME_PENDING": "0", + "ESC_SENSOR_FRAME_COMPLETE": "1", + "ESC_SENSOR_FRAME_FAILED": "2" + }, + "escSensorState_t": { + "_source": "../../../src/main/sensors/esc_sensor.c", + "ESC_SENSOR_WAIT_STARTUP": "0", + "ESC_SENSOR_READY": "1", + "ESC_SENSOR_WAITING": "2" + }, + "failsafeChannelBehavior_e": { + "_source": "../../../src/main/flight/failsafe.c", + "FAILSAFE_CHANNEL_HOLD": "0", + "FAILSAFE_CHANNEL_NEUTRAL": "1" + }, + "failsafePhase_e": { + "_source": "../../../src/main/flight/failsafe.h", + "FAILSAFE_IDLE": "0", + "FAILSAFE_RX_LOSS_DETECTED": "1", + "FAILSAFE_RX_LOSS_IDLE": "2", + "FAILSAFE_RETURN_TO_HOME": "3", + "FAILSAFE_LANDING": "4", + "FAILSAFE_LANDED": "5", + "FAILSAFE_RX_LOSS_MONITORING": "6", + "FAILSAFE_RX_LOSS_RECOVERED": "7" + }, + "failsafeProcedure_e": { + "_source": "../../../src/main/flight/failsafe.h", + "FAILSAFE_PROCEDURE_AUTO_LANDING": "0", + "FAILSAFE_PROCEDURE_DROP_IT": "1", + "FAILSAFE_PROCEDURE_RTH": "2", + "FAILSAFE_PROCEDURE_NONE": "3" + }, + "failsafeRxLinkState_e": { + "_source": "../../../src/main/flight/failsafe.h", + "FAILSAFE_RXLINK_DOWN": "0", + "FAILSAFE_RXLINK_UP": "1" + }, + "failureMode_e": { + "_source": "../../../src/main/drivers/system.h", + "FAILURE_DEVELOPER": "0", + "FAILURE_MISSING_ACC": "1", + "FAILURE_ACC_INIT": "2", + "FAILURE_ACC_INCOMPATIBLE": "3", + "FAILURE_INVALID_EEPROM_CONTENTS": "4", + "FAILURE_FLASH_WRITE_FAILED": "5", + "FAILURE_GYRO_INIT_FAILED": "6", + "FAILURE_FLASH_READ_FAILED": "7" + }, + "fatFilesystemType_e": { + "_source": "../../../src/main/io/asyncfatfs/fat_standard.h", + "FAT_FILESYSTEM_TYPE_INVALID": "0", + "FAT_FILESYSTEM_TYPE_FAT12": "1", + "FAT_FILESYSTEM_TYPE_FAT16": "2", + "FAT_FILESYSTEM_TYPE_FAT32": "3" + }, + "features_e": { + "_source": "../../../src/main/fc/config.h", + "FEATURE_THR_VBAT_COMP": "1 << 0", + "FEATURE_VBAT": "1 << 1", + "FEATURE_TX_PROF_SEL": "1 << 2", + "FEATURE_BAT_PROFILE_AUTOSWITCH": "1 << 3", + "FEATURE_GEOZONE": "1 << 4", + "FEATURE_UNUSED_1": "1 << 5", + "FEATURE_SOFTSERIAL": "1 << 6", + "FEATURE_GPS": "1 << 7", + "FEATURE_UNUSED_3": "1 << 8", + "FEATURE_UNUSED_4": "1 << 9", + "FEATURE_TELEMETRY": "1 << 10", + "FEATURE_CURRENT_METER": "1 << 11", + "FEATURE_REVERSIBLE_MOTORS": "1 << 12", + "FEATURE_UNUSED_5": "1 << 13", + "FEATURE_UNUSED_6": "1 << 14", + "FEATURE_RSSI_ADC": "1 << 15", + "FEATURE_LED_STRIP": "1 << 16", + "FEATURE_DASHBOARD": "1 << 17", + "FEATURE_UNUSED_7": "1 << 18", + "FEATURE_BLACKBOX": "1 << 19", + "FEATURE_UNUSED_10": "1 << 20", + "FEATURE_TRANSPONDER": "1 << 21", + "FEATURE_AIRMODE": "1 << 22", + "FEATURE_SUPEREXPO_RATES": "1 << 23", + "FEATURE_VTX": "1 << 24", + "FEATURE_UNUSED_8": "1 << 25", + "FEATURE_UNUSED_9": "1 << 26", + "FEATURE_UNUSED_11": "1 << 27", + "FEATURE_PWM_OUTPUT_ENABLE": "1 << 28", + "FEATURE_OSD": "1 << 29", + "FEATURE_FW_LAUNCH": "1 << 30", + "FEATURE_FW_AUTOTRIM": "1 << 31" + }, + "filterType_e": { + "_source": "../../../src/main/common/filter.h", + "FILTER_PT1": "0", + "FILTER_BIQUAD": "1", + "FILTER_PT2": "2", + "FILTER_PT3": "3", + "FILTER_LULU": "4" + }, + "fixedWingLaunchEvent_t": { + "_source": "../../../src/main/navigation/navigation_fw_launch.c", + "FW_LAUNCH_EVENT_NONE": "0", + "FW_LAUNCH_EVENT_SUCCESS": "1", + "FW_LAUNCH_EVENT_GOTO_DETECTION": "2", + "FW_LAUNCH_EVENT_ABORT": "3", + "FW_LAUNCH_EVENT_THROTTLE_LOW": "4", + "FW_LAUNCH_EVENT_COUNT": "5" + }, + "fixedWingLaunchMessage_t": { + "_source": "../../../src/main/navigation/navigation_fw_launch.c", + "FW_LAUNCH_MESSAGE_TYPE_NONE": "0", + "FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE": "1", + "FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE": "2", + "FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION": "3", + "FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS": "4", + "FW_LAUNCH_MESSAGE_TYPE_FINISHING": "5" + }, + "fixedWingLaunchState_t": { + "_source": "../../../src/main/navigation/navigation_fw_launch.c", + "FW_LAUNCH_STATE_WAIT_THROTTLE": "0", + "FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT": "1", + "FW_LAUNCH_STATE_IDLE_MOTOR_DELAY": "2", + "FW_LAUNCH_STATE_MOTOR_IDLE": "3", + "FW_LAUNCH_STATE_WAIT_DETECTION": "4", + "FW_LAUNCH_STATE_DETECTED": "5", + "FW_LAUNCH_STATE_MOTOR_DELAY": "6", + "FW_LAUNCH_STATE_MOTOR_SPINUP": "7", + "FW_LAUNCH_STATE_IN_PROGRESS": "8", + "FW_LAUNCH_STATE_FINISH": "9", + "FW_LAUNCH_STATE_ABORTED": "10", + "FW_LAUNCH_STATE_FLYING": "11", + "FW_LAUNCH_STATE_COUNT": "12" + }, + "flashPartitionType_e": { + "_source": "../../../src/main/drivers/flash.h", + "FLASH_PARTITION_TYPE_UNKNOWN": "0", + "FLASH_PARTITION_TYPE_PARTITION_TABLE": "1", + "FLASH_PARTITION_TYPE_FLASHFS": "2", + "FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT": "3", + "FLASH_PARTITION_TYPE_FIRMWARE": "4", + "FLASH_PARTITION_TYPE_CONFIG": "5", + "FLASH_PARTITION_TYPE_FULL_BACKUP": "6", + "FLASH_PARTITION_TYPE_FIRMWARE_UPDATE_META": "7", + "FLASH_PARTITION_TYPE_UPDATE_FIRMWARE": "8", + "FLASH_MAX_PARTITIONS": "9" + }, + "flashType_e": { + "_source": "../../../src/main/drivers/flash.h", + "FLASH_TYPE_NOR": "0", + "FLASH_TYPE_NAND": "1" + }, + "flight_dynamics_index_t": { + "_source": "../../../src/main/common/axis.h", + "FD_ROLL": "0", + "FD_PITCH": "1", + "FD_YAW": "2" + }, + "flightModeFlags_e": { + "_source": "../../../src/main/fc/runtime_config.h", + "ANGLE_MODE": "(1 << 0)", + "HORIZON_MODE": "(1 << 1)", + "HEADING_MODE": "(1 << 2)", + "NAV_ALTHOLD_MODE": "(1 << 3)", + "NAV_RTH_MODE": "(1 << 4)", + "NAV_POSHOLD_MODE": "(1 << 5)", + "HEADFREE_MODE": "(1 << 6)", + "NAV_LAUNCH_MODE": "(1 << 7)", + "MANUAL_MODE": "(1 << 8)", + "FAILSAFE_MODE": "(1 << 9)", + "AUTO_TUNE": "(1 << 10)", + "NAV_WP_MODE": "(1 << 11)", + "NAV_COURSE_HOLD_MODE": "(1 << 12)", + "FLAPERON": "(1 << 13)", + "TURN_ASSISTANT": "(1 << 14)", + "TURTLE_MODE": "(1 << 15)", + "SOARING_MODE": "(1 << 16)", + "ANGLEHOLD_MODE": "(1 << 17)", + "NAV_FW_AUTOLAND": "(1 << 18)", + "NAV_SEND_TO": "(1 << 19)" + }, + "flightModeForTelemetry_e": { + "_source": "../../../src/main/fc/runtime_config.h", + "FLM_MANUAL": "0", + "FLM_ACRO": "1", + "FLM_ACRO_AIR": "2", + "FLM_ANGLE": "3", + "FLM_HORIZON": "4", + "FLM_ALTITUDE_HOLD": "5", + "FLM_POSITION_HOLD": "6", + "FLM_RTH": "7", + "FLM_MISSION": "8", + "FLM_COURSE_HOLD": "9", + "FLM_CRUISE": "10", + "FLM_LAUNCH": "11", + "FLM_FAILSAFE": "12", + "FLM_ANGLEHOLD": "13", + "FLM_COUNT": "14" + }, + "flyingPlatformType_e": { + "_source": "../../../src/main/flight/mixer.h", + "PLATFORM_MULTIROTOR": "0", + "PLATFORM_AIRPLANE": "1", + "PLATFORM_HELICOPTER": "2", + "PLATFORM_TRICOPTER": "3", + "PLATFORM_ROVER": "4", + "PLATFORM_BOAT": "5" + }, + "fport2_control_frame_type_e": { + "_source": "../../../src/main/rx/fport2.c", + "CFT_RC": "255", + "CFT_OTA_START": "240", + "CFT_OTA_DATA": "241", + "CFT_OTA_STOP": "242" + }, + "frame_state_e": { + "_source": "../../../src/main/rx/fport2.c", + "FS_CONTROL_FRAME_START": "0", + "FS_CONTROL_FRAME_TYPE": "1", + "FS_CONTROL_FRAME_DATA": "2", + "FS_DOWNLINK_FRAME_START": "3", + "FS_DOWNLINK_FRAME_DATA": "4" + }, + "frame_type_e": { + "_source": "../../../src/main/rx/fport2.c", + "FT_CONTROL": "0", + "FT_DOWNLINK": "1" + }, + "frskyOSDColor_e": { + "_source": "../../../src/main/io/frsky_osd.h", + "FRSKY_OSD_COLOR_BLACK": "0", + "FRSKY_OSD_COLOR_TRANSPARENT": "1", + "FRSKY_OSD_COLOR_WHITE": "2", + "FRSKY_OSD_COLOR_GRAY": "3" + }, + "frskyOSDLineOutlineType_e": { + "_source": "../../../src/main/io/frsky_osd.h", + "FRSKY_OSD_OUTLINE_TYPE_NONE": "0", + "FRSKY_OSD_OUTLINE_TYPE_TOP": "1 << 0", + "FRSKY_OSD_OUTLINE_TYPE_RIGHT": "1 << 1", + "FRSKY_OSD_OUTLINE_TYPE_BOTTOM": "1 << 2", + "FRSKY_OSD_OUTLINE_TYPE_LEFT": "1 << 3" + }, + "frskyOSDRecvState_e": { + "_source": "../../../src/main/io/frsky_osd.c", + "RECV_STATE_NONE": "0", + "RECV_STATE_SYNC": "1", + "RECV_STATE_LENGTH": "2", + "RECV_STATE_DATA": "3", + "RECV_STATE_CHECKSUM": "4", + "RECV_STATE_DONE": "5" + }, + "frskyOSDTransactionOptions_e": { + "_source": "../../../src/main/io/frsky_osd.h", + "FRSKY_OSD_TRANSACTION_OPT_PROFILED": "1 << 0", + "FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING": "1 << 1" + }, + "fw_autotune_rate_adjustment_e": { + "_source": "../../../src/main/flight/pid.h", + "FIXED": "0", + "LIMIT": "1", + "AUTO": "2" + }, + "fwAutolandApproachDirection_e": { + "_source": "../../../src/main/navigation/navigation.h", + "FW_AUTOLAND_APPROACH_DIRECTION_LEFT": "0", + "FW_AUTOLAND_APPROACH_DIRECTION_RIGHT": "1" + }, + "fwAutolandState_t": { + "_source": "../../../src/main/navigation/navigation.h", + "FW_AUTOLAND_STATE_IDLE": "0", + "FW_AUTOLAND_STATE_LOITER": "1", + "FW_AUTOLAND_STATE_DOWNWIND": "2", + "FW_AUTOLAND_STATE_BASE_LEG": "3", + "FW_AUTOLAND_STATE_FINAL_APPROACH": "4", + "FW_AUTOLAND_STATE_GLIDE": "5", + "FW_AUTOLAND_STATE_FLARE": "6" + }, + "fwAutolandWaypoint_t": { + "_source": "../../../src/main/navigation/navigation_private.h", + "FW_AUTOLAND_WP_TURN": "0", + "FW_AUTOLAND_WP_FINAL_APPROACH": "1", + "FW_AUTOLAND_WP_LAND": "2", + "FW_AUTOLAND_WP_COUNT": "3" + }, + "geoAltitudeConversionMode_e": { + "_source": "../../../src/main/navigation/navigation.h", + "GEO_ALT_ABSOLUTE": "0", + "GEO_ALT_RELATIVE": "1" + }, + "geoAltitudeDatumFlag_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NAV_WP_TAKEOFF_DATUM": "0", + "NAV_WP_MSL_DATUM": "1" + }, + "geoOriginResetMode_e": { + "_source": "../../../src/main/navigation/navigation.h", + "GEO_ORIGIN_SET": "0", + "GEO_ORIGIN_RESET_ALTITUDE": "1" + }, + "geozoneActionState_e": { + "_source": "../../../src/main/navigation/navigation_geozone.c", + "GEOZONE_ACTION_STATE_NONE": "0", + "GEOZONE_ACTION_STATE_AVOIDING": "1", + "GEOZONE_ACTION_STATE_AVOIDING_UPWARD": "2", + "GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE": "3", + "GEOZONE_ACTION_STATE_RETURN_TO_FZ": "4", + "GEOZONE_ACTION_STATE_FLYOUT_NFZ": "5", + "GEOZONE_ACTION_STATE_POSHOLD": "6", + "GEOZONE_ACTION_STATE_RTH": "7" + }, + "geozoneMessageState_e": { + "_source": "../../../src/main/navigation/navigation.h", + "GEOZONE_MESSAGE_STATE_NONE": "0", + "GEOZONE_MESSAGE_STATE_NFZ": "1", + "GEOZONE_MESSAGE_STATE_LEAVING_FZ": "2", + "GEOZONE_MESSAGE_STATE_OUTSIDE_FZ": "3", + "GEOZONE_MESSAGE_STATE_ENTERING_NFZ": "4", + "GEOZONE_MESSAGE_STATE_AVOIDING_FB": "5", + "GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE": "6", + "GEOZONE_MESSAGE_STATE_FLYOUT_NFZ": "7", + "GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH": "8", + "GEOZONE_MESSAGE_STATE_POS_HOLD": "9" + }, + "ghstAddr_e": { + "_source": "../../../src/main/rx/ghst_protocol.h", + "GHST_ADDR_RADIO": "128", + "GHST_ADDR_TX_MODULE_SYM": "129", + "GHST_ADDR_TX_MODULE_ASYM": "136", + "GHST_ADDR_FC": "130", + "GHST_ADDR_GOGGLES": "131", + "GHST_ADDR_QUANTUM_TEE1": "132", + "GHST_ADDR_QUANTUM_TEE2": "133", + "GHST_ADDR_QUANTUM_GW1": "134", + "GHST_ADDR_5G_CLK": "135", + "GHST_ADDR_RX": "137" + }, + "ghstDl_e": { + "_source": "../../../src/main/rx/ghst_protocol.h", + "GHST_DL_OPENTX_SYNC": "32", + "GHST_DL_LINK_STAT": "33", + "GHST_DL_VTX_STAT": "34", + "GHST_DL_PACK_STAT": "35", + "GHST_DL_GPS_PRIMARY": "37", + "GHST_DL_GPS_SECONDARY": "38" + }, + "ghstFrameTypeIndex_e": { + "_source": "../../../src/main/telemetry/ghst.c", + "GHST_FRAME_START_INDEX": "0", + "GHST_FRAME_PACK_INDEX": "GHST_FRAME_START_INDEX", + "GHST_FRAME_GPS_PRIMARY_INDEX": "", + "GHST_FRAME_GPS_SECONDARY_INDEX": "", + "GHST_SCHEDULE_COUNT_MAX": "" + }, + "ghstUl_e": { + "_source": "../../../src/main/rx/ghst_protocol.h", + "GHST_UL_RC_CHANS_HS4_FIRST": "16", + "GHST_UL_RC_CHANS_HS4_5TO8": "16", + "GHST_UL_RC_CHANS_HS4_9TO12": "17", + "GHST_UL_RC_CHANS_HS4_13TO16": "18", + "GHST_UL_RC_CHANS_HS4_RSSI": "19", + "GHST_UL_RC_CHANS_HS4_LAST": "31" + }, + "gimbal_htk_mode_e": { + "_source": "../../../src/main/drivers/gimbal_common.h", + "GIMBAL_MODE_FOLLOW": "0", + "GIMBAL_MODE_TILT_LOCK": "(1<<0)", + "GIMBAL_MODE_ROLL_LOCK": "(1<<1)", + "GIMBAL_MODE_PAN_LOCK": "(1<<2)" + }, + "gimbalDevType_e": { + "_source": "../../../src/main/drivers/gimbal_common.h", + "GIMBAL_DEV_UNSUPPORTED": "0", + "GIMBAL_DEV_SERIAL": "1", + "GIMBAL_DEV_UNKNOWN": "255" + }, + "gimbalHeadtrackerState_e": { + "_source": "../../../src/main/io/gimbal_serial.h", + "WAITING_HDR1": "0", + "WAITING_HDR2": "1", + "WAITING_PAYLOAD": "2", + "WAITING_CRCH": "3", + "WAITING_CRCL": "4" + }, + "gpsAutoBaud_e": { + "_source": "../../../src/main/io/gps.h", + "GPS_AUTOBAUD_OFF": "0", + "GPS_AUTOBAUD_ON": "1" + }, + "gpsAutoConfig_e": { + "_source": "../../../src/main/io/gps.h", + "GPS_AUTOCONFIG_OFF": "0", + "GPS_AUTOCONFIG_ON": "1" + }, + "gpsBaudRate_e": { + "_source": "../../../src/main/io/gps.h", + "GPS_BAUDRATE_115200": "0", + "GPS_BAUDRATE_57600": "1", + "GPS_BAUDRATE_38400": "2", + "GPS_BAUDRATE_19200": "3", + "GPS_BAUDRATE_9600": "4", + "GPS_BAUDRATE_230400": "5", + "GPS_BAUDRATE_460800": "6", + "GPS_BAUDRATE_921600": "7", + "GPS_BAUDRATE_COUNT": "8" + }, + "gpsDynModel_e": { + "_source": "../../../src/main/io/gps.h", + "GPS_DYNMODEL_PEDESTRIAN": "0", + "GPS_DYNMODEL_AUTOMOTIVE": "1", + "GPS_DYNMODEL_AIR_1G": "2", + "GPS_DYNMODEL_AIR_2G": "3", + "GPS_DYNMODEL_AIR_4G": "4", + "GPS_DYNMODEL_SEA": "5", + "GPS_DYNMODEL_MOWER": "6" + }, + "gpsFixChar_e": { + "_source": "../../../src/main/telemetry/hott.c", + "GPS_FIX_CHAR_NONE": "'-'", + "GPS_FIX_CHAR_2D": "'2'", + "GPS_FIX_CHAR_3D": "'3'", + "GPS_FIX_CHAR_DGPS": "'D'" + }, + "gpsFixType_e": { + "_source": "../../../src/main/io/gps.h", + "GPS_NO_FIX": "0", + "GPS_FIX_2D": "1", + "GPS_FIX_3D": "2" + }, + "gpsProvider_e": { + "_source": "../../../src/main/io/gps.h", + "GPS_UBLOX": "0", + "GPS_MSP": "1", + "GPS_FAKE": "2", + "GPS_PROVIDER_COUNT": "3" + }, + "gpsState_e": { + "_source": "../../../src/main/io/gps_private.h", + "GPS_UNKNOWN": "0", + "GPS_INITIALIZING": "1", + "GPS_RUNNING": "2", + "GPS_LOST_COMMUNICATION": "3" + }, + "gyroFilterMode_e": { + "_source": "../../../src/main/sensors/gyro.h", + "GYRO_FILTER_MODE_OFF": "0", + "GYRO_FILTER_MODE_STATIC": "1", + "GYRO_FILTER_MODE_DYNAMIC": "2", + "GYRO_FILTER_MODE_ADAPTIVE": "3" + }, + "gyroHardwareLpf_e": { + "_source": "../../../src/main/drivers/accgyro/accgyro_lsm6dxx.h", + "GYRO_HARDWARE_LPF_NORMAL": "0", + "GYRO_HARDWARE_LPF_OPTION_1": "1", + "GYRO_HARDWARE_LPF_OPTION_2": "2", + "GYRO_HARDWARE_LPF_EXPERIMENTAL": "3", + "GYRO_HARDWARE_LPF_COUNT": "4" + }, + "gyroSensor_e": { + "_source": "../../../src/main/sensors/gyro.h", + "GYRO_NONE": "0", + "GYRO_AUTODETECT": "1", + "GYRO_MPU6000": "2", + "GYRO_MPU6500": "3", + "GYRO_MPU9250": "4", + "GYRO_BMI160": "5", + "GYRO_ICM20689": "6", + "GYRO_BMI088": "7", + "GYRO_ICM42605": "8", + "GYRO_BMI270": "9", + "GYRO_LSM6DXX": "10", + "GYRO_FAKE": "11" + }, + "HardwareMotorTypes_e": { + "_source": "../../../src/main/drivers/pwm_esc_detect.h", + "MOTOR_UNKNOWN": "0", + "MOTOR_BRUSHED": "1", + "MOTOR_BRUSHLESS": "2" + }, + "hardwareSensorStatus_e": { + "_source": "../../../src/main/sensors/diagnostics.h", + "HW_SENSOR_NONE": "0", + "HW_SENSOR_OK": "1", + "HW_SENSOR_UNAVAILABLE": "2", + "HW_SENSOR_UNHEALTHY": "3" + }, + "headTrackerDevType_e": { + "_source": "../../../src/main/drivers/headtracker_common.h", + "HEADTRACKER_NONE": "0", + "HEADTRACKER_SERIAL": "1", + "HEADTRACKER_MSP": "2", + "HEADTRACKER_UNKNOWN": "255" + }, + "hottEamAlarm1Flag_e": { + "_source": "../../../src/main/telemetry/hott.h", + "HOTT_EAM_ALARM1_FLAG_NONE": "0", + "HOTT_EAM_ALARM1_FLAG_MAH": "(1 << 0)", + "HOTT_EAM_ALARM1_FLAG_BATTERY_1": "(1 << 1)", + "HOTT_EAM_ALARM1_FLAG_BATTERY_2": "(1 << 2)", + "HOTT_EAM_ALARM1_FLAG_TEMPERATURE_1": "(1 << 3)", + "HOTT_EAM_ALARM1_FLAG_TEMPERATURE_2": "(1 << 4)", + "HOTT_EAM_ALARM1_FLAG_ALTITUDE": "(1 << 5)", + "HOTT_EAM_ALARM1_FLAG_CURRENT": "(1 << 6)", + "HOTT_EAM_ALARM1_FLAG_MAIN_VOLTAGE": "(1 << 7)" + }, + "hottEamAlarm2Flag_e": { + "_source": "../../../src/main/telemetry/hott.h", + "HOTT_EAM_ALARM2_FLAG_NONE": "0", + "HOTT_EAM_ALARM2_FLAG_MS": "(1 << 0)", + "HOTT_EAM_ALARM2_FLAG_M3S": "(1 << 1)", + "HOTT_EAM_ALARM2_FLAG_ALTITUDE_DUPLICATE": "(1 << 2)", + "HOTT_EAM_ALARM2_FLAG_MS_DUPLICATE": "(1 << 3)", + "HOTT_EAM_ALARM2_FLAG_M3S_DUPLICATE": "(1 << 4)", + "HOTT_EAM_ALARM2_FLAG_UNKNOWN_1": "(1 << 5)", + "HOTT_EAM_ALARM2_FLAG_UNKNOWN_2": "(1 << 6)", + "HOTT_EAM_ALARM2_FLAG_ON_SIGN_OR_TEXT_ACTIVE": "(1 << 7)" + }, + "hottState_e": { + "_source": "../../../src/main/telemetry/hott.c", + "HOTT_WAITING_FOR_REQUEST": "0", + "HOTT_RECEIVING_REQUEST": "1", + "HOTT_WAITING_FOR_TX_WINDOW": "2", + "HOTT_TRANSMITTING": "3", + "HOTT_ENDING_TRANSMISSION": "4" + }, + "hsvColorComponent_e": { + "_source": "../../../src/main/common/color.h", + "HSV_HUE": "0", + "HSV_SATURATION": "1", + "HSV_VALUE": "2" + }, + "I2CSpeed": { + "_source": "../../../src/main/drivers/bus_i2c.h", + "I2C_SPEED_100KHZ": "2", + "I2C_SPEED_200KHZ": "3", + "I2C_SPEED_400KHZ": "0", + "I2C_SPEED_800KHZ": "1" + }, + "i2cState_t": { + "_source": "../../../src/main/drivers/bus_i2c_stm32f40x.c", + "I2C_STATE_STOPPED": "0", + "I2C_STATE_STOPPING": "1", + "I2C_STATE_STARTING": "2", + "I2C_STATE_STARTING_WAIT": "3", + "I2C_STATE_R_ADDR": "4", + "I2C_STATE_R_ADDR_WAIT": "5", + "I2C_STATE_R_REGISTER": "6", + "I2C_STATE_R_REGISTER_WAIT": "7", + "I2C_STATE_R_RESTARTING": "8", + "I2C_STATE_R_RESTARTING_WAIT": "9", + "I2C_STATE_R_RESTART_ADDR": "10", + "I2C_STATE_R_RESTART_ADDR_WAIT": "11", + "I2C_STATE_R_TRANSFER_EQ1": "12", + "I2C_STATE_R_TRANSFER_EQ2": "13", + "I2C_STATE_R_TRANSFER_GE2": "14", + "I2C_STATE_W_ADDR": "15", + "I2C_STATE_W_ADDR_WAIT": "16", + "I2C_STATE_W_REGISTER": "17", + "I2C_STATE_W_TRANSFER_WAIT": "18", + "I2C_STATE_W_TRANSFER": "19", + "I2C_STATE_NACK": "20", + "I2C_STATE_BUS_ERROR": "21" + }, + "i2cTransferDirection_t": { + "_source": "../../../src/main/drivers/bus_i2c_stm32f40x.c", + "I2C_TXN_READ": "0", + "I2C_TXN_WRITE": "1" + }, + "ibusCommand_e": { + "_source": "../../../src/main/telemetry/ibus_shared.c", + "IBUS_COMMAND_DISCOVER_SENSOR": "128", + "IBUS_COMMAND_SENSOR_TYPE": "144", + "IBUS_COMMAND_MEASUREMENT": "160" + }, + "ibusSensorType1_e": { + "_source": "../../../src/main/telemetry/ibus_shared.h", + "IBUS_MEAS_TYPE1_INTV": "0", + "IBUS_MEAS_TYPE1_TEM": "1", + "IBUS_MEAS_TYPE1_MOT": "2", + "IBUS_MEAS_TYPE1_EXTV": "3", + "IBUS_MEAS_TYPE1_CELL": "4", + "IBUS_MEAS_TYPE1_BAT_CURR": "5", + "IBUS_MEAS_TYPE1_FUEL": "6", + "IBUS_MEAS_TYPE1_RPM": "7", + "IBUS_MEAS_TYPE1_CMP_HEAD": "8", + "IBUS_MEAS_TYPE1_CLIMB_RATE": "9", + "IBUS_MEAS_TYPE1_COG": "10", + "IBUS_MEAS_TYPE1_GPS_STATUS": "11", + "IBUS_MEAS_TYPE1_ACC_X": "12", + "IBUS_MEAS_TYPE1_ACC_Y": "13", + "IBUS_MEAS_TYPE1_ACC_Z": "14", + "IBUS_MEAS_TYPE1_ROLL": "15", + "IBUS_MEAS_TYPE1_PITCH": "16", + "IBUS_MEAS_TYPE1_YAW": "17", + "IBUS_MEAS_TYPE1_VERTICAL_SPEED": "18", + "IBUS_MEAS_TYPE1_GROUND_SPEED": "19", + "IBUS_MEAS_TYPE1_GPS_DIST": "20", + "IBUS_MEAS_TYPE1_ARMED": "21", + "IBUS_MEAS_TYPE1_FLIGHT_MODE": "22", + "IBUS_MEAS_TYPE1_PRES": "65", + "IBUS_MEAS_TYPE1_SPE": "126", + "IBUS_MEAS_TYPE1_GPS_LAT": "128", + "IBUS_MEAS_TYPE1_GPS_LON": "129", + "IBUS_MEAS_TYPE1_GPS_ALT": "130", + "IBUS_MEAS_TYPE1_ALT": "131", + "IBUS_MEAS_TYPE1_S84": "132", + "IBUS_MEAS_TYPE1_S85": "133", + "IBUS_MEAS_TYPE1_S86": "134", + "IBUS_MEAS_TYPE1_S87": "135", + "IBUS_MEAS_TYPE1_S88": "136", + "IBUS_MEAS_TYPE1_S89": "137", + "IBUS_MEAS_TYPE1_S8a": "138" + }, + "ibusSensorType_e": { + "_source": "../../../src/main/telemetry/ibus_shared.h", + "IBUS_MEAS_TYPE_INTERNAL_VOLTAGE": "0", + "IBUS_MEAS_TYPE_TEMPERATURE": "1", + "IBUS_MEAS_TYPE_RPM": "2", + "IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE": "3", + "IBUS_MEAS_TYPE_HEADING": "4", + "IBUS_MEAS_TYPE_CURRENT": "5", + "IBUS_MEAS_TYPE_CLIMB": "6", + "IBUS_MEAS_TYPE_ACC_Z": "7", + "IBUS_MEAS_TYPE_ACC_Y": "8", + "IBUS_MEAS_TYPE_ACC_X": "9", + "IBUS_MEAS_TYPE_VSPEED": "10", + "IBUS_MEAS_TYPE_SPEED": "11", + "IBUS_MEAS_TYPE_DIST": "12", + "IBUS_MEAS_TYPE_ARMED": "13", + "IBUS_MEAS_TYPE_MODE": "14", + "IBUS_MEAS_TYPE_PRES": "65", + "IBUS_MEAS_TYPE_SPE": "126", + "IBUS_MEAS_TYPE_COG": "128", + "IBUS_MEAS_TYPE_GPS_STATUS": "129", + "IBUS_MEAS_TYPE_GPS_LON": "130", + "IBUS_MEAS_TYPE_GPS_LAT": "131", + "IBUS_MEAS_TYPE_ALT": "132", + "IBUS_MEAS_TYPE_S85": "133", + "IBUS_MEAS_TYPE_S86": "134", + "IBUS_MEAS_TYPE_S87": "135", + "IBUS_MEAS_TYPE_S88": "136", + "IBUS_MEAS_TYPE_S89": "137", + "IBUS_MEAS_TYPE_S8A": "138", + "IBUS_MEAS_TYPE_GALT": "249", + "IBUS_MEAS_TYPE_GPS": "253" + }, + "ibusSensorValue_e": { + "_source": "../../../src/main/telemetry/ibus_shared.h", + "IBUS_MEAS_VALUE_NONE": "0", + "IBUS_MEAS_VALUE_TEMPERATURE": "1", + "IBUS_MEAS_VALUE_MOT": "2", + "IBUS_MEAS_VALUE_EXTERNAL_VOLTAGE": "3", + "IBUS_MEAS_VALUE_CELL": "4", + "IBUS_MEAS_VALUE_CURRENT": "5", + "IBUS_MEAS_VALUE_FUEL": "6", + "IBUS_MEAS_VALUE_RPM": "7", + "IBUS_MEAS_VALUE_HEADING": "8", + "IBUS_MEAS_VALUE_CLIMB": "9", + "IBUS_MEAS_VALUE_COG": "10", + "IBUS_MEAS_VALUE_GPS_STATUS": "11", + "IBUS_MEAS_VALUE_ACC_X": "12", + "IBUS_MEAS_VALUE_ACC_Y": "13", + "IBUS_MEAS_VALUE_ACC_Z": "14", + "IBUS_MEAS_VALUE_ROLL": "15", + "IBUS_MEAS_VALUE_PITCH": "16", + "IBUS_MEAS_VALUE_YAW": "17", + "IBUS_MEAS_VALUE_VSPEED": "18", + "IBUS_MEAS_VALUE_SPEED": "19", + "IBUS_MEAS_VALUE_DIST": "20", + "IBUS_MEAS_VALUE_ARMED": "21", + "IBUS_MEAS_VALUE_MODE": "22", + "IBUS_MEAS_VALUE_PRES": "65", + "IBUS_MEAS_VALUE_SPE": "126", + "IBUS_MEAS_VALUE_GPS_LAT": "128", + "IBUS_MEAS_VALUE_GPS_LON": "129", + "IBUS_MEAS_VALUE_GALT4": "130", + "IBUS_MEAS_VALUE_ALT4": "131", + "IBUS_MEAS_VALUE_GALT": "132", + "IBUS_MEAS_VALUE_ALT": "133", + "IBUS_MEAS_VALUE_STATUS": "135", + "IBUS_MEAS_VALUE_GPS_LAT1": "136", + "IBUS_MEAS_VALUE_GPS_LON1": "137", + "IBUS_MEAS_VALUE_GPS_LAT2": "144", + "IBUS_MEAS_VALUE_GPS_LON2": "145", + "IBUS_MEAS_VALUE_GPS": "253" + }, + "inputSource_e": { + "_source": "../../../src/main/flight/servos.h", + "INPUT_STABILIZED_ROLL": "0", + "INPUT_STABILIZED_PITCH": "1", + "INPUT_STABILIZED_YAW": "2", + "INPUT_STABILIZED_THROTTLE": "3", + "INPUT_RC_ROLL": "4", + "INPUT_RC_PITCH": "5", + "INPUT_RC_YAW": "6", + "INPUT_RC_THROTTLE": "7", + "INPUT_RC_CH5": "8", + "INPUT_RC_CH6": "9", + "INPUT_RC_CH7": "10", + "INPUT_RC_CH8": "11", + "INPUT_GIMBAL_PITCH": "12", + "INPUT_GIMBAL_ROLL": "13", + "INPUT_FEATURE_FLAPS": "14", + "INPUT_RC_CH9": "15", + "INPUT_RC_CH10": "16", + "INPUT_RC_CH11": "17", + "INPUT_RC_CH12": "18", + "INPUT_RC_CH13": "19", + "INPUT_RC_CH14": "20", + "INPUT_RC_CH15": "21", + "INPUT_RC_CH16": "22", + "INPUT_STABILIZED_ROLL_PLUS": "23", + "INPUT_STABILIZED_ROLL_MINUS": "24", + "INPUT_STABILIZED_PITCH_PLUS": "25", + "INPUT_STABILIZED_PITCH_MINUS": "26", + "INPUT_STABILIZED_YAW_PLUS": "27", + "INPUT_STABILIZED_YAW_MINUS": "28", + "INPUT_MAX": "29", + "INPUT_GVAR_0": "30", + "INPUT_GVAR_1": "31", + "INPUT_GVAR_2": "32", + "INPUT_GVAR_3": "33", + "INPUT_GVAR_4": "34", + "INPUT_GVAR_5": "35", + "INPUT_GVAR_6": "36", + "INPUT_GVAR_7": "37", + "INPUT_MIXER_TRANSITION": "38", + "INPUT_HEADTRACKER_PAN": "39", + "INPUT_HEADTRACKER_TILT": "40", + "INPUT_HEADTRACKER_ROLL": "41", + "INPUT_RC_CH17": "42", + "INPUT_RC_CH18": "43", + "INPUT_RC_CH19": "44", + "INPUT_RC_CH20": "45", + "INPUT_RC_CH21": "46", + "INPUT_RC_CH22": "47", + "INPUT_RC_CH23": "48", + "INPUT_RC_CH24": "49", + "INPUT_RC_CH25": "50", + "INPUT_RC_CH26": "51", + "INPUT_RC_CH27": "52", + "INPUT_RC_CH28": "53", + "INPUT_RC_CH29": "54", + "INPUT_RC_CH30": "55", + "INPUT_RC_CH31": "56", + "INPUT_RC_CH32": "57", + "INPUT_RC_CH33": "58", + "INPUT_RC_CH34": "59", + "INPUT_MIXER_SWITCH_HELPER": "60", + "INPUT_SOURCE_COUNT": "61" + }, + "itermRelax_e": { + "_source": "../../../src/main/flight/pid.h", + "ITERM_RELAX_OFF": "0", + "ITERM_RELAX_RP": "1", + "ITERM_RELAX_RPY": "2" + }, + "led_pin_pwm_mode_e": { + "_source": "../../../src/main/drivers/light_ws2811strip.h", + "LED_PIN_PWM_MODE_SHARED_LOW": "0", + "LED_PIN_PWM_MODE_SHARED_HIGH": "1", + "LED_PIN_PWM_MODE_LOW": "2", + "LED_PIN_PWM_MODE_HIGH": "3" + }, + "ledBaseFunctionId_e": { + "_source": "../../../src/main/io/ledstrip.h", + "LED_FUNCTION_COLOR": "0", + "LED_FUNCTION_FLIGHT_MODE": "1", + "LED_FUNCTION_ARM_STATE": "2", + "LED_FUNCTION_BATTERY": "3", + "LED_FUNCTION_RSSI": "4", + "LED_FUNCTION_GPS": "5", + "LED_FUNCTION_THRUST_RING": "6", + "LED_FUNCTION_CHANNEL": "7" + }, + "ledDirectionId_e": { + "_source": "../../../src/main/io/ledstrip.h", + "LED_DIRECTION_NORTH": "0", + "LED_DIRECTION_EAST": "1", + "LED_DIRECTION_SOUTH": "2", + "LED_DIRECTION_WEST": "3", + "LED_DIRECTION_UP": "4", + "LED_DIRECTION_DOWN": "5" + }, + "ledModeIndex_e": { + "_source": "../../../src/main/io/ledstrip.h", + "LED_MODE_ORIENTATION": "0", + "LED_MODE_HEADFREE": "1", + "LED_MODE_HORIZON": "2", + "LED_MODE_ANGLE": "3", + "LED_MODE_MAG": "4", + "LED_MODE_BARO": "5", + "LED_SPECIAL": "6" + }, + "ledOverlayId_e": { + "_source": "../../../src/main/io/ledstrip.h", + "LED_OVERLAY_THROTTLE": "0", + "LED_OVERLAY_LARSON_SCANNER": "1", + "LED_OVERLAY_BLINK": "2", + "LED_OVERLAY_LANDING_FLASH": "3", + "LED_OVERLAY_INDICATOR": "4", + "LED_OVERLAY_WARNING": "5", + "LED_OVERLAY_STROBE": "6" + }, + "ledSpecialColorIds_e": { + "_source": "../../../src/main/io/ledstrip.h", + "LED_SCOLOR_DISARMED": "0", + "LED_SCOLOR_ARMED": "1", + "LED_SCOLOR_ANIMATION": "2", + "LED_SCOLOR_BACKGROUND": "3", + "LED_SCOLOR_BLINKBACKGROUND": "4", + "LED_SCOLOR_GPSNOSATS": "5", + "LED_SCOLOR_GPSNOLOCK": "6", + "LED_SCOLOR_GPSLOCKED": "7", + "LED_SCOLOR_STROBE": "8" + }, + "logicConditionFlags_e": { + "_source": "../../../src/main/programming/logic_condition.h", + "LOGIC_CONDITION_FLAG_LATCH": "1 << 0", + "LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED": "1 << 1" + }, + "logicConditionsGlobalFlags_t": { + "_source": "../../../src/main/programming/logic_condition.h", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY": "(1 << 0)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE": "(1 << 1)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW": "(1 << 2)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL": "(1 << 3)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH": "(1 << 4)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW": "(1 << 5)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE": "(1 << 6)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT": "(1 << 7)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL": "(1 << 8)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS": "(1 << 9)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS": "(1 << 10)", + "LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX": [ + "(1 << 11)", + "USE_GPS_FIX_ESTIMATION" + ], + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED": "(1 << 12)" + }, + "logicFlightModeOperands_e": { + "_source": "../../../src/main/programming/logic_condition.h", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE": "0", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL": "1", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH": "2", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD": "3", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE": "4", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD": "5", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE": "6", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON": "7", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR": "8", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1": "9", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2": "10", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD": "11", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3": "12", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4": "13", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO": "14", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION": "15", + "LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD": "16" + }, + "logicFlightOperands_e": { + "_source": "../../../src/main/programming/logic_condition.h", + "LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER": "0", + "LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE": "1", + "LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE": "2", + "LOGIC_CONDITION_OPERAND_FLIGHT_RSSI": "3", + "LOGIC_CONDITION_OPERAND_FLIGHT_VBAT": "4", + "LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE": "5", + "LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT": "6", + "LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN": "7", + "LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS": "8", + "LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED": "9", + "LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED": "10", + "LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED": "11", + "LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE": "12", + "LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED": "13", + "LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS": "14", + "LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL": "15", + "LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH": "16", + "LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED": "17", + "LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH": "18", + "LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL": "19", + "LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL": "20", + "LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING": "21", + "LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH": "22", + "LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING": "23", + "LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE": "24", + "LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL": "25", + "LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH": "26", + "LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW": "27", + "LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE": "28", + "LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK": "29", + "LOGIC_CONDITION_OPERAND_FLIGHT_SNR": "30", + "LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID": "31", + "LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS": "32", + "LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE": "33", + "LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS": "34", + "LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS": "35", + "LOGIC_CONDITION_OPERAND_FLIGHT_AGL": "36", + "LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW": "37", + "LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE": "38", + "LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE": "39", + "LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW": "40", + "LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE": "41", + "LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE": "42", + "LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS": "43", + "LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK": "44", + "LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM": "45", + "LOGIC_CONDITION_OPERAND_FLIGHT_MIN_GROUND_SPEED": "46", + "LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED": "47", + "LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION": "48", + "LOGIC_CONDITION_OPERAND_FLIGHT_RELATIVE_WIND_OFFSET": "49" + }, + "logicOperation_e": { + "_source": "../../../src/main/programming/logic_condition.h", + "LOGIC_CONDITION_TRUE": "0", + "LOGIC_CONDITION_EQUAL": "1", + "LOGIC_CONDITION_GREATER_THAN": "2", + "LOGIC_CONDITION_LOWER_THAN": "3", + "LOGIC_CONDITION_LOW": "4", + "LOGIC_CONDITION_MID": "5", + "LOGIC_CONDITION_HIGH": "6", + "LOGIC_CONDITION_AND": "7", + "LOGIC_CONDITION_OR": "8", + "LOGIC_CONDITION_XOR": "9", + "LOGIC_CONDITION_NAND": "10", + "LOGIC_CONDITION_NOR": "11", + "LOGIC_CONDITION_NOT": "12", + "LOGIC_CONDITION_STICKY": "13", + "LOGIC_CONDITION_ADD": "14", + "LOGIC_CONDITION_SUB": "15", + "LOGIC_CONDITION_MUL": "16", + "LOGIC_CONDITION_DIV": "17", + "LOGIC_CONDITION_GVAR_SET": "18", + "LOGIC_CONDITION_GVAR_INC": "19", + "LOGIC_CONDITION_GVAR_DEC": "20", + "LOGIC_CONDITION_PORT_SET": "21", + "LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY": "22", + "LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE": "23", + "LOGIC_CONDITION_SWAP_ROLL_YAW": "24", + "LOGIC_CONDITION_SET_VTX_POWER_LEVEL": "25", + "LOGIC_CONDITION_INVERT_ROLL": "26", + "LOGIC_CONDITION_INVERT_PITCH": "27", + "LOGIC_CONDITION_INVERT_YAW": "28", + "LOGIC_CONDITION_OVERRIDE_THROTTLE": "29", + "LOGIC_CONDITION_SET_VTX_BAND": "30", + "LOGIC_CONDITION_SET_VTX_CHANNEL": "31", + "LOGIC_CONDITION_SET_OSD_LAYOUT": "32", + "LOGIC_CONDITION_SIN": "33", + "LOGIC_CONDITION_COS": "34", + "LOGIC_CONDITION_TAN": "35", + "LOGIC_CONDITION_MAP_INPUT": "36", + "LOGIC_CONDITION_MAP_OUTPUT": "37", + "LOGIC_CONDITION_RC_CHANNEL_OVERRIDE": "38", + "LOGIC_CONDITION_SET_HEADING_TARGET": "39", + "LOGIC_CONDITION_MODULUS": "40", + "LOGIC_CONDITION_LOITER_OVERRIDE": "41", + "LOGIC_CONDITION_SET_PROFILE": "42", + "LOGIC_CONDITION_MIN": "43", + "LOGIC_CONDITION_MAX": "44", + "LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE": "45", + "LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE": "46", + "LOGIC_CONDITION_EDGE": "47", + "LOGIC_CONDITION_DELAY": "48", + "LOGIC_CONDITION_TIMER": "49", + "LOGIC_CONDITION_DELTA": "50", + "LOGIC_CONDITION_APPROX_EQUAL": "51", + "LOGIC_CONDITION_LED_PIN_PWM": "52", + "LOGIC_CONDITION_DISABLE_GPS_FIX": "53", + "LOGIC_CONDITION_RESET_MAG_CALIBRATION": "54", + "LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY": "55", + "LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED": "56", + "LOGIC_CONDITION_LAST": "57" + }, + "logicWaypointOperands_e": { + "_source": "../../../src/main/programming/logic_condition.h", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP": "0", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX": "1", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION": "2", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION": "3", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE": "4", + "LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT": "5", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION": "6", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION": "7", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION": "8", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION": "9", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP": "10", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP": "11", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP": "12", + "LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP": "13" + }, + "logTopic_e": { + "_source": "../../../src/main/common/log.h", + "LOG_TOPIC_SYSTEM": "0", + "LOG_TOPIC_GYRO": "1", + "LOG_TOPIC_BARO": "2", + "LOG_TOPIC_PITOT": "3", + "LOG_TOPIC_PWM": "4", + "LOG_TOPIC_TIMER": "5", + "LOG_TOPIC_IMU": "6", + "LOG_TOPIC_TEMPERATURE": "7", + "LOG_TOPIC_POS_ESTIMATOR": "8", + "LOG_TOPIC_VTX": "9", + "LOG_TOPIC_OSD": "10", + "LOG_TOPIC_COUNT": "11" + }, + "lsm6dxxConfigMasks_e": { + "_source": "../../../src/main/drivers/accgyro/accgyro_lsm6dxx.h", + "LSM6DXX_MASK_COUNTER_BDR1": "128", + "LSM6DXX_MASK_CTRL3_C": "60", + "LSM6DXX_MASK_CTRL3_C_RESET": "BIT(0)", + "LSM6DXX_MASK_CTRL4_C": "14", + "LSM6DXX_MASK_CTRL6_C": "23", + "LSM6DXX_MASK_CTRL7_G": "112", + "LSM6DXX_MASK_CTRL9_XL": "2", + "LSM6DSL_MASK_CTRL6_C": "19" + }, + "lsm6dxxConfigValues_e": { + "_source": "../../../src/main/drivers/accgyro/accgyro_lsm6dxx.h", + "LSM6DXX_VAL_COUNTER_BDR1_DDRY_PM": "BIT(7)", + "LSM6DXX_VAL_INT1_CTRL": "2", + "LSM6DXX_VAL_INT2_CTRL": "0", + "LSM6DXX_VAL_CTRL1_XL_ODR833": "7", + "LSM6DXX_VAL_CTRL1_XL_ODR1667": "8", + "LSM6DXX_VAL_CTRL1_XL_ODR3332": "9", + "LSM6DXX_VAL_CTRL1_XL_ODR3333": "10", + "LSM6DXX_VAL_CTRL1_XL_8G": "3", + "LSM6DXX_VAL_CTRL1_XL_16G": "1", + "LSM6DXX_VAL_CTRL1_XL_LPF1": "0", + "LSM6DXX_VAL_CTRL1_XL_LPF2": "1", + "LSM6DXX_VAL_CTRL2_G_ODR6664": "10", + "LSM6DXX_VAL_CTRL2_G_2000DPS": "3", + "LSM6DXX_VAL_CTRL3_C_H_LACTIVE": "0", + "LSM6DXX_VAL_CTRL3_C_PP_OD": "0", + "LSM6DXX_VAL_CTRL3_C_SIM": "0", + "LSM6DXX_VAL_CTRL3_C_IF_INC": "BIT(2)", + "LSM6DXX_VAL_CTRL4_C_DRDY_MASK": "BIT(3)", + "LSM6DXX_VAL_CTRL4_C_I2C_DISABLE": "BIT(2)", + "LSM6DXX_VAL_CTRL4_C_LPF1_SEL_G": "BIT(1)", + "LSM6DXX_VAL_CTRL6_C_XL_HM_MODE": "0", + "LSM6DXX_VAL_CTRL6_C_FTYPE_300HZ": "0", + "LSM6DXX_VAL_CTRL6_C_FTYPE_201HZ": "1", + "LSM6DXX_VAL_CTRL6_C_FTYPE_102HZ": "2", + "LSM6DXX_VAL_CTRL6_C_FTYPE_603HZ": "3", + "LSM6DXX_VAL_CTRL7_G_HP_EN_G": "BIT(6)", + "LSM6DXX_VAL_CTRL7_G_HPM_G_16": "0", + "LSM6DXX_VAL_CTRL7_G_HPM_G_65": "1", + "LSM6DXX_VAL_CTRL7_G_HPM_G_260": "2", + "LSM6DXX_VAL_CTRL7_G_HPM_G_1040": "3", + "LSM6DXX_VAL_CTRL9_XL_I3C_DISABLE": "BIT(1)" + }, + "lsm6dxxRegister_e": { + "_source": "../../../src/main/drivers/accgyro/accgyro_lsm6dxx.h", + "LSM6DXX_REG_COUNTER_BDR1": "11", + "LSM6DXX_REG_INT1_CTRL": "13", + "LSM6DXX_REG_INT2_CTRL": "14", + "LSM6DXX_REG_WHO_AM_I": "15", + "LSM6DXX_REG_CTRL1_XL": "16", + "LSM6DXX_REG_CTRL2_G": "17", + "LSM6DXX_REG_CTRL3_C": "18", + "LSM6DXX_REG_CTRL4_C": "19", + "LSM6DXX_REG_CTRL5_C": "20", + "LSM6DXX_REG_CTRL6_C": "21", + "LSM6DXX_REG_CTRL7_G": "22", + "LSM6DXX_REG_CTRL8_XL": "23", + "LSM6DXX_REG_CTRL9_XL": "24", + "LSM6DXX_REG_CTRL10_C": "25", + "LSM6DXX_REG_STATUS": "30", + "LSM6DXX_REG_OUT_TEMP_L": "32", + "LSM6DXX_REG_OUT_TEMP_H": "33", + "LSM6DXX_REG_OUTX_L_G": "34", + "LSM6DXX_REG_OUTX_H_G": "35", + "LSM6DXX_REG_OUTY_L_G": "36", + "LSM6DXX_REG_OUTY_H_G": "37", + "LSM6DXX_REG_OUTZ_L_G": "38", + "LSM6DXX_REG_OUTZ_H_G": "39", + "LSM6DXX_REG_OUTX_L_A": "40", + "LSM6DXX_REG_OUTX_H_A": "41", + "LSM6DXX_REG_OUTY_L_A": "42", + "LSM6DXX_REG_OUTY_H_A": "43", + "LSM6DXX_REG_OUTZ_L_A": "44", + "LSM6DXX_REG_OUTZ_H_A": "45" + }, + "ltm_frame_e": { + "_source": "../../../src/main/telemetry/ltm.h", + "LTM_FRAME_START": "0", + "LTM_AFRAME": "LTM_FRAME_START", + "LTM_SFRAME": "", + "LTM_GFRAME": [ + "", + "USE_GPS" + ], + "LTM_OFRAME": [ + "", + "USE_GPS" + ], + "LTM_XFRAME": [ + "", + "USE_GPS" + ], + "LTM_NFRAME": "", + "LTM_FRAME_COUNT": "" + }, + "ltm_modes_e": { + "_source": "../../../src/main/telemetry/ltm.h", + "LTM_MODE_MANUAL": "0", + "LTM_MODE_RATE": "1", + "LTM_MODE_ANGLE": "2", + "LTM_MODE_HORIZON": "3", + "LTM_MODE_ACRO": "4", + "LTM_MODE_STABALIZED1": "5", + "LTM_MODE_STABALIZED2": "6", + "LTM_MODE_STABILIZED3": "7", + "LTM_MODE_ALTHOLD": "8", + "LTM_MODE_GPSHOLD": "9", + "LTM_MODE_WAYPOINTS": "10", + "LTM_MODE_HEADHOLD": "11", + "LTM_MODE_CIRCLE": "12", + "LTM_MODE_RTH": "13", + "LTM_MODE_FOLLOWWME": "14", + "LTM_MODE_LAND": "15", + "LTM_MODE_FLYBYWIRE1": "16", + "LTM_MODE_FLYBYWIRE2": "17", + "LTM_MODE_CRUISE": "18", + "LTM_MODE_UNKNOWN": "19", + "LTM_MODE_LAUNCH": "20", + "LTM_MODE_AUTOTUNE": "21" + }, + "ltmUpdateRate_e": { + "_source": "../../../src/main/telemetry/telemetry.h", + "LTM_RATE_NORMAL": "0", + "LTM_RATE_MEDIUM": "1", + "LTM_RATE_SLOW": "2" + }, + "magSensor_e": { + "_source": "../../../src/main/sensors/compass.h", + "MAG_NONE": "0", + "MAG_AUTODETECT": "1", + "MAG_HMC5883": "2", + "MAG_AK8975": "3", + "MAG_MAG3110": "4", + "MAG_AK8963": "5", + "MAG_IST8310": "6", + "MAG_QMC5883": "7", + "MAG_QMC5883P": "8", + "MAG_MPU9250": "9", + "MAG_IST8308": "10", + "MAG_LIS3MDL": "11", + "MAG_MSP": "12", + "MAG_RM3100": "13", + "MAG_VCM5883": "14", + "MAG_MLX90393": "15", + "MAG_FAKE": "16", + "MAG_MAX": "MAG_FAKE" + }, + "mavFrameSupportMask_e": { + "_source": "../../../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": "../../../src/main/telemetry/telemetry.h", + "MAVLINK_AUTOPILOT_GENERIC": "0", + "MAVLINK_AUTOPILOT_ARDUPILOT": "1" + }, + "mavlinkRadio_e": { + "_source": "../../../src/main/telemetry/telemetry.h", + "MAVLINK_RADIO_GENERIC": "0", + "MAVLINK_RADIO_ELRS": "1", + "MAVLINK_RADIO_SIK": "2" + }, + "measurementSteps_e": { + "_source": "../../../src/main/drivers/rangefinder/rangefinder_vl53l0x.c", + "MEASUREMENT_START": "0", + "MEASUREMENT_WAIT": "1", + "MEASUREMENT_READ": "2" + }, + "mixerProfileATRequest_e": { + "_source": "../../../src/main/flight/mixer_profile.h", + "MIXERAT_REQUEST_NONE": "0", + "MIXERAT_REQUEST_RTH": "1", + "MIXERAT_REQUEST_LAND": "2", + "MIXERAT_REQUEST_ABORT": "3" + }, + "mixerProfileATState_e": { + "_source": "../../../src/main/flight/mixer_profile.h", + "MIXERAT_PHASE_IDLE": "0", + "MIXERAT_PHASE_TRANSITION_INITIALIZE": "1", + "MIXERAT_PHASE_TRANSITIONING": "2", + "MIXERAT_PHASE_DONE": "3" + }, + "modeActivationOperator_e": { + "_source": "../../../src/main/fc/rc_modes.h", + "MODE_OPERATOR_OR": "0", + "MODE_OPERATOR_AND": "1" + }, + "motorPwmProtocolTypes_e": { + "_source": "../../../src/main/drivers/pwm_mapping.h", + "PWM_TYPE_STANDARD": "0", + "PWM_TYPE_ONESHOT125": "1", + "PWM_TYPE_MULTISHOT": "2", + "PWM_TYPE_BRUSHED": "3", + "PWM_TYPE_DSHOT150": "4", + "PWM_TYPE_DSHOT300": "5", + "PWM_TYPE_DSHOT600": "6" + }, + "motorStatus_e": { + "_source": "../../../src/main/flight/mixer.h", + "MOTOR_STOPPED_USER": "0", + "MOTOR_STOPPED_AUTO": "1", + "MOTOR_RUNNING": "2" + }, + "mpu9250CompassReadState_e": { + "_source": "../../../src/main/drivers/compass/compass_mpu9250.c", + "CHECK_STATUS": "0", + "WAITING_FOR_STATUS": "1", + "WAITING_FOR_DATA": "2" + }, + "mspFlashfsFlags_e": { + "_source": "../../../src/main/fc/fc_msp.c", + "MSP_FLASHFS_BIT_READY": "1", + "MSP_FLASHFS_BIT_SUPPORTED": "2" + }, + "mspPassthroughType_e": { + "_source": "../../../src/main/fc/fc_msp.c", + "MSP_PASSTHROUGH_SERIAL_ID": "253", + "MSP_PASSTHROUGH_SERIAL_FUNCTION_ID": "254", + "MSP_PASSTHROUGH_ESC_4WAY": "255" + }, + "mspSDCardFlags_e": { + "_source": "../../../src/main/fc/fc_msp.c", + "MSP_SDCARD_FLAG_SUPPORTTED": "1" + }, + "mspSDCardState_e": { + "_source": "../../../src/main/fc/fc_msp.c", + "MSP_SDCARD_STATE_NOT_PRESENT": "0", + "MSP_SDCARD_STATE_FATAL": "1", + "MSP_SDCARD_STATE_CARD_INIT": "2", + "MSP_SDCARD_STATE_FS_INIT": "3", + "MSP_SDCARD_STATE_READY": "4" + }, + "multi_function_e": { + "_source": "../../../src/main/fc/multifunction.h", + "MULTI_FUNC_NONE": "0", + "MULTI_FUNC_1": "1", + "MULTI_FUNC_2": "2", + "MULTI_FUNC_3": "3", + "MULTI_FUNC_4": "4", + "MULTI_FUNC_5": "5", + "MULTI_FUNC_6": "6", + "MULTI_FUNC_END": "7" + }, + "multiFunctionFlags_e": { + "_source": "../../../src/main/fc/multifunction.h", + "MF_SUSPEND_SAFEHOMES": "(1 << 0)", + "MF_SUSPEND_TRACKBACK": "(1 << 1)", + "MF_TURTLE_MODE": "(1 << 2)" + }, + "nav_reset_type_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NAV_RESET_NEVER": "0", + "NAV_RESET_ON_FIRST_ARM": "1", + "NAV_RESET_ON_EACH_ARM": "2" + }, + "navAGLEstimateQuality_e": { + "_source": "../../../src/main/navigation/navigation_pos_estimator_private.h", + "SURFACE_QUAL_LOW": "0", + "SURFACE_QUAL_MID": "1", + "SURFACE_QUAL_HIGH": "2" + }, + "navArmingBlocker_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NAV_ARMING_BLOCKER_NONE": "0", + "NAV_ARMING_BLOCKER_MISSING_GPS_FIX": "1", + "NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE": "2", + "NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR": "3", + "NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR": "4" + }, + "navDefaultAltitudeSensor_e": { + "_source": "../../../src/main/navigation/navigation_pos_estimator_private.h", + "ALTITUDE_SOURCE_GPS": "0", + "ALTITUDE_SOURCE_BARO": "1", + "ALTITUDE_SOURCE_GPS_ONLY": "2", + "ALTITUDE_SOURCE_BARO_ONLY": "3" + }, + "navExtraArmingSafety_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NAV_EXTRA_ARMING_SAFETY_ON": "0", + "NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS": "1" + }, + "navFwLaunchStatus_e": { + "_source": "../../../src/main/navigation/navigation.h", + "FW_LAUNCH_DETECTED": "5", + "FW_LAUNCH_ABORTED": "10", + "FW_LAUNCH_FLYING": "11" + }, + "navigationEstimateStatus_e": { + "_source": "../../../src/main/navigation/navigation_private.h", + "EST_NONE": "0", + "EST_USABLE": "1", + "EST_TRUSTED": "2" + }, + "navigationFSMEvent_t": { + "_source": "../../../src/main/navigation/navigation_private.h", + "NAV_FSM_EVENT_NONE": "0", + "NAV_FSM_EVENT_TIMEOUT": "1", + "NAV_FSM_EVENT_SUCCESS": "2", + "NAV_FSM_EVENT_ERROR": "3", + "NAV_FSM_EVENT_SWITCH_TO_IDLE": "4", + "NAV_FSM_EVENT_SWITCH_TO_ALTHOLD": "5", + "NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D": "6", + "NAV_FSM_EVENT_SWITCH_TO_RTH": "7", + "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT": "8", + "NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING": "9", + "NAV_FSM_EVENT_SWITCH_TO_LAUNCH": "10", + "NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD": "11", + "NAV_FSM_EVENT_SWITCH_TO_CRUISE": "12", + "NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ": "13", + "NAV_FSM_EVENT_SWITCH_TO_MIXERAT": "14", + "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING": "15", + "NAV_FSM_EVENT_SWITCH_TO_SEND_TO": "16", + "NAV_FSM_EVENT_STATE_SPECIFIC_1": "17", + "NAV_FSM_EVENT_STATE_SPECIFIC_2": "18", + "NAV_FSM_EVENT_STATE_SPECIFIC_3": "19", + "NAV_FSM_EVENT_STATE_SPECIFIC_4": "20", + "NAV_FSM_EVENT_STATE_SPECIFIC_5": "21", + "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT": "NAV_FSM_EVENT_STATE_SPECIFIC_1", + "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED": "NAV_FSM_EVENT_STATE_SPECIFIC_2", + "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME": "NAV_FSM_EVENT_STATE_SPECIFIC_1", + "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND": "NAV_FSM_EVENT_STATE_SPECIFIC_2", + "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED": "NAV_FSM_EVENT_STATE_SPECIFIC_3", + "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE": "NAV_FSM_EVENT_STATE_SPECIFIC_1", + "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK": "NAV_FSM_EVENT_STATE_SPECIFIC_2", + "NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME": "NAV_FSM_EVENT_STATE_SPECIFIC_3", + "NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME": "NAV_FSM_EVENT_STATE_SPECIFIC_4", + "NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING": "NAV_FSM_EVENT_STATE_SPECIFIC_5", + "NAV_FSM_EVENT_COUNT": "" + }, + "navigationFSMState_t": { + "_source": "../../../src/main/navigation/navigation_private.h", + "NAV_STATE_UNDEFINED": "0", + "NAV_STATE_IDLE": "1", + "NAV_STATE_ALTHOLD_INITIALIZE": "2", + "NAV_STATE_ALTHOLD_IN_PROGRESS": "3", + "NAV_STATE_POSHOLD_3D_INITIALIZE": "4", + "NAV_STATE_POSHOLD_3D_IN_PROGRESS": "5", + "NAV_STATE_RTH_INITIALIZE": "6", + "NAV_STATE_RTH_CLIMB_TO_SAFE_ALT": "7", + "NAV_STATE_RTH_TRACKBACK": "8", + "NAV_STATE_RTH_HEAD_HOME": "9", + "NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING": "10", + "NAV_STATE_RTH_LOITER_ABOVE_HOME": "11", + "NAV_STATE_RTH_LANDING": "12", + "NAV_STATE_RTH_FINISHING": "13", + "NAV_STATE_RTH_FINISHED": "14", + "NAV_STATE_WAYPOINT_INITIALIZE": "15", + "NAV_STATE_WAYPOINT_PRE_ACTION": "16", + "NAV_STATE_WAYPOINT_IN_PROGRESS": "17", + "NAV_STATE_WAYPOINT_REACHED": "18", + "NAV_STATE_WAYPOINT_HOLD_TIME": "19", + "NAV_STATE_WAYPOINT_NEXT": "20", + "NAV_STATE_WAYPOINT_FINISHED": "21", + "NAV_STATE_WAYPOINT_RTH_LAND": "22", + "NAV_STATE_EMERGENCY_LANDING_INITIALIZE": "23", + "NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS": "24", + "NAV_STATE_EMERGENCY_LANDING_FINISHED": "25", + "NAV_STATE_LAUNCH_INITIALIZE": "26", + "NAV_STATE_LAUNCH_WAIT": "27", + "NAV_STATE_LAUNCH_IN_PROGRESS": "28", + "NAV_STATE_COURSE_HOLD_INITIALIZE": "29", + "NAV_STATE_COURSE_HOLD_IN_PROGRESS": "30", + "NAV_STATE_COURSE_HOLD_ADJUSTING": "31", + "NAV_STATE_CRUISE_INITIALIZE": "32", + "NAV_STATE_CRUISE_IN_PROGRESS": "33", + "NAV_STATE_CRUISE_ADJUSTING": "34", + "NAV_STATE_FW_LANDING_CLIMB_TO_LOITER": "35", + "NAV_STATE_FW_LANDING_LOITER": "36", + "NAV_STATE_FW_LANDING_APPROACH": "37", + "NAV_STATE_FW_LANDING_GLIDE": "38", + "NAV_STATE_FW_LANDING_FLARE": "39", + "NAV_STATE_FW_LANDING_FINISHED": "40", + "NAV_STATE_FW_LANDING_ABORT": "41", + "NAV_STATE_MIXERAT_INITIALIZE": "42", + "NAV_STATE_MIXERAT_IN_PROGRESS": "43", + "NAV_STATE_MIXERAT_ABORT": "44", + "NAV_STATE_SEND_TO_INITALIZE": "45", + "NAV_STATE_SEND_TO_IN_PROGESS": "46", + "NAV_STATE_SEND_TO_FINISHED": "47", + "NAV_STATE_COUNT": "48" + }, + "navigationFSMStateFlags_t": { + "_source": "../../../src/main/navigation/navigation_private.h", + "NAV_CTL_ALT": "(1 << 0)", + "NAV_CTL_POS": "(1 << 1)", + "NAV_CTL_YAW": "(1 << 2)", + "NAV_CTL_EMERG": "(1 << 3)", + "NAV_CTL_LAUNCH": "(1 << 4)", + "NAV_REQUIRE_ANGLE": "(1 << 5)", + "NAV_REQUIRE_ANGLE_FW": "(1 << 6)", + "NAV_REQUIRE_MAGHOLD": "(1 << 7)", + "NAV_REQUIRE_THRTILT": "(1 << 8)", + "NAV_AUTO_RTH": "(1 << 9)", + "NAV_AUTO_WP": "(1 << 10)", + "NAV_RC_ALT": "(1 << 11)", + "NAV_RC_POS": "(1 << 12)", + "NAV_RC_YAW": "(1 << 13)", + "NAV_CTL_LAND": "(1 << 14)", + "NAV_AUTO_WP_DONE": "(1 << 15)", + "NAV_MIXERAT": "(1 << 16)", + "NAV_CTL_HOLD": "(1 << 17)" + }, + "navigationHomeFlags_t": { + "_source": "../../../src/main/navigation/navigation_private.h", + "NAV_HOME_INVALID": "0", + "NAV_HOME_VALID_XY": "1 << 0", + "NAV_HOME_VALID_Z": "1 << 1", + "NAV_HOME_VALID_HEADING": "1 << 2", + "NAV_HOME_VALID_ALL": "NAV_HOME_VALID_XY | NAV_HOME_VALID_Z | NAV_HOME_VALID_HEADING" + }, + "navigationPersistentId_e": { + "_source": "../../../src/main/navigation/navigation_private.h", + "NAV_PERSISTENT_ID_UNDEFINED": "0", + "NAV_PERSISTENT_ID_IDLE": "1", + "NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE": "2", + "NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS": "3", + "NAV_PERSISTENT_ID_UNUSED_1": "4", + "NAV_PERSISTENT_ID_UNUSED_2": "5", + "NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE": "6", + "NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS": "7", + "NAV_PERSISTENT_ID_RTH_INITIALIZE": "8", + "NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT": "9", + "NAV_PERSISTENT_ID_RTH_HEAD_HOME": "10", + "NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING": "11", + "NAV_PERSISTENT_ID_RTH_LANDING": "12", + "NAV_PERSISTENT_ID_RTH_FINISHING": "13", + "NAV_PERSISTENT_ID_RTH_FINISHED": "14", + "NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE": "15", + "NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION": "16", + "NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS": "17", + "NAV_PERSISTENT_ID_WAYPOINT_REACHED": "18", + "NAV_PERSISTENT_ID_WAYPOINT_NEXT": "19", + "NAV_PERSISTENT_ID_WAYPOINT_FINISHED": "20", + "NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND": "21", + "NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE": "22", + "NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS": "23", + "NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED": "24", + "NAV_PERSISTENT_ID_LAUNCH_INITIALIZE": "25", + "NAV_PERSISTENT_ID_LAUNCH_WAIT": "26", + "NAV_PERSISTENT_ID_UNUSED_3": "27", + "NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS": "28", + "NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE": "29", + "NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS": "30", + "NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING": "31", + "NAV_PERSISTENT_ID_CRUISE_INITIALIZE": "32", + "NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS": "33", + "NAV_PERSISTENT_ID_CRUISE_ADJUSTING": "34", + "NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME": "35", + "NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME": "36", + "NAV_PERSISTENT_ID_UNUSED_4": "37", + "NAV_PERSISTENT_ID_RTH_TRACKBACK": "38", + "NAV_PERSISTENT_ID_MIXERAT_INITIALIZE": "39", + "NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS": "40", + "NAV_PERSISTENT_ID_MIXERAT_ABORT": "41", + "NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER": "42", + "NAV_PERSISTENT_ID_FW_LANDING_LOITER": "43", + "NAV_PERSISTENT_ID_FW_LANDING_APPROACH": "44", + "NAV_PERSISTENT_ID_FW_LANDING_GLIDE": "45", + "NAV_PERSISTENT_ID_FW_LANDING_FLARE": "46", + "NAV_PERSISTENT_ID_FW_LANDING_ABORT": "47", + "NAV_PERSISTENT_ID_FW_LANDING_FINISHED": "48", + "NAV_PERSISTENT_ID_SEND_TO_INITALIZE": "49", + "NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES": "50", + "NAV_PERSISTENT_ID_SEND_TO_FINISHED": "51" + }, + "navMcAltHoldThrottle_e": { + "_source": "../../../src/main/navigation/navigation.h", + "MC_ALT_HOLD_STICK": "0", + "MC_ALT_HOLD_MID": "1", + "MC_ALT_HOLD_HOVER": "2" + }, + "navMissionRestart_e": { + "_source": "../../../src/main/navigation/navigation.h", + "WP_MISSION_START": "0", + "WP_MISSION_RESUME": "1", + "WP_MISSION_SWITCH": "2" + }, + "navOverridesMotorStop_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NOMS_OFF_ALWAYS": "0", + "NOMS_OFF": "1", + "NOMS_AUTO_ONLY": "2", + "NOMS_ALL_NAV": "3" + }, + "navPositionEstimationFlags_e": { + "_source": "../../../src/main/navigation/navigation_pos_estimator_private.h", + "EST_GPS_XY_VALID": "(1 << 0)", + "EST_GPS_Z_VALID": "(1 << 1)", + "EST_BARO_VALID": "(1 << 2)", + "EST_SURFACE_VALID": "(1 << 3)", + "EST_FLOW_VALID": "(1 << 4)", + "EST_XY_VALID": "(1 << 5)", + "EST_Z_VALID": "(1 << 6)" + }, + "navRTHAllowLanding_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NAV_RTH_ALLOW_LANDING_NEVER": "0", + "NAV_RTH_ALLOW_LANDING_ALWAYS": "1", + "NAV_RTH_ALLOW_LANDING_FS_ONLY": "2" + }, + "navRTHClimbFirst_e": { + "_source": "../../../src/main/navigation/navigation.h", + "RTH_CLIMB_OFF": "0", + "RTH_CLIMB_ON": "1", + "RTH_CLIMB_ON_FW_SPIRAL": "2" + }, + "navSetWaypointFlags_t": { + "_source": "../../../src/main/navigation/navigation_private.h", + "NAV_POS_UPDATE_NONE": "0", + "NAV_POS_UPDATE_Z": "1 << 1", + "NAV_POS_UPDATE_XY": "1 << 0", + "NAV_POS_UPDATE_HEADING": "1 << 2", + "NAV_POS_UPDATE_BEARING": "1 << 3", + "NAV_POS_UPDATE_BEARING_TAIL_FIRST": "1 << 4" + }, + "navSystemStatus_Error_e": { + "_source": "../../../src/main/navigation/navigation.h", + "MW_NAV_ERROR_NONE": "0", + "MW_NAV_ERROR_TOOFAR": "1", + "MW_NAV_ERROR_SPOILED_GPS": "2", + "MW_NAV_ERROR_WP_CRC": "3", + "MW_NAV_ERROR_FINISH": "4", + "MW_NAV_ERROR_TIMEWAIT": "5", + "MW_NAV_ERROR_INVALID_JUMP": "6", + "MW_NAV_ERROR_INVALID_DATA": "7", + "MW_NAV_ERROR_WAIT_FOR_RTH_ALT": "8", + "MW_NAV_ERROR_GPS_FIX_LOST": "9", + "MW_NAV_ERROR_DISARMED": "10", + "MW_NAV_ERROR_LANDING": "11" + }, + "navSystemStatus_Flags_e": { + "_source": "../../../src/main/navigation/navigation.h", + "MW_NAV_FLAG_ADJUSTING_POSITION": "1 << 0", + "MW_NAV_FLAG_ADJUSTING_ALTITUDE": "1 << 1" + }, + "navSystemStatus_Mode_e": { + "_source": "../../../src/main/navigation/navigation.h", + "MW_GPS_MODE_NONE": "0", + "MW_GPS_MODE_HOLD": "1", + "MW_GPS_MODE_RTH": "2", + "MW_GPS_MODE_NAV": "3", + "MW_GPS_MODE_EMERG": "15" + }, + "navSystemStatus_State_e": { + "_source": "../../../src/main/navigation/navigation.h", + "MW_NAV_STATE_NONE": "0", + "MW_NAV_STATE_RTH_START": "1", + "MW_NAV_STATE_RTH_ENROUTE": "2", + "MW_NAV_STATE_HOLD_INFINIT": "3", + "MW_NAV_STATE_HOLD_TIMED": "4", + "MW_NAV_STATE_WP_ENROUTE": "5", + "MW_NAV_STATE_PROCESS_NEXT": "6", + "MW_NAV_STATE_DO_JUMP": "7", + "MW_NAV_STATE_LAND_START": "8", + "MW_NAV_STATE_LAND_IN_PROGRESS": "9", + "MW_NAV_STATE_LANDED": "10", + "MW_NAV_STATE_LAND_SETTLE": "11", + "MW_NAV_STATE_LAND_START_DESCENT": "12", + "MW_NAV_STATE_HOVER_ABOVE_HOME": "13", + "MW_NAV_STATE_EMERGENCY_LANDING": "14", + "MW_NAV_STATE_RTH_CLIMB": "15" + }, + "navWaypointActions_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NAV_WP_ACTION_WAYPOINT": "1", + "NAV_WP_ACTION_HOLD_TIME": "3", + "NAV_WP_ACTION_RTH": "4", + "NAV_WP_ACTION_SET_POI": "5", + "NAV_WP_ACTION_JUMP": "6", + "NAV_WP_ACTION_SET_HEAD": "7", + "NAV_WP_ACTION_LAND": "8" + }, + "navWaypointFlags_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NAV_WP_FLAG_HOME": "72", + "NAV_WP_FLAG_LAST": "165" + }, + "navWaypointHeadings_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NAV_WP_HEAD_MODE_NONE": "0", + "NAV_WP_HEAD_MODE_POI": "1", + "NAV_WP_HEAD_MODE_FIXED": "2" + }, + "navWaypointP3Flags_e": { + "_source": "../../../src/main/navigation/navigation.h", + "NAV_WP_ALTMODE": "(1<<0)", + "NAV_WP_USER1": "(1<<1)", + "NAV_WP_USER2": "(1<<2)", + "NAV_WP_USER3": "(1<<3)", + "NAV_WP_USER4": "(1<<4)" + }, + "opflowQuality_e": { + "_source": "../../../src/main/sensors/opflow.h", + "OPFLOW_QUALITY_INVALID": "0", + "OPFLOW_QUALITY_VALID": "1" + }, + "opticalFlowSensor_e": { + "_source": "../../../src/main/sensors/opflow.h", + "OPFLOW_NONE": "0", + "OPFLOW_CXOF": "1", + "OPFLOW_MSP": "2", + "OPFLOW_FAKE": "3" + }, + "osd_adsb_warning_style_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_ADSB_WARNING_STYLE_COMPACT": "0", + "OSD_ADSB_WARNING_STYLE_EXTENDED": "1" + }, + "osd_ahi_style_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_AHI_STYLE_DEFAULT": "0", + "OSD_AHI_STYLE_LINE": "1" + }, + "osd_alignment_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_ALIGN_LEFT": "0", + "OSD_ALIGN_RIGHT": "1" + }, + "osd_crosshairs_style_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_CROSSHAIRS_STYLE_DEFAULT": "0", + "OSD_CROSSHAIRS_STYLE_AIRCRAFT": "1", + "OSD_CROSSHAIRS_STYLE_TYPE3": "2", + "OSD_CROSSHAIRS_STYLE_TYPE4": "3", + "OSD_CROSSHAIRS_STYLE_TYPE5": "4", + "OSD_CROSSHAIRS_STYLE_TYPE6": "5", + "OSD_CROSSHAIRS_STYLE_TYPE7": "6" + }, + "osd_crsf_lq_format_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_CRSF_LQ_TYPE1": "0", + "OSD_CRSF_LQ_TYPE2": "1", + "OSD_CRSF_LQ_TYPE3": "2" + }, + "osd_items_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_RSSI_VALUE": "0", + "OSD_MAIN_BATT_VOLTAGE": "1", + "OSD_CROSSHAIRS": "2", + "OSD_ARTIFICIAL_HORIZON": "3", + "OSD_HORIZON_SIDEBARS": "4", + "OSD_ONTIME": "5", + "OSD_FLYTIME": "6", + "OSD_FLYMODE": "7", + "OSD_CRAFT_NAME": "8", + "OSD_THROTTLE_POS": "9", + "OSD_VTX_CHANNEL": "10", + "OSD_CURRENT_DRAW": "11", + "OSD_MAH_DRAWN": "12", + "OSD_GPS_SPEED": "13", + "OSD_GPS_SATS": "14", + "OSD_ALTITUDE": "15", + "OSD_ROLL_PIDS": "16", + "OSD_PITCH_PIDS": "17", + "OSD_YAW_PIDS": "18", + "OSD_POWER": "19", + "OSD_GPS_LON": "20", + "OSD_GPS_LAT": "21", + "OSD_HOME_DIR": "22", + "OSD_HOME_DIST": "23", + "OSD_HEADING": "24", + "OSD_VARIO": "25", + "OSD_VERTICAL_SPEED_INDICATOR": "26", + "OSD_AIR_SPEED": "27", + "OSD_ONTIME_FLYTIME": "28", + "OSD_RTC_TIME": "29", + "OSD_MESSAGES": "30", + "OSD_GPS_HDOP": "31", + "OSD_MAIN_BATT_CELL_VOLTAGE": "32", + "OSD_SCALED_THROTTLE_POS": "33", + "OSD_HEADING_GRAPH": "34", + "OSD_EFFICIENCY_MAH_PER_KM": "35", + "OSD_WH_DRAWN": "36", + "OSD_BATTERY_REMAINING_CAPACITY": "37", + "OSD_BATTERY_REMAINING_PERCENT": "38", + "OSD_EFFICIENCY_WH_PER_KM": "39", + "OSD_TRIP_DIST": "40", + "OSD_ATTITUDE_PITCH": "41", + "OSD_ATTITUDE_ROLL": "42", + "OSD_MAP_NORTH": "43", + "OSD_MAP_TAKEOFF": "44", + "OSD_RADAR": "45", + "OSD_WIND_SPEED_HORIZONTAL": "46", + "OSD_WIND_SPEED_VERTICAL": "47", + "OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH": "48", + "OSD_REMAINING_DISTANCE_BEFORE_RTH": "49", + "OSD_HOME_HEADING_ERROR": "50", + "OSD_COURSE_HOLD_ERROR": "51", + "OSD_COURSE_HOLD_ADJUSTMENT": "52", + "OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE": "53", + "OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE": "54", + "OSD_POWER_SUPPLY_IMPEDANCE": "55", + "OSD_LEVEL_PIDS": "56", + "OSD_POS_XY_PIDS": "57", + "OSD_POS_Z_PIDS": "58", + "OSD_VEL_XY_PIDS": "59", + "OSD_VEL_Z_PIDS": "60", + "OSD_HEADING_P": "61", + "OSD_BOARD_ALIGN_ROLL": "62", + "OSD_BOARD_ALIGN_PITCH": "63", + "OSD_RC_EXPO": "64", + "OSD_RC_YAW_EXPO": "65", + "OSD_THROTTLE_EXPO": "66", + "OSD_PITCH_RATE": "67", + "OSD_ROLL_RATE": "68", + "OSD_YAW_RATE": "69", + "OSD_MANUAL_RC_EXPO": "70", + "OSD_MANUAL_RC_YAW_EXPO": "71", + "OSD_MANUAL_PITCH_RATE": "72", + "OSD_MANUAL_ROLL_RATE": "73", + "OSD_MANUAL_YAW_RATE": "74", + "OSD_NAV_FW_CRUISE_THR": "75", + "OSD_NAV_FW_PITCH2THR": "76", + "OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE": "77", + "OSD_DEBUG": "78", + "OSD_FW_ALT_PID_OUTPUTS": "79", + "OSD_FW_POS_PID_OUTPUTS": "80", + "OSD_MC_VEL_X_PID_OUTPUTS": "81", + "OSD_MC_VEL_Y_PID_OUTPUTS": "82", + "OSD_MC_VEL_Z_PID_OUTPUTS": "83", + "OSD_MC_POS_XYZ_P_OUTPUTS": "84", + "OSD_3D_SPEED": "85", + "OSD_IMU_TEMPERATURE": "86", + "OSD_BARO_TEMPERATURE": "87", + "OSD_TEMP_SENSOR_0_TEMPERATURE": "88", + "OSD_TEMP_SENSOR_1_TEMPERATURE": "89", + "OSD_TEMP_SENSOR_2_TEMPERATURE": "90", + "OSD_TEMP_SENSOR_3_TEMPERATURE": "91", + "OSD_TEMP_SENSOR_4_TEMPERATURE": "92", + "OSD_TEMP_SENSOR_5_TEMPERATURE": "93", + "OSD_TEMP_SENSOR_6_TEMPERATURE": "94", + "OSD_TEMP_SENSOR_7_TEMPERATURE": "95", + "OSD_ALTITUDE_MSL": "96", + "OSD_PLUS_CODE": "97", + "OSD_MAP_SCALE": "98", + "OSD_MAP_REFERENCE": "99", + "OSD_GFORCE": "100", + "OSD_GFORCE_X": "101", + "OSD_GFORCE_Y": "102", + "OSD_GFORCE_Z": "103", + "OSD_RC_SOURCE": "104", + "OSD_VTX_POWER": "105", + "OSD_ESC_RPM": "106", + "OSD_ESC_TEMPERATURE": "107", + "OSD_AZIMUTH": "108", + "OSD_RSSI_DBM": "109", + "OSD_LQ_UPLINK": "110", + "OSD_SNR_DB": "111", + "OSD_TX_POWER_UPLINK": "112", + "OSD_GVAR_0": "113", + "OSD_GVAR_1": "114", + "OSD_GVAR_2": "115", + "OSD_GVAR_3": "116", + "OSD_TPA": "117", + "OSD_NAV_FW_CONTROL_SMOOTHNESS": "118", + "OSD_VERSION": "119", + "OSD_RANGEFINDER": "120", + "OSD_PLIMIT_REMAINING_BURST_TIME": "121", + "OSD_PLIMIT_ACTIVE_CURRENT_LIMIT": "122", + "OSD_PLIMIT_ACTIVE_POWER_LIMIT": "123", + "OSD_GLIDESLOPE": "124", + "OSD_GPS_MAX_SPEED": "125", + "OSD_3D_MAX_SPEED": "126", + "OSD_AIR_MAX_SPEED": "127", + "OSD_ACTIVE_PROFILE": "128", + "OSD_MISSION": "129", + "OSD_SWITCH_INDICATOR_0": "130", + "OSD_SWITCH_INDICATOR_1": "131", + "OSD_SWITCH_INDICATOR_2": "132", + "OSD_SWITCH_INDICATOR_3": "133", + "OSD_TPA_TIME_CONSTANT": "134", + "OSD_FW_LEVEL_TRIM": "135", + "OSD_GLIDE_TIME_REMAINING": "136", + "OSD_GLIDE_RANGE": "137", + "OSD_CLIMB_EFFICIENCY": "138", + "OSD_NAV_WP_MULTI_MISSION_INDEX": "139", + "OSD_GROUND_COURSE": "140", + "OSD_CROSS_TRACK_ERROR": "141", + "OSD_PILOT_NAME": "142", + "OSD_PAN_SERVO_CENTRED": "143", + "OSD_MULTI_FUNCTION": "144", + "OSD_ODOMETER": "145", + "OSD_PILOT_LOGO": "146", + "OSD_CUSTOM_ELEMENT_1": "147", + "OSD_CUSTOM_ELEMENT_2": "148", + "OSD_CUSTOM_ELEMENT_3": "149", + "OSD_ADSB_WARNING": "150", + "OSD_ADSB_INFO": "151", + "OSD_BLACKBOX": "152", + "OSD_FORMATION_FLIGHT": "153", + "OSD_CUSTOM_ELEMENT_4": "154", + "OSD_CUSTOM_ELEMENT_5": "155", + "OSD_CUSTOM_ELEMENT_6": "156", + "OSD_CUSTOM_ELEMENT_7": "157", + "OSD_CUSTOM_ELEMENT_8": "158", + "OSD_LQ_DOWNLINK": "159", + "OSD_RX_POWER_DOWNLINK": "160", + "OSD_RX_BAND": "161", + "OSD_RX_MODE": "162", + "OSD_COURSE_TO_FENCE": "163", + "OSD_H_DIST_TO_FENCE": "164", + "OSD_V_DIST_TO_FENCE": "165", + "OSD_NAV_FW_ALT_CONTROL_RESPONSE": "166", + "OSD_NAV_MIN_GROUND_SPEED": "167", + "OSD_THROTTLE_GAUGE": "168", + "OSD_ITEM_COUNT": "169" + }, + "osd_sidebar_arrow_e": { + "_source": "../../../src/main/io/osd_grid.c", + "OSD_SIDEBAR_ARROW_NONE": "0", + "OSD_SIDEBAR_ARROW_UP": "1", + "OSD_SIDEBAR_ARROW_DOWN": "2" + }, + "osd_sidebar_scroll_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_SIDEBAR_SCROLL_NONE": "0", + "OSD_SIDEBAR_SCROLL_ALTITUDE": "1", + "OSD_SIDEBAR_SCROLL_SPEED": "2", + "OSD_SIDEBAR_SCROLL_HOME_DISTANCE": "3", + "OSD_SIDEBAR_SCROLL_MAX": "OSD_SIDEBAR_SCROLL_HOME_DISTANCE" + }, + "osd_SpeedTypes_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_SPEED_TYPE_GROUND": "0", + "OSD_SPEED_TYPE_AIR": "1", + "OSD_SPEED_TYPE_3D": "2", + "OSD_SPEED_TYPE_MIN_GROUND": "3" + }, + "osd_stats_energy_unit_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_STATS_ENERGY_UNIT_MAH": "0", + "OSD_STATS_ENERGY_UNIT_WH": "1" + }, + "osd_unit_e": { + "_source": "../../../src/main/io/osd.h", + "OSD_UNIT_IMPERIAL": "0", + "OSD_UNIT_METRIC": "1", + "OSD_UNIT_METRIC_MPH": "2", + "OSD_UNIT_UK": "3", + "OSD_UNIT_GA": "4", + "OSD_UNIT_MAX": "OSD_UNIT_GA" + }, + "osdCustomElementType_e": { + "_source": "../../../src/main/io/osd/custom_elements.h", + "CUSTOM_ELEMENT_TYPE_NONE": "0", + "CUSTOM_ELEMENT_TYPE_TEXT": "1", + "CUSTOM_ELEMENT_TYPE_ICON_STATIC": "2", + "CUSTOM_ELEMENT_TYPE_ICON_GV": "3", + "CUSTOM_ELEMENT_TYPE_ICON_LC": "4", + "CUSTOM_ELEMENT_TYPE_GV_1": "5", + "CUSTOM_ELEMENT_TYPE_GV_2": "6", + "CUSTOM_ELEMENT_TYPE_GV_3": "7", + "CUSTOM_ELEMENT_TYPE_GV_4": "8", + "CUSTOM_ELEMENT_TYPE_GV_5": "9", + "CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1": "10", + "CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_2": "11", + "CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1": "12", + "CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2": "13", + "CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1": "14", + "CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2": "15", + "CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1": "16", + "CUSTOM_ELEMENT_TYPE_LC_1": "17", + "CUSTOM_ELEMENT_TYPE_LC_2": "18", + "CUSTOM_ELEMENT_TYPE_LC_3": "19", + "CUSTOM_ELEMENT_TYPE_LC_4": "20", + "CUSTOM_ELEMENT_TYPE_LC_5": "21", + "CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1": "22", + "CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_2": "23", + "CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1": "24", + "CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2": "25", + "CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1": "26", + "CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2": "27", + "CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1": "28", + "CUSTOM_ELEMENT_TYPE_END": "29" + }, + "osdCustomElementTypeVisibility_e": { + "_source": "../../../src/main/io/osd/custom_elements.h", + "CUSTOM_ELEMENT_VISIBILITY_ALWAYS": "0", + "CUSTOM_ELEMENT_VISIBILITY_GV": "1", + "CUSTOM_ELEMENT_VISIBILITY_LOGIC_CON": "2" + }, + "osdDrawPointType_e": { + "_source": "../../../src/main/io/osd_common.h", + "OSD_DRAW_POINT_TYPE_GRID": "0", + "OSD_DRAW_POINT_TYPE_PIXEL": "1" + }, + "osdDriver_e": { + "_source": "../../../src/main/drivers/osd.h", + "OSD_DRIVER_NONE": "0", + "OSD_DRIVER_MAX7456": "1" + }, + "osdSpeedSource_e": { + "_source": "../../../src/main/io/osd_common.h", + "OSD_SPEED_SOURCE_GROUND": "0", + "OSD_SPEED_SOURCE_3D": "1", + "OSD_SPEED_SOURCE_AIR": "2" + }, + "outputMode_e": { + "_source": "../../../src/main/flight/mixer.h", + "OUTPUT_MODE_AUTO": "0", + "OUTPUT_MODE_MOTORS": "1", + "OUTPUT_MODE_SERVOS": "2", + "OUTPUT_MODE_LED": "3" + }, + "pageId_e": { + "_source": "../../../src/main/io/dashboard.h", + "PAGE_WELCOME": "0", + "PAGE_ARMED": "1", + "PAGE_STATUS": "2" + }, + "persistentObjectId_e": { + "_source": "../../../src/main/drivers/persistent.h", + "PERSISTENT_OBJECT_MAGIC": "0", + "PERSISTENT_OBJECT_RESET_REASON": "1", + "PERSISTENT_OBJECT_COUNT": "2" + }, + "pidAutotuneState_e": { + "_source": "../../../src/main/flight/pid_autotune.c", + "DEMAND_TOO_LOW": "0", + "DEMAND_UNDERSHOOT": "1", + "DEMAND_OVERSHOOT": "2", + "TUNE_UPDATED": "3" + }, + "pidControllerFlags_e": { + "_source": "../../../src/main/common/fp_pid.h", + "PID_DTERM_FROM_ERROR": "1 << 0", + "PID_ZERO_INTEGRATOR": "1 << 1", + "PID_SHRINK_INTEGRATOR": "1 << 2", + "PID_LIMIT_INTEGRATOR": "1 << 3", + "PID_FREEZE_INTEGRATOR": "1 << 4" + }, + "pidIndex_e": { + "_source": "../../../src/main/flight/pid.h", + "PID_ROLL": "0", + "PID_PITCH": "1", + "PID_YAW": "2", + "PID_POS_Z": "3", + "PID_POS_XY": "4", + "PID_VEL_XY": "5", + "PID_SURFACE": "6", + "PID_LEVEL": "7", + "PID_HEADING": "8", + "PID_VEL_Z": "9", + "PID_POS_HEADING": "10", + "PID_ITEM_COUNT": "11" + }, + "pidType_e": { + "_source": "../../../src/main/flight/pid.h", + "PID_TYPE_NONE": "0", + "PID_TYPE_PID": "1", + "PID_TYPE_PIFF": "2", + "PID_TYPE_AUTO": "3" + }, + "pinLabel_e": { + "_source": "../../../src/main/drivers/pwm_mapping.h", + "PIN_LABEL_NONE": "0", + "PIN_LABEL_LED": "1" + }, + "pitotSensor_e": { + "_source": "../../../src/main/sensors/pitotmeter.h", + "PITOT_NONE": "0", + "PITOT_AUTODETECT": "1", + "PITOT_MS4525": "2", + "PITOT_ADC": "3", + "PITOT_VIRTUAL": "4", + "PITOT_FAKE": "5", + "PITOT_MSP": "6", + "PITOT_DLVR": "7" + }, + "pollType_e": { + "_source": "../../../src/main/io/smartport_master.c", + "PT_ACTIVE_ID": "0", + "PT_INACTIVE_ID": "1" + }, + "portSharing_e": { + "_source": "../../../src/main/io/serial.h", + "PORTSHARING_UNUSED": "0", + "PORTSHARING_NOT_SHARED": "1", + "PORTSHARING_SHARED": "2" + }, + "pwmInitError_e": { + "_source": "../../../src/main/drivers/pwm_mapping.h", + "PWM_INIT_ERROR_NONE": "0", + "PWM_INIT_ERROR_TOO_MANY_MOTORS": "1", + "PWM_INIT_ERROR_TOO_MANY_SERVOS": "2", + "PWM_INIT_ERROR_NOT_ENOUGH_MOTOR_OUTPUTS": "3", + "PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS": "4", + "PWM_INIT_ERROR_TIMER_INIT_FAILED": "5" + }, + "quadrant_e": { + "_source": "../../../src/main/io/ledstrip.c", + "QUADRANT_NORTH": "1 << 0", + "QUADRANT_SOUTH": "1 << 1", + "QUADRANT_EAST": "1 << 2", + "QUADRANT_WEST": "1 << 3", + "QUADRANT_NORTH_EAST": "1 << 4", + "QUADRANT_SOUTH_EAST": "1 << 5", + "QUADRANT_NORTH_WEST": "1 << 6", + "QUADRANT_SOUTH_WEST": "1 << 7", + "QUADRANT_NONE": "1 << 8", + "QUADRANT_NOTDIAG": "1 << 9", + "QUADRANT_ANY": "QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST | QUADRANT_NONE" + }, + "QUADSPIClockDivider_e": { + "_source": "../../../src/main/drivers/bus_quadspi.h", + "QUADSPI_CLOCK_INITIALISATION": "255", + "QUADSPI_CLOCK_SLOW": "19", + "QUADSPI_CLOCK_STANDARD": "9", + "QUADSPI_CLOCK_FAST": "3", + "QUADSPI_CLOCK_ULTRAFAST": "1" + }, + "quadSpiMode_e": { + "_source": "../../../src/main/drivers/bus_quadspi.h", + "QUADSPI_MODE_BK1_ONLY": "0", + "QUADSPI_MODE_BK2_ONLY": "1", + "QUADSPI_MODE_DUAL_FLASH": "2" + }, + "rangefinderType_e": { + "_source": "../../../src/main/sensors/rangefinder.h", + "RANGEFINDER_NONE": "0", + "RANGEFINDER_SRF10": "1", + "RANGEFINDER_VL53L0X": "2", + "RANGEFINDER_MSP": "3", + "RANGEFINDER_BENEWAKE": "4", + "RANGEFINDER_VL53L1X": "5", + "RANGEFINDER_US42": "6", + "RANGEFINDER_TOF10102I2C": "7", + "RANGEFINDER_FAKE": "8", + "RANGEFINDER_TERARANGER_EVO": "9", + "RANGEFINDER_USD1_V0": "10", + "RANGEFINDER_NANORADAR": "11" + }, + "RCDEVICE_5key_connection_event_e": { + "_source": "../../../src/main/io/rcdevice.h", + "RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN": "1", + "RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE": "2" + }, + "rcdevice_5key_simulation_operation_e": { + "_source": "../../../src/main/io/rcdevice.h", + "RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE": "0", + "RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET": "1", + "RCDEVICE_PROTOCOL_5KEY_SIMULATION_LEFT": "2", + "RCDEVICE_PROTOCOL_5KEY_SIMULATION_RIGHT": "3", + "RCDEVICE_PROTOCOL_5KEY_SIMULATION_UP": "4", + "RCDEVICE_PROTOCOL_5KEY_SIMULATION_DOWN": "5" + }, + "rcdevice_camera_control_opeation_e": { + "_source": "../../../src/main/io/rcdevice.h", + "RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN": "0", + "RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN": "1", + "RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE": "2", + "RCDEVICE_PROTOCOL_CAM_CTRL_START_RECORDING": "3", + "RCDEVICE_PROTOCOL_CAM_CTRL_STOP_RECORDING": "4", + "RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION": "255" + }, + "rcdevice_features_e": { + "_source": "../../../src/main/io/rcdevice.h", + "RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON": "(1 << 0)", + "RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON": "(1 << 1)", + "RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE": "(1 << 2)", + "RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE": "(1 << 3)", + "RCDEVICE_PROTOCOL_FEATURE_START_RECORDING": "(1 << 6)", + "RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING": "(1 << 7)", + "RCDEVICE_PROTOCOL_FEATURE_CMS_MENU": "(1 << 8)" + }, + "rcdevice_protocol_version_e": { + "_source": "../../../src/main/io/rcdevice.h", + "RCDEVICE_PROTOCOL_RCSPLIT_VERSION": "0", + "RCDEVICE_PROTOCOL_VERSION_1_0": "1", + "RCDEVICE_PROTOCOL_UNKNOWN": "2" + }, + "rcdeviceCamSimulationKeyEvent_e": { + "_source": "../../../src/main/io/rcdevice.h", + "RCDEVICE_CAM_KEY_NONE": "0", + "RCDEVICE_CAM_KEY_ENTER": "1", + "RCDEVICE_CAM_KEY_LEFT": "2", + "RCDEVICE_CAM_KEY_UP": "3", + "RCDEVICE_CAM_KEY_RIGHT": "4", + "RCDEVICE_CAM_KEY_DOWN": "5", + "RCDEVICE_CAM_KEY_CONNECTION_CLOSE": "6", + "RCDEVICE_CAM_KEY_CONNECTION_OPEN": "7", + "RCDEVICE_CAM_KEY_RELEASE": "8" + }, + "rcdeviceResponseStatus_e": { + "_source": "../../../src/main/io/rcdevice.h", + "RCDEVICE_RESP_SUCCESS": "0", + "RCDEVICE_RESP_INCORRECT_CRC": "1", + "RCDEVICE_RESP_TIMEOUT": "2" + }, + "resolutionType_e": { + "_source": "../../../src/main/io/displayport_msp_osd.c", + "SD_3016": "0", + "HD_5018": "1", + "HD_3016": "2", + "HD_6022": "3", + "HD_5320": "4" + }, + "resourceOwner_e": { + "_source": "../../../src/main/drivers/resource.h", + "OWNER_FREE": "0", + "OWNER_PWMIO": "1", + "OWNER_MOTOR": "2", + "OWNER_SERVO": "3", + "OWNER_SOFTSERIAL": "4", + "OWNER_ADC": "5", + "OWNER_SERIAL": "6", + "OWNER_TIMER": "7", + "OWNER_RANGEFINDER": "8", + "OWNER_SYSTEM": "9", + "OWNER_SPI": "10", + "OWNER_QUADSPI": "11", + "OWNER_I2C": "12", + "OWNER_SDCARD": "13", + "OWNER_FLASH": "14", + "OWNER_USB": "15", + "OWNER_BEEPER": "16", + "OWNER_OSD": "17", + "OWNER_BARO": "18", + "OWNER_MPU": "19", + "OWNER_INVERTER": "20", + "OWNER_LED_STRIP": "21", + "OWNER_LED": "22", + "OWNER_RX": "23", + "OWNER_TX": "24", + "OWNER_VTX": "25", + "OWNER_SPI_PREINIT": "26", + "OWNER_COMPASS": "27", + "OWNER_TEMPERATURE": "28", + "OWNER_1WIRE": "29", + "OWNER_AIRSPEED": "30", + "OWNER_OLED_DISPLAY": "31", + "OWNER_PINIO": "32", + "OWNER_IRLOCK": "33", + "OWNER_TOTAL_COUNT": "34" + }, + "resourceType_e": { + "_source": "../../../src/main/drivers/resource.h", + "RESOURCE_NONE": "0", + "RESOURCE_INPUT": "1", + "RESOURCE_TIMER": "2", + "RESOURCE_UART_TX": "3", + "RESOURCE_EXTI": "4", + "RESOURCE_I2C_SCL": "5", + "RESOURCE_SPI_SCK": "6", + "RESOURCE_QUADSPI_CLK": "7", + "RESOURCE_QUADSPI_BK1IO2": "8", + "RESOURCE_QUADSPI_BK2IO0": "9", + "RESOURCE_QUADSPI_BK2IO3": "10", + "RESOURCE_ADC_CH1": "11", + "RESOURCE_RX_CE": "12", + "RESOURCE_TOTAL_COUNT": "13" + }, + "reversibleMotorsThrottleState_e": { + "_source": "../../../src/main/flight/mixer.h", + "MOTOR_DIRECTION_FORWARD": "0", + "MOTOR_DIRECTION_BACKWARD": "1", + "MOTOR_DIRECTION_DEADBAND": "2" + }, + "rollPitchStatus_e": { + "_source": "../../../src/main/fc/rc_controls.h", + "NOT_CENTERED": "0", + "CENTERED": "1" + }, + "rssiSource_e": { + "_source": "../../../src/main/rx/rx.h", + "RSSI_SOURCE_NONE": "0", + "RSSI_SOURCE_AUTO": "1", + "RSSI_SOURCE_ADC": "2", + "RSSI_SOURCE_RX_CHANNEL": "3", + "RSSI_SOURCE_RX_PROTOCOL": "4", + "RSSI_SOURCE_MSP": "5" + }, + "rthState_e": { + "_source": "../../../src/main/flight/failsafe.h", + "RTH_IDLE": "0", + "RTH_IN_PROGRESS": "1", + "RTH_HAS_LANDED": "2" + }, + "rthTargetMode_e": { + "_source": "../../../src/main/navigation/navigation_private.h", + "RTH_HOME_ENROUTE_INITIAL": "0", + "RTH_HOME_ENROUTE_PROPORTIONAL": "1", + "RTH_HOME_ENROUTE_FINAL": "2", + "RTH_HOME_FINAL_LOITER": "3", + "RTH_HOME_FINAL_LAND": "4" + }, + "rthTrackbackMode_e": { + "_source": "../../../src/main/navigation/navigation.h", + "RTH_TRACKBACK_OFF": "0", + "RTH_TRACKBACK_ON": "1", + "RTH_TRACKBACK_FS": "2" + }, + "rxFrameState_e": { + "_source": "../../../src/main/rx/rx.h", + "RX_FRAME_PENDING": "0", + "RX_FRAME_COMPLETE": "(1 << 0)", + "RX_FRAME_FAILSAFE": "(1 << 1)", + "RX_FRAME_PROCESSING_REQUIRED": "(1 << 2)", + "RX_FRAME_DROPPED": "(1 << 3)" + }, + "rxReceiverType_e": { + "_source": "../../../src/main/rx/rx.h", + "RX_TYPE_NONE": "0", + "RX_TYPE_SERIAL": "1", + "RX_TYPE_MSP": "2", + "RX_TYPE_SIM": "3" + }, + "rxSerialReceiverType_e": { + "_source": "../../../src/main/rx/rx.h", + "SERIALRX_SPEKTRUM1024": "0", + "SERIALRX_SPEKTRUM2048": "1", + "SERIALRX_SBUS": "2", + "SERIALRX_SUMD": "3", + "SERIALRX_IBUS": "4", + "SERIALRX_JETIEXBUS": "5", + "SERIALRX_CRSF": "6", + "SERIALRX_FPORT": "7", + "SERIALRX_SBUS_FAST": "8", + "SERIALRX_FPORT2": "9", + "SERIALRX_SRXL2": "10", + "SERIALRX_GHST": "11", + "SERIALRX_MAVLINK": "12", + "SERIALRX_FBUS": "13", + "SERIALRX_SBUS2": "14" + }, + "safehomeUsageMode_e": { + "_source": "../../../src/main/navigation/navigation.h", + "SAFEHOME_USAGE_OFF": "0", + "SAFEHOME_USAGE_RTH": "1", + "SAFEHOME_USAGE_RTH_FS": "2" + }, + "sbasMode_e": { + "_source": "../../../src/main/io/gps.h", + "SBAS_AUTO": "0", + "SBAS_EGNOS": "1", + "SBAS_WAAS": "2", + "SBAS_MSAS": "3", + "SBAS_GAGAN": "4", + "SBAS_SPAN": "5", + "SBAS_NONE": "6" + }, + "sbusDecoderState_e": { + "_source": "../../../src/main/rx/sbus.c", + "STATE_SBUS_SYNC": "0", + "STATE_SBUS_PAYLOAD": "1", + "STATE_SBUS26_PAYLOAD": "2", + "STATE_SBUS_WAIT_SYNC": "3" + }, + "sdcardBlockOperation_e": { + "_source": "../../../src/main/drivers/sdcard/sdcard.h", + "SDCARD_BLOCK_OPERATION_READ": "0", + "SDCARD_BLOCK_OPERATION_WRITE": "1", + "SDCARD_BLOCK_OPERATION_ERASE": "2" + }, + "sdcardOperationStatus_e": { + "_source": "../../../src/main/drivers/sdcard/sdcard.h", + "SDCARD_OPERATION_IN_PROGRESS": "0", + "SDCARD_OPERATION_BUSY": "1", + "SDCARD_OPERATION_SUCCESS": "2", + "SDCARD_OPERATION_FAILURE": "3" + }, + "sdcardReceiveBlockStatus_e": { + "_source": "../../../src/main/drivers/sdcard/sdcard_spi.c", + "SDCARD_RECEIVE_SUCCESS": "0", + "SDCARD_RECEIVE_BLOCK_IN_PROGRESS": "1", + "SDCARD_RECEIVE_ERROR": "2" + }, + "sdcardState_e": { + "_source": "../../../src/main/drivers/sdcard/sdcard_impl.h", + "SDCARD_STATE_NOT_PRESENT": "0", + "SDCARD_STATE_RESET": "1", + "SDCARD_STATE_CARD_INIT_IN_PROGRESS": "2", + "SDCARD_STATE_INITIALIZATION_RECEIVE_CID": "3", + "SDCARD_STATE_READY": "4", + "SDCARD_STATE_READING": "5", + "SDCARD_STATE_SENDING_WRITE": "6", + "SDCARD_STATE_WAITING_FOR_WRITE": "7", + "SDCARD_STATE_WRITING_MULTIPLE_BLOCKS": "8", + "SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE": "9" + }, + "SDIODevice": { + "_source": "../../../src/main/drivers/sdio.h", + "SDIOINVALID": "-1", + "SDIODEV_1": "0", + "SDIODEV_2": "1" + }, + "sensor_align_e": { + "_source": "../../../src/main/drivers/sensor.h", + "ALIGN_DEFAULT": "0", + "CW0_DEG": "1", + "CW90_DEG": "2", + "CW180_DEG": "3", + "CW270_DEG": "4", + "CW0_DEG_FLIP": "5", + "CW90_DEG_FLIP": "6", + "CW180_DEG_FLIP": "7", + "CW270_DEG_FLIP": "8" + }, + "sensorIndex_e": { + "_source": "../../../src/main/sensors/sensors.h", + "SENSOR_INDEX_GYRO": "0", + "SENSOR_INDEX_ACC": "1", + "SENSOR_INDEX_BARO": "2", + "SENSOR_INDEX_MAG": "3", + "SENSOR_INDEX_RANGEFINDER": "4", + "SENSOR_INDEX_PITOT": "5", + "SENSOR_INDEX_OPFLOW": "6", + "SENSOR_INDEX_COUNT": "7" + }, + "sensors_e": { + "_source": "../../../src/main/sensors/sensors.h", + "SENSOR_GYRO": "1 << 0", + "SENSOR_ACC": "1 << 1", + "SENSOR_BARO": "1 << 2", + "SENSOR_MAG": "1 << 3", + "SENSOR_RANGEFINDER": "1 << 4", + "SENSOR_PITOT": "1 << 5", + "SENSOR_OPFLOW": "1 << 6", + "SENSOR_GPS": "1 << 7", + "SENSOR_GPSMAG": "1 << 8", + "SENSOR_TEMP": "1 << 9" + }, + "sensorTempCalState_e": { + "_source": "../../../src/main/sensors/sensors.h", + "SENSOR_TEMP_CAL_INITIALISE": "0", + "SENSOR_TEMP_CAL_IN_PROGRESS": "1", + "SENSOR_TEMP_CAL_COMPLETE": "2" + }, + "serialPortFunction_e": { + "_source": "../../../src/main/io/serial.h", + "FUNCTION_NONE": "0", + "FUNCTION_MSP": "(1 << 0)", + "FUNCTION_GPS": "(1 << 1)", + "FUNCTION_UNUSED_3": "(1 << 2)", + "FUNCTION_TELEMETRY_HOTT": "(1 << 3)", + "FUNCTION_TELEMETRY_LTM": "(1 << 4)", + "FUNCTION_TELEMETRY_SMARTPORT": "(1 << 5)", + "FUNCTION_RX_SERIAL": "(1 << 6)", + "FUNCTION_BLACKBOX": "(1 << 7)", + "FUNCTION_TELEMETRY_MAVLINK": "(1 << 8)", + "FUNCTION_TELEMETRY_IBUS": "(1 << 9)", + "FUNCTION_RCDEVICE": "(1 << 10)", + "FUNCTION_VTX_SMARTAUDIO": "(1 << 11)", + "FUNCTION_VTX_TRAMP": "(1 << 12)", + "FUNCTION_UNUSED_1": "(1 << 13)", + "FUNCTION_OPTICAL_FLOW": "(1 << 14)", + "FUNCTION_LOG": "(1 << 15)", + "FUNCTION_RANGEFINDER": "(1 << 16)", + "FUNCTION_VTX_FFPV": "(1 << 17)", + "FUNCTION_ESCSERIAL": "(1 << 18)", + "FUNCTION_TELEMETRY_SIM": "(1 << 19)", + "FUNCTION_FRSKY_OSD": "(1 << 20)", + "FUNCTION_DJI_HD_OSD": "(1 << 21)", + "FUNCTION_SERVO_SERIAL": "(1 << 22)", + "FUNCTION_TELEMETRY_SMARTPORT_MASTER": "(1 << 23)", + "FUNCTION_UNUSED_2": "(1 << 24)", + "FUNCTION_MSP_OSD": "(1 << 25)", + "FUNCTION_GIMBAL": "(1 << 26)", + "FUNCTION_GIMBAL_HEADTRACKER": "(1 << 27)" + }, + "serialPortIdentifier_e": { + "_source": "../../../src/main/io/serial.h", + "SERIAL_PORT_NONE": "-1", + "SERIAL_PORT_USART1": "0", + "SERIAL_PORT_USART2": "1", + "SERIAL_PORT_USART3": "2", + "SERIAL_PORT_USART4": "3", + "SERIAL_PORT_USART5": "4", + "SERIAL_PORT_USART6": "5", + "SERIAL_PORT_USART7": "6", + "SERIAL_PORT_USART8": "7", + "SERIAL_PORT_USB_VCP": "20", + "SERIAL_PORT_SOFTSERIAL1": "30", + "SERIAL_PORT_SOFTSERIAL2": "31", + "SERIAL_PORT_IDENTIFIER_MAX": "SERIAL_PORT_SOFTSERIAL2" + }, + "servoAutotrimState_e": { + "_source": "../../../src/main/flight/servos.c", + "AUTOTRIM_IDLE": "0", + "AUTOTRIM_COLLECTING": "1", + "AUTOTRIM_SAVE_PENDING": "2", + "AUTOTRIM_DONE": "3" + }, + "servoIndex_e": { + "_source": "../../../src/main/flight/servos.h", + "SERVO_GIMBAL_PITCH": "0", + "SERVO_GIMBAL_ROLL": "1", + "SERVO_ELEVATOR": "2", + "SERVO_FLAPPERON_1": "3", + "SERVO_FLAPPERON_2": "4", + "SERVO_RUDDER": "5", + "SERVO_BICOPTER_LEFT": "4", + "SERVO_BICOPTER_RIGHT": "5", + "SERVO_DUALCOPTER_LEFT": "4", + "SERVO_DUALCOPTER_RIGHT": "5", + "SERVO_SINGLECOPTER_1": "3", + "SERVO_SINGLECOPTER_2": "4", + "SERVO_SINGLECOPTER_3": "5", + "SERVO_SINGLECOPTER_4": "6" + }, + "servoProtocolType_e": { + "_source": "../../../src/main/drivers/pwm_mapping.h", + "SERVO_TYPE_PWM": "0", + "SERVO_TYPE_SBUS": "1", + "SERVO_TYPE_SBUS_PWM": "2" + }, + "setting_mode_e": { + "_source": "../../../src/main/fc/settings.h", + "MODE_DIRECT": "(0 << SETTING_MODE_OFFSET)", + "MODE_LOOKUP": "(1 << SETTING_MODE_OFFSET)" + }, + "setting_section_e": { + "_source": "../../../src/main/fc/settings.h", + "MASTER_VALUE": "(0 << SETTING_SECTION_OFFSET)", + "PROFILE_VALUE": "(1 << SETTING_SECTION_OFFSET)", + "CONTROL_VALUE": "(2 << SETTING_SECTION_OFFSET)", + "BATTERY_CONFIG_VALUE": "(3 << SETTING_SECTION_OFFSET)", + "MIXER_CONFIG_VALUE": "(4 << SETTING_SECTION_OFFSET)", + "EZ_TUNE_VALUE": "(5 << SETTING_SECTION_OFFSET)" + }, + "setting_type_e": { + "_source": "../../../src/main/fc/settings.h", + "VAR_UINT8": "(0 << SETTING_TYPE_OFFSET)", + "VAR_INT8": "(1 << SETTING_TYPE_OFFSET)", + "VAR_UINT16": "(2 << SETTING_TYPE_OFFSET)", + "VAR_INT16": "(3 << SETTING_TYPE_OFFSET)", + "VAR_UINT32": "(4 << SETTING_TYPE_OFFSET)", + "VAR_FLOAT": "(5 << SETTING_TYPE_OFFSET)", + "VAR_STRING": "(6 << SETTING_TYPE_OFFSET)" + }, + "simATCommandState_e": { + "_source": "../../../src/main/telemetry/sim.c", + "SIM_AT_OK": "0", + "SIM_AT_ERROR": "1", + "SIM_AT_WAITING_FOR_RESPONSE": "2" + }, + "simModuleState_e": { + "_source": "../../../src/main/telemetry/sim.c", + "SIM_MODULE_NOT_DETECTED": "0", + "SIM_MODULE_NOT_REGISTERED": "1", + "SIM_MODULE_REGISTERED": "2" + }, + "simReadState_e": { + "_source": "../../../src/main/telemetry/sim.c", + "SIM_READSTATE_RESPONSE": "0", + "SIM_READSTATE_SMS": "1", + "SIM_READSTATE_SKIP": "2" + }, + "simTelemetryState_e": { + "_source": "../../../src/main/telemetry/sim.c", + "SIM_STATE_INIT": "0", + "SIM_STATE_INIT2": "1", + "SIM_STATE_INIT_ENTER_PIN": "2", + "SIM_STATE_SET_MODES": "3", + "SIM_STATE_SEND_SMS": "4", + "SIM_STATE_SEND_SMS_ENTER_MESSAGE": "5" + }, + "simTransmissionState_e": { + "_source": "../../../src/main/telemetry/sim.c", + "SIM_TX_NO": "0", + "SIM_TX_FS": "1", + "SIM_TX": "2" + }, + "simTxFlags_e": { + "_source": "../../../src/main/telemetry/sim.h", + "SIM_TX_FLAG": "(1 << 0)", + "SIM_TX_FLAG_FAILSAFE": "(1 << 1)", + "SIM_TX_FLAG_GPS": "(1 << 2)", + "SIM_TX_FLAG_ACC": "(1 << 3)", + "SIM_TX_FLAG_LOW_ALT": "(1 << 4)", + "SIM_TX_FLAG_RESPONSE": "(1 << 5)" + }, + "simulatorFlags_t": { + "_source": "../../../src/main/fc/runtime_config.h", + "HITL_RESET_FLAGS": "(0 << 0)", + "HITL_ENABLE": "(1 << 0)", + "HITL_SIMULATE_BATTERY": "(1 << 1)", + "HITL_MUTE_BEEPER": "(1 << 2)", + "HITL_USE_IMU": "(1 << 3)", + "HITL_HAS_NEW_GPS_DATA": "(1 << 4)", + "HITL_EXT_BATTERY_VOLTAGE": "(1 << 5)", + "HITL_AIRSPEED": "(1 << 6)", + "HITL_EXTENDED_FLAGS": "(1 << 7)", + "HITL_GPS_TIMEOUT": "(1 << 8)", + "HITL_PITOT_FAILURE": "(1 << 9)" + }, + "smartAudioVersion_e": { + "_source": "../../../src/main/io/vtx_smartaudio.h", + "SA_UNKNOWN": "0", + "SA_1_0": "1", + "SA_2_0": "2", + "SA_2_1": "3" + }, + "smartportFuelUnit_e": { + "_source": "../../../src/main/telemetry/telemetry.h", + "SMARTPORT_FUEL_UNIT_PERCENT": "0", + "SMARTPORT_FUEL_UNIT_MAH": "1", + "SMARTPORT_FUEL_UNIT_MWH": "2" + }, + "softSerialPortIndex_e": { + "_source": "../../../src/main/drivers/serial_softserial.h", + "SOFTSERIAL1": "0", + "SOFTSERIAL2": "1" + }, + "SPIClockSpeed_e": { + "_source": "../../../src/main/drivers/bus_spi.h", + "SPI_CLOCK_INITIALIZATON": "0", + "SPI_CLOCK_SLOW": "1", + "SPI_CLOCK_STANDARD": "2", + "SPI_CLOCK_FAST": "3", + "SPI_CLOCK_ULTRAFAST": "4" + }, + "Srxl2BindRequest": { + "_source": "../../../src/main/rx/srxl2_types.h", + "EnterBindMode": "235", + "RequestBindStatus": "181", + "BoundDataReport": "219", + "SetBindInfo": "91" + }, + "Srxl2BindType": { + "_source": "../../../src/main/rx/srxl2_types.h", + "NotBound": "0", + "DSM2_1024_22ms": "1", + "DSM2_1024_MC24": "2", + "DMS2_2048_11ms": "18", + "DMSX_22ms": "162", + "DMSX_11ms": "178", + "Surface_DSM2_16_5ms": "99", + "DSMR_11ms_22ms": "226", + "DSMR_5_5ms": "228" + }, + "Srxl2ControlDataCommand": { + "_source": "../../../src/main/rx/srxl2_types.h", + "ChannelData": "0", + "FailsafeChannelData": "1", + "VTXData": "2" + }, + "Srxl2DeviceId": { + "_source": "../../../src/main/rx/srxl2_types.h", + "FlightControllerDefault": "48", + "FlightControllerMax": "63", + "Broadcast": "255" + }, + "Srxl2DeviceType": { + "_source": "../../../src/main/rx/srxl2_types.h", + "NoDevice": "0", + "RemoteReceiver": "1", + "Receiver": "2", + "FlightController": "3", + "ESC": "4", + "Reserved": "5", + "SRXLServo": "6", + "SRXLServo_2": "7", + "VTX": "8" + }, + "Srxl2PacketType": { + "_source": "../../../src/main/rx/srxl2_types.h", + "Handshake": "33", + "BindInfo": "65", + "ParameterConfiguration": "80", + "SignalQuality": "85", + "TelemetrySensorData": "128", + "ControlData": "205" + }, + "Srxl2State": { + "_source": "../../../src/main/rx/srxl2_types.h", + "Disabled": "0", + "ListenForActivity": "1", + "SendHandshake": "2", + "ListenForHandshake": "3", + "Running": "4" + }, + "stateFlags_t": { + "_source": "../../../src/main/fc/runtime_config.h", + "GPS_FIX_HOME": "(1 << 0)", + "GPS_FIX": "(1 << 1)", + "CALIBRATE_MAG": "(1 << 2)", + "SMALL_ANGLE": "(1 << 3)", + "FIXED_WING_LEGACY": "(1 << 4)", + "ANTI_WINDUP": "(1 << 5)", + "FLAPERON_AVAILABLE": "(1 << 6)", + "NAV_MOTOR_STOP_OR_IDLE": "(1 << 7)", + "COMPASS_CALIBRATED": "(1 << 8)", + "ACCELEROMETER_CALIBRATED": "(1 << 9)", + "GPS_ESTIMATED_FIX": [ + "(1 << 10)", + "USE_GPS_FIX_ESTIMATION" + ], + "NAV_CRUISE_BRAKING": "(1 << 11)", + "NAV_CRUISE_BRAKING_BOOST": "(1 << 12)", + "NAV_CRUISE_BRAKING_LOCKED": "(1 << 13)", + "NAV_EXTRA_ARMING_SAFETY_BYPASSED": "(1 << 14)", + "AIRMODE_ACTIVE": "(1 << 15)", + "ESC_SENSOR_ENABLED": "(1 << 16)", + "AIRPLANE": "(1 << 17)", + "MULTIROTOR": "(1 << 18)", + "ROVER": "(1 << 19)", + "BOAT": "(1 << 20)", + "ALTITUDE_CONTROL": "(1 << 21)", + "MOVE_FORWARD_ONLY": "(1 << 22)", + "SET_REVERSIBLE_MOTORS_FORWARD": "(1 << 23)", + "FW_HEADING_USE_YAW": "(1 << 24)", + "ANTI_WINDUP_DEACTIVATED": "(1 << 25)", + "LANDING_DETECTED": "(1 << 26)", + "IN_FLIGHT_EMERG_REARM": "(1 << 27)", + "TAILSITTER": "(1 << 28)" + }, + "stickPositions_e": { + "_source": "../../../src/main/fc/rc_controls.h", + "ROL_LO": "(1 << (2 * ROLL))", + "ROL_CE": "(3 << (2 * ROLL))", + "ROL_HI": "(2 << (2 * ROLL))", + "PIT_LO": "(1 << (2 * PITCH))", + "PIT_CE": "(3 << (2 * PITCH))", + "PIT_HI": "(2 << (2 * PITCH))", + "YAW_LO": "(1 << (2 * YAW))", + "YAW_CE": "(3 << (2 * YAW))", + "YAW_HI": "(2 << (2 * YAW))", + "THR_LO": "(1 << (2 * THROTTLE))", + "THR_CE": "(3 << (2 * THROTTLE))", + "THR_HI": "(2 << (2 * THROTTLE))" + }, + "systemState_e": { + "_source": "../../../src/main/fc/fc_init.h", + "SYSTEM_STATE_INITIALISING": "0", + "SYSTEM_STATE_CONFIG_LOADED": "(1 << 0)", + "SYSTEM_STATE_SENSORS_READY": "(1 << 1)", + "SYSTEM_STATE_MOTORS_READY": "(1 << 2)", + "SYSTEM_STATE_TRANSPONDER_ENABLED": "(1 << 3)", + "SYSTEM_STATE_READY": "(1 << 7)" + }, + "tchDmaState_e": { + "_source": "../../../src/main/drivers/timer.h", + "TCH_DMA_IDLE": "0", + "TCH_DMA_READY": "1", + "TCH_DMA_ACTIVE": "2" + }, + "tempSensorType_e": { + "_source": "../../../src/main/sensors/temperature.h", + "TEMP_SENSOR_NONE": "0", + "TEMP_SENSOR_LM75": "1", + "TEMP_SENSOR_DS18B20": "2" + }, + "throttleStatus_e": { + "_source": "../../../src/main/fc/rc_controls.h", + "THROTTLE_LOW": "0", + "THROTTLE_HIGH": "1" + }, + "throttleStatusType_e": { + "_source": "../../../src/main/fc/rc_controls.h", + "THROTTLE_STATUS_TYPE_RC": "0", + "THROTTLE_STATUS_TYPE_COMMAND": "1" + }, + "timerMode_e": { + "_source": "../../../src/main/drivers/serial_softserial.c", + "TIMER_MODE_SINGLE": "0", + "TIMER_MODE_DUAL": "1" + }, + "timerUsageFlag_e": { + "_source": "../../../src/main/drivers/timer.h", + "TIM_USE_ANY": "0", + "TIM_USE_PPM": "(1 << 0)", + "TIM_USE_PWM": "(1 << 1)", + "TIM_USE_MOTOR": "(1 << 2)", + "TIM_USE_SERVO": "(1 << 3)", + "TIM_USE_MC_CHNFW": "(1 << 4)", + "TIM_USE_LED": "(1 << 24)", + "TIM_USE_BEEPER": "(1 << 25)" + }, + "timId_e": { + "_source": "../../../src/main/io/ledstrip.c", + "timBlink": "0", + "timLarson": "1", + "timBattery": "2", + "timRssi": "3", + "timGps": [ + "(4)", + "USE_GPS" + ], + "timWarning": "5", + "timIndicator": "6", + "timAnimation": [ + "(7)", + "USE_LED_ANIMATION" + ], + "timRing": "8", + "timTimerCount": "9" + }, + "tristate_e": { + "_source": "../../../src/main/common/tristate.h", + "TRISTATE_AUTO": "0", + "TRISTATE_ON": "1", + "TRISTATE_OFF": "2" + }, + "tz_automatic_dst_e": { + "_source": "../../../src/main/common/time.h", + "TZ_AUTO_DST_OFF": "0", + "TZ_AUTO_DST_EU": "1", + "TZ_AUTO_DST_USA": "2" + }, + "UARTDevice_e": { + "_source": "../../../src/main/drivers/serial_uart.h", + "UARTDEV_1": "0", + "UARTDEV_2": "1", + "UARTDEV_3": "2", + "UARTDEV_4": "3", + "UARTDEV_5": "4", + "UARTDEV_6": "5", + "UARTDEV_7": "6", + "UARTDEV_8": "7", + "UARTDEV_MAX": "8" + }, + "uartInverterLine_e": { + "_source": "../../../src/main/drivers/uart_inverter.h", + "UART_INVERTER_LINE_NONE": "0", + "UART_INVERTER_LINE_RX": "1 << 0", + "UART_INVERTER_LINE_TX": "1 << 1" + }, + "ublox_nav_sig_health_e": { + "_source": "../../../src/main/io/gps_ublox.h", + "UBLOX_SIG_HEALTH_UNKNOWN": "0", + "UBLOX_SIG_HEALTH_HEALTHY": "1", + "UBLOX_SIG_HEALTH_UNHEALTHY": "2" + }, + "ublox_nav_sig_quality": { + "_source": "../../../src/main/io/gps_ublox.h", + "UBLOX_SIG_QUALITY_NOSIGNAL": "0", + "UBLOX_SIG_QUALITY_SEARCHING": "1", + "UBLOX_SIG_QUALITY_ACQUIRED": "2", + "UBLOX_SIG_QUALITY_UNUSABLE": "3", + "UBLOX_SIG_QUALITY_CODE_LOCK_TIME_SYNC": "4", + "UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC": "5", + "UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC2": "6", + "UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC3": "7" + }, + "ubs_nav_fix_type_t": { + "_source": "../../../src/main/io/gps_ublox.h", + "FIX_NONE": "0", + "FIX_DEAD_RECKONING": "1", + "FIX_2D": "2", + "FIX_3D": "3", + "FIX_GPS_DEAD_RECKONING": "4", + "FIX_TIME": "5" + }, + "ubx_ack_state_t": { + "_source": "../../../src/main/io/gps_ublox.h", + "UBX_ACK_WAITING": "0", + "UBX_ACK_GOT_ACK": "1", + "UBX_ACK_GOT_NAK": "2" + }, + "ubx_nav_status_bits_t": { + "_source": "../../../src/main/io/gps_ublox.h", + "NAV_STATUS_FIX_VALID": "1" + }, + "ubx_protocol_bytes_t": { + "_source": "../../../src/main/io/gps_ublox.h", + "PREAMBLE1": "181", + "PREAMBLE2": "98", + "CLASS_NAV": "1", + "CLASS_ACK": "5", + "CLASS_CFG": "6", + "CLASS_MON": "10", + "MSG_CLASS_UBX": "1", + "MSG_CLASS_NMEA": "240", + "MSG_VER": "4", + "MSG_ACK_NACK": "0", + "MSG_ACK_ACK": "1", + "MSG_NMEA_GGA": "0", + "MSG_NMEA_GLL": "1", + "MSG_NMEA_GSA": "2", + "MSG_NMEA_GSV": "3", + "MSG_NMEA_RMC": "4", + "MSG_NMEA_VGS": "5", + "MSG_POSLLH": "2", + "MSG_STATUS": "3", + "MSG_SOL": "6", + "MSG_PVT": "7", + "MSG_VELNED": "18", + "MSG_TIMEUTC": "33", + "MSG_SVINFO": "48", + "MSG_NAV_SAT": "53", + "MSG_CFG_PRT": "0", + "MSG_CFG_RATE": "8", + "MSG_CFG_SET_RATE": "1", + "MSG_CFG_NAV_SETTINGS": "36", + "MSG_CFG_SBAS": "22", + "MSG_CFG_GNSS": "62", + "MSG_MON_GNSS": "40", + "MSG_NAV_SIG": "67" + }, + "vcselPeriodType_e": { + "_source": "../../../src/main/drivers/rangefinder/rangefinder_vl53l0x.c", + "VcselPeriodPreRange": "0", + "VcselPeriodFinalRange": "1" + }, + "videoSystem_e": { + "_source": "../../../src/main/drivers/osd.h", + "VIDEO_SYSTEM_AUTO": "0", + "VIDEO_SYSTEM_PAL": "1", + "VIDEO_SYSTEM_NTSC": "2", + "VIDEO_SYSTEM_HDZERO": "3", + "VIDEO_SYSTEM_DJIWTF": "4", + "VIDEO_SYSTEM_AVATAR": "5", + "VIDEO_SYSTEM_DJICOMPAT": "6", + "VIDEO_SYSTEM_DJICOMPAT_HD": "7", + "VIDEO_SYSTEM_DJI_NATIVE": "8" + }, + "voltageSensor_e": { + "_source": "../../../src/main/sensors/battery_config_structs.h", + "VOLTAGE_SENSOR_NONE": "0", + "VOLTAGE_SENSOR_ADC": "1", + "VOLTAGE_SENSOR_ESC": "2", + "VOLTAGE_SENSOR_FAKE": "3", + "VOLTAGE_SENSOR_SMARTPORT": "4", + "VOLTAGE_SENSOR_MAX": "VOLTAGE_SENSOR_SMARTPORT" + }, + "vs600Band_e": { + "_source": "../../../src/main/io/smartport_master.h", + "VS600_BAND_A": "0", + "VS600_BAND_B": "1", + "VS600_BAND_C": "2", + "VS600_BAND_D": "3", + "VS600_BAND_E": "4", + "VS600_BAND_F": "5" + }, + "vs600Power_e": { + "_source": "../../../src/main/io/smartport_master.h", + "VS600_POWER_PIT": "0", + "VS600_POWER_25MW": "1", + "VS600_POWER_200MW": "2", + "VS600_POWER_600MW": "3" + }, + "vtxDevType_e": { + "_source": "../../../src/main/drivers/vtx_common.h", + "VTXDEV_UNSUPPORTED": "0", + "VTXDEV_RTC6705": "1", + "VTXDEV_SMARTAUDIO": "3", + "VTXDEV_TRAMP": "4", + "VTXDEV_FFPV": "5", + "VTXDEV_MSP": "6", + "VTXDEV_UNKNOWN": "255" + }, + "vtxFrequencyGroups_e": { + "_source": "../../../src/main/drivers/vtx_common.h", + "FREQUENCYGROUP_5G8": "0", + "FREQUENCYGROUP_2G4": "1", + "FREQUENCYGROUP_1G3": "2" + }, + "vtxLowerPowerDisarm_e": { + "_source": "../../../src/main/io/vtx.h", + "VTX_LOW_POWER_DISARM_OFF": "0", + "VTX_LOW_POWER_DISARM_ALWAYS": "1", + "VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM": "2" + }, + "vtxProtoResponseType_e": { + "_source": "../../../src/main/io/vtx_tramp.c", + "VTX_RESPONSE_TYPE_NONE": "0", + "VTX_RESPONSE_TYPE_CAPABILITIES": "1", + "VTX_RESPONSE_TYPE_STATUS": "2" + }, + "vtxProtoState_e": { + "_source": "../../../src/main/io/vtx_tramp.c", + "VTX_STATE_RESET": "0", + "VTX_STATE_OFFILE": "1", + "VTX_STATE_DETECTING": "2", + "VTX_STATE_IDLE": "3", + "VTX_STATE_QUERY_DELAY": "4", + "VTX_STATE_QUERY_STATUS": "5", + "VTX_STATE_WAIT_STATUS": "6" + }, + "vtxScheduleParams_e": { + "_source": "../../../src/main/io/vtx.c", + "VTX_PARAM_POWER": "0", + "VTX_PARAM_BANDCHAN": "1", + "VTX_PARAM_PITMODE": "2", + "VTX_PARAM_COUNT": "3" + }, + "warningFlags_e": { + "_source": "../../../src/main/io/ledstrip.c", + "WARNING_ARMING_DISABLED": "0", + "WARNING_LOW_BATTERY": "1", + "WARNING_FAILSAFE": "2", + "WARNING_HW_ERROR": "3" + }, + "warningLedState_e": { + "_source": "../../../src/main/io/statusindicator.c", + "WARNING_LED_OFF": "0", + "WARNING_LED_ON": "1", + "WARNING_LED_FLASH": "2" + }, + "widgetAHIOptions_t": { + "_source": "../../../src/main/drivers/display_widgets.h", + "DISPLAY_WIDGET_AHI_OPTION_SHOW_CORNERS": "1 << 0" + }, + "widgetAHIStyle_e": { + "_source": "../../../src/main/drivers/display_widgets.h", + "DISPLAY_WIDGET_AHI_STYLE_STAIRCASE": "0", + "DISPLAY_WIDGET_AHI_STYLE_LINE": "1" + }, + "wpFwTurnSmoothing_e": { + "_source": "../../../src/main/navigation/navigation.h", + "WP_TURN_SMOOTHING_OFF": "0", + "WP_TURN_SMOOTHING_ON": "1", + "WP_TURN_SMOOTHING_CUT": "2" + }, + "wpMissionPlannerStatus_e": { + "_source": "../../../src/main/navigation/navigation.h", + "WP_PLAN_WAIT": "0", + "WP_PLAN_SAVE": "1", + "WP_PLAN_OK": "2", + "WP_PLAN_FULL": "3" + }, + "zeroCalibrationState_e": { + "_source": "../../../src/main/common/calibration.h", + "ZERO_CALIBRATION_NONE": "0", + "ZERO_CALIBRATION_IN_PROGRESS": "1", + "ZERO_CALIBRATION_DONE": "2", + "ZERO_CALIBRATION_FAIL": "3" + } +} \ No newline at end of file diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 2d60f270183..e4476e833e4 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -162,6 +162,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) @@ -3128,6 +3129,19 @@ | `MAG_FAKE` | 16 | | | `MAG_MAX` | MAG_FAKE | | +--- +## `mavFrameSupportMask_e` + +> Source: ../../../src/main/telemetry/mavlink.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAV_FRAME_SUPPORTED_NONE` | 0 | | +| `MAV_FRAME_SUPPORTED_GLOBAL` | (1 << 0) | | +| `MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT` | (1 << 1) | | +| `MAV_FRAME_SUPPORTED_GLOBAL_INT` | (1 << 2) | | +| `MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT` | (1 << 3) | | + --- ## `mavlinkAutopilotType_e` diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index fe0aebe26b7..b9e654bdbd8 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(); } @@ -1943,34 +1958,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 9143acc60c824adf37ea678aa57866c58f743c4f 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/42] 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 b9b11c065207722d1caea8b6f48c23f672895cd0 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Tue, 3 Mar 2026 16:17:16 -0500 Subject: [PATCH 21/42] add altitude command --- src/main/telemetry/mavlink.c | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index b9e654bdbd8..3c68aa01794 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1857,6 +1857,32 @@ 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: + { + const float altitudeMeters = param1; + 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); + return true; + } +#endif case MAV_CMD_SET_MESSAGE_INTERVAL: { uint8_t stream; From 454dd48c7a18149ba83865ee5dce6f1ac0a0fac3 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 22/42] docgen --- dm.txt | 0 docs/development/msp/README.md | 2 +- docs/development/msp/inav_enums.json | 10 +++++++++- docs/development/msp/inav_enums_ref.md | 4 ++-- docs/development/msp/msp_messages.checksum | 2 +- docs/development/msp/rev | 2 +- 6 files changed, 14 insertions(+), 6 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 2ca0fff6675..c4b6d06b58d 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -9,7 +9,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i -**JSON file rev: 4** +**JSON file rev: 0** **Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty** diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index ffe24b3f24c..3844e59cee9 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -2238,6 +2238,14 @@ "MAG_FAKE": "16", "MAG_MAX": "MAG_FAKE" }, + "mavFrameSupportMask_e": { + "_source": "inav/src/main/telemetry/mavlink.c", + "MAV_FRAME_SUPPORTED_NONE": "0", + "MAV_FRAME_SUPPORTED_GLOBAL": "(1 << 0)", + "MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT": "(1 << 1)", + "MAV_FRAME_SUPPORTED_GLOBAL_INT": "(1 << 2)", + "MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT": "(1 << 3)" + }, "mavlinkAutopilotType_e": { "_source": "inav/src/main/telemetry/telemetry.h", "MAVLINK_AUTOPILOT_GENERIC": "0", @@ -3831,7 +3839,7 @@ "THR_HI": "(2 << (2 * THROTTLE))" }, "systemState_e": { - "_source": "inav/src/main/fc/fc_init.c", + "_source": "inav/src/main/fc/fc_init.h", "SYSTEM_STATE_INITIALISING": "0", "SYSTEM_STATE_CONFIG_LOADED": "(1 << 0)", "SYSTEM_STATE_SENSORS_READY": "(1 << 1)", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 2ac04b18f18..164ebd9bc14 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -5643,7 +5643,7 @@ --- ## `systemState_e` -> Source: ../../../src/main/fc/fc_init.h +> Source: ../../../src/main/fc/fc_init.c | Enumerator | Value | Condition | |---|---:|---| @@ -5657,7 +5657,7 @@ --- ## `systemState_e` -> Source: ../../../src/main/fc/fc_init.c +> Source: ../../../src/main/fc/fc_init.h | Enumerator | Value | Condition | |---|---:|---| diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum index ae689c4b8eb..14e143e04b9 100644 --- a/docs/development/msp/msp_messages.checksum +++ b/docs/development/msp/msp_messages.checksum @@ -1 +1 @@ -6e8861395a4275489a12012e6985d733 +7146802c1d6c89bce6aeda0d5e95c137 diff --git a/docs/development/msp/rev b/docs/development/msp/rev index bf0d87ab1b2..c227083464f 100644 --- a/docs/development/msp/rev +++ b/docs/development/msp/rev @@ -1 +1 @@ -4 \ No newline at end of file +0 \ No newline at end of file From 1250399aec927b57b6af9a6e3d8e8b3d0ed07cf6 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 23/42] qodo fixes + set_pos_tgt_global alt change + docs --- docs/Mavlink.md | 7 +-- src/main/telemetry/mavlink.c | 99 +++++++++++++++++++++++++----------- 2 files changed, 73 insertions(+), 33 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 3c68aa01794..d7713ac6833 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,32 +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: { - const float altitudeMeters = param1; - 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 +2182,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 +2213,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 +2262,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 eda3b3f284f4e3c4514cee09d862b0c04ed25a89 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Thu, 26 Feb 2026 19:02:29 -0500 Subject: [PATCH 24/42] multi mav port + router, iteration 1 --- docs/Settings.md | 280 +++++++++++++ src/main/fc/settings.yaml | 206 +++++++++- src/main/rx/mavlink.h | 6 +- src/main/telemetry/mavlink.c | 700 ++++++++++++++++++++++++++++----- src/main/telemetry/mavlink.h | 1 - src/main/telemetry/telemetry.c | 58 ++- src/main/telemetry/telemetry.h | 34 +- 7 files changed, 1142 insertions(+), 143 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 4e577bbedd5..610834d0d7f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2702,6 +2702,286 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry --- +### mavlink_port1_compid + +MAVLink Component ID for MAVLink port 1 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 255 | + +--- + +### mavlink_port1_high_latency + +Enable MAVLink high-latency mode on port 1 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port2_autopilot_type + +Autopilot type to advertise for MAVLink telemetry on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + +### mavlink_port2_compid + +MAVLink Component ID for MAVLink port 2 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 255 | + +--- + +### mavlink_port2_ext_status_rate + +Rate of the extended status message for MAVLink telemetry on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port2_extra1_rate + +Rate of the extra1 message for MAVLink telemetry on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port2_extra2_rate + +Rate of the extra2 message for MAVLink telemetry on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port2_extra3_rate + +Rate of the extra3 message for MAVLink telemetry on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 255 | + +--- + +### mavlink_port2_high_latency + +Enable MAVLink high-latency mode on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port2_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 2. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port2_pos_rate + +Rate of the position message for MAVLink telemetry on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port2_radio_type + +MAVLink radio type for port 2. Affects RSSI and LQ reporting on OSD. + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + +### mavlink_port2_rc_chan_rate + +Rate of the RC channels message for MAVLink telemetry on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 255 | + +--- + +### mavlink_port2_sysid + +MAVLink System ID for MAVLink port 2 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 255 | + +--- + +### mavlink_port2_version + +Version of MAVLink to use on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 1 | 2 | + +--- + +### mavlink_port3_autopilot_type + +Autopilot type to advertise for MAVLink telemetry on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + +### mavlink_port3_compid + +MAVLink Component ID for MAVLink port 3 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 255 | + +--- + +### mavlink_port3_ext_status_rate + +Rate of the extended status message for MAVLink telemetry on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port3_extra1_rate + +Rate of the extra1 message for MAVLink telemetry on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port3_extra2_rate + +Rate of the extra2 message for MAVLink telemetry on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port3_extra3_rate + +Rate of the extra3 message for MAVLink telemetry on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 255 | + +--- + +### mavlink_port3_high_latency + +Enable MAVLink high-latency mode on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port3_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 3. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port3_pos_rate + +Rate of the position message for MAVLink telemetry on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port3_radio_type + +MAVLink radio type for port 3. Affects RSSI and LQ reporting on OSD. + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + +### mavlink_port3_rc_chan_rate + +Rate of the RC channels message for MAVLink telemetry on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 255 | + +--- + +### mavlink_port3_sysid + +MAVLink System ID for MAVLink port 3 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 255 | + +--- + +### mavlink_port3_version + +Version of MAVLink to use on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 1 | 2 | + +--- + ### mavlink_pos_rate Rate of the position message for MAVLink telemetry diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1e1932531e5..c7342f50b17 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3225,79 +3225,263 @@ groups: min: INT16_MIN max: INT16_MAX - name: mavlink_ext_status_rate - field: mavlink.extended_status_rate + field: mavlink[0].extended_status_rate description: "Rate of the extended status message for MAVLink telemetry" type: uint8_t min: 0 max: 255 default_value: 2 - name: mavlink_autopilot_type - field: mavlink.autopilot_type + field: mavlink[0].autopilot_type description: "Autopilot type to advertise for MAVLink telemetry" table: mavlink_autopilot_type default_value: "GENERIC" type: uint8_t - name: mavlink_rc_chan_rate description: "Rate of the RC channels message for MAVLink telemetry" - field: mavlink.rc_channels_rate + field: mavlink[0].rc_channels_rate type: uint8_t min: 0 max: 255 default_value: 1 - name: mavlink_pos_rate description: "Rate of the position message for MAVLink telemetry" - field: mavlink.position_rate + field: mavlink[0].position_rate type: uint8_t min: 0 max: 255 default_value: 2 - name: mavlink_extra1_rate description: "Rate of the extra1 message for MAVLink telemetry" - field: mavlink.extra1_rate + field: mavlink[0].extra1_rate type: uint8_t min: 0 max: 255 default_value: 2 - name: mavlink_extra2_rate description: "Rate of the extra2 message for MAVLink telemetry" - field: mavlink.extra2_rate + field: mavlink[0].extra2_rate type: uint8_t min: 0 max: 255 default_value: 2 - name: mavlink_extra3_rate description: "Rate of the extra3 message for MAVLink telemetry" - field: mavlink.extra3_rate + field: mavlink[0].extra3_rate type: uint8_t min: 0 max: 255 default_value: 1 - name: mavlink_version - field: mavlink.version + field: mavlink[0].version description: "Version of MAVLink to use" type: uint8_t min: 1 max: 2 default_value: 2 - name: mavlink_min_txbuffer - field: mavlink.min_txbuff + field: mavlink[0].min_txbuff description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." default_value: 33 min: 0 max: 100 - name: mavlink_radio_type description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD." - field: mavlink.radio_type + field: mavlink[0].radio_type table: mavlink_radio_type default_value: "GENERIC" type: uint8_t - name: mavlink_sysid - field: mavlink.sysid + field: mavlink[0].sysid description: "MAVLink System ID" type: uint8_t min: 1 max: 255 default_value: 1 + - name: mavlink_port1_compid + field: mavlink[0].compid + description: "MAVLink Component ID for MAVLink port 1" + type: uint8_t + min: 1 + max: 255 + default_value: 1 + - name: mavlink_port1_high_latency + field: mavlink[0].high_latency + description: "Enable MAVLink high-latency mode on port 1" + type: bool + default_value: OFF + - name: mavlink_port2_ext_status_rate + field: mavlink[1].extended_status_rate + description: "Rate of the extended status message for MAVLink telemetry on port 2" + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port2_autopilot_type + field: mavlink[1].autopilot_type + description: "Autopilot type to advertise for MAVLink telemetry on port 2" + table: mavlink_autopilot_type + default_value: "GENERIC" + type: uint8_t + - name: mavlink_port2_rc_chan_rate + description: "Rate of the RC channels message for MAVLink telemetry on port 2" + field: mavlink[1].rc_channels_rate + type: uint8_t + min: 0 + max: 255 + default_value: 1 + - name: mavlink_port2_pos_rate + description: "Rate of the position message for MAVLink telemetry on port 2" + field: mavlink[1].position_rate + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port2_extra1_rate + description: "Rate of the extra1 message for MAVLink telemetry on port 2" + field: mavlink[1].extra1_rate + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port2_extra2_rate + description: "Rate of the extra2 message for MAVLink telemetry on port 2" + field: mavlink[1].extra2_rate + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port2_extra3_rate + description: "Rate of the extra3 message for MAVLink telemetry on port 2" + field: mavlink[1].extra3_rate + type: uint8_t + min: 0 + max: 255 + default_value: 1 + - name: mavlink_port2_version + field: mavlink[1].version + description: "Version of MAVLink to use on port 2" + type: uint8_t + min: 1 + max: 2 + default_value: 2 + - name: mavlink_port2_min_txbuffer + field: mavlink[1].min_txbuff + description: "Minimum percent of TX buffer space free for MAVLink port 2. Requires RADIO_STATUS messages." + default_value: 33 + min: 0 + max: 100 + - name: mavlink_port2_radio_type + description: "MAVLink radio type for port 2. Affects RSSI and LQ reporting on OSD." + field: mavlink[1].radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t + - name: mavlink_port2_sysid + field: mavlink[1].sysid + description: "MAVLink System ID for MAVLink port 2" + type: uint8_t + min: 1 + max: 255 + default_value: 1 + - name: mavlink_port2_compid + field: mavlink[1].compid + description: "MAVLink Component ID for MAVLink port 2" + type: uint8_t + min: 1 + max: 255 + default_value: 1 + - name: mavlink_port2_high_latency + field: mavlink[1].high_latency + description: "Enable MAVLink high-latency mode on port 2" + type: bool + default_value: OFF + - name: mavlink_port3_ext_status_rate + field: mavlink[2].extended_status_rate + description: "Rate of the extended status message for MAVLink telemetry on port 3" + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port3_autopilot_type + field: mavlink[2].autopilot_type + description: "Autopilot type to advertise for MAVLink telemetry on port 3" + table: mavlink_autopilot_type + default_value: "GENERIC" + type: uint8_t + - name: mavlink_port3_rc_chan_rate + description: "Rate of the RC channels message for MAVLink telemetry on port 3" + field: mavlink[2].rc_channels_rate + type: uint8_t + min: 0 + max: 255 + default_value: 1 + - name: mavlink_port3_pos_rate + description: "Rate of the position message for MAVLink telemetry on port 3" + field: mavlink[2].position_rate + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port3_extra1_rate + description: "Rate of the extra1 message for MAVLink telemetry on port 3" + field: mavlink[2].extra1_rate + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port3_extra2_rate + description: "Rate of the extra2 message for MAVLink telemetry on port 3" + field: mavlink[2].extra2_rate + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port3_extra3_rate + description: "Rate of the extra3 message for MAVLink telemetry on port 3" + field: mavlink[2].extra3_rate + type: uint8_t + min: 0 + max: 255 + default_value: 1 + - name: mavlink_port3_version + field: mavlink[2].version + description: "Version of MAVLink to use on port 3" + type: uint8_t + min: 1 + max: 2 + default_value: 2 + - name: mavlink_port3_min_txbuffer + field: mavlink[2].min_txbuff + description: "Minimum percent of TX buffer space free for MAVLink port 3. Requires RADIO_STATUS messages." + default_value: 33 + min: 0 + max: 100 + - name: mavlink_port3_radio_type + description: "MAVLink radio type for port 3. Affects RSSI and LQ reporting on OSD." + field: mavlink[2].radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t + - name: mavlink_port3_sysid + field: mavlink[2].sysid + description: "MAVLink System ID for MAVLink port 3" + type: uint8_t + min: 1 + max: 255 + default_value: 1 + - name: mavlink_port3_compid + field: mavlink[2].compid + description: "MAVLink Component ID for MAVLink port 3" + type: uint8_t + min: 1 + max: 255 + default_value: 1 + - name: mavlink_port3_high_latency + field: mavlink[2].high_latency + description: "Enable MAVLink high-latency mode on port 3" + type: bool + default_value: OFF - name: PG_OSD_CONFIG type: osdConfig_t diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h index 951b0eba105..644e71d4f8d 100644 --- a/src/main/rx/mavlink.h +++ b/src/main/rx/mavlink.h @@ -17,9 +17,13 @@ #pragma once +#ifndef MAX_MAVLINK_PORTS +#define MAX_MAVLINK_PORTS 3 +#endif + #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" -#define MAVLINK_COMM_NUM_BUFFERS 1 +#define MAVLINK_COMM_NUM_BUFFERS MAX_MAVLINK_PORTS #include "common/mavlink.h" #pragma GCC diagnostic pop diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index d7713ac6833..74665525ec0 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -58,6 +58,7 @@ #include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/wind_estimator.h" #include "io/adsb.h" #include "io/gps.h" @@ -91,17 +92,22 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" -#define MAVLINK_COMM_NUM_BUFFERS 1 +#ifndef MAVLINK_COMM_NUM_BUFFERS +#define MAVLINK_COMM_NUM_BUFFERS MAX_MAVLINK_PORTS +#endif #include "common/mavlink.h" #pragma GCC diagnostic pop #define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX #define TELEMETRY_MAVLINK_MAXRATE 50 #define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) +#define TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US (5 * 1000 * 1000) #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 MAVLINK_MAX_ROUTES 32 +#define MAVLINK_PORT_MASK(portIndex) (1U << (portIndex)) typedef enum { MAV_FRAME_SUPPORTED_NONE = 0, @@ -181,16 +187,14 @@ typedef enum APM_COPTER_MODE COPTER_MODE_ENUM_END=29, } APM_COPTER_MODE; -static serialPort_t *mavlinkPort = NULL; -static serialPortConfig_t *portConfig; - -static bool mavlinkTelemetryEnabled = false; -static portSharing_e mavlinkPortSharing; -static uint8_t txbuff_free = 100; -static bool txbuff_valid = false; +typedef struct mavlinkRouteEntry_s { + uint8_t sysid; + uint8_t compid; + uint8_t ingressPortIndex; +} mavlinkRouteEntry_t; /* MAVLink datastream rates in Hz */ -static uint8_t mavRates[] = { +static const uint8_t mavDefaultRates[] = { [MAV_DATA_STREAM_EXTENDED_STATUS] = 2, // 2Hz [MAV_DATA_STREAM_RC_CHANNELS] = 1, // 1Hz [MAV_DATA_STREAM_POSITION] = 2, // 2Hz @@ -200,16 +204,36 @@ static uint8_t mavRates[] = { [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]; +typedef struct mavlinkPortRuntime_s { + serialPort_t *port; + const serialPortConfig_t *portConfig; + portSharing_e portSharing; + bool telemetryEnabled; + bool txbuffValid; + uint8_t txbuffFree; + timeUs_t lastMavlinkMessageUs; + timeUs_t lastHighLatencyMessageUs; + bool highLatencyEnabled; + uint8_t mavRates[ARRAYLEN(mavDefaultRates)]; + uint8_t mavRatesConfigured[ARRAYLEN(mavDefaultRates)]; + uint8_t mavTicks[ARRAYLEN(mavDefaultRates)]; + mavlink_message_t mavRecvMsg; + mavlink_status_t mavRecvStatus; +} mavlinkPortRuntime_t; + +#define MAXSTREAMS (ARRAYLEN(mavDefaultRates)) + +static mavlinkPortRuntime_t mavPortStates[MAX_MAVLINK_PORTS]; +static uint8_t mavPortCount = 0; +static mavlinkRouteEntry_t mavRouteTable[MAVLINK_MAX_ROUTES]; +static uint8_t mavRouteCount = 0; +static uint8_t mavSendMask = 0; +static mavlinkPortRuntime_t *mavActivePort = NULL; +static const mavlinkTelemetryPortConfig_t *mavActiveConfig = NULL; static mavlink_message_t mavSendMsg; static mavlink_message_t mavRecvMsg; -static mavlink_status_t mavRecvStatus; -// Set mavSystemId from telemetryConfig()->mavlink.sysid +// Set active MAV identity from active port settings. static uint8_t mavSystemId = 1; static uint8_t mavAutopilotType; static uint8_t mavComponentId = MAV_COMP_ID_AUTOPILOT1; @@ -328,40 +352,73 @@ static mavlinkModeSelection_t selectMavlinkMode(bool isPlane) return modeSelection; } +static const mavlinkTelemetryPortConfig_t *mavlinkGetPortConfig(uint8_t portIndex) +{ + return &telemetryConfig()->mavlink[portIndex]; +} + +static void mavlinkApplyActivePortOutputVersion(void) +{ + mavlink_status_t *chanState = mavlink_get_channel_status(MAVLINK_COMM_0); + if (mavActiveConfig->version == 1) { + chanState->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } else { + chanState->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } +} + +static void mavlinkSetActivePortContext(uint8_t portIndex) +{ + mavActivePort = &mavPortStates[portIndex]; + mavActiveConfig = mavlinkGetPortConfig(portIndex); + mavAutopilotType = mavActiveConfig->autopilot_type; + mavSystemId = mavActiveConfig->sysid; + mavComponentId = mavActiveConfig->compid; + mavlinkApplyActivePortOutputVersion(); +} + static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum) { - uint8_t rate = (uint8_t) mavRates[streamNum]; + if (!mavActivePort || streamNum >= MAXSTREAMS) { + return 0; + } + + uint8_t rate = mavActivePort->mavRates[streamNum]; if (rate == 0) { return 0; } - if (mavTicks[streamNum] == 0) { + if (mavActivePort->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); + mavActivePort->mavTicks[streamNum] = (TELEMETRY_MAVLINK_MAXRATE / rate); return 1; } // count down at TASK_RATE_HZ - mavTicks[streamNum]--; + mavActivePort->mavTicks[streamNum]--; return 0; } static void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate) { - if (streamNum >= MAXSTREAMS) { + if (!mavActivePort || streamNum >= MAXSTREAMS) { return; } - mavRates[streamNum] = rate; - mavTicks[streamNum] = 0; + mavActivePort->mavRates[streamNum] = rate; + mavActivePort->mavTicks[streamNum] = 0; } static int32_t mavlinkStreamIntervalUs(uint8_t streamNum) { - uint8_t rate = mavRates[streamNum]; + if (!mavActivePort || streamNum >= MAXSTREAMS) { + return -1; + } + + uint8_t rate = mavActivePort->mavRates[streamNum]; if (rate == 0) { return -1; } @@ -369,91 +426,153 @@ static int32_t mavlinkStreamIntervalUs(uint8_t streamNum) return 1000000 / rate; } +static void configureMAVLinkStreamRates(uint8_t portIndex) +{ + mavlinkSetActivePortContext(portIndex); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_STATUS, mavActiveConfig->extended_status_rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_RC_CHANNELS, mavActiveConfig->rc_channels_rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_POSITION, mavActiveConfig->position_rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA1, mavActiveConfig->extra1_rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA2, mavActiveConfig->extra2_rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA3, mavActiveConfig->extra3_rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_SYS_STATE, mavActiveConfig->extra3_rate); + memcpy(mavActivePort->mavRatesConfigured, mavActivePort->mavRates, sizeof(mavActivePort->mavRatesConfigured)); +} + +static void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) +{ + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + if (state->port) { + closeSerialPort(state->port); + } + + state->port = NULL; + state->telemetryEnabled = false; + state->txbuffValid = false; + state->txbuffFree = 100; + state->lastMavlinkMessageUs = 0; + state->lastHighLatencyMessageUs = 0; + state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; + memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); + memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); +} + void freeMAVLinkTelemetryPort(void) { - closeSerialPort(mavlinkPort); - mavlinkPort = NULL; - mavlinkTelemetryEnabled = false; + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + freeMAVLinkTelemetryPortByIndex(portIndex); + } + + mavSendMask = 0; + mavRouteCount = 0; } void initMAVLinkTelemetry(void) { - portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); - mavlinkPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_MAVLINK); - memcpy(mavRatesConfigured, mavRates, sizeof(mavRatesConfigured)); + memset(mavPortStates, 0, sizeof(mavPortStates)); + memset(mavRouteTable, 0, sizeof(mavRouteTable)); + mavPortCount = 0; + mavRouteCount = 0; + mavSendMask = 0; + + const serialPortConfig_t *serialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); + while (serialPortConfig && mavPortCount < MAX_MAVLINK_PORTS) { + mavlinkPortRuntime_t *state = &mavPortStates[mavPortCount]; + state->portConfig = serialPortConfig; + state->portSharing = determinePortSharing(serialPortConfig, FUNCTION_TELEMETRY_MAVLINK); + state->txbuffFree = 100; + state->highLatencyEnabled = mavlinkGetPortConfig(mavPortCount)->high_latency; + configureMAVLinkStreamRates(mavPortCount); + + mavPortCount++; + serialPortConfig = findNextSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); + } + + mavActivePort = NULL; + mavActiveConfig = NULL; } -void configureMAVLinkTelemetryPort(void) +static void configureMAVLinkTelemetryPort(uint8_t portIndex) { - if (!portConfig) { + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + if (!state->portConfig) { return; } - baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex; + baudRate_e baudRateIndex = state->portConfig->telemetry_baudrateIndex; if (baudRateIndex == BAUD_AUTO) { // default rate for minimOSD baudRateIndex = BAUD_57600; } - mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_PORT_MODE, SERIAL_NOT_INVERTED); - mavAutopilotType = telemetryConfig()->mavlink.autopilot_type; - mavSystemId = telemetryConfig()->mavlink.sysid; - - if (!mavlinkPort) { + state->port = openSerialPort(state->portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_PORT_MODE, SERIAL_NOT_INVERTED); + if (!state->port) { return; } - mavlinkTelemetryEnabled = true; -} - -static void configureMAVLinkStreamRates(void) -{ - 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)); + state->telemetryEnabled = true; + state->txbuffValid = false; + state->txbuffFree = 100; + state->lastMavlinkMessageUs = 0; + state->lastHighLatencyMessageUs = 0; + state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; + memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); + memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); } void checkMAVLinkTelemetryState(void) { - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing); + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(state->portSharing); + if ((state->portConfig->functionMask & FUNCTION_RX_SERIAL) && + rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { + newTelemetryEnabledValue = true; + } - if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) { - return; - } + if (newTelemetryEnabledValue == state->telemetryEnabled) { + continue; + } - if (newTelemetryEnabledValue) { - configureMAVLinkTelemetryPort(); - configureMAVLinkStreamRates(); - } else - freeMAVLinkTelemetryPort(); + if (newTelemetryEnabledValue) { + configureMAVLinkTelemetryPort(portIndex); + if (state->telemetryEnabled) { + configureMAVLinkStreamRates(portIndex); + } + } else { + freeMAVLinkTelemetryPortByIndex(portIndex); + } + } } static void mavlinkSendMessage(void) { uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; + int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg); - mavlink_status_t* chan_state = mavlink_get_channel_status(MAVLINK_COMM_0); - if (telemetryConfig()->mavlink.version == 1) { - chan_state->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - } else { - chan_state->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - } + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if ((mavSendMask & MAVLINK_PORT_MASK(portIndex)) == 0) { + continue; + } - int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg); + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } - for (int i = 0; i < msgLength; i++) { - serialWrite(mavlinkPort, mavBuffer[i]); + for (int i = 0; i < msgLength; i++) { + serialWrite(state->port, mavBuffer[i]); + } } } static void mavlinkSendAutopilotVersion(void) { - if (telemetryConfig()->mavlink.version == 1) return; + if (mavActiveConfig->version == 1) return; // Capabilities aligned with what we actually support. uint64_t capabilities = 0; @@ -486,11 +605,11 @@ static void mavlinkSendAutopilotVersion(void) static void mavlinkSendProtocolVersion(void) { - if (telemetryConfig()->mavlink.version == 1) return; + if (mavActiveConfig->version == 1) return; uint8_t specHash[8] = {0}; uint8_t libHash[8] = {0}; - const uint16_t protocolVersion = (uint16_t)telemetryConfig()->mavlink.version * 100; + const uint16_t protocolVersion = (uint16_t)mavActiveConfig->version * 100; mavlink_msg_protocol_version_pack( mavSystemId, @@ -903,7 +1022,7 @@ void mavlinkSendSystemStatus(void) void mavlinkSendRCChannelsAndRSSI(void) { #define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) - if (telemetryConfig()->mavlink.version == 1) { + if (mavActiveConfig->version == 1) { mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, // time_boot_ms Timestamp (milliseconds since system boot) millis(), @@ -1329,8 +1448,200 @@ void mavlinkSendBatteryTemperatureStatusText(void) } +static uint8_t mavlinkGetAutopilotEnum(void) +{ + if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { + return MAV_AUTOPILOT_ARDUPILOTMEGA; + } + + return MAV_AUTOPILOT_GENERIC; +} + +static uint8_t mavlinkHeadingDeg2FromCd(int32_t headingCd) +{ + return (uint8_t)(wrap_36000(headingCd) / 200); +} + +static uint16_t mavlinkComputeHighLatencyFailures(void) +{ + uint16_t flags = 0; + +#if defined(USE_GPS) + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) || gpsSol.fixType == GPS_NO_FIX) { + flags |= HL_FAILURE_FLAG_GPS; + } +#endif + +#ifdef USE_PITOT + if (getHwPitotmeterStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; + } +#endif + + if (getHwBarometerStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; + } + + if (getHwAccelerometerStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_ACCEL; + } + + if (getHwGyroStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_GYRO; + } + + if (getHwCompassStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_MAG; + } + + if (getHwRangefinderStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_TERRAIN; + } + + const batteryState_e batteryState = getBatteryState(); + if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { + flags |= HL_FAILURE_FLAG_BATTERY; + } + + if (!rxIsReceivingSignal() || !rxAreFlightChannelsValid()) { + flags |= HL_FAILURE_FLAG_RC_RECEIVER; + } + + return flags; +} + +static void mavlinkSendHighLatency2(timeUs_t currentTimeUs) +{ + const bool isPlane = (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE); + const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); + + int32_t latitude = 0; + int32_t longitude = 0; + int16_t altitude = (int16_t)constrain((int32_t)(getEstimatedActualPosition(Z) / 100), INT16_MIN, INT16_MAX); + int16_t targetAltitude = (int16_t)constrain((int32_t)(posControl.desiredState.pos.z / 100), INT16_MIN, INT16_MAX); + uint16_t targetDistance = 0; + uint16_t wpNum = 0; + uint8_t heading = mavlinkHeadingDeg2FromCd(DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)); + uint8_t targetHeading = mavlinkHeadingDeg2FromCd(posControl.desiredState.yaw); + uint8_t groundspeed = 0; + uint8_t airspeed = 0; + uint8_t airspeedSp = 0; + uint8_t windspeed = 0; + uint8_t windHeading = 0; + uint8_t eph = UINT8_MAX; + uint8_t epv = UINT8_MAX; + int8_t temperatureAir = 0; + int8_t climbRate = (int8_t)constrain(lrintf(getEstimatedActualVelocity(Z) / 10.0f), INT8_MIN, INT8_MAX); + int8_t battery = feature(FEATURE_VBAT) ? (int8_t)calculateBatteryPercentage() : -1; + +#if defined(USE_GPS) + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + latitude = gpsSol.llh.lat; + longitude = gpsSol.llh.lon; + altitude = (int16_t)constrain((int32_t)(gpsSol.llh.alt / 100), INT16_MIN, INT16_MAX); + + const int32_t desiredAltCm = lrintf(posControl.desiredState.pos.z); + const int32_t targetAltCm = GPS_home.alt + desiredAltCm; + targetAltitude = (int16_t)constrain(targetAltCm / 100, INT16_MIN, INT16_MAX); + + groundspeed = (uint8_t)constrain(lrintf(gpsSol.groundSpeed / 20.0f), 0, UINT8_MAX); + + if (gpsSol.flags.validEPE) { + eph = (uint8_t)constrain((gpsSol.eph + 5) / 10, 0, UINT8_MAX); + epv = (uint8_t)constrain((gpsSol.epv + 5) / 10, 0, UINT8_MAX); + } + + if (posControl.activeWaypointIndex >= 0) { + wpNum = (uint16_t)posControl.activeWaypointIndex; + targetDistance = (uint16_t)constrain(lrintf(posControl.wpDistance / 1000.0f), 0, UINT16_MAX); + } + } +#endif + +#if defined(USE_PITOT) + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + airspeed = (uint8_t)constrain(lrintf(getAirspeedEstimate() / 20.0f), 0, UINT8_MAX); + airspeedSp = airspeed; + } +#endif + + if (airspeedSp == 0) { + const float desiredVelXY = calc_length_pythagorean_2D(posControl.desiredState.vel.x, posControl.desiredState.vel.y); + airspeedSp = (uint8_t)constrain(lrintf(desiredVelXY / 20.0f), 0, UINT8_MAX); + } + + if (airspeed == 0) { + airspeed = groundspeed; + } + +#ifdef USE_WIND_ESTIMATOR + if (isEstimatedWindSpeedValid()) { + uint16_t windAngleCd = 0; + const float windHoriz = getEstimatedHorizontalWindSpeed(&windAngleCd); + windspeed = (uint8_t)constrain(lrintf(windHoriz / 20.0f), 0, UINT8_MAX); + windHeading = mavlinkHeadingDeg2FromCd(windAngleCd); + } +#endif + + int16_t temperature; + sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); + temperatureAir = (int8_t)constrain(temperature, INT8_MIN, INT8_MAX); + + const uint16_t failureFlags = mavlinkComputeHighLatencyFailures(); + + mavlink_msg_high_latency2_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + (uint32_t)(currentTimeUs / 1000), + mavlinkGetVehicleType(), + mavlinkGetAutopilotEnum(), + modeSelection.customMode, + latitude, + longitude, + altitude, + targetAltitude, + heading, + targetHeading, + targetDistance, + (uint8_t)constrain(getThrottlePercent(osdUsingScaledThrottle()), 0, 100), + airspeed, + airspeedSp, + groundspeed, + windspeed, + windHeading, + eph, + epv, + temperatureAir, + climbRate, + battery, + wpNum, + failureFlags, + 0, + 0, + 0); + + mavlinkSendMessage(); +} + void processMAVLinkTelemetry(timeUs_t currentTimeUs) { + if (mavActivePort->highLatencyEnabled && mavActiveConfig->version != 1) { + if ((currentTimeUs - mavActivePort->lastHighLatencyMessageUs) >= TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US) { + mavlinkSendHighLatency2(currentTimeUs); + mavActivePort->lastHighLatencyMessageUs = currentTimeUs; + } + return; + } + // is executed @ TELEMETRY_MAVLINK_MAXRATE rate if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) { mavlinkSendSystemStatus(); @@ -1897,7 +2208,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem if (mavlinkMessageToStream((uint16_t)param1, &stream)) { if (param2 == 0.0f) { - mavlinkSetStreamRate(stream, mavRatesConfigured[stream]); + mavlinkSetStreamRate(stream, mavActivePort->mavRatesConfigured[stream]); result = MAV_RESULT_ACCEPTED; } else if (param2 < 0.0f) { mavlinkSetStreamRate(stream, 0); @@ -1939,8 +2250,24 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); return true; } + case MAV_CMD_CONTROL_HIGH_LATENCY: + if (param1 == 0.0f || param1 == 1.0f) { + if (mavActiveConfig->version == 1 && param1 > 0.5f) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + mavActivePort->highLatencyEnabled = param1 > 0.5f; + if (mavActivePort->highLatencyEnabled) { + mavActivePort->lastHighLatencyMessageUs = 0; + } + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); + } + return true; case MAV_CMD_REQUEST_PROTOCOL_VERSION: - if (telemetryConfig()->mavlink.version == 1) { + if (mavActiveConfig->version == 1) { mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); } else { mavlinkSendProtocolVersion(); @@ -1948,7 +2275,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem } return true; case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: - if (telemetryConfig()->mavlink.version == 1) { + if (mavActiveConfig->version == 1) { mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); } else { mavlinkSendAutopilotVersion(); @@ -1966,13 +2293,13 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem sent = true; break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION: - if (telemetryConfig()->mavlink.version != 1) { + if (mavActiveConfig->version != 1) { mavlinkSendAutopilotVersion(); sent = true; } break; case MAVLINK_MSG_ID_PROTOCOL_VERSION: - if (telemetryConfig()->mavlink.version != 1) { + if (mavActiveConfig->version != 1) { mavlinkSendProtocolVersion(); sent = true; } @@ -2283,7 +2610,7 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { } static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { - switch(telemetryConfig()->mavlink.radio_type) { + switch(mavActiveConfig->radio_type) { case MAVLINK_RADIO_SIK: // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; @@ -2307,8 +2634,13 @@ static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { static bool handleIncoming_RADIO_STATUS(void) { mavlink_radio_status_t msg; mavlink_msg_radio_status_decode(&mavRecvMsg, &msg); - txbuff_valid = true; - txbuff_free = msg.txbuf; + if (msg.txbuf > 0) { + mavActivePort->txbuffValid = true; + mavActivePort->txbuffFree = msg.txbuf; + } else { + mavActivePort->txbuffValid = false; + mavActivePort->txbuffFree = 100; + } if (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { @@ -2360,14 +2692,164 @@ static bool handleIncoming_ADSB_VEHICLE(void) { } #endif +static bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid) +{ + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + const mavlinkTelemetryPortConfig_t *cfg = mavlinkGetPortConfig(portIndex); + if (cfg->sysid == sysid && cfg->compid == compid) { + return true; + } + } + + return false; +} + +static void mavlinkLearnRoute(uint8_t ingressPortIndex) +{ + if (mavRecvMsg.sysid == 0 || mavlinkIsFromLocalIdentity(mavRecvMsg.sysid, mavRecvMsg.compid)) { + return; + } + + for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { + mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; + if (route->sysid == mavRecvMsg.sysid && route->compid == mavRecvMsg.compid) { + route->ingressPortIndex = ingressPortIndex; + return; + } + } + + if (mavRouteCount >= MAVLINK_MAX_ROUTES) { + return; + } + + mavRouteTable[mavRouteCount].sysid = mavRecvMsg.sysid; + mavRouteTable[mavRouteCount].compid = mavRecvMsg.compid; + mavRouteTable[mavRouteCount].ingressPortIndex = ingressPortIndex; + mavRouteCount++; +} + +static void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent) +{ + *targetSystem = -1; + *targetComponent = -1; + + const mavlink_msg_entry_t *msgEntry = mavlink_get_msg_entry(msg->msgid); + if (!msgEntry) { + return; + } + + if (msgEntry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { + *targetSystem = _MAV_RETURN_uint8_t(msg, msgEntry->target_system_ofs); + } + + if (msgEntry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { + *targetComponent = _MAV_RETURN_uint8_t(msg, msgEntry->target_component_ofs); + } +} + +static bool mavlinkRouteMatchesTargetOnPort(uint8_t portIndex, int16_t targetSystem, int16_t targetComponent) +{ + for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { + const mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; + if (route->ingressPortIndex != portIndex || route->sysid != targetSystem) { + continue; + } + + if (targetComponent <= 0 || route->compid == targetComponent) { + return true; + } + } + + return false; +} + +static void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent) +{ + if (mavRecvMsg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + return; + } + + uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; + const int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavRecvMsg); + + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if (portIndex == ingressPortIndex) { + continue; + } + + const mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + bool shouldForward = false; + if (targetSystem <= 0) { + shouldForward = true; + } else if (mavlinkRouteMatchesTargetOnPort(portIndex, targetSystem, targetComponent)) { + shouldForward = true; + } + + if (!shouldForward) { + continue; + } + + for (int i = 0; i < msgLength; i++) { + serialWrite(state->port, mavBuffer[i]); + } + } +} + +static int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t targetComponent, uint8_t ingressPortIndex) +{ + if (targetSystem <= 0) { + return ingressPortIndex; + } + + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + const mavlinkTelemetryPortConfig_t *cfg = mavlinkGetPortConfig(portIndex); + if (cfg->sysid != targetSystem) { + continue; + } + + if (targetComponent <= 0 || cfg->compid == targetComponent) { + return portIndex; + } + } + + return -1; +} + // Returns whether a message was processed -static bool processMAVLinkIncomingTelemetry(void) +static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) { - while (serialRxBytesWaiting(mavlinkPort) > 0) { + mavlinkPortRuntime_t *state = &mavPortStates[ingressPortIndex]; + + while (serialRxBytesWaiting(state->port) > 0) { // Limit handling to one message per cycle - char c = serialRead(mavlinkPort); - uint8_t result = mavlink_parse_char(0, c, &mavRecvMsg, &mavRecvStatus); + const char c = serialRead(state->port); + const uint8_t result = mavlink_parse_char(ingressPortIndex, c, &state->mavRecvMsg, &state->mavRecvStatus); if (result == MAVLINK_FRAMING_OK) { + mavRecvMsg = state->mavRecvMsg; + + if (mavlinkIsFromLocalIdentity(mavRecvMsg.sysid, mavRecvMsg.compid)) { + return false; + } + + mavlinkLearnRoute(ingressPortIndex); + + int16_t targetSystem; + int16_t targetComponent; + mavlinkExtractTargets(&mavRecvMsg, &targetSystem, &targetComponent); + mavlinkForwardMessage(ingressPortIndex, targetSystem, targetComponent); + + const int8_t localPortIndex = mavlinkResolveLocalPortForTarget(targetSystem, targetComponent, ingressPortIndex); + if (localPortIndex < 0) { + return false; + } + + mavlinkSetActivePortContext((uint8_t)localPortIndex); + mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); + switch (mavRecvMsg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: return handleIncoming_HEARTBEAT(); @@ -2424,32 +2906,40 @@ static bool isMAVLinkTelemetryHalfDuplex(void) { void handleMAVLinkTelemetry(timeUs_t currentTimeUs) { - if (!mavlinkTelemetryEnabled) { - return; - } + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } - if (!mavlinkPort) { - return; - } + mavlinkSetActivePortContext(portIndex); - // Process incoming MAVLink - bool receivedMessage = processMAVLinkIncomingTelemetry(); - bool shouldSendTelemetry = false; + // Process incoming MAVLink on this port and forward when needed. + const bool receivedMessage = processMAVLinkIncomingTelemetry(portIndex); - // Determine whether to send telemetry back based on flow control / pacing - if (txbuff_valid) { - // Use flow control if available - shouldSendTelemetry = txbuff_free >= telemetryConfig()->mavlink.min_txbuff; - } else { - // If not, use blind frame pacing - and back off for collision avoidance if half-duplex - bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage); - shouldSendTelemetry = ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) && !halfDuplexBackoff; - } + // Restore context back to this port before periodic send decisions. + mavlinkSetActivePortContext(portIndex); + bool shouldSendTelemetry = false; - if (shouldSendTelemetry) { + if (state->txbuffValid) { + // Use flow control if available. + shouldSendTelemetry = state->txbuffFree >= mavActiveConfig->min_txbuff; + } else { + // If not, use blind frame pacing and half-duplex backoff after RX activity. + const bool halfDuplexBackoff = isMAVLinkTelemetryHalfDuplex() && receivedMessage; + shouldSendTelemetry = ((currentTimeUs - state->lastMavlinkMessageUs) >= TELEMETRY_MAVLINK_DELAY) && !halfDuplexBackoff; + } + + if (!shouldSendTelemetry) { + continue; + } + + mavSendMask = MAVLINK_PORT_MASK(portIndex); processMAVLinkTelemetry(currentTimeUs); - lastMavlinkMessage = currentTimeUs; + state->lastMavlinkMessageUs = currentTimeUs; } + + mavSendMask = 0; } #endif diff --git a/src/main/telemetry/mavlink.h b/src/main/telemetry/mavlink.h index f5b51e1e3ef..899674194de 100755 --- a/src/main/telemetry/mavlink.h +++ b/src/main/telemetry/mavlink.h @@ -22,4 +22,3 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs); void checkMAVLinkTelemetryState(void); void freeMAVLinkTelemetryPort(void); -void configureMAVLinkTelemetryPort(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 2c896945843..b771fc5c86c 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -56,7 +56,7 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 9); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, @@ -87,17 +87,51 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, #endif .mavlink = { - .autopilot_type = SETTING_MAVLINK_AUTOPILOT_TYPE_DEFAULT, - .extended_status_rate = SETTING_MAVLINK_EXT_STATUS_RATE_DEFAULT, - .rc_channels_rate = SETTING_MAVLINK_RC_CHAN_RATE_DEFAULT, - .position_rate = SETTING_MAVLINK_POS_RATE_DEFAULT, - .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, - .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, - .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, - .version = SETTING_MAVLINK_VERSION_DEFAULT, - .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, - .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT, - .sysid = SETTING_MAVLINK_SYSID_DEFAULT + { + .autopilot_type = SETTING_MAVLINK_AUTOPILOT_TYPE_DEFAULT, + .extended_status_rate = SETTING_MAVLINK_EXT_STATUS_RATE_DEFAULT, + .rc_channels_rate = SETTING_MAVLINK_RC_CHAN_RATE_DEFAULT, + .position_rate = SETTING_MAVLINK_POS_RATE_DEFAULT, + .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, + .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, + .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, + .version = SETTING_MAVLINK_VERSION_DEFAULT, + .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT, + .sysid = SETTING_MAVLINK_SYSID_DEFAULT, + .compid = SETTING_MAVLINK_PORT1_COMPID_DEFAULT, + .high_latency = SETTING_MAVLINK_PORT1_HIGH_LATENCY_DEFAULT + }, + { + .autopilot_type = SETTING_MAVLINK_PORT2_AUTOPILOT_TYPE_DEFAULT, + .extended_status_rate = SETTING_MAVLINK_PORT2_EXT_STATUS_RATE_DEFAULT, + .rc_channels_rate = SETTING_MAVLINK_PORT2_RC_CHAN_RATE_DEFAULT, + .position_rate = SETTING_MAVLINK_PORT2_POS_RATE_DEFAULT, + .extra1_rate = SETTING_MAVLINK_PORT2_EXTRA1_RATE_DEFAULT, + .extra2_rate = SETTING_MAVLINK_PORT2_EXTRA2_RATE_DEFAULT, + .extra3_rate = SETTING_MAVLINK_PORT2_EXTRA3_RATE_DEFAULT, + .version = SETTING_MAVLINK_PORT2_VERSION_DEFAULT, + .min_txbuff = SETTING_MAVLINK_PORT2_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_PORT2_RADIO_TYPE_DEFAULT, + .sysid = SETTING_MAVLINK_PORT2_SYSID_DEFAULT, + .compid = SETTING_MAVLINK_PORT2_COMPID_DEFAULT, + .high_latency = SETTING_MAVLINK_PORT2_HIGH_LATENCY_DEFAULT + }, + { + .autopilot_type = SETTING_MAVLINK_PORT3_AUTOPILOT_TYPE_DEFAULT, + .extended_status_rate = SETTING_MAVLINK_PORT3_EXT_STATUS_RATE_DEFAULT, + .rc_channels_rate = SETTING_MAVLINK_PORT3_RC_CHAN_RATE_DEFAULT, + .position_rate = SETTING_MAVLINK_PORT3_POS_RATE_DEFAULT, + .extra1_rate = SETTING_MAVLINK_PORT3_EXTRA1_RATE_DEFAULT, + .extra2_rate = SETTING_MAVLINK_PORT3_EXTRA2_RATE_DEFAULT, + .extra3_rate = SETTING_MAVLINK_PORT3_EXTRA3_RATE_DEFAULT, + .version = SETTING_MAVLINK_PORT3_VERSION_DEFAULT, + .min_txbuff = SETTING_MAVLINK_PORT3_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_PORT3_RADIO_TYPE_DEFAULT, + .sysid = SETTING_MAVLINK_PORT3_SYSID_DEFAULT, + .compid = SETTING_MAVLINK_PORT3_COMPID_DEFAULT, + .high_latency = SETTING_MAVLINK_PORT3_HIGH_LATENCY_DEFAULT + } } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 7fb26781c11..bc363c930a8 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -30,6 +30,10 @@ #include "io/serial.h" +#ifndef MAX_MAVLINK_PORTS +#define MAX_MAVLINK_PORTS 3 +#endif + typedef enum { LTM_RATE_NORMAL, LTM_RATE_MEDIUM, @@ -53,6 +57,22 @@ typedef enum { SMARTPORT_FUEL_UNIT_MWH } smartportFuelUnit_e; +typedef struct mavlinkTelemetryPortConfig_s { + uint8_t autopilot_type; + uint8_t extended_status_rate; + uint8_t rc_channels_rate; + uint8_t position_rate; + uint8_t extra1_rate; + uint8_t extra2_rate; + uint8_t extra3_rate; + uint8_t version; + uint8_t min_txbuff; + uint8_t radio_type; + uint8_t sysid; + uint8_t compid; + bool high_latency; +} mavlinkTelemetryPortConfig_t; + typedef struct telemetryConfig_s { uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inverted; // Flip the default inversion of the protocol - Same as serialrx_inverted in rx.c, but for telemetry. @@ -76,19 +96,7 @@ typedef struct telemetryConfig_s { uint16_t accEventThresholdLow; uint16_t accEventThresholdNegX; #endif - struct { - uint8_t autopilot_type; - uint8_t extended_status_rate; - uint8_t rc_channels_rate; - uint8_t position_rate; - uint8_t extra1_rate; - uint8_t extra2_rate; - uint8_t extra3_rate; - uint8_t version; - uint8_t min_txbuff; - uint8_t radio_type; - uint8_t sysid; - } mavlink; + mavlinkTelemetryPortConfig_t mavlink[MAX_MAVLINK_PORTS]; } telemetryConfig_t; PG_DECLARE(telemetryConfig_t, telemetryConfig); From 4436964f110549822b6abed3b966a3236d5ce150 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Thu, 26 Feb 2026 23:15:43 -0500 Subject: [PATCH 25/42] small refactor + settings + common.h max mav ports define --- docs/Mavlink.md | 19 ++- docs/Settings.md | 160 ++++++++++++++++---------- src/main/fc/settings.yaml | 144 +++++++++++++---------- src/main/io/serial.c | 2 - src/main/rx/mavlink.h | 4 +- src/main/target/common.h | 5 +- src/main/telemetry/mavlink.c | 203 +++++++-------------------------- src/main/telemetry/mavlink.h | 152 ++++++++++++++++++++++++ src/main/telemetry/telemetry.c | 44 ++++--- src/main/telemetry/telemetry.h | 17 +-- 10 files changed, 427 insertions(+), 323 deletions(-) diff --git a/docs/Mavlink.md b/docs/Mavlink.md index b29dcdf8f89..d39c1165c25 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -16,9 +16,9 @@ INAV has a partial implementation of MAVLink that is intended primarily for simp - `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. +- Stream rates (Hz): `mavlink_port1_ext_status_rate`, `mavlink_port1_rc_chan_rate`, `mavlink_port1_pos_rate`, `mavlink_port1_extra1_rate`, `mavlink_port1_extra2_rate`, `mavlink_port1_extra3_rate`. Each group is polled up to 50 Hz; a rate of 0 disables the group. +- `mavlink_port1_min_txbuffer` – minimum remote TX buffer level before sending when `RADIO_STATUS` provides flow control. +- `mavlink_port1_radio_type` – scales `RADIO_STATUS` RSSI/SNR for **generic**, **ELRS**, or **SiK** links. ## Supported Outgoing Messages @@ -46,10 +46,10 @@ 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. `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). +- `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_port1_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,7 +59,6 @@ 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`. @@ -105,12 +104,12 @@ Default rates (Hz) are shown; adjust with the CLI keys above. | `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`) | +| `EXT_SYS_STATE` | `EXTENDED_SYS_STATE` | 1 Hz (defaults to `mavlink_port1_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`. +- Set `mavlink_port1_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. diff --git a/docs/Settings.md b/docs/Settings.md index 610834d0d7f..939991a9792 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2652,9 +2652,19 @@ Autopilot type to advertise for MAVLink telemetry --- -### mavlink_ext_status_rate +### mavlink_port1_compid + +MAVLink Component ID for MAVLink port 1 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 255 | + +--- -Rate of the extended status message for MAVLink telemetry +### mavlink_port1_ext_status_rate + +Rate of the extended status message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2662,9 +2672,9 @@ Rate of the extended status message for MAVLink telemetry --- -### mavlink_extra1_rate +### mavlink_port1_extra1_rate -Rate of the extra1 message for MAVLink telemetry +Rate of the extra1 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2672,9 +2682,9 @@ Rate of the extra1 message for MAVLink telemetry --- -### mavlink_extra2_rate +### mavlink_port1_extra2_rate -Rate of the extra2 message for MAVLink telemetry +Rate of the extra2 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2682,9 +2692,9 @@ Rate of the extra2 message for MAVLink telemetry --- -### mavlink_extra3_rate +### mavlink_port1_extra3_rate -Rate of the extra3 message for MAVLink telemetry +Rate of the extra3 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2692,39 +2702,39 @@ Rate of the extra3 message for MAVLink telemetry --- -### mavlink_min_txbuffer +### mavlink_port1_high_latency -Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. +Enable MAVLink high-latency mode on port 1 | Default | Min | Max | | --- | --- | --- | -| 33 | 0 | 100 | +| OFF | OFF | ON | --- -### mavlink_port1_compid +### mavlink_port1_min_txbuffer -MAVLink Component ID for MAVLink port 1 +Minimum percent of TX buffer space free for MAVLink port 1. Requires RADIO_STATUS messages. | Default | Min | Max | | --- | --- | --- | -| 1 | 1 | 255 | +| 33 | 0 | 100 | --- -### mavlink_port1_high_latency +### mavlink_port1_pos_rate -Enable MAVLink high-latency mode on port 1 +Rate of the position message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| 2 | 0 | 255 | --- -### mavlink_port2_autopilot_type +### mavlink_port1_radio_type -Autopilot type to advertise for MAVLink telemetry on port 2 +MAVLink radio type for port 1. Affects RSSI and LQ reporting on OSD. | Default | Min | Max | | --- | --- | --- | @@ -2732,6 +2742,16 @@ Autopilot type to advertise for MAVLink telemetry on port 2 --- +### mavlink_port1_rc_chan_rate + +Rate of the RC channels message for MAVLink telemetry on port 1 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 255 | + +--- + ### mavlink_port2_compid MAVLink Component ID for MAVLink port 2 @@ -2832,36 +2852,6 @@ Rate of the RC channels message for MAVLink telemetry on port 2 --- -### mavlink_port2_sysid - -MAVLink System ID for MAVLink port 2 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 1 | 255 | - ---- - -### mavlink_port2_version - -Version of MAVLink to use on port 2 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 1 | 2 | - ---- - -### mavlink_port3_autopilot_type - -Autopilot type to advertise for MAVLink telemetry on port 3 - -| Default | Min | Max | -| --- | --- | --- | -| GENERIC | | | - ---- - ### mavlink_port3_compid MAVLink Component ID for MAVLink port 3 @@ -2962,9 +2952,9 @@ Rate of the RC channels message for MAVLink telemetry on port 3 --- -### mavlink_port3_sysid +### mavlink_port4_compid -MAVLink System ID for MAVLink port 3 +MAVLink Component ID for MAVLink port 4 | Default | Min | Max | | --- | --- | --- | @@ -2972,19 +2962,69 @@ MAVLink System ID for MAVLink port 3 --- -### mavlink_port3_version +### mavlink_port4_ext_status_rate -Version of MAVLink to use on port 3 +Rate of the extended status message for MAVLink telemetry on port 4 | Default | Min | Max | | --- | --- | --- | -| 2 | 1 | 2 | +| 2 | 0 | 255 | + +--- + +### mavlink_port4_extra1_rate + +Rate of the extra1 message for MAVLink telemetry on port 4 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port4_extra2_rate + +Rate of the extra2 message for MAVLink telemetry on port 4 + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_port4_extra3_rate + +Rate of the extra3 message for MAVLink telemetry on port 4 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 255 | + +--- + +### mavlink_port4_high_latency + +Enable MAVLink high-latency mode on port 4 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port4_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 4. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | --- -### mavlink_pos_rate +### mavlink_port4_pos_rate -Rate of the position message for MAVLink telemetry +Rate of the position message for MAVLink telemetry on port 4 | Default | Min | Max | | --- | --- | --- | @@ -2992,9 +3032,9 @@ Rate of the position message for MAVLink telemetry --- -### mavlink_radio_type +### mavlink_port4_radio_type -Mavlink radio type. Affects how RSSI and LQ are reported on OSD. +MAVLink radio type for port 4. Affects RSSI and LQ reporting on OSD. | Default | Min | Max | | --- | --- | --- | @@ -3002,9 +3042,9 @@ Mavlink radio type. Affects how RSSI and LQ are reported on OSD. --- -### mavlink_rc_chan_rate +### mavlink_port4_rc_chan_rate -Rate of the RC channels message for MAVLink telemetry +Rate of the RC channels message for MAVLink telemetry on port 4 | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c7342f50b17..4d262c9b63e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3224,75 +3224,75 @@ groups: type: int16_t min: INT16_MIN max: INT16_MAX - - name: mavlink_ext_status_rate + - name: mavlink_port1_ext_status_rate field: mavlink[0].extended_status_rate - description: "Rate of the extended status message for MAVLink telemetry" + description: "Rate of the extended status message for MAVLink telemetry on port 1" type: uint8_t min: 0 max: 255 default_value: 2 - name: mavlink_autopilot_type - field: mavlink[0].autopilot_type + field: mavlink_common.autopilot_type description: "Autopilot type to advertise for MAVLink telemetry" table: mavlink_autopilot_type default_value: "GENERIC" type: uint8_t - - name: mavlink_rc_chan_rate - description: "Rate of the RC channels message for MAVLink telemetry" + - name: mavlink_port1_rc_chan_rate + description: "Rate of the RC channels message for MAVLink telemetry on port 1" field: mavlink[0].rc_channels_rate type: uint8_t min: 0 max: 255 default_value: 1 - - name: mavlink_pos_rate - description: "Rate of the position message for MAVLink telemetry" + - name: mavlink_port1_pos_rate + description: "Rate of the position message for MAVLink telemetry on port 1" field: mavlink[0].position_rate type: uint8_t min: 0 max: 255 default_value: 2 - - name: mavlink_extra1_rate - description: "Rate of the extra1 message for MAVLink telemetry" + - name: mavlink_port1_extra1_rate + description: "Rate of the extra1 message for MAVLink telemetry on port 1" field: mavlink[0].extra1_rate type: uint8_t min: 0 max: 255 default_value: 2 - - name: mavlink_extra2_rate - description: "Rate of the extra2 message for MAVLink telemetry" + - name: mavlink_port1_extra2_rate + description: "Rate of the extra2 message for MAVLink telemetry on port 1" field: mavlink[0].extra2_rate type: uint8_t min: 0 max: 255 default_value: 2 - - name: mavlink_extra3_rate - description: "Rate of the extra3 message for MAVLink telemetry" + - name: mavlink_port1_extra3_rate + description: "Rate of the extra3 message for MAVLink telemetry on port 1" field: mavlink[0].extra3_rate type: uint8_t min: 0 max: 255 default_value: 1 - name: mavlink_version - field: mavlink[0].version + field: mavlink_common.version description: "Version of MAVLink to use" type: uint8_t min: 1 max: 2 default_value: 2 - - name: mavlink_min_txbuffer + - name: mavlink_port1_min_txbuffer field: mavlink[0].min_txbuff - description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." + description: "Minimum percent of TX buffer space free for MAVLink port 1. Requires RADIO_STATUS messages." default_value: 33 min: 0 max: 100 - - name: mavlink_radio_type - description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD." + - name: mavlink_port1_radio_type + description: "MAVLink radio type for port 1. Affects RSSI and LQ reporting on OSD." field: mavlink[0].radio_type table: mavlink_radio_type default_value: "GENERIC" type: uint8_t - name: mavlink_sysid - field: mavlink[0].sysid + field: mavlink_common.sysid description: "MAVLink System ID" type: uint8_t min: 1 @@ -3317,12 +3317,6 @@ groups: min: 0 max: 255 default_value: 2 - - name: mavlink_port2_autopilot_type - field: mavlink[1].autopilot_type - description: "Autopilot type to advertise for MAVLink telemetry on port 2" - table: mavlink_autopilot_type - default_value: "GENERIC" - type: uint8_t - name: mavlink_port2_rc_chan_rate description: "Rate of the RC channels message for MAVLink telemetry on port 2" field: mavlink[1].rc_channels_rate @@ -3358,13 +3352,6 @@ groups: min: 0 max: 255 default_value: 1 - - name: mavlink_port2_version - field: mavlink[1].version - description: "Version of MAVLink to use on port 2" - type: uint8_t - min: 1 - max: 2 - default_value: 2 - name: mavlink_port2_min_txbuffer field: mavlink[1].min_txbuff description: "Minimum percent of TX buffer space free for MAVLink port 2. Requires RADIO_STATUS messages." @@ -3377,13 +3364,6 @@ groups: table: mavlink_radio_type default_value: "GENERIC" type: uint8_t - - name: mavlink_port2_sysid - field: mavlink[1].sysid - description: "MAVLink System ID for MAVLink port 2" - type: uint8_t - min: 1 - max: 255 - default_value: 1 - name: mavlink_port2_compid field: mavlink[1].compid description: "MAVLink Component ID for MAVLink port 2" @@ -3403,12 +3383,6 @@ groups: min: 0 max: 255 default_value: 2 - - name: mavlink_port3_autopilot_type - field: mavlink[2].autopilot_type - description: "Autopilot type to advertise for MAVLink telemetry on port 3" - table: mavlink_autopilot_type - default_value: "GENERIC" - type: uint8_t - name: mavlink_port3_rc_chan_rate description: "Rate of the RC channels message for MAVLink telemetry on port 3" field: mavlink[2].rc_channels_rate @@ -3444,13 +3418,6 @@ groups: min: 0 max: 255 default_value: 1 - - name: mavlink_port3_version - field: mavlink[2].version - description: "Version of MAVLink to use on port 3" - type: uint8_t - min: 1 - max: 2 - default_value: 2 - name: mavlink_port3_min_txbuffer field: mavlink[2].min_txbuff description: "Minimum percent of TX buffer space free for MAVLink port 3. Requires RADIO_STATUS messages." @@ -3463,13 +3430,6 @@ groups: table: mavlink_radio_type default_value: "GENERIC" type: uint8_t - - name: mavlink_port3_sysid - field: mavlink[2].sysid - description: "MAVLink System ID for MAVLink port 3" - type: uint8_t - min: 1 - max: 255 - default_value: 1 - name: mavlink_port3_compid field: mavlink[2].compid description: "MAVLink Component ID for MAVLink port 3" @@ -3482,6 +3442,72 @@ groups: description: "Enable MAVLink high-latency mode on port 3" type: bool default_value: OFF + - name: mavlink_port4_ext_status_rate + field: mavlink[3].extended_status_rate + description: "Rate of the extended status message for MAVLink telemetry on port 4" + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port4_rc_chan_rate + description: "Rate of the RC channels message for MAVLink telemetry on port 4" + field: mavlink[3].rc_channels_rate + type: uint8_t + min: 0 + max: 255 + default_value: 1 + - name: mavlink_port4_pos_rate + description: "Rate of the position message for MAVLink telemetry on port 4" + field: mavlink[3].position_rate + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port4_extra1_rate + description: "Rate of the extra1 message for MAVLink telemetry on port 4" + field: mavlink[3].extra1_rate + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port4_extra2_rate + description: "Rate of the extra2 message for MAVLink telemetry on port 4" + field: mavlink[3].extra2_rate + type: uint8_t + min: 0 + max: 255 + default_value: 2 + - name: mavlink_port4_extra3_rate + description: "Rate of the extra3 message for MAVLink telemetry on port 4" + field: mavlink[3].extra3_rate + type: uint8_t + min: 0 + max: 255 + default_value: 1 + - name: mavlink_port4_min_txbuffer + field: mavlink[3].min_txbuff + description: "Minimum percent of TX buffer space free for MAVLink port 4. Requires RADIO_STATUS messages." + default_value: 33 + min: 0 + max: 100 + - name: mavlink_port4_radio_type + description: "MAVLink radio type for port 4. Affects RSSI and LQ reporting on OSD." + field: mavlink[3].radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t + - name: mavlink_port4_compid + field: mavlink[3].compid + description: "MAVLink Component ID for MAVLink port 4" + type: uint8_t + min: 1 + max: 255 + default_value: 1 + - name: mavlink_port4_high_latency + field: mavlink[3].high_latency + description: "Enable MAVLink high-latency mode on port 4" + type: bool + default_value: OFF - name: PG_OSD_CONFIG type: osdConfig_t diff --git a/src/main/io/serial.c b/src/main/io/serial.c index d6b82da02b8..e7865f3fac2 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -296,10 +296,8 @@ bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) if ((portConfig->functionMask & FUNCTION_MSP) && (portConfig->functionMask & ALL_FUNCTIONS_SHARABLE_WITH_MSP)) { // MSP & telemetry -#ifdef USE_TELEMETRY } else if (telemetryCheckRxPortShared(portConfig)) { // serial RX & telemetry -#endif } else { // some other combination return false; diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h index 644e71d4f8d..c891039ddb8 100644 --- a/src/main/rx/mavlink.h +++ b/src/main/rx/mavlink.h @@ -17,9 +17,7 @@ #pragma once -#ifndef MAX_MAVLINK_PORTS -#define MAX_MAVLINK_PORTS 3 -#endif +#include "target/common.h" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" diff --git a/src/main/target/common.h b/src/main/target/common.h index 45eb12ac4bc..e63eff4785d 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -31,6 +31,10 @@ #define DYNAMIC_HEAP_SIZE 2048 +#ifndef MAX_MAVLINK_PORTS +#define MAX_MAVLINK_PORTS 4 +#endif + #define I2C1_OVERCLOCK false #define I2C2_OVERCLOCK false #define USE_I2C_PULLUP // Enable built-in pullups on all boards in case external ones are too week @@ -226,4 +230,3 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER - diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 74665525ec0..3ccf552c77b 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -90,111 +90,8 @@ #include "scheduler/scheduler.h" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-function" -#ifndef MAVLINK_COMM_NUM_BUFFERS -#define MAVLINK_COMM_NUM_BUFFERS MAX_MAVLINK_PORTS -#endif -#include "common/mavlink.h" -#pragma GCC diagnostic pop - -#define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX -#define TELEMETRY_MAVLINK_MAXRATE 50 -#define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) -#define TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US (5 * 1000 * 1000) -#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 MAVLINK_MAX_ROUTES 32 -#define MAVLINK_PORT_MASK(portIndex) (1U << (portIndex)) - -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. - * This converts angles from a range of 0..Pi to -Pi..Pi - */ -#define RADIANS_TO_MAVLINK_RANGE(angle) (angle > M_PIf) ? angle - (2 * M_PIf) : angle - -/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ -typedef enum APM_PLANE_MODE -{ - PLANE_MODE_MANUAL=0, - PLANE_MODE_CIRCLE=1, - PLANE_MODE_STABILIZE=2, - PLANE_MODE_TRAINING=3, - PLANE_MODE_ACRO=4, - PLANE_MODE_FLY_BY_WIRE_A=5, - PLANE_MODE_FLY_BY_WIRE_B=6, - PLANE_MODE_CRUISE=7, - PLANE_MODE_AUTOTUNE=8, - PLANE_MODE_AUTO=10, - PLANE_MODE_RTL=11, - PLANE_MODE_LOITER=12, - PLANE_MODE_TAKEOFF=13, - PLANE_MODE_AVOID_ADSB=14, - PLANE_MODE_GUIDED=15, - PLANE_MODE_INITIALIZING=16, - PLANE_MODE_QSTABILIZE=17, - PLANE_MODE_QHOVER=18, - PLANE_MODE_QLOITER=19, - PLANE_MODE_QLAND=20, - PLANE_MODE_QRTL=21, - PLANE_MODE_QAUTOTUNE=22, - 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. */ -typedef enum APM_COPTER_MODE -{ - COPTER_MODE_STABILIZE=0, - COPTER_MODE_ACRO=1, - COPTER_MODE_ALT_HOLD=2, - COPTER_MODE_AUTO=3, - COPTER_MODE_GUIDED=4, - COPTER_MODE_LOITER=5, - COPTER_MODE_RTL=6, - COPTER_MODE_CIRCLE=7, - COPTER_MODE_LAND=9, - COPTER_MODE_DRIFT=11, - COPTER_MODE_SPORT=13, - COPTER_MODE_FLIP=14, - COPTER_MODE_AUTOTUNE=15, - COPTER_MODE_POSHOLD=16, - COPTER_MODE_BRAKE=17, - COPTER_MODE_THROW=18, - COPTER_MODE_AVOID_ADSB=19, - COPTER_MODE_GUIDED_NOGPS=20, - COPTER_MODE_SMART_RTL=21, - 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; - -typedef struct mavlinkRouteEntry_s { - uint8_t sysid; - uint8_t compid; - uint8_t ingressPortIndex; -} mavlinkRouteEntry_t; - /* MAVLink datastream rates in Hz */ -static const uint8_t mavDefaultRates[] = { +const uint8_t mavDefaultRates[MAVLINK_STREAM_COUNT] = { [MAV_DATA_STREAM_EXTENDED_STATUS] = 2, // 2Hz [MAV_DATA_STREAM_RC_CHANNELS] = 1, // 1Hz [MAV_DATA_STREAM_POSITION] = 2, // 2Hz @@ -204,25 +101,6 @@ static const uint8_t mavDefaultRates[] = { [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = 1 // 1Hz }; -typedef struct mavlinkPortRuntime_s { - serialPort_t *port; - const serialPortConfig_t *portConfig; - portSharing_e portSharing; - bool telemetryEnabled; - bool txbuffValid; - uint8_t txbuffFree; - timeUs_t lastMavlinkMessageUs; - timeUs_t lastHighLatencyMessageUs; - bool highLatencyEnabled; - uint8_t mavRates[ARRAYLEN(mavDefaultRates)]; - uint8_t mavRatesConfigured[ARRAYLEN(mavDefaultRates)]; - uint8_t mavTicks[ARRAYLEN(mavDefaultRates)]; - mavlink_message_t mavRecvMsg; - mavlink_status_t mavRecvStatus; -} mavlinkPortRuntime_t; - -#define MAXSTREAMS (ARRAYLEN(mavDefaultRates)) - static mavlinkPortRuntime_t mavPortStates[MAX_MAVLINK_PORTS]; static uint8_t mavPortCount = 0; static mavlinkRouteEntry_t mavRouteTable[MAVLINK_MAX_ROUTES]; @@ -357,10 +235,20 @@ static const mavlinkTelemetryPortConfig_t *mavlinkGetPortConfig(uint8_t portInde return &telemetryConfig()->mavlink[portIndex]; } +static const mavlinkTelemetryCommonConfig_t *mavlinkGetCommonConfig(void) +{ + return &telemetryConfig()->mavlink_common; +} + +static uint8_t mavlinkGetProtocolVersion(void) +{ + return mavlinkGetCommonConfig()->version; +} + static void mavlinkApplyActivePortOutputVersion(void) { mavlink_status_t *chanState = mavlink_get_channel_status(MAVLINK_COMM_0); - if (mavActiveConfig->version == 1) { + if (mavlinkGetProtocolVersion() == 1) { chanState->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; } else { chanState->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; @@ -371,8 +259,9 @@ static void mavlinkSetActivePortContext(uint8_t portIndex) { mavActivePort = &mavPortStates[portIndex]; mavActiveConfig = mavlinkGetPortConfig(portIndex); - mavAutopilotType = mavActiveConfig->autopilot_type; - mavSystemId = mavActiveConfig->sysid; + const mavlinkTelemetryCommonConfig_t *commonConfig = mavlinkGetCommonConfig(); + mavAutopilotType = commonConfig->autopilot_type; + mavSystemId = commonConfig->sysid; mavComponentId = mavActiveConfig->compid; mavlinkApplyActivePortOutputVersion(); } @@ -572,7 +461,7 @@ static void mavlinkSendMessage(void) static void mavlinkSendAutopilotVersion(void) { - if (mavActiveConfig->version == 1) return; + if (mavlinkGetProtocolVersion() == 1) return; // Capabilities aligned with what we actually support. uint64_t capabilities = 0; @@ -605,11 +494,11 @@ static void mavlinkSendAutopilotVersion(void) static void mavlinkSendProtocolVersion(void) { - if (mavActiveConfig->version == 1) return; + if (mavlinkGetProtocolVersion() == 1) return; uint8_t specHash[8] = {0}; uint8_t libHash[8] = {0}; - const uint16_t protocolVersion = (uint16_t)mavActiveConfig->version * 100; + const uint16_t protocolVersion = (uint16_t)mavlinkGetProtocolVersion() * 100; mavlink_msg_protocol_version_pack( mavSystemId, @@ -692,12 +581,7 @@ static uint8_t navWaypointFrame(const navWaypoint_t *wp, bool useIntMessages) return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT; } -typedef struct { - uint8_t customMode; - const char *name; -} mavlinkModeDescriptor_t; - -static const mavlinkModeDescriptor_t planeModes[] = { +const mavlinkModeDescriptor_t planeModes[] = { { PLANE_MODE_MANUAL, "MANUAL" }, { PLANE_MODE_ACRO, "ACRO" }, { PLANE_MODE_STABILIZE, "STABILIZE" }, @@ -710,8 +594,9 @@ static const mavlinkModeDescriptor_t planeModes[] = { { PLANE_MODE_TAKEOFF, "TAKEOFF" }, { PLANE_MODE_GUIDED, "GUIDED" }, }; +const uint8_t planeModesCount = (uint8_t)ARRAYLEN(planeModes); -static const mavlinkModeDescriptor_t copterModes[] = { +const mavlinkModeDescriptor_t copterModes[] = { { COPTER_MODE_ACRO, "ACRO" }, { COPTER_MODE_STABILIZE, "STABILIZE" }, { COPTER_MODE_ALT_HOLD, "ALT_HOLD" }, @@ -726,6 +611,7 @@ static const mavlinkModeDescriptor_t copterModes[] = { { COPTER_MODE_SMART_RTL, "SMART_RTL" }, { COPTER_MODE_DRIFT, "DRIFT" }, }; +const uint8_t copterModesCount = (uint8_t)ARRAYLEN(copterModes); static bool mavlinkPlaneModeIsConfigured(uint8_t customMode) { @@ -1022,7 +908,7 @@ void mavlinkSendSystemStatus(void) void mavlinkSendRCChannelsAndRSSI(void) { #define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) - if (mavActiveConfig->version == 1) { + if (mavlinkGetProtocolVersion() == 1) { mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, // time_boot_ms Timestamp (milliseconds since system boot) millis(), @@ -1634,7 +1520,7 @@ static void mavlinkSendHighLatency2(timeUs_t currentTimeUs) void processMAVLinkTelemetry(timeUs_t currentTimeUs) { - if (mavActivePort->highLatencyEnabled && mavActiveConfig->version != 1) { + if (mavActivePort->highLatencyEnabled && mavlinkGetProtocolVersion() != 1) { if ((currentTimeUs - mavActivePort->lastHighLatencyMessageUs) >= TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US) { mavlinkSendHighLatency2(currentTimeUs); mavActivePort->lastHighLatencyMessageUs = currentTimeUs; @@ -1988,18 +1874,6 @@ 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}; @@ -2252,7 +2126,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem } case MAV_CMD_CONTROL_HIGH_LATENCY: if (param1 == 0.0f || param1 == 1.0f) { - if (mavActiveConfig->version == 1 && param1 > 0.5f) { + if (mavlinkGetProtocolVersion() == 1 && param1 > 0.5f) { mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); return true; } @@ -2267,7 +2141,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem } return true; case MAV_CMD_REQUEST_PROTOCOL_VERSION: - if (mavActiveConfig->version == 1) { + if (mavlinkGetProtocolVersion() == 1) { mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); } else { mavlinkSendProtocolVersion(); @@ -2275,7 +2149,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem } return true; case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: - if (mavActiveConfig->version == 1) { + if (mavlinkGetProtocolVersion() == 1) { mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); } else { mavlinkSendAutopilotVersion(); @@ -2293,13 +2167,13 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem sent = true; break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION: - if (mavActiveConfig->version != 1) { + if (mavlinkGetProtocolVersion() != 1) { mavlinkSendAutopilotVersion(); sent = true; } break; case MAVLINK_MSG_ID_PROTOCOL_VERSION: - if (mavActiveConfig->version != 1) { + if (mavlinkGetProtocolVersion() != 1) { mavlinkSendProtocolVersion(); sent = true; } @@ -2321,9 +2195,9 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem 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); + mavlinkSendAvailableModes(planeModes, planeModesCount, modeSelection.customMode, mavlinkPlaneModeIsConfigured); } else { - mavlinkSendAvailableModes(copterModes, ARRAYLEN(copterModes), modeSelection.customMode, mavlinkCopterModeIsConfigured); + mavlinkSendAvailableModes(copterModes, copterModesCount, modeSelection.customMode, mavlinkCopterModeIsConfigured); } sent = true; } @@ -2694,9 +2568,14 @@ static bool handleIncoming_ADSB_VEHICLE(void) { static bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid) { + const uint8_t localSystemId = mavlinkGetCommonConfig()->sysid; + if (sysid != localSystemId) { + return false; + } + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { const mavlinkTelemetryPortConfig_t *cfg = mavlinkGetPortConfig(portIndex); - if (cfg->sysid == sysid && cfg->compid == compid) { + if (cfg->compid == compid) { return true; } } @@ -2805,12 +2684,12 @@ static int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t tar return ingressPortIndex; } + if ((uint8_t)targetSystem != mavlinkGetCommonConfig()->sysid) { + return -1; + } + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { const mavlinkTelemetryPortConfig_t *cfg = mavlinkGetPortConfig(portIndex); - if (cfg->sysid != targetSystem) { - continue; - } - if (targetComponent <= 0 || cfg->compid == targetComponent) { return portIndex; } diff --git a/src/main/telemetry/mavlink.h b/src/main/telemetry/mavlink.h index 899674194de..57d03e1d64e 100755 --- a/src/main/telemetry/mavlink.h +++ b/src/main/telemetry/mavlink.h @@ -17,6 +17,158 @@ #pragma once +#include "common/time.h" + +#include "io/serial.h" + +#include "target/common.h" + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#ifndef MAVLINK_COMM_NUM_BUFFERS +#define MAVLINK_COMM_NUM_BUFFERS MAX_MAVLINK_PORTS +#endif +#include "common/mavlink.h" +#pragma GCC diagnostic pop + +#define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX +#define TELEMETRY_MAVLINK_MAXRATE 50 +#define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) +#define TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US (5 * 1000 * 1000) +#define MAV_DATA_STREAM_EXTENDED_SYS_STATE (MAV_DATA_STREAM_EXTRA3 + 1) +#define MAVLINK_STREAM_COUNT (MAV_DATA_STREAM_EXTENDED_SYS_STATE + 1) +#define ARDUPILOT_VERSION_MAJOR 4 +#define ARDUPILOT_VERSION_MINOR 6 +#define ARDUPILOT_VERSION_PATCH 3 +#define MAVLINK_MAX_ROUTES 32 +#define MAVLINK_PORT_MASK(portIndex) (1U << (portIndex)) +#define MAXSTREAMS MAVLINK_STREAM_COUNT + +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. + * This converts angles from a range of 0..Pi to -Pi..Pi + */ +#define RADIANS_TO_MAVLINK_RANGE(angle) (angle > M_PIf) ? angle - (2 * M_PIf) : angle + +/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ +typedef enum APM_PLANE_MODE +{ + PLANE_MODE_MANUAL=0, + PLANE_MODE_CIRCLE=1, + PLANE_MODE_STABILIZE=2, + PLANE_MODE_TRAINING=3, + PLANE_MODE_ACRO=4, + PLANE_MODE_FLY_BY_WIRE_A=5, + PLANE_MODE_FLY_BY_WIRE_B=6, + PLANE_MODE_CRUISE=7, + PLANE_MODE_AUTOTUNE=8, + PLANE_MODE_AUTO=10, + PLANE_MODE_RTL=11, + PLANE_MODE_LOITER=12, + PLANE_MODE_TAKEOFF=13, + PLANE_MODE_AVOID_ADSB=14, + PLANE_MODE_GUIDED=15, + PLANE_MODE_INITIALIZING=16, + PLANE_MODE_QSTABILIZE=17, + PLANE_MODE_QHOVER=18, + PLANE_MODE_QLOITER=19, + PLANE_MODE_QLAND=20, + PLANE_MODE_QRTL=21, + PLANE_MODE_QAUTOTUNE=22, + 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. */ +typedef enum APM_COPTER_MODE +{ + COPTER_MODE_STABILIZE=0, + COPTER_MODE_ACRO=1, + COPTER_MODE_ALT_HOLD=2, + COPTER_MODE_AUTO=3, + COPTER_MODE_GUIDED=4, + COPTER_MODE_LOITER=5, + COPTER_MODE_RTL=6, + COPTER_MODE_CIRCLE=7, + COPTER_MODE_LAND=9, + COPTER_MODE_DRIFT=11, + COPTER_MODE_SPORT=13, + COPTER_MODE_FLIP=14, + COPTER_MODE_AUTOTUNE=15, + COPTER_MODE_POSHOLD=16, + COPTER_MODE_BRAKE=17, + COPTER_MODE_THROW=18, + COPTER_MODE_AVOID_ADSB=19, + COPTER_MODE_GUIDED_NOGPS=20, + COPTER_MODE_SMART_RTL=21, + 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; + +typedef struct mavlinkRouteEntry_s { + uint8_t sysid; + uint8_t compid; + uint8_t ingressPortIndex; +} mavlinkRouteEntry_t; + +extern const uint8_t mavDefaultRates[MAVLINK_STREAM_COUNT]; + +typedef struct mavlinkPortRuntime_s { + serialPort_t *port; + const serialPortConfig_t *portConfig; + portSharing_e portSharing; + bool telemetryEnabled; + bool txbuffValid; + uint8_t txbuffFree; + timeUs_t lastMavlinkMessageUs; + timeUs_t lastHighLatencyMessageUs; + bool highLatencyEnabled; + uint8_t mavRates[MAVLINK_STREAM_COUNT]; + uint8_t mavRatesConfigured[MAVLINK_STREAM_COUNT]; + uint8_t mavTicks[MAVLINK_STREAM_COUNT]; + mavlink_message_t mavRecvMsg; + mavlink_status_t mavRecvStatus; +} mavlinkPortRuntime_t; + +typedef struct mavlinkModeDescriptor_s { + uint8_t customMode; + const char *name; +} mavlinkModeDescriptor_t; + +extern const mavlinkModeDescriptor_t planeModes[]; +extern const uint8_t planeModesCount; +extern const mavlinkModeDescriptor_t copterModes[]; +extern const uint8_t copterModesCount; + +typedef struct mavlinkMissionItemData_s { + uint8_t frame; + uint16_t command; + float param1; + float param2; + float param3; + float param4; + int32_t lat; + int32_t lon; + float alt; +} mavlinkMissionItemData_t; + void initMAVLinkTelemetry(void); void handleMAVLinkTelemetry(timeUs_t currentTimeUs); void checkMAVLinkTelemetryState(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index b771fc5c86c..c77f80a2c57 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -56,7 +56,7 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 10); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, @@ -86,51 +86,59 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .accEventThresholdNegX = SETTING_ACC_EVENT_THRESHOLD_NEG_X_DEFAULT, #endif + .mavlink_common = { + .autopilot_type = SETTING_MAVLINK_AUTOPILOT_TYPE_DEFAULT, + .version = SETTING_MAVLINK_VERSION_DEFAULT, + .sysid = SETTING_MAVLINK_SYSID_DEFAULT, + }, .mavlink = { { - .autopilot_type = SETTING_MAVLINK_AUTOPILOT_TYPE_DEFAULT, - .extended_status_rate = SETTING_MAVLINK_EXT_STATUS_RATE_DEFAULT, - .rc_channels_rate = SETTING_MAVLINK_RC_CHAN_RATE_DEFAULT, - .position_rate = SETTING_MAVLINK_POS_RATE_DEFAULT, - .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, - .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, - .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, - .version = SETTING_MAVLINK_VERSION_DEFAULT, - .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, - .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT, - .sysid = SETTING_MAVLINK_SYSID_DEFAULT, + .extended_status_rate = SETTING_MAVLINK_PORT1_EXT_STATUS_RATE_DEFAULT, + .rc_channels_rate = SETTING_MAVLINK_PORT1_RC_CHAN_RATE_DEFAULT, + .position_rate = SETTING_MAVLINK_PORT1_POS_RATE_DEFAULT, + .extra1_rate = SETTING_MAVLINK_PORT1_EXTRA1_RATE_DEFAULT, + .extra2_rate = SETTING_MAVLINK_PORT1_EXTRA2_RATE_DEFAULT, + .extra3_rate = SETTING_MAVLINK_PORT1_EXTRA3_RATE_DEFAULT, + .min_txbuff = SETTING_MAVLINK_PORT1_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_PORT1_RADIO_TYPE_DEFAULT, .compid = SETTING_MAVLINK_PORT1_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT1_HIGH_LATENCY_DEFAULT }, { - .autopilot_type = SETTING_MAVLINK_PORT2_AUTOPILOT_TYPE_DEFAULT, .extended_status_rate = SETTING_MAVLINK_PORT2_EXT_STATUS_RATE_DEFAULT, .rc_channels_rate = SETTING_MAVLINK_PORT2_RC_CHAN_RATE_DEFAULT, .position_rate = SETTING_MAVLINK_PORT2_POS_RATE_DEFAULT, .extra1_rate = SETTING_MAVLINK_PORT2_EXTRA1_RATE_DEFAULT, .extra2_rate = SETTING_MAVLINK_PORT2_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_PORT2_EXTRA3_RATE_DEFAULT, - .version = SETTING_MAVLINK_PORT2_VERSION_DEFAULT, .min_txbuff = SETTING_MAVLINK_PORT2_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_PORT2_RADIO_TYPE_DEFAULT, - .sysid = SETTING_MAVLINK_PORT2_SYSID_DEFAULT, .compid = SETTING_MAVLINK_PORT2_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT2_HIGH_LATENCY_DEFAULT }, { - .autopilot_type = SETTING_MAVLINK_PORT3_AUTOPILOT_TYPE_DEFAULT, .extended_status_rate = SETTING_MAVLINK_PORT3_EXT_STATUS_RATE_DEFAULT, .rc_channels_rate = SETTING_MAVLINK_PORT3_RC_CHAN_RATE_DEFAULT, .position_rate = SETTING_MAVLINK_PORT3_POS_RATE_DEFAULT, .extra1_rate = SETTING_MAVLINK_PORT3_EXTRA1_RATE_DEFAULT, .extra2_rate = SETTING_MAVLINK_PORT3_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_PORT3_EXTRA3_RATE_DEFAULT, - .version = SETTING_MAVLINK_PORT3_VERSION_DEFAULT, .min_txbuff = SETTING_MAVLINK_PORT3_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_PORT3_RADIO_TYPE_DEFAULT, - .sysid = SETTING_MAVLINK_PORT3_SYSID_DEFAULT, .compid = SETTING_MAVLINK_PORT3_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT3_HIGH_LATENCY_DEFAULT + }, + { + .extended_status_rate = SETTING_MAVLINK_PORT4_EXT_STATUS_RATE_DEFAULT, + .rc_channels_rate = SETTING_MAVLINK_PORT4_RC_CHAN_RATE_DEFAULT, + .position_rate = SETTING_MAVLINK_PORT4_POS_RATE_DEFAULT, + .extra1_rate = SETTING_MAVLINK_PORT4_EXTRA1_RATE_DEFAULT, + .extra2_rate = SETTING_MAVLINK_PORT4_EXTRA2_RATE_DEFAULT, + .extra3_rate = SETTING_MAVLINK_PORT4_EXTRA3_RATE_DEFAULT, + .min_txbuff = SETTING_MAVLINK_PORT4_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_PORT4_RADIO_TYPE_DEFAULT, + .compid = SETTING_MAVLINK_PORT4_COMPID_DEFAULT, + .high_latency = SETTING_MAVLINK_PORT4_HIGH_LATENCY_DEFAULT } } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index bc363c930a8..46937dd7386 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -29,10 +29,7 @@ #include "config/parameter_group.h" #include "io/serial.h" - -#ifndef MAX_MAVLINK_PORTS -#define MAX_MAVLINK_PORTS 3 -#endif +#include "target/common.h" typedef enum { LTM_RATE_NORMAL, @@ -51,6 +48,12 @@ typedef enum { MAVLINK_RADIO_SIK, } mavlinkRadio_e; +typedef struct mavlinkTelemetryCommonConfig_s { + uint8_t autopilot_type; + uint8_t version; + uint8_t sysid; +} mavlinkTelemetryCommonConfig_t; + typedef enum { SMARTPORT_FUEL_UNIT_PERCENT, SMARTPORT_FUEL_UNIT_MAH, @@ -58,17 +61,14 @@ typedef enum { } smartportFuelUnit_e; typedef struct mavlinkTelemetryPortConfig_s { - uint8_t autopilot_type; uint8_t extended_status_rate; uint8_t rc_channels_rate; uint8_t position_rate; uint8_t extra1_rate; uint8_t extra2_rate; uint8_t extra3_rate; - uint8_t version; uint8_t min_txbuff; uint8_t radio_type; - uint8_t sysid; uint8_t compid; bool high_latency; } mavlinkTelemetryPortConfig_t; @@ -96,12 +96,13 @@ typedef struct telemetryConfig_s { uint16_t accEventThresholdLow; uint16_t accEventThresholdNegX; #endif + mavlinkTelemetryCommonConfig_t mavlink_common; mavlinkTelemetryPortConfig_t mavlink[MAX_MAVLINK_PORTS]; } telemetryConfig_t; PG_DECLARE(telemetryConfig_t, telemetryConfig); -#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_IBUS) +#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_IBUS | FUNCTION_TELEMETRY_MAVLINK) extern serialPort_t *telemetrySharedPort; void telemetryInit(void); From e45c0903ec97df8eceda9875990f0ddd365db6d1 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Tue, 3 Mar 2026 14:19:39 -0500 Subject: [PATCH 26/42] broadcast fixing --- src/main/telemetry/mavlink.c | 112 ++- src/utils/mavlink_tests/ROUTING_TEST.md | 32 + .../mav_fix2_singleport_benchmark.py | 465 ++++++++++ .../mavlink_tests/mav_multi_benchmark.py | 804 ++++++++++++++++++ src/utils/mavlink_tests/mav_multi_sweep.py | 152 ++++ .../mavlink_routing_compliance_test.py | 499 +++++++++++ src/utils/mavlink_tests/mavlink_test_rc.py | 141 +++ src/utils/mavlink_tests/msp_test_rc.py | 145 ++++ .../mavlink_tests/routing_test_config.yaml | 58 ++ ..._multiport4_sweep_rc460800_tele115200.yaml | 51 ++ 10 files changed, 2435 insertions(+), 24 deletions(-) create mode 100644 src/utils/mavlink_tests/ROUTING_TEST.md create mode 100644 src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py create mode 100644 src/utils/mavlink_tests/mav_multi_benchmark.py create mode 100644 src/utils/mavlink_tests/mav_multi_sweep.py create mode 100644 src/utils/mavlink_tests/mavlink_routing_compliance_test.py create mode 100644 src/utils/mavlink_tests/mavlink_test_rc.py create mode 100644 src/utils/mavlink_tests/msp_test_rc.py create mode 100644 src/utils/mavlink_tests/routing_test_config.yaml create mode 100644 src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 3ccf552c77b..9198e982ac8 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -2024,11 +2024,14 @@ static void mavlinkSendCommandAck(uint16_t command, MAV_RESULT result, uint8_t a 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) +static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent, 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; } + if (targetComponent != 0 && targetComponent != mavComponentId) { + return false; + } UNUSED(param3); switch (command) { @@ -2276,7 +2279,7 @@ 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); + return handleIncoming_COMMAND(msg.target_system, msg.target_component, 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) @@ -2285,7 +2288,7 @@ static bool handleIncoming_COMMAND_LONG(void) mavlink_msg_command_long_decode(&mavRecvMsg, &msg); // COMMAND_LONG has no frame field; location commands are WGS84 global by definition. - return handleIncoming_COMMAND(msg.target_system, mavRecvMsg.sysid, mavRecvMsg.compid, msg.command, MAV_FRAME_GLOBAL, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7); + return handleIncoming_COMMAND(msg.target_system, msg.target_component, 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) @@ -2382,6 +2385,9 @@ static bool handleIncoming_REQUEST_DATA_STREAM(void) if (msg.target_system != mavSystemId) { return false; } + if (msg.target_component != 0 && msg.target_component != mavComponentId) { + return false; + } uint8_t rate = 0; if (msg.start_stop != 0) { @@ -2698,6 +2704,27 @@ static int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t tar return -1; } +static bool mavlinkShouldFanOutLocalBroadcast(const mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_REQUEST_DATA_STREAM) { + return true; + } + + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + mavlink_command_long_t cmd; + mavlink_msg_command_long_decode(msg, &cmd); + return cmd.command == MAV_CMD_SET_MESSAGE_INTERVAL || cmd.command == MAV_CMD_CONTROL_HIGH_LATENCY; + } + + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_INT) { + mavlink_command_int_t cmd; + mavlink_msg_command_int_decode(msg, &cmd); + return cmd.command == MAV_CMD_SET_MESSAGE_INTERVAL || cmd.command == MAV_CMD_CONTROL_HIGH_LATENCY; + } + + return false; +} + // Returns whether a message was processed static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) { @@ -2726,52 +2753,89 @@ static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) return false; } - mavlinkSetActivePortContext((uint8_t)localPortIndex); - mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); + uint8_t localPortMask = MAVLINK_PORT_MASK((uint8_t)localPortIndex); + const bool isLocalOrSystemBroadcast = targetSystem == 0 || ((targetSystem > 0) && ((uint8_t)targetSystem == mavlinkGetCommonConfig()->sysid)); + if (targetComponent == 0 && isLocalOrSystemBroadcast && mavlinkShouldFanOutLocalBroadcast(&mavRecvMsg)) { + localPortMask = 0; + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + localPortMask |= MAVLINK_PORT_MASK(portIndex); + } + } + + bool handled = false; + for (uint8_t localIndex = 0; localIndex < mavPortCount; localIndex++) { + if (!(localPortMask & MAVLINK_PORT_MASK(localIndex))) { + continue; + } + + mavlinkSetActivePortContext(localIndex); + mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); + bool localHandled = false; - switch (mavRecvMsg.msgid) { + switch (mavRecvMsg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: - return handleIncoming_HEARTBEAT(); + localHandled = handleIncoming_HEARTBEAT(); + break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - return handleIncoming_PARAM_REQUEST_LIST(); + localHandled = handleIncoming_PARAM_REQUEST_LIST(); + break; case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: - return handleIncoming_MISSION_CLEAR_ALL(); + localHandled = handleIncoming_MISSION_CLEAR_ALL(); + break; case MAVLINK_MSG_ID_MISSION_COUNT: - return handleIncoming_MISSION_COUNT(); + localHandled = handleIncoming_MISSION_COUNT(); + break; case MAVLINK_MSG_ID_MISSION_ITEM: - return handleIncoming_MISSION_ITEM(); + localHandled = handleIncoming_MISSION_ITEM(); + break; case MAVLINK_MSG_ID_MISSION_ITEM_INT: - return handleIncoming_MISSION_ITEM_INT(); + localHandled = handleIncoming_MISSION_ITEM_INT(); + break; case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: - return handleIncoming_MISSION_REQUEST_LIST(); - + localHandled = handleIncoming_MISSION_REQUEST_LIST(); + break; case MAVLINK_MSG_ID_COMMAND_LONG: - return handleIncoming_COMMAND_LONG(); + localHandled = handleIncoming_COMMAND_LONG(); + break; 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(); + localHandled = handleIncoming_COMMAND_INT(); + break; case MAVLINK_MSG_ID_MISSION_REQUEST: - return handleIncoming_MISSION_REQUEST(); + localHandled = handleIncoming_MISSION_REQUEST(); + break; case MAVLINK_MSG_ID_MISSION_REQUEST_INT: - return handleIncoming_MISSION_REQUEST_INT(); + localHandled = handleIncoming_MISSION_REQUEST_INT(); + break; case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - return handleIncoming_REQUEST_DATA_STREAM(); + localHandled = handleIncoming_REQUEST_DATA_STREAM(); + break; case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: handleIncoming_RC_CHANNELS_OVERRIDE(); // Don't set that we handled a message, otherwise RC channel packets will block telemetry messages - return false; + localHandled = false; + break; case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: - return handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(); + localHandled = handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(); + break; #ifdef USE_ADSB case MAVLINK_MSG_ID_ADSB_VEHICLE: - return handleIncoming_ADSB_VEHICLE(); + localHandled = handleIncoming_ADSB_VEHICLE(); + break; #endif case MAVLINK_MSG_ID_RADIO_STATUS: handleIncoming_RADIO_STATUS(); // Don't set that we handled a message, otherwise radio status packets will block telemetry messages. - return false; + localHandled = false; + break; default: - return false; + localHandled = false; + break; + } + + handled = handled || localHandled; } + + return handled; } } diff --git a/src/utils/mavlink_tests/ROUTING_TEST.md b/src/utils/mavlink_tests/ROUTING_TEST.md new file mode 100644 index 00000000000..6a67b31822b --- /dev/null +++ b/src/utils/mavlink_tests/ROUTING_TEST.md @@ -0,0 +1,32 @@ +# MAVLink Routing Compliance Test + +- fc_system_id: `1` +- links: `['link_camera', 'link_components_a', 'link_gcs', 'link_gimbal']` +- gcs_link: `link_gcs` +- components: `[('mav1_elrs_receiver', 1, 68, 'link_components_a'), ('mav2_companion_computer', 1, 191, 'link_components_a'), ('mav3_camera', 1, 100, 'link_camera'), ('mav4_gimbal', 1, 154, 'link_gimbal')]` + +## Rule References + +- broadcast_forward: `routing.md:25-27,48-49` +- known_target_forward: `routing.md:49-50; MAVLink_routing.cpp:66-80` +- unknown_target_blocked: `routing.md:26-27,53` +- no_ingress_loop: `MAVLink_routing.cpp:197-209` +- no_repack: `routing.md:29-31` + +## Results + +| Case | Source(link) | Target(sys,comp) | Expected Links | Observed Links | Routing | No Ingress Loop | Payload Intact | Pass | +| --- | --- | --- | --- | --- | --- | --- | --- | --- | +| `gcs_broadcast` | `gcs (link_gcs)` | `(0,0)` | `['link_camera', 'link_components_a', 'link_gimbal']` | `['link_camera', 'link_components_a', 'link_gimbal']` | `True` | `True` | `True` | `True` | +| `gcs_target_mav1_elrs_receiver` | `gcs (link_gcs)` | `(1,68)` | `['link_components_a']` | `['link_components_a']` | `True` | `True` | `True` | `True` | +| `gcs_target_mav2_companion_computer` | `gcs (link_gcs)` | `(1,191)` | `['link_components_a']` | `['link_components_a']` | `True` | `True` | `True` | `True` | +| `gcs_target_mav3_camera` | `gcs (link_gcs)` | `(1,100)` | `['link_camera']` | `['link_camera']` | `True` | `True` | `True` | `True` | +| `gcs_target_mav4_gimbal` | `gcs (link_gcs)` | `(1,154)` | `['link_gimbal']` | `['link_gimbal']` | `True` | `True` | `True` | `True` | +| `gcs_target_unknown_component` | `gcs (link_gcs)` | `(1,199)` | `[]` | `[]` | `True` | `True` | `True` | `True` | +| `gcs_target_unknown_system` | `gcs (link_gcs)` | `(42,1)` | `[]` | `[]` | `True` | `True` | `True` | `True` | +| `mav1_elrs_receiver_broadcast` | `mav1_elrs_receiver (link_components_a)` | `(0,0)` | `['link_camera', 'link_gcs', 'link_gimbal']` | `['link_camera', 'link_gcs', 'link_gimbal']` | `True` | `True` | `True` | `True` | +| `mav1_elrs_receiver_to_mav3_camera` | `mav1_elrs_receiver (link_components_a)` | `(1,100)` | `['link_camera']` | `['link_camera']` | `True` | `True` | `True` | `True` | +| `mav1_elrs_receiver_to_gcs` | `mav1_elrs_receiver (link_components_a)` | `(255,190)` | `['link_gcs']` | `['link_gcs']` | `True` | `True` | `True` | `True` | + +summary pass_count=10 fail_count=0 total=10 + diff --git a/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py b/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py new file mode 100644 index 00000000000..f510b28b270 --- /dev/null +++ b/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py @@ -0,0 +1,465 @@ +#!/usr/bin/env python3 +""" +Usage: + /home/anon/miniconda3/envs/drone/bin/python mydev/branch/mav_multi/mav_fix2_singleport_benchmark.py --config mydev/branch/mav_multi/test_config_mav_fix2.yaml +""" + +from __future__ import annotations + +import argparse +import contextlib +import math +import socket +import struct +import subprocess +import time +from datetime import datetime, timezone +from pathlib import Path +from typing import Any, Dict, List, Tuple + +import yaml +from pymavlink import mavutil +from serial.serialutil import SerialException + +try: + from mspapi2.msp_api import MSPApi + from mspapi2.lib import InavMSP +except ModuleNotFoundError: + repo_root_guess = Path(__file__).resolve().parents[3] + mspapi2_repo = repo_root_guess.parent / "mspapi2" + if mspapi2_repo.exists(): + import sys + + sys.path.insert(0, str(mspapi2_repo)) + from mspapi2.msp_api import MSPApi + from mspapi2.lib import InavMSP + + +CHANNELS = [1500, 1500, 900, 1500] + [900] * 14 +MAV_CMD_REQUEST_MESSAGE = int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE) + + +def load_config(config_path: Path) -> Dict[str, Any]: + return yaml.safe_load(config_path.read_text(encoding="utf-8")) + + +def wait_for_tcp_port(host: str, port: int, timeout_s: float) -> None: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + try: + with socket.create_connection((host, port), timeout=1.0): + return + except (ConnectionRefusedError, socket.timeout, OSError): + time.sleep(0.2) + raise TimeoutError(f"TCP port {host}:{port} did not become available within {timeout_s}s") + + +def cli_read_until_prompt(cli_socket: socket.socket) -> str: + data = b"" + while b"\n# " not in data: + chunk = cli_socket.recv(65536) + if not chunk: + raise ConnectionError("CLI socket closed before prompt") + data += chunk + return data.decode("utf-8", errors="replace") + + +def run_cli_commands(host: str, port: int, commands: List[str]) -> None: + with socket.create_connection((host, port), timeout=5.0) as cli_socket: + cli_socket.settimeout(3.0) + cli_socket.sendall(b"#\n") + cli_read_until_prompt(cli_socket) + for command in commands: + cli_socket.sendall(command.encode("utf-8") + b"\n") + if command == "save": + break + cli_read_until_prompt(cli_socket) + time.sleep(1.0) + + +def build_serial_command(index: int, function_mask: int, baud: int) -> str: + return f"serial {index} {function_mask} {baud} {baud} 0 {baud}" + + +def build_cli_commands(config: Dict[str, Any]) -> List[str]: + cli_cfg = config["cli"] + rc_baud = int(cli_cfg["rc_baud"]) + commands = [ + "feature TELEMETRY", + "set receiver_type = SERIAL", + "set serialrx_provider = MAVLINK", + build_serial_command(0, 1, 115200), # UART1 MSP + build_serial_command(1, 256, rc_baud), # UART2 MAVLINK single port (RC-capable) + build_serial_command(2, 0, rc_baud), + build_serial_command(3, 0, rc_baud), + "set mavlink_version = 2", + f"set mavlink_port1_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", + f"set mavlink_port1_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", + f"set mavlink_port1_pos_rate = {int(cli_cfg['pos_rate_hz'])}", + f"set mavlink_port1_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", + f"set mavlink_port1_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", + f"set mavlink_port1_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", + "save", + ] + return commands + + +def run_workload( + config: Dict[str, Any], + sitl_pid: int, + duration_s: float, + command_rate_hz: float, + rc_tx_hz_override: float | None = None, +) -> Dict[str, Any]: + tests_cfg = config["tests"] + ports_cfg = config["ports"] + rc_port = int(ports_cfg["rc"]) + msp_port = int(ports_cfg["msp"]) + + wait_for_tcp_port("127.0.0.1", rc_port, float(tests_cfg["port_ready_timeout_s"])) + wait_for_tcp_port("127.0.0.1", msp_port, float(tests_cfg["port_ready_timeout_s"])) + msp_enabled = msp_port != rc_port + + heartbeat_expected_hz = float(tests_cfg["heartbeat_expected_hz"]) + rc_expected_hz = float(tests_cfg["rc_expected_hz"]) + rc_tx_hz = float(tests_cfg["rc_tx_hz"]) if rc_tx_hz_override is None else float(rc_tx_hz_override) + msp_poll_hz = float(tests_cfg["msp_poll_hz"]) + msp_request_timeout_s = float(tests_cfg["msp_request_timeout_s"]) + command_message_id = int(tests_cfg["stress_command_message_id"]) + target_system = int(tests_cfg["rc_target_system"]) + target_component = int(tests_cfg["rc_target_component"]) + + listener = mavutil.mavlink_connection( + f"tcp:127.0.0.1:{rc_port}", + source_system=170, + source_component=190, + autoreconnect=True, + ) + sender = mavutil.mavlink_connection( + f"tcp:127.0.0.1:{rc_port}", + source_system=239, + source_component=191, + autoreconnect=True, + ) + + # Prime telemetry stream startup from GCS side. + handshake_deadline = time.monotonic() + 3.0 + while time.monotonic() < handshake_deadline: + sender.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, + 0, + 0, + ) + msg = listener.recv_match(type="HEARTBEAT", blocking=True, timeout=0.2) + if msg is not None: + target_system = int(msg.get_srcSystem()) + target_component = int(msg.get_srcComponent()) + break + + hb_count = 0 + hb_est_lost = 0 + last_hb_t: float | None = None + rc_count = 0 + rc_est_lost = 0 + last_rc_t: float | None = None + rc_mismatch = 0 + command_sent = 0 + command_ack_total = 0 + command_ack_ok = 0 + msp_success = 0 + msp_fail = 0 + msp_mismatch = 0 + rc_sent = 0 + fc_cpu_load_samples: List[float] = [] + fc_cycle_time_samples: List[float] = [] + fc_status_fail = 0 + fc_status_last_error = "" + + rc_period_s = 1.0 / rc_tx_hz + next_rc_send_t = time.monotonic() + command_period_s = 0.0 if command_rate_hz <= 0 else (1.0 / command_rate_hz) + next_command_send_t = next_rc_send_t + next_heartbeat_send_t = next_rc_send_t + + prev_sample_t = time.monotonic() + next_resource_sample_t = prev_sample_t + 1.0 + next_msp_poll_t = prev_sample_t + (1.0 / msp_poll_hz) + workload_end_t = prev_sample_t + duration_s + + with (MSPApi(tcp_endpoint=f"127.0.0.1:{msp_port}") if msp_enabled else contextlib.nullcontext()) as msp_api: + if msp_enabled: + msp_api._serial._max_retries = 1 + msp_api._serial._reconnect_delay = 0.05 + while time.monotonic() < workload_end_t: + now = time.monotonic() + + if now >= next_heartbeat_send_t: + listener.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, + 0, + 0, + ) + sender.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, + 0, + 0, + ) + next_heartbeat_send_t += 1.0 + if next_heartbeat_send_t < now: + next_heartbeat_send_t = now + 1.0 + + if now >= next_rc_send_t: + sender.mav.rc_channels_override_send(target_system, target_component, *CHANNELS[:8]) + rc_sent += 1 + next_rc_send_t += rc_period_s + if next_rc_send_t < now: + next_rc_send_t = now + rc_period_s + + if command_period_s > 0 and now >= next_command_send_t: + sender.mav.command_long_send( + target_system, + target_component, + MAV_CMD_REQUEST_MESSAGE, + 0, + float(command_message_id), + 0, + 0, + 0, + 0, + 0, + 0, + ) + command_sent += 1 + next_command_send_t += command_period_s + if next_command_send_t < now: + next_command_send_t = now + command_period_s + + while True: + try: + msg = listener.recv_match(blocking=False) + except (ConnectionRefusedError, OSError): + wait_for_tcp_port("127.0.0.1", rc_port, float(tests_cfg["port_ready_timeout_s"])) + break + if msg is None: + break + msg_type = msg.get_type() + if msg_type == "HEARTBEAT": + target_system = int(msg.get_srcSystem()) + target_component = int(msg.get_srcComponent()) + hb_count += 1 + if last_hb_t is not None: + dt = now - last_hb_t + hb_est_lost += max(int(math.floor(dt * heartbeat_expected_hz) - 1), 0) + last_hb_t = now + elif msg_type == "RC_CHANNELS": + rc_count += 1 + if last_rc_t is not None: + dt = now - last_rc_t + rc_est_lost += max(int(math.floor(dt * rc_expected_hz) - 1), 0) + last_rc_t = now + if ( + abs(int(msg.chan1_raw) - CHANNELS[0]) > 20 + or abs(int(msg.chan2_raw) - CHANNELS[1]) > 20 + or abs(int(msg.chan3_raw) - CHANNELS[2]) > 20 + or abs(int(msg.chan4_raw) - CHANNELS[3]) > 20 + ): + rc_mismatch += 1 + elif msg_type == "COMMAND_ACK": + if int(msg.command) == MAV_CMD_REQUEST_MESSAGE: + command_ack_total += 1 + if int(msg.result) == int(mavutil.mavlink.MAV_RESULT_ACCEPTED): + command_ack_ok += 1 + + if msp_enabled and now >= next_msp_poll_t: + try: + _, payload = msp_api._request_raw(InavMSP.MSP_RC, timeout=msp_request_timeout_s) + if len(payload) % 2: + raise ValueError("MSP RC payload length must be even") + channel_count = len(payload) // 2 + channels = list(struct.unpack(f"<{channel_count}H", payload)) + msp_success += 1 + if ( + abs(channels[0] - CHANNELS[0]) > 40 + or abs(channels[1] - CHANNELS[1]) > 40 + or abs(channels[2] - CHANNELS[2]) > 40 + or abs(channels[3] - CHANNELS[3]) > 40 + ): + msp_mismatch += 1 + except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError): + msp_fail += 1 + next_msp_poll_t += 1.0 / msp_poll_hz + + if now >= next_resource_sample_t: + if msp_enabled: + try: + status = msp_api.get_inav_status() + fc_cpu_load_samples.append(float(status["cpuLoad"])) + fc_cycle_time_samples.append(float(status["cycleTime"])) + except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError, KeyError, ValueError) as exc: + fc_status_fail += 1 + fc_status_last_error = str(exc) + prev_sample_t = now + next_resource_sample_t += 1.0 + + time.sleep(0.001) + + if msp_enabled and not fc_cpu_load_samples: + raise RuntimeError(f"No MSP2_INAV_STATUS samples collected; last_error={fc_status_last_error} fail_count={fc_status_fail}") + + listener.close() + sender.close() + + hb_total = hb_count + hb_est_lost + rc_total = rc_count + rc_est_lost + return { + "duration_s": duration_s, + "heartbeat_count": hb_count, + "heartbeat_est_lost": hb_est_lost, + "heartbeat_loss_rate": (hb_est_lost / hb_total) if hb_total > 0 else 0.0, + "rc_count": rc_count, + "rc_est_lost": rc_est_lost, + "rc_loss_rate": (rc_est_lost / rc_total) if rc_total > 0 else 0.0, + "rc_mismatch": rc_mismatch, + "rc_mismatch_rate": (rc_mismatch / max(rc_count, 1)), + "msp_success": msp_success, + "msp_fail": msp_fail, + "msp_failure_rate": (msp_fail / max(msp_success + msp_fail, 1)), + "msp_mismatch": msp_mismatch, + "msp_mismatch_rate": (msp_mismatch / max(msp_success, 1)), + "command_sent": command_sent, + "command_ack_total": command_ack_total, + "command_ack_ok": command_ack_ok, + "command_ack_failure_rate": ((command_sent - command_ack_total) / command_sent) if command_sent > 0 else 0.0, + "command_ack_non_ok_rate": ((command_ack_total - command_ack_ok) / command_ack_total) if command_ack_total > 0 else 0.0, + "rc_sent": rc_sent, + "rc_sent_hz": (rc_sent / max(duration_s, 1e-6)), + "fc_cpu_load_avg_pct": (sum(fc_cpu_load_samples) / len(fc_cpu_load_samples)) if fc_cpu_load_samples else 0.0, + "fc_cpu_load_max_pct": max(fc_cpu_load_samples) if fc_cpu_load_samples else 0.0, + "fc_cycle_time_avg_us": (sum(fc_cycle_time_samples) / len(fc_cycle_time_samples)) if fc_cycle_time_samples else 0.0, + "fc_cycle_time_max_us": max(fc_cycle_time_samples) if fc_cycle_time_samples else 0.0, + "fc_status_samples": len(fc_cpu_load_samples), + } + + +def write_report(config: Dict[str, Any], baseline: Dict[str, Any], stress: Dict[str, Any]) -> None: + out_path = Path(config["output"]["testing_md"]) + branch_name = str(config.get("meta", {}).get("branch_name", "unknown")) + timestamp = datetime.now(timezone.utc).isoformat() + lines: List[str] = [] + lines.append(f"# MAVLink Single-Port Baseline (`{branch_name}`)") + lines.append("") + lines.append(f"- Generated: `{timestamp}`") + lines.append(f"- Branch: `{branch_name}`") + lines.append(f"- SITL binary: `{config['sitl']['binary']}`") + lines.append(f"- EEPROM: `{config['sitl']['eeprom_path']}`") + lines.append("- MSP kept on UART1 by design (`serial 0 ... functionMask=1`).") + lines.append("- CLI under test sets `receiver_type=SERIAL` and `serialrx_provider=MAVLINK`.") + lines.append( + f"- CLI baud config: `rc_baud={config['cli']['rc_baud']}`, " + f"`telemetry_baud={config['cli'].get('telemetry_baud', config['cli']['rc_baud'])}`." + ) + lines.append("") + lines.append("## Baseline: Single MAVLink RC+Telemetry Port") + lines.append("") + lines.append( + f"- Duration: `{baseline['duration_s']}s` | FC cpuLoad avg/max: `{baseline['fc_cpu_load_avg_pct']:.2f}% / {baseline['fc_cpu_load_max_pct']:.2f}%` | " + f"FC cycleTime avg/max: `{baseline['fc_cycle_time_avg_us']:.1f} / {baseline['fc_cycle_time_max_us']:.1f}` us | " + f"Status samples: `{baseline['fc_status_samples']}`" + ) + lines.append( + f"- MSP success/fail: `{baseline['msp_success']}/{baseline['msp_fail']}` | MSP failure rate: `{baseline['msp_failure_rate']:.4f}` | " + f"MSP RC mismatch rate: `{baseline['msp_mismatch_rate']:.4f}`" + ) + lines.append(f"- RC override send rate: `{baseline['rc_sent_hz']:.2f} Hz` (`{baseline['rc_sent']}` packets)") + lines.append( + f"- HB count/loss: `{baseline['heartbeat_count']}/{baseline['heartbeat_est_lost']}` | HB loss rate: `{baseline['heartbeat_loss_rate']:.4f}`" + ) + lines.append( + f"- RC count/loss: `{baseline['rc_count']}/{baseline['rc_est_lost']}` | RC loss rate: `{baseline['rc_loss_rate']:.4f}` | " + f"RC mismatch rate: `{baseline['rc_mismatch_rate']:.4f}`" + ) + lines.append("") + lines.append("## Stress: Single Port Overload") + lines.append("") + lines.append( + f"- Command load: `{config['tests']['stress_rate_hz']} req/s` for `{stress['duration_s']}s` | " + f"FC cpuLoad avg/max: `{stress['fc_cpu_load_avg_pct']:.2f}% / {stress['fc_cpu_load_max_pct']:.2f}%` | " + f"FC cycleTime avg/max: `{stress['fc_cycle_time_avg_us']:.1f} / {stress['fc_cycle_time_max_us']:.1f}` us | " + f"Status samples: `{stress['fc_status_samples']}`" + ) + lines.append( + f"- MSP success/fail: `{stress['msp_success']}/{stress['msp_fail']}` | MSP failure rate: `{stress['msp_failure_rate']:.4f}` | " + f"MSP RC mismatch rate: `{stress['msp_mismatch_rate']:.4f}`" + ) + lines.append(f"- RC override send rate: `{stress['rc_sent_hz']:.2f} Hz` (`{stress['rc_sent']}` packets)") + lines.append( + f"- HB count/loss: `{stress['heartbeat_count']}/{stress['heartbeat_est_lost']}` | HB loss rate: `{stress['heartbeat_loss_rate']:.4f}`" + ) + lines.append( + f"- RC count/loss: `{stress['rc_count']}/{stress['rc_est_lost']}` | RC loss rate: `{stress['rc_loss_rate']:.4f}` | " + f"RC mismatch rate: `{stress['rc_mismatch_rate']:.4f}`" + ) + lines.append( + f"- Command sent/ack: `{stress['command_sent']}/{stress['command_ack_total']}` | " + f"ack failure rate: `{stress['command_ack_failure_rate']:.4f}` | non-ok ack rate: `{stress['command_ack_non_ok_rate']:.4f}`" + ) + out_path.parent.mkdir(parents=True, exist_ok=True) + out_path.write_text("\n".join(lines) + "\n", encoding="utf-8") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Run single-port MAVLink benchmark and write a baseline markdown report.") + parser.add_argument("--config", required=True, help="YAML configuration path") + args = parser.parse_args() + + config = load_config(Path(args.config)) + sitl_bin = Path(config["sitl"]["binary"]) + sitl_workdir = Path(config["sitl"]["workdir"]) + sitl_eeprom = str(config["sitl"]["eeprom_path"]) + sitl_log = Path(config["sitl"]["runtime_log"]) + sitl_log.parent.mkdir(parents=True, exist_ok=True) + + log_handle = sitl_log.open("w", encoding="utf-8") + sitl_proc = subprocess.Popen( + [str(sitl_bin), f"--path={sitl_eeprom}"], + cwd=str(sitl_workdir), + stdout=log_handle, + stderr=subprocess.STDOUT, + text=True, + ) + + try: + wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(config["tests"]["port_ready_timeout_s"])) + run_cli_commands("127.0.0.1", int(config["ports"]["msp"]), build_cli_commands(config)) + wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(config["tests"]["port_ready_timeout_s"])) + wait_for_tcp_port("127.0.0.1", int(config["ports"]["rc"]), float(config["tests"]["port_ready_timeout_s"])) + + baseline = run_workload( + config=config, + sitl_pid=sitl_proc.pid, + duration_s=float(config["tests"]["baseline_duration_s"]), + command_rate_hz=0.0, + ) + stress = run_workload( + config=config, + sitl_pid=sitl_proc.pid, + duration_s=float(config["tests"]["stress_duration_s"]), + command_rate_hz=float(config["tests"]["stress_rate_hz"]), + rc_tx_hz_override=float(config["tests"]["rc_tx_hz_max"]), + ) + write_report(config, baseline, stress) + print(f"report_written={config['output']['testing_md']}", flush=True) + finally: + sitl_proc.terminate() + try: + sitl_proc.wait(timeout=5.0) + except subprocess.TimeoutExpired: + sitl_proc.kill() + sitl_proc.wait(timeout=5.0) + log_handle.close() diff --git a/src/utils/mavlink_tests/mav_multi_benchmark.py b/src/utils/mavlink_tests/mav_multi_benchmark.py new file mode 100644 index 00000000000..6483fa5806e --- /dev/null +++ b/src/utils/mavlink_tests/mav_multi_benchmark.py @@ -0,0 +1,804 @@ +#!/usr/bin/env python3 +""" +Usage: + conda run -n drone python mydev/branch/mav_multi/mav_multi_benchmark.py + conda run -n drone python mydev/branch/mav_multi/mav_multi_benchmark.py --config mydev/branch/mav_multi/test_config.yaml +""" + +from __future__ import annotations + +import argparse +import asyncio +import socket +import subprocess +import time +from datetime import datetime, timezone +from pathlib import Path +from typing import Any, Dict, List, Tuple + +import yaml +from mavsdk import System +from pymavlink import mavutil +from serial.serialutil import SerialException + +try: + from mspapi2.msp_api import MSPApi + from mspapi2.lib import InavMSP +except ModuleNotFoundError: + repo_root_guess = Path(__file__).resolve().parents[3] + mspapi2_repo = repo_root_guess.parent / "mspapi2" + if mspapi2_repo.exists(): + import sys + + sys.path.insert(0, str(mspapi2_repo)) + from mspapi2.msp_api import MSPApi + from mspapi2.lib import InavMSP + + +DEFAULT_CONFIG_PATH = Path("mydev/branch/mav_multi/test_config.yaml") +CHANNELS = [1500, 1500, 900, 1500] + [900] * 14 +MAV_CMD_REQUEST_MESSAGE = int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE) +MAVLINK_MSG_ID_HEARTBEAT = int(mavutil.mavlink.MAVLINK_MSG_ID_HEARTBEAT) + + +def load_config(config_path: Path) -> Dict[str, Any]: + return yaml.safe_load(config_path.read_text(encoding="utf-8")) + + +def wait_for_tcp_port(host: str, port: int, timeout_s: float) -> None: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + try: + with socket.create_connection((host, port), timeout=1.0): + return + except ConnectionRefusedError: + time.sleep(0.2) + continue + except socket.timeout: + time.sleep(0.2) + continue + raise TimeoutError(f"TCP port {host}:{port} did not become available within {timeout_s}s") + + +def cli_read_until_prompt(cli_socket: socket.socket) -> str: + data = b"" + while b"\n# " not in data: + chunk = cli_socket.recv(65536) + if not chunk: + raise ConnectionError("CLI socket closed before prompt") + data += chunk + return data.decode("utf-8", errors="replace") + + +def wait_for_cli_ready(host: str, port: int, timeout_s: float) -> None: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + try: + with socket.create_connection((host, port), timeout=1.0) as cli_socket: + cli_socket.settimeout(1.0) + cli_socket.sendall(b"#\n") + cli_read_until_prompt(cli_socket) + return + except (ConnectionRefusedError, ConnectionError, socket.timeout, OSError): + time.sleep(0.2) + raise TimeoutError(f"CLI prompt did not become available on {host}:{port} within {timeout_s}s") + + +def run_cli_commands(host: str, port: int, commands: List[str], reboot_timeout_s: float) -> None: + sent_save = False + with socket.create_connection((host, port), timeout=5.0) as cli_socket: + cli_socket.settimeout(3.0) + cli_socket.sendall(b"#\n") + cli_read_until_prompt(cli_socket) + for command in commands: + cli_socket.sendall(command.encode("utf-8") + b"\n") + if command == "save": + sent_save = True + break + cli_read_until_prompt(cli_socket) + if sent_save: + # Save triggers reboot; do not send '#' again because that re-enters CLI mode. + time.sleep(min(reboot_timeout_s, 1.0)) + + +def build_serial_command(index: int, function_mask: int, baud: int) -> str: + return f"serial {index} {function_mask} {baud} {baud} 0 {baud}" + + +def build_cli_config_commands(config: Dict[str, Any], mavlink_port_count: int) -> List[str]: + cli_cfg = config["cli"] + rc_baud = int(cli_cfg["rc_baud"]) + telemetry_baud = int(cli_cfg["telemetry_baud"]) + uart2_mask = 320 # FUNCTION_RX_SERIAL + FUNCTION_TELEMETRY_MAVLINK + uart3_mask = 256 if mavlink_port_count >= 2 else 0 # FUNCTION_TELEMETRY_MAVLINK + uart4_mask = 256 if mavlink_port_count >= 3 else 0 # FUNCTION_TELEMETRY_MAVLINK + uart5_mask = 256 if mavlink_port_count >= 4 else 0 # FUNCTION_TELEMETRY_MAVLINK + commands = [ + "feature TELEMETRY", + "set receiver_type = SERIAL", + "set serialrx_provider = MAVLINK", + build_serial_command(0, 1, 115200), # MSP on UART1, always. + build_serial_command(1, uart2_mask, rc_baud), + build_serial_command(2, uart3_mask, telemetry_baud), + build_serial_command(3, uart4_mask, telemetry_baud), + build_serial_command(4, uart5_mask, telemetry_baud), + "set mavlink_version = 2", + "set mavlink_port1_compid = 1", + "set mavlink_port2_compid = 2", + "set mavlink_port3_compid = 3", + "set mavlink_port4_compid = 4", + "set mavlink_port1_high_latency = OFF", + "set mavlink_port2_high_latency = OFF", + "set mavlink_port3_high_latency = OFF", + "set mavlink_port4_high_latency = OFF", + f"set mavlink_port1_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", + f"set mavlink_port1_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", + f"set mavlink_port1_pos_rate = {int(cli_cfg['pos_rate_hz'])}", + f"set mavlink_port1_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", + f"set mavlink_port1_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", + f"set mavlink_port1_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", + f"set mavlink_port2_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", + f"set mavlink_port2_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", + f"set mavlink_port2_pos_rate = {int(cli_cfg['pos_rate_hz'])}", + f"set mavlink_port2_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", + f"set mavlink_port2_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", + f"set mavlink_port2_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", + f"set mavlink_port3_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", + f"set mavlink_port3_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", + f"set mavlink_port3_pos_rate = {int(cli_cfg['pos_rate_hz'])}", + f"set mavlink_port3_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", + f"set mavlink_port3_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", + f"set mavlink_port3_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", + f"set mavlink_port4_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", + f"set mavlink_port4_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", + f"set mavlink_port4_pos_rate = {int(cli_cfg['pos_rate_hz'])}", + f"set mavlink_port4_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", + f"set mavlink_port4_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", + f"set mavlink_port4_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", + "save", + ] + return commands + + +def apply_config_and_wait(config: Dict[str, Any], mavlink_port_count: int) -> None: + ports = config["ports"] + commands = build_cli_config_commands(config, mavlink_port_count) + run_cli_commands( + "127.0.0.1", + int(ports["msp"]), + commands, + reboot_timeout_s=float(config["tests"]["save_reboot_timeout_s"]), + ) + wait_for_tcp_port("127.0.0.1", int(ports["msp"]), float(config["tests"]["port_ready_timeout_s"])) + wait_for_tcp_port("127.0.0.1", int(ports["rc"]), float(config["tests"]["port_ready_timeout_s"])) + if mavlink_port_count >= 2: + wait_for_tcp_port("127.0.0.1", int(ports["telemetry"][0]), float(config["tests"]["port_ready_timeout_s"])) + if mavlink_port_count >= 3: + wait_for_tcp_port("127.0.0.1", int(ports["telemetry"][1]), float(config["tests"]["port_ready_timeout_s"])) + if mavlink_port_count >= 4: + wait_for_tcp_port("127.0.0.1", int(ports["telemetry"][2]), float(config["tests"]["port_ready_timeout_s"])) + + +async def mavsdk_probe(address: str, timeout_s: float) -> Dict[str, Any]: + drone = System() + result: Dict[str, Any] = { + "address": address, + "connected": False, + "flight_mode": None, + } + await drone.connect(system_address=address) + async with asyncio.timeout(timeout_s): + async for state in drone.core.connection_state(): + if state.is_connected: + result["connected"] = True + break + async with asyncio.timeout(timeout_s): + async for flight_mode in drone.telemetry.flight_mode(): + result["flight_mode"] = str(flight_mode) + break + return result + + +def probe_mavsdk_sync(address: str, timeout_s: float) -> Dict[str, Any]: + return asyncio.run(mavsdk_probe(address, timeout_s)) + + +def open_mavlink_tcp_connection(port: int, source_system: int, source_component: int, timeout_s: float) -> Any: + deadline = time.monotonic() + timeout_s + while True: + try: + return mavutil.mavlink_connection( + f"tcp:127.0.0.1:{port}", + source_system=source_system, + source_component=source_component, + autoreconnect=True, + ) + except OSError: + if time.monotonic() >= deadline: + raise + time.sleep(0.2) + + +def run_workload( + config: Dict[str, Any], + sitl_pid: int, + telemetry_ports: List[int], + rc_port: int, + load_rates_hz: Dict[int, float], + duration_s: float, + rc_tx_hz_override: float | None = None, +) -> Dict[str, Any]: + print( + f"run_workload_start duration_s={duration_s} telemetry_ports={telemetry_ports} rc_port={rc_port} " + f"load_rates_hz={load_rates_hz}", + flush=True, + ) + tests_cfg = config["tests"] + rc_tx_hz = float(tests_cfg["rc_tx_hz"]) if rc_tx_hz_override is None else float(rc_tx_hz_override) + msp_poll_hz = float(tests_cfg["msp_poll_hz"]) + inav_status_hz = float(tests_cfg.get("inav_status_hz", 1.0)) + msp_request_timeout_s = float(tests_cfg["msp_request_timeout_s"]) + command_message_id = int(tests_cfg["stress_command_message_id"]) + rc_target_system = int(tests_cfg["rc_target_system"]) + rc_target_component = int(tests_cfg["rc_target_component"]) + port_ready_timeout_s = float(tests_cfg["port_ready_timeout_s"]) + warmup_s = float(tests_cfg.get("warmup_s", 3.0)) + warmup_heartbeat_count = int(tests_cfg.get("warmup_heartbeat_count", 3)) + warmup_timeout_s = float(tests_cfg.get("warmup_timeout_s", 30.0)) + + listeners: Dict[int, Any] = {} + targets: Dict[int, Tuple[int, int]] = {} + port_stats: Dict[int, Dict[str, Any]] = {} + load_senders: Dict[int, Any] = {} + load_sent: Dict[int, int] = {} + load_next_send: Dict[int, float] = {} + msp_success = 0 + msp_fail = 0 + rc_sent = 0 + fc_cpu_load_samples: List[float] = [] + fc_cycle_time_samples: List[float] = [] + fc_status_fail = 0 + fc_status_last_error = "" + + source_base = 170 + ports_to_open = set(telemetry_ports) + ports_to_open.update(load_rates_hz.keys()) + ports_to_open.add(rc_port) + for port in sorted(ports_to_open): + wait_for_tcp_port("127.0.0.1", int(port), port_ready_timeout_s) + + for idx, port in enumerate(telemetry_ports): + conn = open_mavlink_tcp_connection( + port=port, + source_system=source_base + idx, + source_component=190, + timeout_s=port_ready_timeout_s, + ) + listeners[port] = conn + targets[port] = (rc_target_system, rc_target_component) + port_stats[port] = { + "heartbeat_count": 0, + "rc_count": 0, + "command_ack_total": 0, + "command_ack_ok": 0, + "mav_msg_count": 0, + "mav_seq_lost": 0, + "last_seq_by_source": {}, + } + + for idx, port in enumerate(sorted(load_rates_hz.keys())): + if port in listeners: + sender = listeners[port] + else: + sender = open_mavlink_tcp_connection( + port=port, + source_system=210 + idx, + source_component=191, + timeout_s=port_ready_timeout_s, + ) + load_senders[port] = sender + load_sent[port] = 0 + load_next_send[port] = time.monotonic() + if port not in targets: + targets[port] = (rc_target_system, rc_target_component) + + if rc_port in listeners: + rc_sender = listeners[rc_port] + else: + rc_sender = open_mavlink_tcp_connection( + port=rc_port, + source_system=239, + source_component=191, + timeout_s=port_ready_timeout_s, + ) + if telemetry_ports: + telemetry_target = targets[telemetry_ports[0]] + rc_target_system, rc_target_component = telemetry_target + + rc_period_s = 1.0 / rc_tx_hz + next_rc_send_t = time.monotonic() + heartbeat_period_s = 1.0 + next_heartbeat_send_t = next_rc_send_t + + warmup_hb_seen: Dict[int, int] = {port: 0 for port in telemetry_ports} + warmup_start_t = time.monotonic() + warmup_deadline_t = warmup_start_t + warmup_timeout_s + + while True: + now = time.monotonic() + + if now >= next_heartbeat_send_t: + heartbeat_conns: Dict[int, Any] = {} + for conn in listeners.values(): + heartbeat_conns[id(conn)] = conn + for conn in load_senders.values(): + heartbeat_conns[id(conn)] = conn + heartbeat_conns[id(rc_sender)] = rc_sender + for conn in heartbeat_conns.values(): + conn.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, + 0, + 0, + ) + next_heartbeat_send_t += heartbeat_period_s + if next_heartbeat_send_t < now: + next_heartbeat_send_t = now + heartbeat_period_s + + if now >= next_rc_send_t: + rc_target = targets.get(rc_port) + if rc_target is not None: + rc_target_system, rc_target_component = rc_target + rc_sender.mav.rc_channels_override_send(rc_target_system, rc_target_component, *CHANNELS[:8]) + next_rc_send_t += rc_period_s + if next_rc_send_t < now: + next_rc_send_t = now + rc_period_s + + for port, conn in listeners.items(): + while True: + msg = conn.recv_match(blocking=False) + if msg is None: + break + msg_type = msg.get_type() + if msg_type == "HEARTBEAT": + targets[port] = (int(msg.get_srcSystem()), int(msg.get_srcComponent())) + warmup_hb_seen[port] += 1 + + if telemetry_ports and all(count >= warmup_heartbeat_count for count in warmup_hb_seen.values()) and (now - warmup_start_t) >= warmup_s: + break + if (not telemetry_ports) and (now - warmup_start_t) >= warmup_s: + break + if now >= warmup_deadline_t: + raise TimeoutError( + f"Warmup failed: heartbeat_counts={warmup_hb_seen} required={warmup_heartbeat_count} timeout_s={warmup_timeout_s}" + ) + + time.sleep(0.001) + + for stats in port_stats.values(): + stats["heartbeat_count"] = 0 + stats["rc_count"] = 0 + stats["command_ack_total"] = 0 + stats["command_ack_ok"] = 0 + stats["mav_msg_count"] = 0 + stats["mav_seq_lost"] = 0 + stats["last_seq_by_source"] = {} + + rc_sent = 0 + measurement_start_t = time.monotonic() + next_rc_send_t = measurement_start_t + next_heartbeat_send_t = measurement_start_t + for port in load_next_send: + load_next_send[port] = measurement_start_t + next_resource_sample_t = measurement_start_t + (1.0 / inav_status_hz) + next_msp_poll_t = measurement_start_t + (1.0 / msp_poll_hz) + workload_end_t = measurement_start_t + duration_s + + with MSPApi(tcp_endpoint=f"127.0.0.1:{int(config['ports']['msp'])}") as msp_api: + msp_api._serial._max_retries = 1 + msp_api._serial._reconnect_delay = 0.05 + while time.monotonic() < workload_end_t: + now = time.monotonic() + + if now >= next_heartbeat_send_t: + heartbeat_conns: Dict[int, Any] = {} + for conn in listeners.values(): + heartbeat_conns[id(conn)] = conn + for conn in load_senders.values(): + heartbeat_conns[id(conn)] = conn + heartbeat_conns[id(rc_sender)] = rc_sender + for conn in heartbeat_conns.values(): + conn.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, + 0, + 0, + ) + next_heartbeat_send_t += heartbeat_period_s + if next_heartbeat_send_t < now: + next_heartbeat_send_t = now + heartbeat_period_s + + if now >= next_rc_send_t: + rc_target = targets.get(rc_port) + if rc_target is not None: + rc_target_system, rc_target_component = rc_target + rc_sender.mav.rc_channels_override_send(rc_target_system, rc_target_component, *CHANNELS[:8]) + rc_sent += 1 + next_rc_send_t += rc_period_s + if next_rc_send_t < now: + next_rc_send_t = now + rc_period_s + + for port, rate_hz in load_rates_hz.items(): + next_send_t = load_next_send[port] + if now >= next_send_t: + target_system, target_component = targets[port] + load_senders[port].mav.command_long_send( + target_system, + target_component, + MAV_CMD_REQUEST_MESSAGE, + 0, + float(command_message_id), + 0, + 0, + 0, + 0, + 0, + 0, + ) + load_sent[port] += 1 + next_send_t += 1.0 / rate_hz + if next_send_t < now: + next_send_t = now + (1.0 / rate_hz) + load_next_send[port] = next_send_t + + for port, conn in listeners.items(): + while True: + msg = conn.recv_match(blocking=False) + if msg is None: + break + msg_type = msg.get_type() + stats = port_stats[port] + seq = None + if hasattr(msg, "get_seq"): + try: + seq = int(msg.get_seq()) + except (TypeError, ValueError): + seq = None + if seq is None: + header = getattr(msg, "_header", None) + if header is not None and hasattr(header, "seq"): + seq = int(header.seq) + if seq is not None: + src_key = (int(msg.get_srcSystem()), int(msg.get_srcComponent())) + last_seq = stats["last_seq_by_source"].get(src_key) + if last_seq is not None: + seq_delta = (seq - int(last_seq)) & 0xFF + if seq_delta > 0: + stats["mav_seq_lost"] += max(seq_delta - 1, 0) + stats["last_seq_by_source"][src_key] = seq + stats["mav_msg_count"] += 1 + + if msg_type == "HEARTBEAT": + targets[port] = (int(msg.get_srcSystem()), int(msg.get_srcComponent())) + stats["heartbeat_count"] += 1 + elif msg_type == "RC_CHANNELS": + stats["rc_count"] += 1 + elif msg_type == "COMMAND_ACK": + if int(msg.command) == MAV_CMD_REQUEST_MESSAGE: + stats["command_ack_total"] += 1 + if int(msg.result) == int(mavutil.mavlink.MAV_RESULT_ACCEPTED): + stats["command_ack_ok"] += 1 + + if now >= next_msp_poll_t: + try: + msp_api._request_raw(InavMSP.MSP_RC, timeout=msp_request_timeout_s) + msp_success += 1 + except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError, OSError): + msp_fail += 1 + next_msp_poll_t += 1.0 / msp_poll_hz + + if now >= next_resource_sample_t: + try: + status = msp_api.get_inav_status() + fc_cpu_load_samples.append(float(status["cpuLoad"])) + fc_cycle_time_samples.append(float(status["cycleTime"])) + except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError, OSError, KeyError, ValueError) as exc: + fc_status_fail += 1 + fc_status_last_error = str(exc) + next_resource_sample_t += 1.0 / inav_status_hz + + time.sleep(0.001) + + all_connections: Dict[int, Any] = {} + for conn in listeners.values(): + all_connections[id(conn)] = conn + for conn in load_senders.values(): + all_connections[id(conn)] = conn + all_connections[id(rc_sender)] = rc_sender + for conn in all_connections.values(): + conn.close() + + if not fc_cpu_load_samples: + raise RuntimeError(f"No MSP2_INAV_STATUS samples collected; last_error={fc_status_last_error} fail_count={fc_status_fail}") + + summarized_ports: Dict[int, Dict[str, Any]] = {} + for port in telemetry_ports: + stats = port_stats[port] + mav_total = int(stats["mav_msg_count"]) + mav_lost = int(stats["mav_seq_lost"]) + mav_expected = mav_total + mav_lost + sent = load_sent.get(port, 0) + ack_total = stats["command_ack_total"] + ack_ok = stats["command_ack_ok"] + ack_missing = max(sent - ack_total, 0) + command_failed_total = max(sent - ack_ok, 0) + summarized_ports[port] = { + "heartbeat_count": stats["heartbeat_count"], + "rc_count": stats["rc_count"], + "mav_msg_count": mav_total, + "mav_seq_lost": mav_lost, + "mav_seq_loss_rate": (mav_lost / mav_expected) if mav_expected > 0 else 0.0, + "command_sent": sent, + "command_ack_total": ack_total, + "command_ack_ok": ack_ok, + "command_failed_total": command_failed_total, + "command_ack_missing": ack_missing, + "command_ack_failure_rate": (command_failed_total / sent) if sent > 0 else 0.0, + } + + result = { + "duration_s": duration_s, + "ports": summarized_ports, + "msp_success": msp_success, + "msp_fail": msp_fail, + "rc_sent": rc_sent, + "rc_sent_hz": (rc_sent / max(duration_s, 1e-6)), + "msp_failure_rate": (msp_fail / max(msp_success + msp_fail, 1)), + "fc_cpu_load_avg_pct": (sum(fc_cpu_load_samples) / len(fc_cpu_load_samples)) if fc_cpu_load_samples else 0.0, + "fc_cpu_load_max_pct": max(fc_cpu_load_samples) if fc_cpu_load_samples else 0.0, + "fc_cycle_time_avg_us": (sum(fc_cycle_time_samples) / len(fc_cycle_time_samples)) if fc_cycle_time_samples else 0.0, + "fc_cycle_time_max_us": max(fc_cycle_time_samples) if fc_cycle_time_samples else 0.0, + "fc_status_samples": len(fc_cpu_load_samples), + } + print( + f"run_workload_done duration_s={duration_s} fc_cpu_load_avg_pct={result['fc_cpu_load_avg_pct']:.2f} " + f"msp_fail_rate={result['msp_failure_rate']:.4f}", + flush=True, + ) + return result + + +def format_port_table(port_result: Dict[int, Dict[str, Any]]) -> str: + if not port_result: + return "_No MAVLink telemetry ports active in this scenario._" + + lines = [ + "| Port | MAV Msg Count | MAV Seq Lost | MAV Seq Loss % | HB Count | RC_CHANNELS Count | Cmd Sent | Cmd Ack OK | Cmd Failed Total | Cmd Missing Ack |", + "| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |", + ] + for port in sorted(port_result.keys()): + data = port_result[port] + lines.append( + "| " + f"{port} | {data['mav_msg_count']} | {data['mav_seq_lost']} | {(data['mav_seq_loss_rate'] * 100.0):.2f}% | " + f"{data['heartbeat_count']} | {data['rc_count']} | {data['command_sent']} | " + f"{data['command_ack_ok']} | {data['command_failed_total']} | {data['command_ack_missing']} |" + ) + return "\n".join(lines) + + +def summarize_command_failures(port_result: Dict[int, Dict[str, Any]]) -> Dict[str, float]: + sent_total = sum(int(item["command_sent"]) for item in port_result.values()) + failed_total = sum(int(item["command_failed_total"]) for item in port_result.values()) + missing_ack_total = sum(int(item["command_ack_missing"]) for item in port_result.values()) + mav_seq_lost_total = sum(int(item["mav_seq_lost"]) for item in port_result.values()) + mav_msg_total = sum(int(item["mav_msg_count"]) for item in port_result.values()) + mav_seq_loss_pct = (mav_seq_lost_total / max(mav_seq_lost_total + mav_msg_total, 1)) * 100.0 + return { + "sent_total": float(sent_total), + "failed_total": float(failed_total), + "failed_pct": (failed_total / max(sent_total, 1)) * 100.0, + "missing_ack_total": float(missing_ack_total), + "mav_seq_loss_pct": mav_seq_loss_pct, + } + + +def write_testing_report(config: Dict[str, Any], report: Dict[str, Any]) -> None: + out_path = Path(config["output"]["testing_md"]) + timestamp = datetime.now(timezone.utc).isoformat() + lines: List[str] = [] + lines.append("# MAVLink Multiport Testing") + lines.append("") + lines.append(f"- Generated: `{timestamp}`") + lines.append(f"- Conda env: `drone`") + lines.append(f"- SITL binary: `{config['sitl']['binary']}`") + lines.append(f"- EEPROM: `{config['sitl']['eeprom_path']}`") + lines.append("- MSP kept on UART1 by design (`serial 0 ... functionMask=1`).") + lines.append("- CLI under test sets `receiver_type=SERIAL` and `serialrx_provider=MAVLINK`.") + lines.append("- `RC_CHANNELS` metrics below are MAVLink telemetry stream metrics, not RX input ownership.") + lines.append("") + lines.append("## MAVSDK Probe") + lines.append("") + lines.append("| Address | Connected | Flight Mode |") + lines.append("| --- | --- | --- |") + if report["mavsdk_probes"]: + for probe in report["mavsdk_probes"]: + lines.append(f"| {probe['address']} | {probe['connected']} | {probe['flight_mode']} |") + else: + lines.append("| _skipped_ | _skipped_ | _skipped_ |") + lines.append("") + lines.append("## Port Count Baseline (1 RC, then +Telemetry ports)") + lines.append("") + for label, result in report["baseline"].items(): + cmd_summary = summarize_command_failures(result["ports"]) + lines.append(f"### {label}") + lines.append("") + lines.append( + f"- Duration: `{result['duration_s']}s` | FC cpuLoad avg/max: `{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}%` | " + f"FC cycleTime avg/max: `{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f}` us | " + f"Status samples: `{result['fc_status_samples']}`" + ) + lines.append( + f"- MSP success/fail: `{result['msp_success']}/{result['msp_fail']}` | MSP failure rate: `{(result['msp_failure_rate'] * 100.0):.2f}%`" + ) + lines.append( + f"- Commands sent/failed: `{int(cmd_summary['sent_total'])}/{int(cmd_summary['failed_total'])}` (`{cmd_summary['failed_pct']:.2f}%`) | " + f"Missing ACK: `{int(cmd_summary['missing_ack_total'])}` | MAVLink seq loss: `{cmd_summary['mav_seq_loss_pct']:.2f}%`" + ) + lines.append(f"- RC override send rate: `{result['rc_sent_hz']:.2f} Hz` (`{result['rc_sent']}` packets)") + lines.append("") + lines.append(format_port_table(result["ports"])) + lines.append("") + lines.append("## Stress Test: Progressive Overload On One MAVLink Port") + lines.append("") + for item in report["single_port_stress"]: + rate = item["rate_hz"] + result = item["result"] + cmd_summary = summarize_command_failures(result["ports"]) + lines.append(f"### Load Rate `{rate} req/s` on telemetry port") + lines.append("") + lines.append( + f"- Duration: `{result['duration_s']}s` | FC cpuLoad avg/max: `{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}%` | " + f"FC cycleTime avg/max: `{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f}` us | " + f"Status samples: `{result['fc_status_samples']}`" + ) + lines.append( + f"- MSP success/fail: `{result['msp_success']}/{result['msp_fail']}` | MSP failure rate: `{(result['msp_failure_rate'] * 100.0):.2f}%`" + ) + lines.append( + f"- Commands sent/failed: `{int(cmd_summary['sent_total'])}/{int(cmd_summary['failed_total'])}` (`{cmd_summary['failed_pct']:.2f}%`) | " + f"Missing ACK: `{int(cmd_summary['missing_ack_total'])}` | MAVLink seq loss: `{cmd_summary['mav_seq_loss_pct']:.2f}%`" + ) + lines.append(f"- RC override send rate: `{result['rc_sent_hz']:.2f} Hz` (`{result['rc_sent']}` packets)") + lines.append("") + lines.append(format_port_table(result["ports"])) + lines.append("") + max_result = report["all_ports_max"] + cmd_summary = summarize_command_failures(max_result["ports"]) + max_ports = len(max_result["ports"]) + lines.append(f"## Stress Test: All {max_ports} MAVLink Ports At Max Load") + lines.append("") + lines.append( + f"- Duration: `{max_result['duration_s']}s` | FC cpuLoad avg/max: `{max_result['fc_cpu_load_avg_pct']:.2f}% / {max_result['fc_cpu_load_max_pct']:.2f}%` | " + f"FC cycleTime avg/max: `{max_result['fc_cycle_time_avg_us']:.1f} / {max_result['fc_cycle_time_max_us']:.1f}` us | " + f"Status samples: `{max_result['fc_status_samples']}`" + ) + lines.append( + f"- MSP success/fail: `{max_result['msp_success']}/{max_result['msp_fail']}` | MSP failure rate: `{(max_result['msp_failure_rate'] * 100.0):.2f}%`" + ) + lines.append( + f"- Commands sent/failed: `{int(cmd_summary['sent_total'])}/{int(cmd_summary['failed_total'])}` (`{cmd_summary['failed_pct']:.2f}%`) | " + f"Missing ACK: `{int(cmd_summary['missing_ack_total'])}` | MAVLink seq loss: `{cmd_summary['mav_seq_loss_pct']:.2f}%`" + ) + lines.append(f"- RC override send rate: `{max_result['rc_sent_hz']:.2f} Hz` (`{max_result['rc_sent']}` packets)") + lines.append("") + lines.append(format_port_table(max_result["ports"])) + lines.append("") + out_path.write_text("\n".join(lines) + "\n", encoding="utf-8") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Run MAVLink multiport RC/telemetry/stress benchmark and write TESTING.md") + parser.add_argument("--config", default=str(DEFAULT_CONFIG_PATH), help="YAML configuration path") + args = parser.parse_args() + + config_path = Path(args.config) + config = load_config(config_path) + + sitl_bin = Path(config["sitl"]["binary"]) + sitl_workdir = Path(config["sitl"]["workdir"]) + sitl_eeprom = str(config["sitl"]["eeprom_path"]) + sitl_log = Path(config["sitl"]["runtime_log"]) + sitl_log.parent.mkdir(parents=True, exist_ok=True) + log_handle = sitl_log.open("w", encoding="utf-8") + sitl_proc = subprocess.Popen( + [str(sitl_bin), f"--path={sitl_eeprom}"], + cwd=str(sitl_workdir), + stdout=log_handle, + stderr=subprocess.STDOUT, + text=True, + ) + + report: Dict[str, Any] = { + "mavsdk_probes": [], + "baseline": {}, + "single_port_stress": [], + "all_ports_max": {}, + } + + try: + wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(config["tests"]["port_ready_timeout_s"])) + + configured_port_count = 1 + len(config["ports"]["telemetry"]) + print(f"single_config_start count={configured_port_count}", flush=True) + apply_config_and_wait(config, configured_port_count) + print(f"single_config_done count={configured_port_count}", flush=True) + + baseline_scenarios: List[Tuple[str, List[int]]] = [] + rc_port = int(config["ports"]["rc"]) + telemetry_ports = [int(port) for port in config["ports"]["telemetry"]] + for count in range(1, configured_port_count + 1): + baseline_scenarios.append((f"{count} MAVLink port(s)", [rc_port] + telemetry_ports[: max(0, count - 1)])) + for label, active_telemetry_ports in baseline_scenarios: + print(f"baseline_run_start label={label} ports={active_telemetry_ports}", flush=True) + result = run_workload( + config=config, + sitl_pid=sitl_proc.pid, + telemetry_ports=active_telemetry_ports, + rc_port=int(config["ports"]["rc"]), + load_rates_hz={}, + duration_s=float(config["tests"]["baseline_duration_s"]), + ) + report["baseline"][label] = result + print(f"baseline_run_done label={label}", flush=True) + + if bool(config["tests"].get("run_mavsdk_probe", True)): + print("mavsdk_probe_start", flush=True) + mavsdk_probe_ports = [int(config["ports"]["rc"])] + [int(p) for p in config["ports"]["telemetry"]] + for port in mavsdk_probe_ports: + probe = probe_mavsdk_sync(f"tcp://127.0.0.1:{port}", float(config["tests"]["mavsdk_probe_timeout_s"])) + report["mavsdk_probes"].append(probe) + print("mavsdk_probe_done", flush=True) + else: + print("mavsdk_probe_skipped", flush=True) + + stress_port = int(config["ports"]["telemetry"][0]) + active_telemetry_ports = [int(config["ports"]["rc"])] + [int(port) for port in config["ports"]["telemetry"]] + for rate in config["tests"]["progressive_rates_hz"]: + print(f"single_port_stress_start rate={rate}", flush=True) + result = run_workload( + config=config, + sitl_pid=sitl_proc.pid, + telemetry_ports=active_telemetry_ports, + rc_port=int(config["ports"]["rc"]), + load_rates_hz={stress_port: float(rate)}, + duration_s=float(config["tests"]["stress_duration_s"]), + ) + report["single_port_stress"].append({"rate_hz": rate, "result": result}) + + print("all_ports_max_start", flush=True) + max_rate = float(config["tests"]["max_rate_hz"]) + max_loads = {port: max_rate for port in active_telemetry_ports} + result = run_workload( + config=config, + sitl_pid=sitl_proc.pid, + telemetry_ports=active_telemetry_ports, + rc_port=int(config["ports"]["rc"]), + load_rates_hz=max_loads, + duration_s=float(config["tests"]["all_ports_max_duration_s"]), + ) + report["all_ports_max"] = result + print("all_ports_max_done", flush=True) + + write_testing_report(config, report) + print(f"report_written={config['output']['testing_md']}") + finally: + sitl_proc.terminate() + try: + sitl_proc.wait(timeout=5.0) + except subprocess.TimeoutExpired: + sitl_proc.kill() + sitl_proc.wait(timeout=5.0) + log_handle.close() diff --git a/src/utils/mavlink_tests/mav_multi_sweep.py b/src/utils/mavlink_tests/mav_multi_sweep.py new file mode 100644 index 00000000000..0915e1a1485 --- /dev/null +++ b/src/utils/mavlink_tests/mav_multi_sweep.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 +""" +Usage: + conda run -n drone python src/utils/mavlink_tests/mav_multi_sweep.py + conda run -n drone python src/utils/mavlink_tests/mav_multi_sweep.py --config src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml +""" + +from __future__ import annotations + +import argparse +import importlib.util +import subprocess +from datetime import datetime, timezone +from pathlib import Path +from typing import Any, Dict, List, Tuple + +import yaml + + +TESTS_DIR = Path(__file__).resolve().parent +DEFAULT_CONFIG_PATH = TESTS_DIR / "test_config_multiport4_sweep_rc460800_tele115200.yaml" +BENCHMARK_MODULE_PATH = TESTS_DIR / "mav_multi_benchmark.py" + +spec = importlib.util.spec_from_file_location("mav_multi_benchmark", BENCHMARK_MODULE_PATH) +benchmark = importlib.util.module_from_spec(spec) +assert spec is not None +assert spec.loader is not None +spec.loader.exec_module(benchmark) + + +parser = argparse.ArgumentParser(description="Run MAVLink load sweep table for 1..4 ports and [50,100,200,max] req/s") +parser.add_argument("--config", default=str(DEFAULT_CONFIG_PATH), help="YAML configuration path") +args = parser.parse_args() + +config_path = Path(args.config) +config: Dict[str, Any] = yaml.safe_load(config_path.read_text(encoding="utf-8")) + +sitl_bin = Path(config["sitl"]["binary"]) +sitl_workdir = Path(config["sitl"]["workdir"]) +sitl_eeprom = str(config["sitl"]["eeprom_path"]) +sitl_log = Path(config["sitl"]["runtime_log"]) +sitl_log.parent.mkdir(parents=True, exist_ok=True) + +rate_labels: List[Tuple[str, float]] = [ + ("50", 50.0), + ("100", 100.0), + ("200", 200.0), + ("max", float(config["tests"]["max_rate_hz"])), +] + +results: Dict[str, Dict[int, Dict[str, Any]]] = {} + +rc_port = int(config["ports"]["rc"]) +telemetry_ports = [int(port) for port in config["ports"]["telemetry"]] +sweep_duration_s = float(config["tests"]["sweep_duration_s"]) + +rate_cache: Dict[float, Dict[int, Dict[str, Any]]] = {} + +for label, rate_hz in rate_labels: + if rate_hz not in rate_cache: + rate_cache[rate_hz] = {} + for port_count in range(1, 5): + scenario_log = sitl_log.with_name(f"{sitl_log.stem}_{label}_{port_count}{sitl_log.suffix}") + log_handle = scenario_log.open("w", encoding="utf-8") + sitl_proc = subprocess.Popen( + [str(sitl_bin), f"--path={sitl_eeprom}"], + cwd=str(sitl_workdir), + stdout=log_handle, + stderr=subprocess.STDOUT, + text=True, + ) + try: + benchmark.wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(config["tests"]["port_ready_timeout_s"])) + benchmark.apply_config_and_wait(config, 4) + active_ports = [rc_port] + telemetry_ports[: max(0, port_count - 1)] + load_rates_hz = {port: rate_hz for port in active_ports} + print(f"sweep_run_start label={label} rate={rate_hz} ports={port_count}", flush=True) + result = benchmark.run_workload( + config=config, + sitl_pid=sitl_proc.pid, + telemetry_ports=active_ports, + rc_port=rc_port, + load_rates_hz=load_rates_hz, + duration_s=sweep_duration_s, + ) + rate_cache[rate_hz][port_count] = result + print(f"sweep_run_done label={label} rate={rate_hz} ports={port_count}", flush=True) + finally: + sitl_proc.terminate() + try: + sitl_proc.wait(timeout=5.0) + except subprocess.TimeoutExpired: + sitl_proc.kill() + sitl_proc.wait(timeout=5.0) + log_handle.close() + scenario_log.unlink(missing_ok=True) + results[label] = rate_cache[rate_hz] + +output_path = Path(config["output"]["testing_md"]) +lines: List[str] = [ + "# MAVLink Port Load Sweep", + "", + f"- Generated: `{datetime.now(timezone.utc).isoformat()}`", + f"- SITL binary: `{config['sitl']['binary']}`", + f"- EEPROM: `{config['sitl']['eeprom_path']}`", + "- UART1 MSP only.", + "- UART2 is MAVLink RC (460800), RC override target is 100 Hz.", + "- Additional MAVLink telemetry ports are 115200 baud.", + "", + "## Comparison Table", + "", + "| Load Label | Msg/s per active port | Active MAVLink ports | FC cpuLoad avg/max | FC cycleTime avg/max (us) | MAV seq loss max | Cmd failed total | Cmd failed % | MSP fail % |", + "| --- | ---: | ---: | --- | --- | ---: | ---: | ---: | ---: |", +] + +for label, rate_hz in rate_labels: + rate_results = results[label] + for port_count in range(1, 5): + result = rate_results[port_count] + per_port = list(result["ports"].values()) + mav_seq_loss_max_pct = max(item["mav_seq_loss_rate"] * 100.0 for item in per_port) + cmd_sent_total = sum(int(item["command_sent"]) for item in per_port) + cmd_failed_total = sum(int(item["command_failed_total"]) for item in per_port) + cmd_failed_pct = (cmd_failed_total / max(cmd_sent_total, 1)) * 100.0 + lines.append( + f"| {label} | {rate_hz:.1f} | {port_count} | " + f"{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}% | " + f"{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f} | " + f"{mav_seq_loss_max_pct:.2f}% | {cmd_failed_total} | {cmd_failed_pct:.2f}% | {(result['msp_failure_rate'] * 100.0):.2f}% |" + ) + +lines.append("") +lines.append("## Raw Scenario Details") +lines.append("") +for label, rate_hz in rate_labels: + lines.append(f"### {label} ({rate_hz:.1f} msg/s per active port)") + lines.append("") + for port_count in range(1, 5): + result = results[label][port_count] + lines.append(f"#### {port_count} active MAVLink port(s)") + lines.append("") + lines.append( + f"- FC cpuLoad avg/max: `{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}%` | " + f"FC cycleTime avg/max: `{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f}` us | " + f"MSP fail rate: `{(result['msp_failure_rate'] * 100.0):.2f}%`" + ) + lines.append("") + lines.append(benchmark.format_port_table(result["ports"])) + lines.append("") + +output_path.write_text("\n".join(lines) + "\n", encoding="utf-8") +print(f"report_written={output_path}", flush=True) diff --git a/src/utils/mavlink_tests/mavlink_routing_compliance_test.py b/src/utils/mavlink_tests/mavlink_routing_compliance_test.py new file mode 100644 index 00000000000..e202e649af8 --- /dev/null +++ b/src/utils/mavlink_tests/mavlink_routing_compliance_test.py @@ -0,0 +1,499 @@ +#!/usr/bin/env python3 +""" +Usage: + conda run -n drone python mydev/branch/mav_multi/mavlink_routing_compliance_test.py --config mydev/branch/mav_multi/routing_test_config.yaml +""" + +from __future__ import annotations + +import argparse +import math +import time +from pathlib import Path +from typing import Any, Dict, List + +import yaml +from pymavlink import mavutil + + +MAV_AUTOPILOT_INVALID = int(mavutil.mavlink.MAV_AUTOPILOT_INVALID) +MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = int(mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) +MAV_STATE_ACTIVE = int(mavutil.mavlink.MAV_STATE_ACTIVE) +MAV_TYPE_GCS = int(mavutil.mavlink.MAV_TYPE_GCS) + +ROUTING_RULE_REFERENCES = { + "broadcast_forward": "routing.md:25-27,48-49", + "known_target_forward": "routing.md:49-50; MAVLink_routing.cpp:66-80", + "unknown_target_blocked": "routing.md:26-27,53", + "no_ingress_loop": "MAVLink_routing.cpp:197-209", + "no_repack": "routing.md:29-31", +} + + +def load_config(config_path: Path) -> Dict[str, Any]: + return yaml.safe_load(config_path.read_text(encoding="utf-8")) + + +def open_mavlink_connection(endpoint: str, source_system: int, source_component: int, timeout_s: float) -> Any: + deadline = time.monotonic() + timeout_s + while True: + try: + return mavutil.mavlink_connection( + endpoint, + source_system=source_system, + source_component=source_component, + autoreconnect=True, + ) + except OSError as error: + if time.monotonic() >= deadline: + raise TimeoutError( + f"endpoint={endpoint} source_system={source_system} source_component={source_component} timeout_s={timeout_s}" + ) from error + time.sleep(0.2) + + +def set_source_identity(connection: Any, system_id: int, component_id: int) -> None: + connection.mav.srcSystem = system_id + connection.mav.srcComponent = component_id + + +def send_heartbeat(connection: Any, system_id: int, component_id: int, mav_type: int) -> None: + set_source_identity(connection, system_id, component_id) + connection.mav.heartbeat_send( + mav_type, + MAV_AUTOPILOT_INVALID, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + 0, + MAV_STATE_ACTIVE, + ) + + +def drain_link(connection: Any, duration_s: float) -> None: + deadline = time.monotonic() + duration_s + while time.monotonic() < deadline: + message = connection.recv_match(blocking=False) + if message is None: + time.sleep(0.002) + + +def drain_all_links(connections: Dict[str, Any], duration_s: float) -> None: + for connection in connections.values(): + drain_link(connection, duration_s) + + +def collect_command_observations(connections: Dict[str, Any], command_id: int, observation_window_s: float) -> Dict[str, List[Dict[str, Any]]]: + observations: Dict[str, List[Dict[str, Any]]] = {link_name: [] for link_name in connections} + deadline = time.monotonic() + observation_window_s + while time.monotonic() < deadline: + saw_message = False + for link_name, connection in connections.items(): + while True: + message = connection.recv_match(type=["COMMAND_LONG"], blocking=False) + if message is None: + break + if int(message.command) != command_id: + continue + observations[link_name].append( + { + "src_system": int(message.get_srcSystem()), + "src_component": int(message.get_srcComponent()), + "target_system": int(message.target_system), + "target_component": int(message.target_component), + "command": int(message.command), + "confirmation": int(message.confirmation), + "params": [ + float(message.param1), + float(message.param2), + float(message.param3), + float(message.param4), + float(message.param5), + float(message.param6), + float(message.param7), + ], + } + ) + saw_message = True + if not saw_message: + time.sleep(0.002) + return observations + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Run MAVLink routing compliance checks against INAV multiport routing.") + parser.add_argument("--config", required=True, type=Path, help="YAML config path") + args = parser.parse_args() + + config = load_config(args.config) + timing = config["timing"] + network = config["network"] + tests = config["tests"] + components_cfg = config["components"] + vehicle = config["vehicle"] + + gcs_link_name = network["gcs_link"] + fc_system_id = int(vehicle["fc_system_id"]) + unknown_component_id = int(tests["unknown_component_id"]) + unknown_system_id = int(tests["unknown_system_id"]) + marker_command_base = int(tests["marker_command_base"]) + report_path = Path(tests["report_markdown"]) + + link_cfgs = network["links"] + if gcs_link_name not in link_cfgs: + raise ValueError(f"gcs_link={gcs_link_name} missing from network.links") + + components: List[Dict[str, Any]] = [] + for component in components_cfg: + if component["link"] not in link_cfgs: + raise ValueError(f"component={component['name']} link={component['link']} missing from network.links") + components.append( + { + "name": component["name"], + "link": component["link"], + "system_id": int(component["system_id"]), + "component_id": int(component["component_id"]), + "mav_type": int(component["mav_type"]), + } + ) + + component_ids = {component["component_id"] for component in components} + component_system_ids = {component["system_id"] for component in components} + if unknown_component_id in component_ids: + raise ValueError(f"unknown_component_id={unknown_component_id} collides with configured component IDs") + if unknown_system_id in component_system_ids or unknown_system_id == fc_system_id: + raise ValueError(f"unknown_system_id={unknown_system_id} collides with configured systems") + + gcs_actor = { + "name": "gcs", + "link": gcs_link_name, + "system_id": int(link_cfgs[gcs_link_name]["source_system"]), + "component_id": int(link_cfgs[gcs_link_name]["source_component"]), + "mav_type": MAV_TYPE_GCS, + } + + inter_source = components[0] + inter_target = None + for component in components: + if component["link"] != inter_source["link"]: + inter_target = component + break + if inter_target is None: + raise ValueError("Need at least one component on a different link for inter-component routing test") + + connections: Dict[str, Any] = {} + fc_heartbeats: Dict[str, Dict[str, int]] = {} + + try: + for link_name, link_cfg in link_cfgs.items(): + connection = open_mavlink_connection( + endpoint=link_cfg["endpoint"], + source_system=int(link_cfg["source_system"]), + source_component=int(link_cfg["source_component"]), + timeout_s=float(timing["connect_timeout_s"]), + ) + connections[link_name] = connection + + for link_name, connection in connections.items(): + heartbeat = connection.wait_heartbeat(timeout=float(timing["connect_timeout_s"])) + if heartbeat is None: + raise TimeoutError(f"link={link_name} wait_heartbeat timed out") + fc_heartbeats[link_name] = { + "system_id": int(heartbeat.get_srcSystem()), + "component_id": int(heartbeat.get_srcComponent()), + } + print( + f"fc_heartbeat link={link_name} system_id={fc_heartbeats[link_name]['system_id']} " + f"component_id={fc_heartbeats[link_name]['component_id']}", + flush=True, + ) + + time.sleep(float(timing["settle_after_connect_s"])) + drain_all_links(connections, float(timing["drain_window_s"])) + + send_heartbeat( + connections[gcs_actor["link"]], + gcs_actor["system_id"], + gcs_actor["component_id"], + gcs_actor["mav_type"], + ) + for component in components: + send_heartbeat( + connections[component["link"]], + component["system_id"], + component["component_id"], + component["mav_type"], + ) + print( + f"route_learn_heartbeat component={component['name']} link={component['link']} " + f"system_id={component['system_id']} component_id={component['component_id']} mav_type={component['mav_type']}", + flush=True, + ) + time.sleep(float(timing["route_learn_settle_s"])) + drain_all_links(connections, float(timing["drain_window_s"])) + + cases: List[Dict[str, Any]] = [] + all_other_than_gcs = sorted([link_name for link_name in connections if link_name != gcs_link_name]) + + cases.append( + { + "name": "gcs_broadcast", + "source": gcs_actor, + "target_system": 0, + "target_component": 0, + "expected_links": all_other_than_gcs, + "rules": [ROUTING_RULE_REFERENCES["broadcast_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], + } + ) + + cases.append( + { + "name": "gcs_target_local_system_component_broadcast", + "source": gcs_actor, + "target_system": fc_system_id, + "target_component": 0, + "expected_links": all_other_than_gcs, + "rules": [ROUTING_RULE_REFERENCES["broadcast_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], + } + ) + + for component in components: + expected_links = [component["link"]] + if component["link"] == gcs_link_name: + expected_links = [] + cases.append( + { + "name": f"gcs_target_{component['name']}", + "source": gcs_actor, + "target_system": component["system_id"], + "target_component": component["component_id"], + "expected_links": sorted(expected_links), + "rules": [ROUTING_RULE_REFERENCES["known_target_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], + } + ) + + cases.append( + { + "name": "gcs_target_unknown_component", + "source": gcs_actor, + "target_system": fc_system_id, + "target_component": unknown_component_id, + "expected_links": [], + "rules": [ROUTING_RULE_REFERENCES["unknown_target_blocked"]], + } + ) + + cases.append( + { + "name": "gcs_target_unknown_system", + "source": gcs_actor, + "target_system": unknown_system_id, + "target_component": 1, + "expected_links": [], + "rules": [ROUTING_RULE_REFERENCES["unknown_target_blocked"]], + } + ) + + component_broadcast_expected = sorted([link_name for link_name in connections if link_name != inter_source["link"]]) + cases.append( + { + "name": f"{inter_source['name']}_broadcast", + "source": inter_source, + "target_system": 0, + "target_component": 0, + "expected_links": component_broadcast_expected, + "rules": [ROUTING_RULE_REFERENCES["broadcast_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], + } + ) + + cases.append( + { + "name": f"{inter_source['name']}_to_{inter_target['name']}", + "source": inter_source, + "target_system": inter_target["system_id"], + "target_component": inter_target["component_id"], + "expected_links": [inter_target["link"]], + "rules": [ROUTING_RULE_REFERENCES["known_target_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], + } + ) + + cases.append( + { + "name": f"{inter_source['name']}_to_gcs", + "source": inter_source, + "target_system": gcs_actor["system_id"], + "target_component": gcs_actor["component_id"], + "expected_links": [gcs_actor["link"]], + "rules": [ROUTING_RULE_REFERENCES["known_target_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], + } + ) + + if marker_command_base + len(cases) > 65535: + raise ValueError(f"marker_command_base={marker_command_base} too high for case_count={len(cases)}") + + results: List[Dict[str, Any]] = [] + for case_index, case in enumerate(cases, start=1): + drain_all_links(connections, float(timing["drain_window_s"])) + command_id = marker_command_base + case_index + confirmation = case_index % 256 + params = [ + float(1000 + case_index), + float(2000 + case_index), + float(3000 + case_index), + float(4000 + case_index), + float(5000 + case_index), + float(6000 + case_index), + float(7000 + case_index), + ] + + source = case["source"] + source_connection = connections[source["link"]] + set_source_identity(source_connection, source["system_id"], source["component_id"]) + source_connection.mav.command_long_send( + int(case["target_system"]), + int(case["target_component"]), + command_id, + confirmation, + params[0], + params[1], + params[2], + params[3], + params[4], + params[5], + params[6], + ) + + observations = collect_command_observations( + connections=connections, + command_id=command_id, + observation_window_s=float(timing["observation_window_s"]), + ) + observed_links = sorted([link_name for link_name, msgs in observations.items() if len(msgs) > 0]) + expected_links = sorted(case["expected_links"]) + no_ingress_loop = source["link"] not in observed_links + routing_match = observed_links == expected_links + + payload_intact = True + payload_issues: List[str] = [] + for link_name, messages in observations.items(): + for message in messages: + if message["src_system"] != int(source["system_id"]) or message["src_component"] != int(source["component_id"]): + payload_intact = False + payload_issues.append( + f"link={link_name} src_mismatch expected=({source['system_id']},{source['component_id']}) " + f"observed=({message['src_system']},{message['src_component']})" + ) + if message["target_system"] != int(case["target_system"]) or message["target_component"] != int(case["target_component"]): + payload_intact = False + payload_issues.append( + f"link={link_name} target_mismatch expected=({case['target_system']},{case['target_component']}) " + f"observed=({message['target_system']},{message['target_component']})" + ) + if message["command"] != command_id or message["confirmation"] != confirmation: + payload_intact = False + payload_issues.append( + f"link={link_name} cmd_mismatch expected=(command={command_id},confirmation={confirmation}) " + f"observed=(command={message['command']},confirmation={message['confirmation']})" + ) + for index, observed_value in enumerate(message["params"]): + expected_value = params[index] + if not math.isclose(observed_value, expected_value, rel_tol=0.0, abs_tol=1e-3): + payload_intact = False + payload_issues.append( + f"link={link_name} param{index + 1}_mismatch expected={expected_value} observed={observed_value}" + ) + + case_pass = routing_match and no_ingress_loop and payload_intact + results.append( + { + "name": case["name"], + "source": source["name"], + "source_link": source["link"], + "target_system": int(case["target_system"]), + "target_component": int(case["target_component"]), + "command_id": command_id, + "expected_links": expected_links, + "observed_links": observed_links, + "routing_match": routing_match, + "no_ingress_loop": no_ingress_loop, + "payload_intact": payload_intact, + "pass": case_pass, + "rules": case["rules"], + "payload_issues": payload_issues, + } + ) + + print( + f"case={case['name']} command_id={command_id} source={source['name']} source_link={source['link']} " + f"target=({case['target_system']},{case['target_component']}) expected_links={expected_links} observed_links={observed_links} " + f"routing_match={routing_match} no_ingress_loop={no_ingress_loop} payload_intact={payload_intact} pass={case_pass}", + flush=True, + ) + time.sleep(float(timing["inter_case_pause_s"])) + + finally: + for connection in connections.values(): + connection.close() + + pass_count = sum(1 for result in results if result["pass"]) + fail_count = len(results) - pass_count + + markdown_lines: List[str] = [] + markdown_lines.append("# MAVLink Routing Compliance Test") + markdown_lines.append("") + markdown_lines.append( + f"- fc_system_id: `{fc_system_id}`" + ) + markdown_lines.append( + f"- links: `{sorted(list(link_cfgs.keys()))}`" + ) + markdown_lines.append( + f"- gcs_link: `{gcs_link_name}`" + ) + markdown_lines.append( + f"- components: `{[(component['name'], component['system_id'], component['component_id'], component['link']) for component in components]}`" + ) + markdown_lines.append("") + markdown_lines.append("## Rule References") + markdown_lines.append("") + markdown_lines.append(f"- broadcast_forward: `{ROUTING_RULE_REFERENCES['broadcast_forward']}`") + markdown_lines.append(f"- known_target_forward: `{ROUTING_RULE_REFERENCES['known_target_forward']}`") + markdown_lines.append(f"- unknown_target_blocked: `{ROUTING_RULE_REFERENCES['unknown_target_blocked']}`") + markdown_lines.append(f"- no_ingress_loop: `{ROUTING_RULE_REFERENCES['no_ingress_loop']}`") + markdown_lines.append(f"- no_repack: `{ROUTING_RULE_REFERENCES['no_repack']}`") + markdown_lines.append("") + markdown_lines.append("## Results") + markdown_lines.append("") + markdown_lines.append("| Case | Source(link) | Target(sys,comp) | Expected Links | Observed Links | Routing | No Ingress Loop | Payload Intact | Pass |") + markdown_lines.append("| --- | --- | --- | --- | --- | --- | --- | --- | --- |") + for result in results: + markdown_lines.append( + f"| `{result['name']}` | `{result['source']} ({result['source_link']})` | " + f"`({result['target_system']},{result['target_component']})` | " + f"`{result['expected_links']}` | `{result['observed_links']}` | " + f"`{result['routing_match']}` | `{result['no_ingress_loop']}` | `{result['payload_intact']}` | `{result['pass']}` |" + ) + markdown_lines.append("") + markdown_lines.append(f"summary pass_count={pass_count} fail_count={fail_count} total={len(results)}") + markdown_lines.append("") + + failures = [result for result in results if not result["pass"]] + if failures: + markdown_lines.append("## Failure Details") + markdown_lines.append("") + for failure in failures: + markdown_lines.append(f"### {failure['name']}") + markdown_lines.append("") + markdown_lines.append( + f"- expected_links: `{failure['expected_links']}` observed_links: `{failure['observed_links']}` " + f"routing_match: `{failure['routing_match']}` no_ingress_loop: `{failure['no_ingress_loop']}` " + f"payload_intact: `{failure['payload_intact']}`" + ) + if failure["payload_issues"]: + markdown_lines.append(f"- payload_issues: `{failure['payload_issues']}`") + markdown_lines.append(f"- rule_refs: `{failure['rules']}`") + markdown_lines.append("") + + report_path.write_text("\n".join(markdown_lines) + "\n", encoding="utf-8") + print(f"report_path={report_path} pass_count={pass_count} fail_count={fail_count} total={len(results)}", flush=True) + + if fail_count > 0: + raise SystemExit(1) diff --git a/src/utils/mavlink_tests/mavlink_test_rc.py b/src/utils/mavlink_tests/mavlink_test_rc.py new file mode 100644 index 00000000000..4862ec7aad2 --- /dev/null +++ b/src/utils/mavlink_tests/mavlink_test_rc.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python3 +""" +Usage: + conda run -n drone python mydev/branch/mav_multi/mavlink_test_rc.py --master tcp:127.0.0.1:5761 + conda run -n drone python mydev/branch/mav_multi/mavlink_test_rc.py --master tcp:127.0.0.1:5761 --duration 20 --tx-hz 100 --roll MID --pitch MID --yaw MID --throttle LOW +""" + +from __future__ import annotations + +import argparse +import time +from collections import Counter +from typing import List + +from pymavlink import mavutil + + +CHANNEL_ORDER = [ + "roll", + "pitch", + "throttle", + "yaw", + "ch5", + "ch6", + "ch7", + "ch8", + "ch9", + "ch10", + "ch11", + "ch12", + "ch13", + "ch14", + "ch15", + "ch16", + "ch17", + "ch18", +] +DEFAULT_CHANNELS = [900] * 18 +DEFAULT_CHANNELS[0] = 1500 +DEFAULT_CHANNELS[1] = 1500 +DEFAULT_CHANNELS[3] = 1500 +LEVEL_TO_PWM = {"LOW": 900, "MID": 1500, "HIGH": 2100} + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Send MAVLink RC_CHANNELS_OVERRIDE stream and monitor RC echo.") + parser.add_argument("--master", required=True, help='pymavlink master endpoint, e.g. "tcp:127.0.0.1:5761"') + parser.add_argument("--tx-hz", type=float, default=100.0, help="RC override transmit rate in Hz") + parser.add_argument("--duration", type=float, default=0.0, help="Test duration in seconds, 0 means run forever") + parser.add_argument("--source-system", type=int, default=240, help="MAVLink source system ID") + parser.add_argument("--source-component", type=int, default=191, help="MAVLink source component ID") + parser.add_argument("--echo-tolerance", type=int, default=20, help="Allowed absolute PWM error for RC_CHANNELS echo checks") + for channel_name in CHANNEL_ORDER: + parser.add_argument(f"--{channel_name}", default=None, help="LOW/MID/HIGH or integer PWM") + args = parser.parse_args() + + channels: List[int] = list(DEFAULT_CHANNELS) + for idx, channel_name in enumerate(CHANNEL_ORDER): + raw_value = getattr(args, channel_name) + if raw_value is None: + continue + level_name = raw_value.upper() + if level_name in LEVEL_TO_PWM: + channels[idx] = LEVEL_TO_PWM[level_name] + else: + channels[idx] = int(raw_value) + + print( + f"master={args.master} tx_hz={args.tx_hz} duration={args.duration} " + f"source_system={args.source_system} source_component={args.source_component} channels={channels}", + flush=True, + ) + + master = mavutil.mavlink_connection( + args.master, + source_system=args.source_system, + source_component=args.source_component, + autoreconnect=True, + ) + heartbeat = master.wait_heartbeat(timeout=10) + if heartbeat is None: + raise TimeoutError("No heartbeat received from FC") + target_system = heartbeat.get_srcSystem() + target_component = heartbeat.get_srcComponent() + print(f"target_system={target_system} target_component={target_component}", flush=True) + + period_s = 1.0 / args.tx_hz + tx_count = 0 + rx_count = 0 + mismatch_count = 0 + decode_errors = 0 + last_rx_time = 0.0 + loop_start = time.monotonic() + next_tx_time = loop_start + next_report_time = loop_start + 1.0 + mismatch_hist = Counter() + + while True: + now = time.monotonic() + if args.duration > 0 and (now - loop_start) >= args.duration: + break + + if now >= next_tx_time: + master.mav.rc_channels_override_send(target_system, target_component, *channels[:8]) + tx_count += 1 + next_tx_time += period_s + if next_tx_time < now: + next_tx_time = now + period_s + + message = master.recv_match(type=["RC_CHANNELS", "RC_CHANNELS_RAW"], blocking=False) + if message is not None: + rx_count += 1 + last_rx_time = now + if message.get_type() == "RC_CHANNELS": + for i in range(4): + key = f"chan{i + 1}_raw" + observed = int(getattr(message, key)) + err = abs(observed - channels[i]) + if err > args.echo_tolerance: + mismatch_count += 1 + mismatch_hist[i + 1] += 1 + else: + decode_errors += 1 + + if now >= next_report_time: + rx_age = -1.0 if last_rx_time == 0.0 else (now - last_rx_time) + print( + f"status_s={now - loop_start:.1f} tx={tx_count} rx={rx_count} mismatches={mismatch_count} " + f"decode_errors={decode_errors} rx_age_s={rx_age:.3f} mismatch_hist={dict(mismatch_hist)}", + flush=True, + ) + next_report_time += 1.0 + + time.sleep(0.001) + + elapsed = max(time.monotonic() - loop_start, 1e-6) + print( + f"summary elapsed_s={elapsed:.2f} tx={tx_count} rx={rx_count} mismatch_count={mismatch_count} " + f"mismatch_rate={mismatch_count / max(rx_count, 1):.6f} tx_hz={tx_count / elapsed:.2f} rx_hz={rx_count / elapsed:.2f}", + flush=True, + ) diff --git a/src/utils/mavlink_tests/msp_test_rc.py b/src/utils/mavlink_tests/msp_test_rc.py new file mode 100644 index 00000000000..9cd41cb6c5d --- /dev/null +++ b/src/utils/mavlink_tests/msp_test_rc.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +""" +Usage: + conda run -n drone python mydev/branch/mav_multi/msp_test_rc.py --msp-endpoint 127.0.0.1:5760 --duration 20 + conda run -n drone python mydev/branch/mav_multi/msp_test_rc.py --msp-endpoint 127.0.0.1:5760 --poll-hz 25 --roll MID --pitch MID --yaw MID --throttle LOW +""" + +from __future__ import annotations + +import argparse +import struct +import time +from collections import Counter +from pathlib import Path +from typing import List + +from serial.serialutil import SerialException + +try: + from mspapi2.msp_api import MSPApi + from mspapi2.lib import InavMSP +except ModuleNotFoundError: + repo_root_guess = Path(__file__).resolve().parents[3] + mspapi2_repo = repo_root_guess.parent / "mspapi2" + if mspapi2_repo.exists(): + import sys + + sys.path.insert(0, str(mspapi2_repo)) + from mspapi2.msp_api import MSPApi + from mspapi2.lib import InavMSP + + +CHANNEL_ORDER = [ + "roll", + "pitch", + "throttle", + "yaw", + "ch5", + "ch6", + "ch7", + "ch8", + "ch9", + "ch10", + "ch11", + "ch12", + "ch13", + "ch14", + "ch15", + "ch16", + "ch17", + "ch18", +] +DEFAULT_CHANNELS = [900] * 18 +DEFAULT_CHANNELS[0] = 1500 +DEFAULT_CHANNELS[1] = 1500 +DEFAULT_CHANNELS[3] = 1500 +LEVEL_TO_PWM = {"LOW": 900, "MID": 1500, "HIGH": 2100} + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Poll MSP_RC and monitor RC values against expected channels.") + parser.add_argument("--msp-endpoint", required=True, help='MSP endpoint, e.g. "127.0.0.1:5760"') + parser.add_argument("--poll-hz", type=float, default=25.0, help="MSP_RC poll rate in Hz") + parser.add_argument("--timeout-s", type=float, default=0.2, help="Per MSP request timeout") + parser.add_argument("--duration", type=float, default=0.0, help="Test duration in seconds, 0 means run forever") + parser.add_argument("--echo-tolerance", type=int, default=40, help="Allowed absolute PWM error per channel") + for channel_name in CHANNEL_ORDER: + parser.add_argument(f"--{channel_name}", default=None, help="LOW/MID/HIGH or integer PWM") + args = parser.parse_args() + + expected_channels: List[int] = list(DEFAULT_CHANNELS) + for idx, channel_name in enumerate(CHANNEL_ORDER): + raw_value = getattr(args, channel_name) + if raw_value is None: + continue + level_name = raw_value.upper() + if level_name in LEVEL_TO_PWM: + expected_channels[idx] = LEVEL_TO_PWM[level_name] + else: + expected_channels[idx] = int(raw_value) + + print( + f"msp_endpoint={args.msp_endpoint} poll_hz={args.poll_hz} duration={args.duration} " + f"timeout_s={args.timeout_s} expected_channels={expected_channels}", + flush=True, + ) + + poll_period_s = 1.0 / args.poll_hz + success_count = 0 + fail_count = 0 + mismatch_count = 0 + mismatch_hist = Counter() + last_success_time = 0.0 + start_t = time.monotonic() + next_poll_t = start_t + next_report_t = start_t + 1.0 + + with MSPApi(tcp_endpoint=args.msp_endpoint) as msp_api: + msp_api._serial._max_retries = 1 + msp_api._serial._reconnect_delay = 0.05 + while True: + now = time.monotonic() + if args.duration > 0 and (now - start_t) >= args.duration: + break + + if now >= next_poll_t: + try: + _, payload = msp_api._request_raw(InavMSP.MSP_RC, timeout=args.timeout_s) + if len(payload) % 2: + raise ValueError("MSP RC payload length must be even") + channel_count = len(payload) // 2 + observed_channels = list(struct.unpack(f"<{channel_count}H", payload)) + success_count += 1 + last_success_time = now + for i in range(min(channel_count, len(expected_channels))): + error = abs(observed_channels[i] - expected_channels[i]) + if error > args.echo_tolerance: + mismatch_count += 1 + mismatch_hist[i + 1] += 1 + except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError): + fail_count += 1 + next_poll_t += poll_period_s + if next_poll_t < now: + next_poll_t = now + poll_period_s + + if now >= next_report_t: + last_ok_age_s = -1.0 if last_success_time == 0.0 else (now - last_success_time) + total = success_count + fail_count + print( + f"status_s={now - start_t:.1f} ok={success_count} fail={fail_count} fail_rate={fail_count / max(total, 1):.6f} " + f"mismatch={mismatch_count} mismatch_rate={mismatch_count / max(success_count, 1):.6f} " + f"last_ok_age_s={last_ok_age_s:.3f} mismatch_hist={dict(mismatch_hist)}", + flush=True, + ) + next_report_t += 1.0 + + time.sleep(0.001) + + elapsed = max(time.monotonic() - start_t, 1e-6) + total = success_count + fail_count + print( + f"summary elapsed_s={elapsed:.2f} ok={success_count} fail={fail_count} fail_rate={fail_count / max(total, 1):.6f} " + f"mismatch={mismatch_count} mismatch_rate={mismatch_count / max(success_count, 1):.6f} poll_hz={success_count / elapsed:.2f}", + flush=True, + ) diff --git a/src/utils/mavlink_tests/routing_test_config.yaml b/src/utils/mavlink_tests/routing_test_config.yaml new file mode 100644 index 00000000000..e4d17865675 --- /dev/null +++ b/src/utils/mavlink_tests/routing_test_config.yaml @@ -0,0 +1,58 @@ +timing: + connect_timeout_s: 10.0 + settle_after_connect_s: 0.5 + route_learn_settle_s: 0.5 + drain_window_s: 0.15 + observation_window_s: 0.8 + inter_case_pause_s: 0.15 + +network: + gcs_link: link_gcs + links: + link_gcs: + endpoint: tcp:127.0.0.1:5761 + source_system: 255 + source_component: 190 + link_components_a: + endpoint: tcp:127.0.0.1:5762 + source_system: 254 + source_component: 200 + link_camera: + endpoint: tcp:127.0.0.1:5763 + source_system: 253 + source_component: 201 + link_gimbal: + endpoint: tcp:127.0.0.1:5764 + source_system: 252 + source_component: 202 + +vehicle: + fc_system_id: 1 + +tests: + marker_command_base: 31000 + unknown_component_id: 199 + unknown_system_id: 42 + report_markdown: mydev/branch/mav_multi/ROUTING_TEST.md + +components: + - name: mav1_elrs_receiver + link: link_components_a + system_id: 1 + component_id: 68 + mav_type: 49 + - name: mav2_companion_computer + link: link_components_a + system_id: 1 + component_id: 191 + mav_type: 18 + - name: mav3_camera + link: link_camera + system_id: 1 + component_id: 100 + mav_type: 30 + - name: mav4_gimbal + link: link_gimbal + system_id: 1 + component_id: 154 + mav_type: 26 diff --git a/src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml b/src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml new file mode 100644 index 00000000000..a73523f0698 --- /dev/null +++ b/src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml @@ -0,0 +1,51 @@ +sitl: + binary: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/cmake/build_SITL/inav_9.0.0_SITL + workdir: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/cmake + eeprom_path: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/mydev/branch/mav_multi/eeprom_multi4_sweep.bin + runtime_log: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/mydev/branch/mav_multi/sitl_runtime_multi4_sweep.log + +ports: + msp: 5760 + rc: 5761 + telemetry: + - 5762 + - 5763 + - 5764 + +cli: + rc_baud: 460800 + telemetry_baud: 115200 + ext_status_rate_hz: 10 + rc_chan_rate_hz: 25 + pos_rate_hz: 10 + extra1_rate_hz: 10 + extra2_rate_hz: 10 + extra3_rate_hz: 5 + +tests: + port_ready_timeout_s: 30 + save_reboot_timeout_s: 45 + mavsdk_probe_timeout_s: 10 + heartbeat_expected_hz: 1.0 + rc_expected_hz: 25.0 + rc_target_system: 1 + rc_target_component: 1 + rc_tx_hz: 100.0 + rc_tx_hz_max: 400.0 + msp_poll_hz: 50.0 + inav_status_hz: 5.0 + msp_request_timeout_s: 0.2 + stress_command_message_id: 33 + baseline_duration_s: 30 + progressive_rates_hz: + - 50 + - 100 + - 200 + - 400 + stress_duration_s: 20 + max_rate_hz: 400 + all_ports_max_duration_s: 30 + sweep_duration_s: 8 + +output: + testing_md: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/mydev/branch/mav_multi/TESTING_multiport4_sweep_rc460800_tele115200.md From 27d98b54f12be2e75eb1fdce91f3ceb81d2404ad Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Tue, 3 Mar 2026 15:47:12 -0500 Subject: [PATCH 27/42] only initialize streams for port 1 --- .gitignore | 1 + docs/Mavlink.md | 6 +- docs/Settings.md | 180 ------------------ src/main/fc/settings.yaml | 126 ------------ src/main/telemetry/mavlink.c | 55 ++++-- src/main/telemetry/mavlink.h | 5 +- src/main/telemetry/telemetry.c | 38 ++-- src/main/telemetry/telemetry.h | 1 + src/test/unit/mavlink_unittest.cc | 38 ++++ src/utils/mavlink_tests/ROUTING_TEST.md | 29 +-- .../mav_fix2_singleport_benchmark.py | 66 ++++++- .../mavlink_tests/mav_multi_benchmark.py | 87 ++++++--- .../mavlink_routing_compliance_test.py | 2 +- .../mavlink_tests/routing_test_config.yaml | 36 ++-- 14 files changed, 260 insertions(+), 410 deletions(-) diff --git a/.gitignore b/.gitignore index 6d7138e6bd5..887f6707141 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,7 @@ *.bak *.uvgui.* *.ubx +*.log .project .settings .cproject diff --git a/docs/Mavlink.md b/docs/Mavlink.md index d39c1165c25..98bc3c681ad 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -95,7 +95,8 @@ Limited implementation of the Command protocol. ## Datastream groups and defaults -Default rates (Hz) are shown; adjust with the CLI keys above. +Default rates (Hz) are shown; adjust with the CLI keys above for port 1. +Ports 2..N use a secondary startup profile (heartbeat at 1 Hz, other streams disabled). | Datastream group | Messages | Default rate | | --- | --- | --- | @@ -103,7 +104,8 @@ Default rates (Hz) are shown; adjust with the CLI keys above. | `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 | +| `EXTRA2` | `VFR_HUD` | 2 Hz | +| `HEARTBEAT` | `HEARTBEAT` | 1 Hz (independent of stream groups) | | `EXT_SYS_STATE` | `EXTENDED_SYS_STATE` | 1 Hz (defaults to `mavlink_port1_extra3_rate`) | | `EXTRA3` | `BATTERY_STATUS`, `SCALED_PRESSURE`, `STATUSTEXT` (when present) | 1 Hz | diff --git a/docs/Settings.md b/docs/Settings.md index 939991a9792..31e8095d34e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2762,46 +2762,6 @@ MAVLink Component ID for MAVLink port 2 --- -### mavlink_port2_ext_status_rate - -Rate of the extended status message for MAVLink telemetry on port 2 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - -### mavlink_port2_extra1_rate - -Rate of the extra1 message for MAVLink telemetry on port 2 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - -### mavlink_port2_extra2_rate - -Rate of the extra2 message for MAVLink telemetry on port 2 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - -### mavlink_port2_extra3_rate - -Rate of the extra3 message for MAVLink telemetry on port 2 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 0 | 255 | - ---- - ### mavlink_port2_high_latency Enable MAVLink high-latency mode on port 2 @@ -2822,16 +2782,6 @@ Minimum percent of TX buffer space free for MAVLink port 2. Requires RADIO_STATU --- -### mavlink_port2_pos_rate - -Rate of the position message for MAVLink telemetry on port 2 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - ### mavlink_port2_radio_type MAVLink radio type for port 2. Affects RSSI and LQ reporting on OSD. @@ -2842,16 +2792,6 @@ MAVLink radio type for port 2. Affects RSSI and LQ reporting on OSD. --- -### mavlink_port2_rc_chan_rate - -Rate of the RC channels message for MAVLink telemetry on port 2 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 0 | 255 | - ---- - ### mavlink_port3_compid MAVLink Component ID for MAVLink port 3 @@ -2862,46 +2802,6 @@ MAVLink Component ID for MAVLink port 3 --- -### mavlink_port3_ext_status_rate - -Rate of the extended status message for MAVLink telemetry on port 3 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - -### mavlink_port3_extra1_rate - -Rate of the extra1 message for MAVLink telemetry on port 3 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - -### mavlink_port3_extra2_rate - -Rate of the extra2 message for MAVLink telemetry on port 3 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - -### mavlink_port3_extra3_rate - -Rate of the extra3 message for MAVLink telemetry on port 3 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 0 | 255 | - ---- - ### mavlink_port3_high_latency Enable MAVLink high-latency mode on port 3 @@ -2922,16 +2822,6 @@ Minimum percent of TX buffer space free for MAVLink port 3. Requires RADIO_STATU --- -### mavlink_port3_pos_rate - -Rate of the position message for MAVLink telemetry on port 3 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - ### mavlink_port3_radio_type MAVLink radio type for port 3. Affects RSSI and LQ reporting on OSD. @@ -2942,16 +2832,6 @@ MAVLink radio type for port 3. Affects RSSI and LQ reporting on OSD. --- -### mavlink_port3_rc_chan_rate - -Rate of the RC channels message for MAVLink telemetry on port 3 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 0 | 255 | - ---- - ### mavlink_port4_compid MAVLink Component ID for MAVLink port 4 @@ -2962,46 +2842,6 @@ MAVLink Component ID for MAVLink port 4 --- -### mavlink_port4_ext_status_rate - -Rate of the extended status message for MAVLink telemetry on port 4 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - -### mavlink_port4_extra1_rate - -Rate of the extra1 message for MAVLink telemetry on port 4 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - -### mavlink_port4_extra2_rate - -Rate of the extra2 message for MAVLink telemetry on port 4 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - -### mavlink_port4_extra3_rate - -Rate of the extra3 message for MAVLink telemetry on port 4 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 0 | 255 | - ---- - ### mavlink_port4_high_latency Enable MAVLink high-latency mode on port 4 @@ -3022,16 +2862,6 @@ Minimum percent of TX buffer space free for MAVLink port 4. Requires RADIO_STATU --- -### mavlink_port4_pos_rate - -Rate of the position message for MAVLink telemetry on port 4 - -| Default | Min | Max | -| --- | --- | --- | -| 2 | 0 | 255 | - ---- - ### mavlink_port4_radio_type MAVLink radio type for port 4. Affects RSSI and LQ reporting on OSD. @@ -3042,16 +2872,6 @@ MAVLink radio type for port 4. Affects RSSI and LQ reporting on OSD. --- -### mavlink_port4_rc_chan_rate - -Rate of the RC channels message for MAVLink telemetry on port 4 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 0 | 255 | - ---- - ### mavlink_sysid MAVLink System ID diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4d262c9b63e..18fda6bc4e6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3310,48 +3310,6 @@ groups: description: "Enable MAVLink high-latency mode on port 1" type: bool default_value: OFF - - name: mavlink_port2_ext_status_rate - field: mavlink[1].extended_status_rate - description: "Rate of the extended status message for MAVLink telemetry on port 2" - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port2_rc_chan_rate - description: "Rate of the RC channels message for MAVLink telemetry on port 2" - field: mavlink[1].rc_channels_rate - type: uint8_t - min: 0 - max: 255 - default_value: 1 - - name: mavlink_port2_pos_rate - description: "Rate of the position message for MAVLink telemetry on port 2" - field: mavlink[1].position_rate - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port2_extra1_rate - description: "Rate of the extra1 message for MAVLink telemetry on port 2" - field: mavlink[1].extra1_rate - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port2_extra2_rate - description: "Rate of the extra2 message for MAVLink telemetry on port 2" - field: mavlink[1].extra2_rate - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port2_extra3_rate - description: "Rate of the extra3 message for MAVLink telemetry on port 2" - field: mavlink[1].extra3_rate - type: uint8_t - min: 0 - max: 255 - default_value: 1 - name: mavlink_port2_min_txbuffer field: mavlink[1].min_txbuff description: "Minimum percent of TX buffer space free for MAVLink port 2. Requires RADIO_STATUS messages." @@ -3376,48 +3334,6 @@ groups: description: "Enable MAVLink high-latency mode on port 2" type: bool default_value: OFF - - name: mavlink_port3_ext_status_rate - field: mavlink[2].extended_status_rate - description: "Rate of the extended status message for MAVLink telemetry on port 3" - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port3_rc_chan_rate - description: "Rate of the RC channels message for MAVLink telemetry on port 3" - field: mavlink[2].rc_channels_rate - type: uint8_t - min: 0 - max: 255 - default_value: 1 - - name: mavlink_port3_pos_rate - description: "Rate of the position message for MAVLink telemetry on port 3" - field: mavlink[2].position_rate - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port3_extra1_rate - description: "Rate of the extra1 message for MAVLink telemetry on port 3" - field: mavlink[2].extra1_rate - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port3_extra2_rate - description: "Rate of the extra2 message for MAVLink telemetry on port 3" - field: mavlink[2].extra2_rate - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port3_extra3_rate - description: "Rate of the extra3 message for MAVLink telemetry on port 3" - field: mavlink[2].extra3_rate - type: uint8_t - min: 0 - max: 255 - default_value: 1 - name: mavlink_port3_min_txbuffer field: mavlink[2].min_txbuff description: "Minimum percent of TX buffer space free for MAVLink port 3. Requires RADIO_STATUS messages." @@ -3442,48 +3358,6 @@ groups: description: "Enable MAVLink high-latency mode on port 3" type: bool default_value: OFF - - name: mavlink_port4_ext_status_rate - field: mavlink[3].extended_status_rate - description: "Rate of the extended status message for MAVLink telemetry on port 4" - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port4_rc_chan_rate - description: "Rate of the RC channels message for MAVLink telemetry on port 4" - field: mavlink[3].rc_channels_rate - type: uint8_t - min: 0 - max: 255 - default_value: 1 - - name: mavlink_port4_pos_rate - description: "Rate of the position message for MAVLink telemetry on port 4" - field: mavlink[3].position_rate - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port4_extra1_rate - description: "Rate of the extra1 message for MAVLink telemetry on port 4" - field: mavlink[3].extra1_rate - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port4_extra2_rate - description: "Rate of the extra2 message for MAVLink telemetry on port 4" - field: mavlink[3].extra2_rate - type: uint8_t - min: 0 - max: 255 - default_value: 2 - - name: mavlink_port4_extra3_rate - description: "Rate of the extra3 message for MAVLink telemetry on port 4" - field: mavlink[3].extra3_rate - type: uint8_t - min: 0 - max: 255 - default_value: 1 - name: mavlink_port4_min_txbuffer field: mavlink[3].min_txbuff description: "Minimum percent of TX buffer space free for MAVLink port 4. Requires RADIO_STATUS messages." diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 9198e982ac8..713c0967f0f 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -90,15 +90,16 @@ #include "scheduler/scheduler.h" -/* MAVLink datastream rates in Hz */ -const uint8_t mavDefaultRates[MAVLINK_STREAM_COUNT] = { - [MAV_DATA_STREAM_EXTENDED_STATUS] = 2, // 2Hz - [MAV_DATA_STREAM_RC_CHANNELS] = 1, // 1Hz - [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_EXTENDED_SYS_STATE] = 1 // 1Hz +/* Secondary profile for ports 2..N: heartbeat only. */ +static const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT] = { + [MAV_DATA_STREAM_EXTENDED_STATUS] = 0, + [MAV_DATA_STREAM_RC_CHANNELS] = 0, + [MAV_DATA_STREAM_POSITION] = 0, + [MAV_DATA_STREAM_EXTRA1] = 0, + [MAV_DATA_STREAM_EXTRA2] = 0, + [MAV_DATA_STREAM_EXTRA3] = 0, + [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = 0, + [MAV_DATA_STREAM_HEARTBEAT] = 1 }; static mavlinkPortRuntime_t mavPortStates[MAX_MAVLINK_PORTS]; @@ -317,15 +318,26 @@ static int32_t mavlinkStreamIntervalUs(uint8_t streamNum) static void configureMAVLinkStreamRates(uint8_t portIndex) { - mavlinkSetActivePortContext(portIndex); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_STATUS, mavActiveConfig->extended_status_rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_RC_CHANNELS, mavActiveConfig->rc_channels_rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_POSITION, mavActiveConfig->position_rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA1, mavActiveConfig->extra1_rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA2, mavActiveConfig->extra2_rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA3, mavActiveConfig->extra3_rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_SYS_STATE, mavActiveConfig->extra3_rate); - memcpy(mavActivePort->mavRatesConfigured, mavActivePort->mavRates, sizeof(mavActivePort->mavRatesConfigured)); + const mavlinkTelemetryPortConfig_t *primaryConfig = &telemetryConfig()->mavlink[0]; + const uint8_t mavPrimaryRates[MAVLINK_STREAM_COUNT] = { + [MAV_DATA_STREAM_EXTENDED_STATUS] = primaryConfig->extended_status_rate, + [MAV_DATA_STREAM_RC_CHANNELS] = primaryConfig->rc_channels_rate, + [MAV_DATA_STREAM_POSITION] = primaryConfig->position_rate, + [MAV_DATA_STREAM_EXTRA1] = primaryConfig->extra1_rate, + [MAV_DATA_STREAM_EXTRA2] = primaryConfig->extra2_rate, + [MAV_DATA_STREAM_EXTRA3] = primaryConfig->extra3_rate, + [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = primaryConfig->extra3_rate, + [MAV_DATA_STREAM_HEARTBEAT] = 1 + }; + + const uint8_t *selectedRates = (portIndex == 0) ? mavPrimaryRates : mavSecondaryRates; + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + for (uint8_t stream = 0; stream < MAVLINK_STREAM_COUNT; stream++) { + state->mavRates[stream] = selectedRates[stream]; + state->mavRatesConfigured[stream] = selectedRates[stream]; + state->mavTicks[stream] = 0; + } } static void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) @@ -1549,6 +1561,9 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs) if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) { mavlinkSendVfrHud(); + } + + if (mavlinkStreamTrigger(MAV_DATA_STREAM_HEARTBEAT)) { mavlinkSendHeartbeat(); } @@ -1980,6 +1995,8 @@ static bool mavlinkMessageToStream(uint16_t messageId, uint8_t *streamNum) { switch (messageId) { case MAVLINK_MSG_ID_HEARTBEAT: + *streamNum = MAV_DATA_STREAM_HEARTBEAT; + return true; case MAVLINK_MSG_ID_VFR_HUD: *streamNum = MAV_DATA_STREAM_EXTRA2; return true; @@ -2491,6 +2508,8 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { switch(mavActiveConfig->radio_type) { + case MAVLINK_RADIO_NONE: + break; case MAVLINK_RADIO_SIK: // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; diff --git a/src/main/telemetry/mavlink.h b/src/main/telemetry/mavlink.h index 57d03e1d64e..d774e6cb07b 100755 --- a/src/main/telemetry/mavlink.h +++ b/src/main/telemetry/mavlink.h @@ -36,7 +36,8 @@ #define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) #define TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US (5 * 1000 * 1000) #define MAV_DATA_STREAM_EXTENDED_SYS_STATE (MAV_DATA_STREAM_EXTRA3 + 1) -#define MAVLINK_STREAM_COUNT (MAV_DATA_STREAM_EXTENDED_SYS_STATE + 1) +#define MAV_DATA_STREAM_HEARTBEAT (MAV_DATA_STREAM_EXTENDED_SYS_STATE + 1) +#define MAVLINK_STREAM_COUNT (MAV_DATA_STREAM_HEARTBEAT + 1) #define ARDUPILOT_VERSION_MAJOR 4 #define ARDUPILOT_VERSION_MINOR 6 #define ARDUPILOT_VERSION_PATCH 3 @@ -128,8 +129,6 @@ typedef struct mavlinkRouteEntry_s { uint8_t ingressPortIndex; } mavlinkRouteEntry_t; -extern const uint8_t mavDefaultRates[MAVLINK_STREAM_COUNT]; - typedef struct mavlinkPortRuntime_s { serialPort_t *port; const serialPortConfig_t *portConfig; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index c77f80a2c57..dba8709e6e9 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -86,6 +86,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .accEventThresholdNegX = SETTING_ACC_EVENT_THRESHOLD_NEG_X_DEFAULT, #endif +#if defined(USE_TELEMETRY_MAVLINK) .mavlink_common = { .autopilot_type = SETTING_MAVLINK_AUTOPILOT_TYPE_DEFAULT, .version = SETTING_MAVLINK_VERSION_DEFAULT, @@ -105,42 +106,43 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .high_latency = SETTING_MAVLINK_PORT1_HIGH_LATENCY_DEFAULT }, { - .extended_status_rate = SETTING_MAVLINK_PORT2_EXT_STATUS_RATE_DEFAULT, - .rc_channels_rate = SETTING_MAVLINK_PORT2_RC_CHAN_RATE_DEFAULT, - .position_rate = SETTING_MAVLINK_PORT2_POS_RATE_DEFAULT, - .extra1_rate = SETTING_MAVLINK_PORT2_EXTRA1_RATE_DEFAULT, - .extra2_rate = SETTING_MAVLINK_PORT2_EXTRA2_RATE_DEFAULT, - .extra3_rate = SETTING_MAVLINK_PORT2_EXTRA3_RATE_DEFAULT, + .extended_status_rate = 0, + .rc_channels_rate = 0, + .position_rate = 0, + .extra1_rate = 0, + .extra2_rate = 0, + .extra3_rate = 0, .min_txbuff = SETTING_MAVLINK_PORT2_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_PORT2_RADIO_TYPE_DEFAULT, .compid = SETTING_MAVLINK_PORT2_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT2_HIGH_LATENCY_DEFAULT }, { - .extended_status_rate = SETTING_MAVLINK_PORT3_EXT_STATUS_RATE_DEFAULT, - .rc_channels_rate = SETTING_MAVLINK_PORT3_RC_CHAN_RATE_DEFAULT, - .position_rate = SETTING_MAVLINK_PORT3_POS_RATE_DEFAULT, - .extra1_rate = SETTING_MAVLINK_PORT3_EXTRA1_RATE_DEFAULT, - .extra2_rate = SETTING_MAVLINK_PORT3_EXTRA2_RATE_DEFAULT, - .extra3_rate = SETTING_MAVLINK_PORT3_EXTRA3_RATE_DEFAULT, + .extended_status_rate = 0, + .rc_channels_rate = 0, + .position_rate = 0, + .extra1_rate = 0, + .extra2_rate = 0, + .extra3_rate = 0, .min_txbuff = SETTING_MAVLINK_PORT3_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_PORT3_RADIO_TYPE_DEFAULT, .compid = SETTING_MAVLINK_PORT3_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT3_HIGH_LATENCY_DEFAULT }, { - .extended_status_rate = SETTING_MAVLINK_PORT4_EXT_STATUS_RATE_DEFAULT, - .rc_channels_rate = SETTING_MAVLINK_PORT4_RC_CHAN_RATE_DEFAULT, - .position_rate = SETTING_MAVLINK_PORT4_POS_RATE_DEFAULT, - .extra1_rate = SETTING_MAVLINK_PORT4_EXTRA1_RATE_DEFAULT, - .extra2_rate = SETTING_MAVLINK_PORT4_EXTRA2_RATE_DEFAULT, - .extra3_rate = SETTING_MAVLINK_PORT4_EXTRA3_RATE_DEFAULT, + .extended_status_rate = 0, + .rc_channels_rate = 0, + .position_rate = 0, + .extra1_rate = 0, + .extra2_rate = 0, + .extra3_rate = 0, .min_txbuff = SETTING_MAVLINK_PORT4_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_PORT4_RADIO_TYPE_DEFAULT, .compid = SETTING_MAVLINK_PORT4_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT4_HIGH_LATENCY_DEFAULT } } +#endif ); void telemetryInit(void) diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 46937dd7386..f2686c5dcac 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -46,6 +46,7 @@ typedef enum { MAVLINK_RADIO_GENERIC, MAVLINK_RADIO_ELRS, MAVLINK_RADIO_SIK, + MAVLINK_RADIO_NONE // Not a radio } mavlinkRadio_e; typedef struct mavlinkTelemetryCommonConfig_s { diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index 6e09307247f..57035ad7393 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -638,6 +638,44 @@ TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream) EXPECT_EQ(interval.interval_us, -1); } +TEST(MavlinkTelemetryTest, HeartbeatIntervalIsIndependentFromExtra2Stream) +{ + initMavlinkTestState(); + + mavlink_message_t stopExtra2Msg; + mavlink_msg_request_data_stream_pack( + 42, 200, &stopExtra2Msg, + 1, MAV_COMP_ID_MISSIONPLANNER, + MAV_DATA_STREAM_EXTRA2, 0, 0); + + pushRxMessage(&stopExtra2Msg); + 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_HEARTBEAT, + 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_HEARTBEAT); + EXPECT_EQ(interval.interval_us, 1000000); +} + TEST(MavlinkTelemetryTest, RequestProtocolVersionUsesConfiguredVersion) { initMavlinkTestState(); diff --git a/src/utils/mavlink_tests/ROUTING_TEST.md b/src/utils/mavlink_tests/ROUTING_TEST.md index 6a67b31822b..7ed080083d4 100644 --- a/src/utils/mavlink_tests/ROUTING_TEST.md +++ b/src/utils/mavlink_tests/ROUTING_TEST.md @@ -1,9 +1,9 @@ # MAVLink Routing Compliance Test - fc_system_id: `1` -- links: `['link_camera', 'link_components_a', 'link_gcs', 'link_gimbal']` -- gcs_link: `link_gcs` -- components: `[('mav1_elrs_receiver', 1, 68, 'link_components_a'), ('mav2_companion_computer', 1, 191, 'link_components_a'), ('mav3_camera', 1, 100, 'link_camera'), ('mav4_gimbal', 1, 154, 'link_gimbal')]` +- links: `['link_camera', 'link_companion', 'link_gimbal', 'link_radio']` +- gcs_link: `link_radio` +- components: `[('mav2_companion_computer', 1, 191, 'link_companion'), ('mav1_elrs_receiver', 1, 68, 'link_radio'), ('mav3_camera', 1, 100, 'link_camera'), ('mav4_gimbal', 1, 154, 'link_gimbal')]` ## Rule References @@ -17,16 +17,17 @@ | Case | Source(link) | Target(sys,comp) | Expected Links | Observed Links | Routing | No Ingress Loop | Payload Intact | Pass | | --- | --- | --- | --- | --- | --- | --- | --- | --- | -| `gcs_broadcast` | `gcs (link_gcs)` | `(0,0)` | `['link_camera', 'link_components_a', 'link_gimbal']` | `['link_camera', 'link_components_a', 'link_gimbal']` | `True` | `True` | `True` | `True` | -| `gcs_target_mav1_elrs_receiver` | `gcs (link_gcs)` | `(1,68)` | `['link_components_a']` | `['link_components_a']` | `True` | `True` | `True` | `True` | -| `gcs_target_mav2_companion_computer` | `gcs (link_gcs)` | `(1,191)` | `['link_components_a']` | `['link_components_a']` | `True` | `True` | `True` | `True` | -| `gcs_target_mav3_camera` | `gcs (link_gcs)` | `(1,100)` | `['link_camera']` | `['link_camera']` | `True` | `True` | `True` | `True` | -| `gcs_target_mav4_gimbal` | `gcs (link_gcs)` | `(1,154)` | `['link_gimbal']` | `['link_gimbal']` | `True` | `True` | `True` | `True` | -| `gcs_target_unknown_component` | `gcs (link_gcs)` | `(1,199)` | `[]` | `[]` | `True` | `True` | `True` | `True` | -| `gcs_target_unknown_system` | `gcs (link_gcs)` | `(42,1)` | `[]` | `[]` | `True` | `True` | `True` | `True` | -| `mav1_elrs_receiver_broadcast` | `mav1_elrs_receiver (link_components_a)` | `(0,0)` | `['link_camera', 'link_gcs', 'link_gimbal']` | `['link_camera', 'link_gcs', 'link_gimbal']` | `True` | `True` | `True` | `True` | -| `mav1_elrs_receiver_to_mav3_camera` | `mav1_elrs_receiver (link_components_a)` | `(1,100)` | `['link_camera']` | `['link_camera']` | `True` | `True` | `True` | `True` | -| `mav1_elrs_receiver_to_gcs` | `mav1_elrs_receiver (link_components_a)` | `(255,190)` | `['link_gcs']` | `['link_gcs']` | `True` | `True` | `True` | `True` | +| `gcs_broadcast` | `gcs (link_radio)` | `(0,0)` | `['link_camera', 'link_companion', 'link_gimbal']` | `['link_camera', 'link_companion', 'link_gimbal']` | `True` | `True` | `True` | `True` | +| `gcs_target_local_system_component_broadcast` | `gcs (link_radio)` | `(1,0)` | `['link_camera', 'link_companion', 'link_gimbal']` | `['link_camera', 'link_companion', 'link_gimbal']` | `True` | `True` | `True` | `True` | +| `gcs_target_mav2_companion_computer` | `gcs (link_radio)` | `(1,191)` | `['link_companion']` | `['link_companion']` | `True` | `True` | `True` | `True` | +| `gcs_target_mav1_elrs_receiver` | `gcs (link_radio)` | `(1,68)` | `[]` | `[]` | `True` | `True` | `True` | `True` | +| `gcs_target_mav3_camera` | `gcs (link_radio)` | `(1,100)` | `['link_camera']` | `['link_camera']` | `True` | `True` | `True` | `True` | +| `gcs_target_mav4_gimbal` | `gcs (link_radio)` | `(1,154)` | `['link_gimbal']` | `['link_gimbal']` | `True` | `True` | `True` | `True` | +| `gcs_target_unknown_component` | `gcs (link_radio)` | `(1,199)` | `[]` | `[]` | `True` | `True` | `True` | `True` | +| `gcs_target_unknown_system` | `gcs (link_radio)` | `(42,1)` | `[]` | `[]` | `True` | `True` | `True` | `True` | +| `mav2_companion_computer_broadcast` | `mav2_companion_computer (link_companion)` | `(0,0)` | `['link_camera', 'link_gimbal', 'link_radio']` | `['link_camera', 'link_gimbal', 'link_radio']` | `True` | `True` | `True` | `True` | +| `mav2_companion_computer_to_mav1_elrs_receiver` | `mav2_companion_computer (link_companion)` | `(1,68)` | `['link_radio']` | `['link_radio']` | `True` | `True` | `True` | `True` | +| `mav2_companion_computer_to_gcs` | `mav2_companion_computer (link_companion)` | `(1,68)` | `['link_radio']` | `['link_radio']` | `True` | `True` | `True` | `True` | -summary pass_count=10 fail_count=0 total=10 +summary pass_count=11 fail_count=0 total=11 diff --git a/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py b/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py index f510b28b270..33cafc762c1 100644 --- a/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py +++ b/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py @@ -37,6 +37,8 @@ CHANNELS = [1500, 1500, 900, 1500] + [900] * 14 MAV_CMD_REQUEST_MESSAGE = int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE) +MAV_CMD_SET_MESSAGE_INTERVAL = int(mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL) +MAVLINK_MSG_ID_HEARTBEAT = int(mavutil.mavlink.MAVLINK_MSG_ID_HEARTBEAT) def load_config(config_path: Path) -> Dict[str, Any]: @@ -93,12 +95,6 @@ def build_cli_commands(config: Dict[str, Any]) -> List[str]: build_serial_command(2, 0, rc_baud), build_serial_command(3, 0, rc_baud), "set mavlink_version = 2", - f"set mavlink_port1_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", - f"set mavlink_port1_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", - f"set mavlink_port1_pos_rate = {int(cli_cfg['pos_rate_hz'])}", - f"set mavlink_port1_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", - f"set mavlink_port1_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", - f"set mavlink_port1_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", "save", ] return commands @@ -128,6 +124,7 @@ def run_workload( command_message_id = int(tests_cfg["stress_command_message_id"]) target_system = int(tests_cfg["rc_target_system"]) target_component = int(tests_cfg["rc_target_component"]) + stream_cfg = config["cli"] listener = mavutil.mavlink_connection( f"tcp:127.0.0.1:{rc_port}", @@ -158,6 +155,62 @@ def run_workload( target_component = int(msg.get_srcComponent()) break + sender.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS), + int(stream_cfg["ext_status_rate_hz"]), + 1 if int(stream_cfg["ext_status_rate_hz"]) > 0 else 0, + ) + sender.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS), + int(stream_cfg["rc_chan_rate_hz"]), + 1 if int(stream_cfg["rc_chan_rate_hz"]) > 0 else 0, + ) + sender.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_POSITION), + int(stream_cfg["pos_rate_hz"]), + 1 if int(stream_cfg["pos_rate_hz"]) > 0 else 0, + ) + sender.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA1), + int(stream_cfg["extra1_rate_hz"]), + 1 if int(stream_cfg["extra1_rate_hz"]) > 0 else 0, + ) + sender.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA2), + int(stream_cfg["extra2_rate_hz"]), + 1 if int(stream_cfg["extra2_rate_hz"]) > 0 else 0, + ) + sender.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA3), + int(stream_cfg["extra3_rate_hz"]), + 1 if int(stream_cfg["extra3_rate_hz"]) > 0 else 0, + ) + sender.mav.command_long_send( + target_system, + target_component, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + float(MAVLINK_MSG_ID_HEARTBEAT), + float(int(1_000_000.0 / heartbeat_expected_hz)), + 0, + 0, + 0, + 0, + 0, + ) + hb_count = 0 hb_est_lost = 0 last_hb_t: float | None = None @@ -360,6 +413,7 @@ def write_report(config: Dict[str, Any], baseline: Dict[str, Any], stress: Dict[ lines.append(f"- EEPROM: `{config['sitl']['eeprom_path']}`") lines.append("- MSP kept on UART1 by design (`serial 0 ... functionMask=1`).") lines.append("- CLI under test sets `receiver_type=SERIAL` and `serialrx_provider=MAVLINK`.") + lines.append("- Stream rates are configured at runtime via MAVLink `REQUEST_DATA_STREAM` and `MAV_CMD_SET_MESSAGE_INTERVAL`.") lines.append( f"- CLI baud config: `rc_baud={config['cli']['rc_baud']}`, " f"`telemetry_baud={config['cli'].get('telemetry_baud', config['cli']['rc_baud'])}`." diff --git a/src/utils/mavlink_tests/mav_multi_benchmark.py b/src/utils/mavlink_tests/mav_multi_benchmark.py index 6483fa5806e..97ce932a7f6 100644 --- a/src/utils/mavlink_tests/mav_multi_benchmark.py +++ b/src/utils/mavlink_tests/mav_multi_benchmark.py @@ -38,6 +38,7 @@ DEFAULT_CONFIG_PATH = Path("mydev/branch/mav_multi/test_config.yaml") CHANNELS = [1500, 1500, 900, 1500] + [900] * 14 MAV_CMD_REQUEST_MESSAGE = int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE) +MAV_CMD_SET_MESSAGE_INTERVAL = int(mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL) MAVLINK_MSG_ID_HEARTBEAT = int(mavutil.mavlink.MAVLINK_MSG_ID_HEARTBEAT) @@ -131,30 +132,6 @@ def build_cli_config_commands(config: Dict[str, Any], mavlink_port_count: int) - "set mavlink_port2_high_latency = OFF", "set mavlink_port3_high_latency = OFF", "set mavlink_port4_high_latency = OFF", - f"set mavlink_port1_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", - f"set mavlink_port1_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", - f"set mavlink_port1_pos_rate = {int(cli_cfg['pos_rate_hz'])}", - f"set mavlink_port1_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", - f"set mavlink_port1_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", - f"set mavlink_port1_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", - f"set mavlink_port2_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", - f"set mavlink_port2_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", - f"set mavlink_port2_pos_rate = {int(cli_cfg['pos_rate_hz'])}", - f"set mavlink_port2_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", - f"set mavlink_port2_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", - f"set mavlink_port2_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", - f"set mavlink_port3_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", - f"set mavlink_port3_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", - f"set mavlink_port3_pos_rate = {int(cli_cfg['pos_rate_hz'])}", - f"set mavlink_port3_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", - f"set mavlink_port3_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", - f"set mavlink_port3_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", - f"set mavlink_port4_ext_status_rate = {int(cli_cfg['ext_status_rate_hz'])}", - f"set mavlink_port4_rc_chan_rate = {int(cli_cfg['rc_chan_rate_hz'])}", - f"set mavlink_port4_pos_rate = {int(cli_cfg['pos_rate_hz'])}", - f"set mavlink_port4_extra1_rate = {int(cli_cfg['extra1_rate_hz'])}", - f"set mavlink_port4_extra2_rate = {int(cli_cfg['extra2_rate_hz'])}", - f"set mavlink_port4_extra3_rate = {int(cli_cfg['extra3_rate_hz'])}", "save", ] return commands @@ -241,6 +218,8 @@ def run_workload( command_message_id = int(tests_cfg["stress_command_message_id"]) rc_target_system = int(tests_cfg["rc_target_system"]) rc_target_component = int(tests_cfg["rc_target_component"]) + heartbeat_expected_hz = float(tests_cfg["heartbeat_expected_hz"]) + stream_cfg = config["cli"] port_ready_timeout_s = float(tests_cfg["port_ready_timeout_s"]) warmup_s = float(tests_cfg.get("warmup_s", 3.0)) warmup_heartbeat_count = int(tests_cfg.get("warmup_heartbeat_count", 3)) @@ -376,6 +355,65 @@ def run_workload( time.sleep(0.001) + for port, conn in listeners.items(): + target_system, target_component = targets[port] + conn.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS), + int(stream_cfg["ext_status_rate_hz"]), + 1 if int(stream_cfg["ext_status_rate_hz"]) > 0 else 0, + ) + conn.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS), + int(stream_cfg["rc_chan_rate_hz"]), + 1 if int(stream_cfg["rc_chan_rate_hz"]) > 0 else 0, + ) + conn.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_POSITION), + int(stream_cfg["pos_rate_hz"]), + 1 if int(stream_cfg["pos_rate_hz"]) > 0 else 0, + ) + conn.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA1), + int(stream_cfg["extra1_rate_hz"]), + 1 if int(stream_cfg["extra1_rate_hz"]) > 0 else 0, + ) + conn.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA2), + int(stream_cfg["extra2_rate_hz"]), + 1 if int(stream_cfg["extra2_rate_hz"]) > 0 else 0, + ) + conn.mav.request_data_stream_send( + target_system, + target_component, + int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA3), + int(stream_cfg["extra3_rate_hz"]), + 1 if int(stream_cfg["extra3_rate_hz"]) > 0 else 0, + ) + conn.mav.command_long_send( + target_system, + target_component, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + float(MAVLINK_MSG_ID_HEARTBEAT), + float(int(1_000_000.0 / heartbeat_expected_hz)), + 0, + 0, + 0, + 0, + 0, + ) + time.sleep(0.1) + for stats in port_stats.values(): stats["heartbeat_count"] = 0 stats["rc_count"] = 0 @@ -617,6 +655,7 @@ def write_testing_report(config: Dict[str, Any], report: Dict[str, Any]) -> None lines.append(f"- EEPROM: `{config['sitl']['eeprom_path']}`") lines.append("- MSP kept on UART1 by design (`serial 0 ... functionMask=1`).") lines.append("- CLI under test sets `receiver_type=SERIAL` and `serialrx_provider=MAVLINK`.") + lines.append("- Stream rates are configured at runtime via MAVLink `REQUEST_DATA_STREAM` and `MAV_CMD_SET_MESSAGE_INTERVAL`.") lines.append("- `RC_CHANNELS` metrics below are MAVLink telemetry stream metrics, not RX input ownership.") lines.append("") lines.append("## MAVSDK Probe") diff --git a/src/utils/mavlink_tests/mavlink_routing_compliance_test.py b/src/utils/mavlink_tests/mavlink_routing_compliance_test.py index e202e649af8..0ea996ec1b5 100644 --- a/src/utils/mavlink_tests/mavlink_routing_compliance_test.py +++ b/src/utils/mavlink_tests/mavlink_routing_compliance_test.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 """ Usage: - conda run -n drone python mydev/branch/mav_multi/mavlink_routing_compliance_test.py --config mydev/branch/mav_multi/routing_test_config.yaml + conda run -n drone python src/utils/mavlink_tests/mavlink_routing_compliance_test.py --config src/utils/mavlink_tests/routing_test_config.yaml """ from __future__ import annotations diff --git a/src/utils/mavlink_tests/routing_test_config.yaml b/src/utils/mavlink_tests/routing_test_config.yaml index e4d17865675..d4677ba4ebf 100644 --- a/src/utils/mavlink_tests/routing_test_config.yaml +++ b/src/utils/mavlink_tests/routing_test_config.yaml @@ -7,24 +7,24 @@ timing: inter_case_pause_s: 0.15 network: - gcs_link: link_gcs + gcs_link: link_radio links: - link_gcs: + link_radio: endpoint: tcp:127.0.0.1:5761 - source_system: 255 - source_component: 190 - link_components_a: + source_system: 1 + source_component: 68 + link_companion: endpoint: tcp:127.0.0.1:5762 - source_system: 254 - source_component: 200 + source_system: 1 + source_component: 191 link_camera: endpoint: tcp:127.0.0.1:5763 - source_system: 253 - source_component: 201 + source_system: 1 + source_component: 100 link_gimbal: endpoint: tcp:127.0.0.1:5764 - source_system: 252 - source_component: 202 + source_system: 1 + source_component: 154 vehicle: fc_system_id: 1 @@ -33,19 +33,19 @@ tests: marker_command_base: 31000 unknown_component_id: 199 unknown_system_id: 42 - report_markdown: mydev/branch/mav_multi/ROUTING_TEST.md + report_markdown: src/utils/mavlink_tests/ROUTING_TEST.md components: - - name: mav1_elrs_receiver - link: link_components_a - system_id: 1 - component_id: 68 - mav_type: 49 - name: mav2_companion_computer - link: link_components_a + link: link_companion system_id: 1 component_id: 191 mav_type: 18 + - name: mav1_elrs_receiver + link: link_radio + system_id: 1 + component_id: 68 + mav_type: 49 - name: mav3_camera link: link_camera system_id: 1 From d2f2866b20e9eb02950a61a975567d2f35afeeeb Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Wed, 4 Mar 2026 16:40:57 -0500 Subject: [PATCH 28/42] . --- docs/Mavlink.md | 39 ++ docs/development/msp/inav_enums.json | 5 +- docs/development/msp/inav_enums_ref.md | 3 +- .../mav_fix2_singleport_benchmark.py | 519 ------------------ src/utils/mavlink_tests/mav_multi_sweep.py | 42 +- 5 files changed, 71 insertions(+), 537 deletions(-) delete mode 100644 src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py diff --git a/docs/Mavlink.md b/docs/Mavlink.md index 98bc3c681ad..1df23459fed 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -20,6 +20,45 @@ INAV has a partial implementation of MAVLink that is intended primarily for simp - `mavlink_port1_min_txbuffer` – minimum remote TX buffer level before sending when `RADIO_STATUS` provides flow control. - `mavlink_port1_radio_type` – scales `RADIO_STATUS` RSSI/SNR for **generic**, **ELRS**, or **SiK** links. +## Multi-port MAVLink + +INAV supports up to 4 concurrent MAVLink telemetry ports (`MAX_MAVLINK_PORTS`), one endpoint per serial port configured with `FUNCTION_TELEMETRY_MAVLINK`. + +### Configuration model + +- Shared across all ports: `mavlink_sysid`, `mavlink_version`, `mavlink_autopilot_type`. +- Per-port: `mavlink_portN_compid`, `mavlink_portN_min_txbuffer`, `mavlink_portN_radio_type`, `mavlink_portN_high_latency`. +- Stream defaults at startup: +- Port 1 uses configured CLI rates (`mavlink_port1_*_rate`). +- Ports 2..4 start with heartbeat only (1 Hz), all other streams disabled. + +### Routing and forwarding behavior + +- INAV learns routes from incoming traffic as `(sysid, compid) -> ingress port`. +- Broadcast messages are forwarded to all other MAVLink ports (except `RADIO_STATUS`, which is not forwarded). +- Targeted messages are forwarded only to ports with a learned route for that target. +- Practical caveat: the first targeted message to a never-seen endpoint may not forward until that endpoint has sent at least one MAVLink frame. + +### Local message handling behavior + +- Local/system broadcasts (`target_system=0` or local system ID with `target_component=0`) are fanned out to all local ports only for: +- `REQUEST_DATA_STREAM` +- `MAV_CMD_SET_MESSAGE_INTERVAL` +- `MAV_CMD_CONTROL_HIGH_LATENCY` +- Other incoming commands/messages are handled on one resolved local port, based on local target matching. + +### High-latency behavior + +- High-latency mode is per-port (`mavlink_portN_high_latency` or `MAV_CMD_CONTROL_HIGH_LATENCY` on that port). +- Requires MAVLink2; MAVLink1 cannot enable it. +- When enabled, normal stream scheduling for that port is replaced by `HIGH_LATENCY2` at 5-second intervals. + +### Usage guidance + +- Assign a unique `mavlink_portN_compid` to each INAV MAVLink port to avoid ambiguous local targeting. +- If a GCS or companion needs telemetry on ports 2..4, explicitly request streams (`REQUEST_DATA_STREAM` or `MAV_CMD_SET_MESSAGE_INTERVAL`) because only heartbeat is enabled by default. +- If you depend on directed forwarding between links, ensure each remote endpoint transmits at least one frame early so route learning is populated. + ## Supported Outgoing Messages Messages are organized into MAVLink datastream groups. Each group sends **one message per trigger** at the configured rate: diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 3844e59cee9..69a74420887 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -2239,7 +2239,7 @@ "MAG_MAX": "MAG_FAKE" }, "mavFrameSupportMask_e": { - "_source": "inav/src/main/telemetry/mavlink.c", + "_source": "inav/src/main/telemetry/mavlink.h", "MAV_FRAME_SUPPORTED_NONE": "0", "MAV_FRAME_SUPPORTED_GLOBAL": "(1 << 0)", "MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT": "(1 << 1)", @@ -2255,7 +2255,8 @@ "_source": "inav/src/main/telemetry/telemetry.h", "MAVLINK_RADIO_GENERIC": "0", "MAVLINK_RADIO_ELRS": "1", - "MAVLINK_RADIO_SIK": "2" + "MAVLINK_RADIO_SIK": "2", + "MAVLINK_RADIO_NONE": "3" }, "measurementSteps_e": { "_source": "inav/src/main/drivers/rangefinder/rangefinder_vl53l0x.c", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 164ebd9bc14..1fd45d97e91 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -3378,7 +3378,7 @@ --- ## `mavFrameSupportMask_e` -> Source: ../../../src/main/telemetry/mavlink.c +> Source: ../../../src/main/telemetry/mavlink.h | Enumerator | Value | Condition | |---|---:|---| @@ -3408,6 +3408,7 @@ | `MAVLINK_RADIO_GENERIC` | 0 | | | `MAVLINK_RADIO_ELRS` | 1 | | | `MAVLINK_RADIO_SIK` | 2 | | +| `MAVLINK_RADIO_NONE` | 3 | | --- ## `measurementSteps_e` diff --git a/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py b/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py deleted file mode 100644 index 33cafc762c1..00000000000 --- a/src/utils/mavlink_tests/mav_fix2_singleport_benchmark.py +++ /dev/null @@ -1,519 +0,0 @@ -#!/usr/bin/env python3 -""" -Usage: - /home/anon/miniconda3/envs/drone/bin/python mydev/branch/mav_multi/mav_fix2_singleport_benchmark.py --config mydev/branch/mav_multi/test_config_mav_fix2.yaml -""" - -from __future__ import annotations - -import argparse -import contextlib -import math -import socket -import struct -import subprocess -import time -from datetime import datetime, timezone -from pathlib import Path -from typing import Any, Dict, List, Tuple - -import yaml -from pymavlink import mavutil -from serial.serialutil import SerialException - -try: - from mspapi2.msp_api import MSPApi - from mspapi2.lib import InavMSP -except ModuleNotFoundError: - repo_root_guess = Path(__file__).resolve().parents[3] - mspapi2_repo = repo_root_guess.parent / "mspapi2" - if mspapi2_repo.exists(): - import sys - - sys.path.insert(0, str(mspapi2_repo)) - from mspapi2.msp_api import MSPApi - from mspapi2.lib import InavMSP - - -CHANNELS = [1500, 1500, 900, 1500] + [900] * 14 -MAV_CMD_REQUEST_MESSAGE = int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE) -MAV_CMD_SET_MESSAGE_INTERVAL = int(mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL) -MAVLINK_MSG_ID_HEARTBEAT = int(mavutil.mavlink.MAVLINK_MSG_ID_HEARTBEAT) - - -def load_config(config_path: Path) -> Dict[str, Any]: - return yaml.safe_load(config_path.read_text(encoding="utf-8")) - - -def wait_for_tcp_port(host: str, port: int, timeout_s: float) -> None: - deadline = time.monotonic() + timeout_s - while time.monotonic() < deadline: - try: - with socket.create_connection((host, port), timeout=1.0): - return - except (ConnectionRefusedError, socket.timeout, OSError): - time.sleep(0.2) - raise TimeoutError(f"TCP port {host}:{port} did not become available within {timeout_s}s") - - -def cli_read_until_prompt(cli_socket: socket.socket) -> str: - data = b"" - while b"\n# " not in data: - chunk = cli_socket.recv(65536) - if not chunk: - raise ConnectionError("CLI socket closed before prompt") - data += chunk - return data.decode("utf-8", errors="replace") - - -def run_cli_commands(host: str, port: int, commands: List[str]) -> None: - with socket.create_connection((host, port), timeout=5.0) as cli_socket: - cli_socket.settimeout(3.0) - cli_socket.sendall(b"#\n") - cli_read_until_prompt(cli_socket) - for command in commands: - cli_socket.sendall(command.encode("utf-8") + b"\n") - if command == "save": - break - cli_read_until_prompt(cli_socket) - time.sleep(1.0) - - -def build_serial_command(index: int, function_mask: int, baud: int) -> str: - return f"serial {index} {function_mask} {baud} {baud} 0 {baud}" - - -def build_cli_commands(config: Dict[str, Any]) -> List[str]: - cli_cfg = config["cli"] - rc_baud = int(cli_cfg["rc_baud"]) - commands = [ - "feature TELEMETRY", - "set receiver_type = SERIAL", - "set serialrx_provider = MAVLINK", - build_serial_command(0, 1, 115200), # UART1 MSP - build_serial_command(1, 256, rc_baud), # UART2 MAVLINK single port (RC-capable) - build_serial_command(2, 0, rc_baud), - build_serial_command(3, 0, rc_baud), - "set mavlink_version = 2", - "save", - ] - return commands - - -def run_workload( - config: Dict[str, Any], - sitl_pid: int, - duration_s: float, - command_rate_hz: float, - rc_tx_hz_override: float | None = None, -) -> Dict[str, Any]: - tests_cfg = config["tests"] - ports_cfg = config["ports"] - rc_port = int(ports_cfg["rc"]) - msp_port = int(ports_cfg["msp"]) - - wait_for_tcp_port("127.0.0.1", rc_port, float(tests_cfg["port_ready_timeout_s"])) - wait_for_tcp_port("127.0.0.1", msp_port, float(tests_cfg["port_ready_timeout_s"])) - msp_enabled = msp_port != rc_port - - heartbeat_expected_hz = float(tests_cfg["heartbeat_expected_hz"]) - rc_expected_hz = float(tests_cfg["rc_expected_hz"]) - rc_tx_hz = float(tests_cfg["rc_tx_hz"]) if rc_tx_hz_override is None else float(rc_tx_hz_override) - msp_poll_hz = float(tests_cfg["msp_poll_hz"]) - msp_request_timeout_s = float(tests_cfg["msp_request_timeout_s"]) - command_message_id = int(tests_cfg["stress_command_message_id"]) - target_system = int(tests_cfg["rc_target_system"]) - target_component = int(tests_cfg["rc_target_component"]) - stream_cfg = config["cli"] - - listener = mavutil.mavlink_connection( - f"tcp:127.0.0.1:{rc_port}", - source_system=170, - source_component=190, - autoreconnect=True, - ) - sender = mavutil.mavlink_connection( - f"tcp:127.0.0.1:{rc_port}", - source_system=239, - source_component=191, - autoreconnect=True, - ) - - # Prime telemetry stream startup from GCS side. - handshake_deadline = time.monotonic() + 3.0 - while time.monotonic() < handshake_deadline: - sender.mav.heartbeat_send( - mavutil.mavlink.MAV_TYPE_GCS, - mavutil.mavlink.MAV_AUTOPILOT_INVALID, - 0, - 0, - 0, - ) - msg = listener.recv_match(type="HEARTBEAT", blocking=True, timeout=0.2) - if msg is not None: - target_system = int(msg.get_srcSystem()) - target_component = int(msg.get_srcComponent()) - break - - sender.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS), - int(stream_cfg["ext_status_rate_hz"]), - 1 if int(stream_cfg["ext_status_rate_hz"]) > 0 else 0, - ) - sender.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS), - int(stream_cfg["rc_chan_rate_hz"]), - 1 if int(stream_cfg["rc_chan_rate_hz"]) > 0 else 0, - ) - sender.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_POSITION), - int(stream_cfg["pos_rate_hz"]), - 1 if int(stream_cfg["pos_rate_hz"]) > 0 else 0, - ) - sender.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA1), - int(stream_cfg["extra1_rate_hz"]), - 1 if int(stream_cfg["extra1_rate_hz"]) > 0 else 0, - ) - sender.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA2), - int(stream_cfg["extra2_rate_hz"]), - 1 if int(stream_cfg["extra2_rate_hz"]) > 0 else 0, - ) - sender.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA3), - int(stream_cfg["extra3_rate_hz"]), - 1 if int(stream_cfg["extra3_rate_hz"]) > 0 else 0, - ) - sender.mav.command_long_send( - target_system, - target_component, - MAV_CMD_SET_MESSAGE_INTERVAL, - 0, - float(MAVLINK_MSG_ID_HEARTBEAT), - float(int(1_000_000.0 / heartbeat_expected_hz)), - 0, - 0, - 0, - 0, - 0, - ) - - hb_count = 0 - hb_est_lost = 0 - last_hb_t: float | None = None - rc_count = 0 - rc_est_lost = 0 - last_rc_t: float | None = None - rc_mismatch = 0 - command_sent = 0 - command_ack_total = 0 - command_ack_ok = 0 - msp_success = 0 - msp_fail = 0 - msp_mismatch = 0 - rc_sent = 0 - fc_cpu_load_samples: List[float] = [] - fc_cycle_time_samples: List[float] = [] - fc_status_fail = 0 - fc_status_last_error = "" - - rc_period_s = 1.0 / rc_tx_hz - next_rc_send_t = time.monotonic() - command_period_s = 0.0 if command_rate_hz <= 0 else (1.0 / command_rate_hz) - next_command_send_t = next_rc_send_t - next_heartbeat_send_t = next_rc_send_t - - prev_sample_t = time.monotonic() - next_resource_sample_t = prev_sample_t + 1.0 - next_msp_poll_t = prev_sample_t + (1.0 / msp_poll_hz) - workload_end_t = prev_sample_t + duration_s - - with (MSPApi(tcp_endpoint=f"127.0.0.1:{msp_port}") if msp_enabled else contextlib.nullcontext()) as msp_api: - if msp_enabled: - msp_api._serial._max_retries = 1 - msp_api._serial._reconnect_delay = 0.05 - while time.monotonic() < workload_end_t: - now = time.monotonic() - - if now >= next_heartbeat_send_t: - listener.mav.heartbeat_send( - mavutil.mavlink.MAV_TYPE_GCS, - mavutil.mavlink.MAV_AUTOPILOT_INVALID, - 0, - 0, - 0, - ) - sender.mav.heartbeat_send( - mavutil.mavlink.MAV_TYPE_GCS, - mavutil.mavlink.MAV_AUTOPILOT_INVALID, - 0, - 0, - 0, - ) - next_heartbeat_send_t += 1.0 - if next_heartbeat_send_t < now: - next_heartbeat_send_t = now + 1.0 - - if now >= next_rc_send_t: - sender.mav.rc_channels_override_send(target_system, target_component, *CHANNELS[:8]) - rc_sent += 1 - next_rc_send_t += rc_period_s - if next_rc_send_t < now: - next_rc_send_t = now + rc_period_s - - if command_period_s > 0 and now >= next_command_send_t: - sender.mav.command_long_send( - target_system, - target_component, - MAV_CMD_REQUEST_MESSAGE, - 0, - float(command_message_id), - 0, - 0, - 0, - 0, - 0, - 0, - ) - command_sent += 1 - next_command_send_t += command_period_s - if next_command_send_t < now: - next_command_send_t = now + command_period_s - - while True: - try: - msg = listener.recv_match(blocking=False) - except (ConnectionRefusedError, OSError): - wait_for_tcp_port("127.0.0.1", rc_port, float(tests_cfg["port_ready_timeout_s"])) - break - if msg is None: - break - msg_type = msg.get_type() - if msg_type == "HEARTBEAT": - target_system = int(msg.get_srcSystem()) - target_component = int(msg.get_srcComponent()) - hb_count += 1 - if last_hb_t is not None: - dt = now - last_hb_t - hb_est_lost += max(int(math.floor(dt * heartbeat_expected_hz) - 1), 0) - last_hb_t = now - elif msg_type == "RC_CHANNELS": - rc_count += 1 - if last_rc_t is not None: - dt = now - last_rc_t - rc_est_lost += max(int(math.floor(dt * rc_expected_hz) - 1), 0) - last_rc_t = now - if ( - abs(int(msg.chan1_raw) - CHANNELS[0]) > 20 - or abs(int(msg.chan2_raw) - CHANNELS[1]) > 20 - or abs(int(msg.chan3_raw) - CHANNELS[2]) > 20 - or abs(int(msg.chan4_raw) - CHANNELS[3]) > 20 - ): - rc_mismatch += 1 - elif msg_type == "COMMAND_ACK": - if int(msg.command) == MAV_CMD_REQUEST_MESSAGE: - command_ack_total += 1 - if int(msg.result) == int(mavutil.mavlink.MAV_RESULT_ACCEPTED): - command_ack_ok += 1 - - if msp_enabled and now >= next_msp_poll_t: - try: - _, payload = msp_api._request_raw(InavMSP.MSP_RC, timeout=msp_request_timeout_s) - if len(payload) % 2: - raise ValueError("MSP RC payload length must be even") - channel_count = len(payload) // 2 - channels = list(struct.unpack(f"<{channel_count}H", payload)) - msp_success += 1 - if ( - abs(channels[0] - CHANNELS[0]) > 40 - or abs(channels[1] - CHANNELS[1]) > 40 - or abs(channels[2] - CHANNELS[2]) > 40 - or abs(channels[3] - CHANNELS[3]) > 40 - ): - msp_mismatch += 1 - except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError): - msp_fail += 1 - next_msp_poll_t += 1.0 / msp_poll_hz - - if now >= next_resource_sample_t: - if msp_enabled: - try: - status = msp_api.get_inav_status() - fc_cpu_load_samples.append(float(status["cpuLoad"])) - fc_cycle_time_samples.append(float(status["cycleTime"])) - except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError, KeyError, ValueError) as exc: - fc_status_fail += 1 - fc_status_last_error = str(exc) - prev_sample_t = now - next_resource_sample_t += 1.0 - - time.sleep(0.001) - - if msp_enabled and not fc_cpu_load_samples: - raise RuntimeError(f"No MSP2_INAV_STATUS samples collected; last_error={fc_status_last_error} fail_count={fc_status_fail}") - - listener.close() - sender.close() - - hb_total = hb_count + hb_est_lost - rc_total = rc_count + rc_est_lost - return { - "duration_s": duration_s, - "heartbeat_count": hb_count, - "heartbeat_est_lost": hb_est_lost, - "heartbeat_loss_rate": (hb_est_lost / hb_total) if hb_total > 0 else 0.0, - "rc_count": rc_count, - "rc_est_lost": rc_est_lost, - "rc_loss_rate": (rc_est_lost / rc_total) if rc_total > 0 else 0.0, - "rc_mismatch": rc_mismatch, - "rc_mismatch_rate": (rc_mismatch / max(rc_count, 1)), - "msp_success": msp_success, - "msp_fail": msp_fail, - "msp_failure_rate": (msp_fail / max(msp_success + msp_fail, 1)), - "msp_mismatch": msp_mismatch, - "msp_mismatch_rate": (msp_mismatch / max(msp_success, 1)), - "command_sent": command_sent, - "command_ack_total": command_ack_total, - "command_ack_ok": command_ack_ok, - "command_ack_failure_rate": ((command_sent - command_ack_total) / command_sent) if command_sent > 0 else 0.0, - "command_ack_non_ok_rate": ((command_ack_total - command_ack_ok) / command_ack_total) if command_ack_total > 0 else 0.0, - "rc_sent": rc_sent, - "rc_sent_hz": (rc_sent / max(duration_s, 1e-6)), - "fc_cpu_load_avg_pct": (sum(fc_cpu_load_samples) / len(fc_cpu_load_samples)) if fc_cpu_load_samples else 0.0, - "fc_cpu_load_max_pct": max(fc_cpu_load_samples) if fc_cpu_load_samples else 0.0, - "fc_cycle_time_avg_us": (sum(fc_cycle_time_samples) / len(fc_cycle_time_samples)) if fc_cycle_time_samples else 0.0, - "fc_cycle_time_max_us": max(fc_cycle_time_samples) if fc_cycle_time_samples else 0.0, - "fc_status_samples": len(fc_cpu_load_samples), - } - - -def write_report(config: Dict[str, Any], baseline: Dict[str, Any], stress: Dict[str, Any]) -> None: - out_path = Path(config["output"]["testing_md"]) - branch_name = str(config.get("meta", {}).get("branch_name", "unknown")) - timestamp = datetime.now(timezone.utc).isoformat() - lines: List[str] = [] - lines.append(f"# MAVLink Single-Port Baseline (`{branch_name}`)") - lines.append("") - lines.append(f"- Generated: `{timestamp}`") - lines.append(f"- Branch: `{branch_name}`") - lines.append(f"- SITL binary: `{config['sitl']['binary']}`") - lines.append(f"- EEPROM: `{config['sitl']['eeprom_path']}`") - lines.append("- MSP kept on UART1 by design (`serial 0 ... functionMask=1`).") - lines.append("- CLI under test sets `receiver_type=SERIAL` and `serialrx_provider=MAVLINK`.") - lines.append("- Stream rates are configured at runtime via MAVLink `REQUEST_DATA_STREAM` and `MAV_CMD_SET_MESSAGE_INTERVAL`.") - lines.append( - f"- CLI baud config: `rc_baud={config['cli']['rc_baud']}`, " - f"`telemetry_baud={config['cli'].get('telemetry_baud', config['cli']['rc_baud'])}`." - ) - lines.append("") - lines.append("## Baseline: Single MAVLink RC+Telemetry Port") - lines.append("") - lines.append( - f"- Duration: `{baseline['duration_s']}s` | FC cpuLoad avg/max: `{baseline['fc_cpu_load_avg_pct']:.2f}% / {baseline['fc_cpu_load_max_pct']:.2f}%` | " - f"FC cycleTime avg/max: `{baseline['fc_cycle_time_avg_us']:.1f} / {baseline['fc_cycle_time_max_us']:.1f}` us | " - f"Status samples: `{baseline['fc_status_samples']}`" - ) - lines.append( - f"- MSP success/fail: `{baseline['msp_success']}/{baseline['msp_fail']}` | MSP failure rate: `{baseline['msp_failure_rate']:.4f}` | " - f"MSP RC mismatch rate: `{baseline['msp_mismatch_rate']:.4f}`" - ) - lines.append(f"- RC override send rate: `{baseline['rc_sent_hz']:.2f} Hz` (`{baseline['rc_sent']}` packets)") - lines.append( - f"- HB count/loss: `{baseline['heartbeat_count']}/{baseline['heartbeat_est_lost']}` | HB loss rate: `{baseline['heartbeat_loss_rate']:.4f}`" - ) - lines.append( - f"- RC count/loss: `{baseline['rc_count']}/{baseline['rc_est_lost']}` | RC loss rate: `{baseline['rc_loss_rate']:.4f}` | " - f"RC mismatch rate: `{baseline['rc_mismatch_rate']:.4f}`" - ) - lines.append("") - lines.append("## Stress: Single Port Overload") - lines.append("") - lines.append( - f"- Command load: `{config['tests']['stress_rate_hz']} req/s` for `{stress['duration_s']}s` | " - f"FC cpuLoad avg/max: `{stress['fc_cpu_load_avg_pct']:.2f}% / {stress['fc_cpu_load_max_pct']:.2f}%` | " - f"FC cycleTime avg/max: `{stress['fc_cycle_time_avg_us']:.1f} / {stress['fc_cycle_time_max_us']:.1f}` us | " - f"Status samples: `{stress['fc_status_samples']}`" - ) - lines.append( - f"- MSP success/fail: `{stress['msp_success']}/{stress['msp_fail']}` | MSP failure rate: `{stress['msp_failure_rate']:.4f}` | " - f"MSP RC mismatch rate: `{stress['msp_mismatch_rate']:.4f}`" - ) - lines.append(f"- RC override send rate: `{stress['rc_sent_hz']:.2f} Hz` (`{stress['rc_sent']}` packets)") - lines.append( - f"- HB count/loss: `{stress['heartbeat_count']}/{stress['heartbeat_est_lost']}` | HB loss rate: `{stress['heartbeat_loss_rate']:.4f}`" - ) - lines.append( - f"- RC count/loss: `{stress['rc_count']}/{stress['rc_est_lost']}` | RC loss rate: `{stress['rc_loss_rate']:.4f}` | " - f"RC mismatch rate: `{stress['rc_mismatch_rate']:.4f}`" - ) - lines.append( - f"- Command sent/ack: `{stress['command_sent']}/{stress['command_ack_total']}` | " - f"ack failure rate: `{stress['command_ack_failure_rate']:.4f}` | non-ok ack rate: `{stress['command_ack_non_ok_rate']:.4f}`" - ) - out_path.parent.mkdir(parents=True, exist_ok=True) - out_path.write_text("\n".join(lines) + "\n", encoding="utf-8") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Run single-port MAVLink benchmark and write a baseline markdown report.") - parser.add_argument("--config", required=True, help="YAML configuration path") - args = parser.parse_args() - - config = load_config(Path(args.config)) - sitl_bin = Path(config["sitl"]["binary"]) - sitl_workdir = Path(config["sitl"]["workdir"]) - sitl_eeprom = str(config["sitl"]["eeprom_path"]) - sitl_log = Path(config["sitl"]["runtime_log"]) - sitl_log.parent.mkdir(parents=True, exist_ok=True) - - log_handle = sitl_log.open("w", encoding="utf-8") - sitl_proc = subprocess.Popen( - [str(sitl_bin), f"--path={sitl_eeprom}"], - cwd=str(sitl_workdir), - stdout=log_handle, - stderr=subprocess.STDOUT, - text=True, - ) - - try: - wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(config["tests"]["port_ready_timeout_s"])) - run_cli_commands("127.0.0.1", int(config["ports"]["msp"]), build_cli_commands(config)) - wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(config["tests"]["port_ready_timeout_s"])) - wait_for_tcp_port("127.0.0.1", int(config["ports"]["rc"]), float(config["tests"]["port_ready_timeout_s"])) - - baseline = run_workload( - config=config, - sitl_pid=sitl_proc.pid, - duration_s=float(config["tests"]["baseline_duration_s"]), - command_rate_hz=0.0, - ) - stress = run_workload( - config=config, - sitl_pid=sitl_proc.pid, - duration_s=float(config["tests"]["stress_duration_s"]), - command_rate_hz=float(config["tests"]["stress_rate_hz"]), - rc_tx_hz_override=float(config["tests"]["rc_tx_hz_max"]), - ) - write_report(config, baseline, stress) - print(f"report_written={config['output']['testing_md']}", flush=True) - finally: - sitl_proc.terminate() - try: - sitl_proc.wait(timeout=5.0) - except subprocess.TimeoutExpired: - sitl_proc.kill() - sitl_proc.wait(timeout=5.0) - log_handle.close() diff --git a/src/utils/mavlink_tests/mav_multi_sweep.py b/src/utils/mavlink_tests/mav_multi_sweep.py index 0915e1a1485..bb7a38b741e 100644 --- a/src/utils/mavlink_tests/mav_multi_sweep.py +++ b/src/utils/mavlink_tests/mav_multi_sweep.py @@ -28,7 +28,7 @@ spec.loader.exec_module(benchmark) -parser = argparse.ArgumentParser(description="Run MAVLink load sweep table for 1..4 ports and [50,100,200,max] req/s") +parser = argparse.ArgumentParser(description="Run MAVLink total-load sweep table for 1..4 ports and [50,100,200,max] req/s total") parser.add_argument("--config", default=str(DEFAULT_CONFIG_PATH), help="YAML configuration path") args = parser.parse_args() @@ -56,9 +56,9 @@ rate_cache: Dict[float, Dict[int, Dict[str, Any]]] = {} -for label, rate_hz in rate_labels: - if rate_hz not in rate_cache: - rate_cache[rate_hz] = {} +for label, total_rate_hz in rate_labels: + if total_rate_hz not in rate_cache: + rate_cache[total_rate_hz] = {} for port_count in range(1, 5): scenario_log = sitl_log.with_name(f"{sitl_log.stem}_{label}_{port_count}{sitl_log.suffix}") log_handle = scenario_log.open("w", encoding="utf-8") @@ -73,8 +73,12 @@ benchmark.wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(config["tests"]["port_ready_timeout_s"])) benchmark.apply_config_and_wait(config, 4) active_ports = [rc_port] + telemetry_ports[: max(0, port_count - 1)] - load_rates_hz = {port: rate_hz for port in active_ports} - print(f"sweep_run_start label={label} rate={rate_hz} ports={port_count}", flush=True) + per_port_rate_hz = total_rate_hz / float(port_count) + load_rates_hz = {port: per_port_rate_hz for port in active_ports} + print( + f"sweep_run_start label={label} total_rate={total_rate_hz} per_port_rate={per_port_rate_hz} ports={port_count}", + flush=True, + ) result = benchmark.run_workload( config=config, sitl_pid=sitl_proc.pid, @@ -83,8 +87,11 @@ load_rates_hz=load_rates_hz, duration_s=sweep_duration_s, ) - rate_cache[rate_hz][port_count] = result - print(f"sweep_run_done label={label} rate={rate_hz} ports={port_count}", flush=True) + rate_cache[total_rate_hz][port_count] = result + print( + f"sweep_run_done label={label} total_rate={total_rate_hz} per_port_rate={per_port_rate_hz} ports={port_count}", + flush=True, + ) finally: sitl_proc.terminate() try: @@ -94,7 +101,7 @@ sitl_proc.wait(timeout=5.0) log_handle.close() scenario_log.unlink(missing_ok=True) - results[label] = rate_cache[rate_hz] + results[label] = rate_cache[total_rate_hz] output_path = Path(config["output"]["testing_md"]) lines: List[str] = [ @@ -106,14 +113,15 @@ "- UART1 MSP only.", "- UART2 is MAVLink RC (460800), RC override target is 100 Hz.", "- Additional MAVLink telemetry ports are 115200 baud.", + "- Total command rate is held constant per load label and split evenly across active MAVLink ports.", "", "## Comparison Table", "", - "| Load Label | Msg/s per active port | Active MAVLink ports | FC cpuLoad avg/max | FC cycleTime avg/max (us) | MAV seq loss max | Cmd failed total | Cmd failed % | MSP fail % |", - "| --- | ---: | ---: | --- | --- | ---: | ---: | ---: | ---: |", + "| Load Label | Total Msg/s | Active MAVLink ports | Msg/s per active port | FC cpuLoad avg/max | FC cycleTime avg/max (us) | MAV seq loss max | Cmd failed total | Cmd failed % | MSP fail % |", + "| --- | ---: | ---: | ---: | --- | --- | ---: | ---: | ---: | ---: |", ] -for label, rate_hz in rate_labels: +for label, total_rate_hz in rate_labels: rate_results = results[label] for port_count in range(1, 5): result = rate_results[port_count] @@ -122,8 +130,9 @@ cmd_sent_total = sum(int(item["command_sent"]) for item in per_port) cmd_failed_total = sum(int(item["command_failed_total"]) for item in per_port) cmd_failed_pct = (cmd_failed_total / max(cmd_sent_total, 1)) * 100.0 + per_port_rate_hz = total_rate_hz / float(port_count) lines.append( - f"| {label} | {rate_hz:.1f} | {port_count} | " + f"| {label} | {total_rate_hz:.1f} | {port_count} | {per_port_rate_hz:.2f} | " f"{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}% | " f"{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f} | " f"{mav_seq_loss_max_pct:.2f}% | {cmd_failed_total} | {cmd_failed_pct:.2f}% | {(result['msp_failure_rate'] * 100.0):.2f}% |" @@ -132,13 +141,16 @@ lines.append("") lines.append("## Raw Scenario Details") lines.append("") -for label, rate_hz in rate_labels: - lines.append(f"### {label} ({rate_hz:.1f} msg/s per active port)") +for label, total_rate_hz in rate_labels: + lines.append(f"### {label} ({total_rate_hz:.1f} total msg/s)") lines.append("") for port_count in range(1, 5): result = results[label][port_count] + per_port_rate_hz = total_rate_hz / float(port_count) lines.append(f"#### {port_count} active MAVLink port(s)") lines.append("") + lines.append(f"- Total command rate: `{total_rate_hz:.1f} msg/s`") + lines.append(f"- Per-port command rate: `{per_port_rate_hz:.2f} msg/s`") lines.append( f"- FC cpuLoad avg/max: `{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}%` | " f"FC cycleTime avg/max: `{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f}` us | " From 13cb589aef26ac68791bd75b771930a20b858c77 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Thu, 5 Mar 2026 14:15:57 -0500 Subject: [PATCH 29/42] mission upload drop guard, split telemetry messages, port handling fixes --- src/main/telemetry/mavlink.c | 217 ++++++++++++++---- src/main/telemetry/mavlink.h | 4 +- .../mavlink_tests/mav_multi_benchmark.py | 61 ++++- 3 files changed, 229 insertions(+), 53 deletions(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 713c0967f0f..508eaea4062 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -355,6 +355,8 @@ static void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) state->lastMavlinkMessageUs = 0; state->lastHighLatencyMessageUs = 0; state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; + state->txSeq = 0; + state->txDroppedFrames = 0; memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); } @@ -419,6 +421,8 @@ static void configureMAVLinkTelemetryPort(uint8_t portIndex) state->lastMavlinkMessageUs = 0; state->lastHighLatencyMessageUs = 0; state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; + state->txSeq = 0; + state->txDroppedFrames = 0; memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); } @@ -452,8 +456,10 @@ void checkMAVLinkTelemetryState(void) static void mavlinkSendMessage(void) { - uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; - int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg); + const mavlink_msg_entry_t *msgEntry = mavlink_get_msg_entry(mavSendMsg.msgid); + if (!msgEntry) { + return; + } for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { if ((mavSendMask & MAVLINK_PORT_MASK(portIndex)) == 0) { @@ -465,9 +471,39 @@ static void mavlinkSendMessage(void) continue; } - for (int i = 0; i < msgLength; i++) { - serialWrite(state->port, mavBuffer[i]); + mavlink_status_t txStatus = { 0 }; + txStatus.current_tx_seq = state->txSeq; + if (mavlinkGetProtocolVersion() == 1) { + txStatus.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } + + mavlink_message_t txMsg = mavSendMsg; + mavlink_finalize_message_buffer( + &txMsg, + txMsg.sysid, + txMsg.compid, + &txStatus, + msgEntry->min_msg_len, + txMsg.len, + msgEntry->crc_extra + ); + state->txSeq = txStatus.current_tx_seq; + + uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; + const int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &txMsg); + if (msgLength <= 0) { + continue; + } + + // Drop the frame on this port if there is no room; do not block telemetry task. + if (serialTxBytesFree(state->port) < (uint32_t)msgLength) { + state->txDroppedFrames++; + continue; } + + serialBeginWrite(state->port); + serialWriteBuf(state->port, mavBuffer, msgLength); + serialEndWrite(state->port); } } @@ -1181,6 +1217,8 @@ void mavlinkSendVfrHud(void) mavlinkSendMessage(); } +static uint8_t mavlinkGetAutopilotEnum(void); + void mavlinkSendHeartbeat(void) { uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; @@ -1230,18 +1268,11 @@ void mavlinkSendHeartbeat(void) mavSystemState = MAV_STATE_STANDBY; } - uint8_t mavType; - if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { - mavType = MAV_AUTOPILOT_ARDUPILOTMEGA; - } else { - mavType = MAV_AUTOPILOT_GENERIC; - } - mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg, // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) mavSystemType, // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - mavType, + mavlinkGetAutopilotEnum(), // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h mavModes, // custom_mode A bitfield for use for autopilot-specific flags. @@ -1252,7 +1283,7 @@ void mavlinkSendHeartbeat(void) mavlinkSendMessage(); } -void mavlinkSendBatteryTemperatureStatusText(void) +static void mavlinkSendBatteryStatus(void) { uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; @@ -1308,8 +1339,10 @@ void mavlinkSendBatteryTemperatureStatusText(void) 0); mavlinkSendMessage(); +} - +static void mavlinkSendScaledPressure(void) +{ int16_t temperature; sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); mavlink_msg_scaled_pressure_pack(mavSystemId, mavComponentId, &mavSendMsg, @@ -1320,7 +1353,10 @@ void mavlinkSendBatteryTemperatureStatusText(void) 0); mavlinkSendMessage(); +} +static bool mavlinkSendStatusText(void) +{ // FIXME - Status text is limited to boards with USE_OSD #ifdef USE_OSD char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "}; @@ -1340,10 +1376,17 @@ void mavlinkSendBatteryTemperatureStatusText(void) 0); mavlinkSendMessage(); + return true; } #endif + return false; +} - +void mavlinkSendBatteryTemperatureStatusText(void) +{ + mavlinkSendBatteryStatus(); + mavlinkSendScaledPressure(); + mavlinkSendStatusText(); } static uint8_t mavlinkGetAutopilotEnum(void) @@ -1577,6 +1620,8 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs) } +static void mavlinkResetIncomingMissionTransaction(void); + static bool handleIncoming_MISSION_CLEAR_ALL(void) { mavlink_mission_clear_all_t msg; @@ -1585,6 +1630,7 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void) // Check if this message is for us if (msg.target_system == mavSystemId) { resetWaypointList(); + mavlinkResetIncomingMissionTransaction(); mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true; @@ -1594,11 +1640,66 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void) } // Static state for MISSION UPLOAD transaction (starting with MISSION_COUNT) +#define MAVLINK_MISSION_UPLOAD_TIMEOUT_MS 10000 static int incomingMissionWpCount = 0; static int incomingMissionWpSequence = 0; +static uint8_t incomingMissionSourceSystem = 0; +static uint8_t incomingMissionSourceComponent = 0; +static timeMs_t incomingMissionLastActivityMs = 0; + +static void mavlinkResetIncomingMissionTransaction(void) +{ + incomingMissionWpCount = 0; + incomingMissionWpSequence = 0; + incomingMissionSourceSystem = 0; + incomingMissionSourceComponent = 0; + incomingMissionLastActivityMs = 0; +} + +static void mavlinkStartIncomingMissionTransaction(int missionCount) +{ + incomingMissionWpCount = missionCount; + incomingMissionWpSequence = 0; + incomingMissionSourceSystem = mavRecvMsg.sysid; + incomingMissionSourceComponent = mavRecvMsg.compid; + incomingMissionLastActivityMs = millis(); +} + +static void mavlinkTouchIncomingMissionTransaction(void) +{ + incomingMissionLastActivityMs = millis(); +} + +static bool mavlinkIsIncomingMissionTransactionActive(void) +{ + if (incomingMissionWpCount <= 0) { + return false; + } + + if ((timeMs_t)(millis() - incomingMissionLastActivityMs) > MAVLINK_MISSION_UPLOAD_TIMEOUT_MS) { + mavlinkResetIncomingMissionTransaction(); + return false; + } + + return true; +} + +static bool mavlinkIsIncomingMissionTransactionOwner(void) +{ + return mavRecvMsg.sysid == incomingMissionSourceSystem && + mavRecvMsg.compid == incomingMissionSourceComponent; +} 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 (!mavlinkIsIncomingMissionTransactionActive() || !mavlinkIsIncomingMissionTransactionOwner()) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + mavlinkTouchIncomingMissionTransaction(); + if (autocontinue == 0) { mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); @@ -1773,6 +1874,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); } + mavlinkResetIncomingMissionTransaction(); } else { if (useIntMessages) { @@ -1786,6 +1888,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u else { // If we get a duplicate of the last accepted item, re-request the next one instead of aborting. if (seq + 1 == incomingMissionWpSequence) { + mavlinkTouchIncomingMissionTransaction(); if (useIntMessages) { mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); } else { @@ -1808,14 +1911,20 @@ static bool handleIncoming_MISSION_COUNT(void) // Check if this message is for us if (msg.target_system == mavSystemId) { + mavlinkResetIncomingMissionTransaction(); 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 == 0) { + resetWaypointList(); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, 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; + mavlinkStartIncomingMissionTransaction(msg.count); mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; @@ -2256,10 +2365,16 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent #endif break; case MAVLINK_MSG_ID_BATTERY_STATUS: + mavlinkSendBatteryStatus(); + sent = true; + break; case MAVLINK_MSG_ID_SCALED_PRESSURE: - mavlinkSendBatteryTemperatureStatusText(); + mavlinkSendScaledPressure(); sent = true; break; + case MAVLINK_MSG_ID_STATUSTEXT: + sent = mavlinkSendStatusText(); + break; case MAVLINK_MSG_ID_HOME_POSITION: #ifdef USE_GPS if (STATE(GPS_FIX_HOME)) { @@ -2651,20 +2766,48 @@ static void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetS } } -static bool mavlinkRouteMatchesTargetOnPort(uint8_t portIndex, int16_t targetSystem, int16_t targetComponent) +static uint8_t mavlinkComputeForwardMask(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent) { + uint8_t mask = 0; + + if (targetSystem <= 0) { + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if (portIndex == ingressPortIndex) { + continue; + } + + const mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + mask |= MAVLINK_PORT_MASK(portIndex); + } + return mask; + } + for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { const mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; - if (route->ingressPortIndex != portIndex || route->sysid != targetSystem) { + + if (route->sysid != targetSystem) { + continue; + } + if (targetComponent > 0 && route->compid != targetComponent) { + continue; + } + if (route->ingressPortIndex == ingressPortIndex || route->ingressPortIndex >= mavPortCount) { continue; } - if (targetComponent <= 0 || route->compid == targetComponent) { - return true; + const mavlinkPortRuntime_t *state = &mavPortStates[route->ingressPortIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; } + + mask |= MAVLINK_PORT_MASK(route->ingressPortIndex); } - return false; + return mask; } static void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent) @@ -2675,31 +2818,25 @@ static void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; const int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavRecvMsg); + if (msgLength <= 0) { + return; + } + const uint8_t forwardMask = mavlinkComputeForwardMask(ingressPortIndex, targetSystem, targetComponent); for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - if (portIndex == ingressPortIndex) { + if ((forwardMask & MAVLINK_PORT_MASK(portIndex)) == 0) { continue; } - const mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; - if (!state->telemetryEnabled || !state->port) { - continue; - } - - bool shouldForward = false; - if (targetSystem <= 0) { - shouldForward = true; - } else if (mavlinkRouteMatchesTargetOnPort(portIndex, targetSystem, targetComponent)) { - shouldForward = true; - } - - if (!shouldForward) { + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (serialTxBytesFree(state->port) < (uint32_t)msgLength) { + state->txDroppedFrames++; continue; } - for (int i = 0; i < msgLength; i++) { - serialWrite(state->port, mavBuffer[i]); - } + serialBeginWrite(state->port); + serialWriteBuf(state->port, mavBuffer, msgLength); + serialEndWrite(state->port); } } diff --git a/src/main/telemetry/mavlink.h b/src/main/telemetry/mavlink.h index d774e6cb07b..60b77237d0a 100755 --- a/src/main/telemetry/mavlink.h +++ b/src/main/telemetry/mavlink.h @@ -57,7 +57,7 @@ typedef enum { * MAVLink requires angles to be in the range -Pi..Pi. * This converts angles from a range of 0..Pi to -Pi..Pi */ -#define RADIANS_TO_MAVLINK_RANGE(angle) (angle > M_PIf) ? angle - (2 * M_PIf) : angle +#define RADIANS_TO_MAVLINK_RANGE(angle) (((angle) > M_PIf) ? ((angle) - (2 * M_PIf)) : (angle)) /** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ typedef enum APM_PLANE_MODE @@ -142,6 +142,8 @@ typedef struct mavlinkPortRuntime_s { uint8_t mavRates[MAVLINK_STREAM_COUNT]; uint8_t mavRatesConfigured[MAVLINK_STREAM_COUNT]; uint8_t mavTicks[MAVLINK_STREAM_COUNT]; + uint8_t txSeq; + uint32_t txDroppedFrames; mavlink_message_t mavRecvMsg; mavlink_status_t mavRecvStatus; } mavlinkPortRuntime_t; diff --git a/src/utils/mavlink_tests/mav_multi_benchmark.py b/src/utils/mavlink_tests/mav_multi_benchmark.py index 97ce932a7f6..bb74da85779 100644 --- a/src/utils/mavlink_tests/mav_multi_benchmark.py +++ b/src/utils/mavlink_tests/mav_multi_benchmark.py @@ -40,6 +40,8 @@ MAV_CMD_REQUEST_MESSAGE = int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE) MAV_CMD_SET_MESSAGE_INTERVAL = int(mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL) MAVLINK_MSG_ID_HEARTBEAT = int(mavutil.mavlink.MAVLINK_MSG_ID_HEARTBEAT) +MAV_AUTOPILOT_INVALID = int(mavutil.mavlink.MAV_AUTOPILOT_INVALID) +MAV_TYPE_GCS = int(mavutil.mavlink.MAV_TYPE_GCS) def load_config(config_path: Path) -> Dict[str, Any]: @@ -61,6 +63,25 @@ def wait_for_tcp_port(host: str, port: int, timeout_s: float) -> None: raise TimeoutError(f"TCP port {host}:{port} did not become available within {timeout_s}s") +def wait_for_tcp_port_cycle(host: str, port: int, timeout_s: float) -> None: + deadline = time.monotonic() + timeout_s + saw_closed = False + while time.monotonic() < deadline: + try: + with socket.create_connection((host, port), timeout=1.0): + if saw_closed: + return + except (ConnectionRefusedError, socket.timeout, OSError): + saw_closed = True + time.sleep(0.2) + + if saw_closed: + raise TimeoutError(f"TCP port {host}:{port} did not return after reboot within {timeout_s}s") + + # If no close window was observed, treat it as already rebooted and just ensure the port is reachable. + wait_for_tcp_port(host, port, timeout_s) + + def cli_read_until_prompt(cli_socket: socket.socket) -> str: data = b"" while b"\n# " not in data: @@ -85,7 +106,7 @@ def wait_for_cli_ready(host: str, port: int, timeout_s: float) -> None: raise TimeoutError(f"CLI prompt did not become available on {host}:{port} within {timeout_s}s") -def run_cli_commands(host: str, port: int, commands: List[str], reboot_timeout_s: float) -> None: +def run_cli_commands(host: str, port: int, commands: List[str], reboot_timeout_s: float) -> bool: sent_save = False with socket.create_connection((host, port), timeout=5.0) as cli_socket: cli_socket.settimeout(3.0) @@ -100,6 +121,7 @@ def run_cli_commands(host: str, port: int, commands: List[str], reboot_timeout_s if sent_save: # Save triggers reboot; do not send '#' again because that re-enters CLI mode. time.sleep(min(reboot_timeout_s, 1.0)) + return sent_save def build_serial_command(index: int, function_mask: int, baud: int) -> str: @@ -140,12 +162,14 @@ def build_cli_config_commands(config: Dict[str, Any], mavlink_port_count: int) - def apply_config_and_wait(config: Dict[str, Any], mavlink_port_count: int) -> None: ports = config["ports"] commands = build_cli_config_commands(config, mavlink_port_count) - run_cli_commands( + sent_save = run_cli_commands( "127.0.0.1", int(ports["msp"]), commands, reboot_timeout_s=float(config["tests"]["save_reboot_timeout_s"]), ) + if sent_save: + wait_for_tcp_port_cycle("127.0.0.1", int(ports["msp"]), float(config["tests"]["save_reboot_timeout_s"])) wait_for_tcp_port("127.0.0.1", int(ports["msp"]), float(config["tests"]["port_ready_timeout_s"])) wait_for_tcp_port("127.0.0.1", int(ports["rc"]), float(config["tests"]["port_ready_timeout_s"])) if mavlink_port_count >= 2: @@ -196,6 +220,10 @@ def open_mavlink_tcp_connection(port: int, source_system: int, source_component: time.sleep(0.2) +def is_fc_heartbeat(msg: Any) -> bool: + return int(msg.autopilot) != MAV_AUTOPILOT_INVALID and int(msg.type) != MAV_TYPE_GCS + + def run_workload( config: Dict[str, Any], sitl_pid: int, @@ -263,6 +291,7 @@ def run_workload( "mav_msg_count": 0, "mav_seq_lost": 0, "last_seq_by_source": {}, + "seq_track_source": None, } for idx, port in enumerate(sorted(load_rates_hz.keys())): @@ -341,8 +370,11 @@ def run_workload( break msg_type = msg.get_type() if msg_type == "HEARTBEAT": - targets[port] = (int(msg.get_srcSystem()), int(msg.get_srcComponent())) - warmup_hb_seen[port] += 1 + src_system = int(msg.get_srcSystem()) + src_component = int(msg.get_srcComponent()) + if is_fc_heartbeat(msg): + targets[port] = (src_system, src_component) + warmup_hb_seen[port] += 1 if telemetry_ports and all(count >= warmup_heartbeat_count for count in warmup_hb_seen.values()) and (now - warmup_start_t) >= warmup_s: break @@ -357,6 +389,7 @@ def run_workload( for port, conn in listeners.items(): target_system, target_component = targets[port] + port_stats[port]["seq_track_source"] = (target_system, target_component) conn.mav.request_data_stream_send( target_system, target_component, @@ -510,16 +543,20 @@ def run_workload( seq = int(header.seq) if seq is not None: src_key = (int(msg.get_srcSystem()), int(msg.get_srcComponent())) - last_seq = stats["last_seq_by_source"].get(src_key) - if last_seq is not None: - seq_delta = (seq - int(last_seq)) & 0xFF - if seq_delta > 0: - stats["mav_seq_lost"] += max(seq_delta - 1, 0) - stats["last_seq_by_source"][src_key] = seq - stats["mav_msg_count"] += 1 + if src_key == stats["seq_track_source"]: + last_seq = stats["last_seq_by_source"].get(src_key) + if last_seq is not None: + seq_delta = (seq - int(last_seq)) & 0xFF + if seq_delta > 0: + stats["mav_seq_lost"] += max(seq_delta - 1, 0) + stats["last_seq_by_source"][src_key] = seq + stats["mav_msg_count"] += 1 if msg_type == "HEARTBEAT": - targets[port] = (int(msg.get_srcSystem()), int(msg.get_srcComponent())) + src_system = int(msg.get_srcSystem()) + src_component = int(msg.get_srcComponent()) + if is_fc_heartbeat(msg): + targets[port] = (src_system, src_component) stats["heartbeat_count"] += 1 elif msg_type == "RC_CHANNELS": stats["rc_count"] += 1 From 5c3b2eb146d5ac8e621a90fc7ed5e8f3907d0446 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Sat, 7 Mar 2026 23:46:15 -0500 Subject: [PATCH 30/42] update mav_fix2 changes --- docs/Mavlink.md | 5 +- docs/development/msp/inav_enums.json | 5 +- docs/development/msp/inav_enums_ref.md | 5 +- src/main/fc/fc_msp.c | 4 +- src/main/navigation/navigation.c | 6 +- src/main/telemetry/mavlink.c | 277 ++++++++++----- src/main/telemetry/mavlink.h | 2 +- src/test/unit/mavlink_unittest.cc | 469 ++++++++++++++++++++++--- src/test/unit/platform.h | 1 + 9 files changed, 628 insertions(+), 146 deletions(-) diff --git a/docs/Mavlink.md b/docs/Mavlink.md index 1df23459fed..78c89e0d648 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -86,7 +86,8 @@ Messages are organized into MAVLink datastream groups. Each group sends **one me - `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. +- `SET_POSITION_TARGET_GLOBAL_INT`: writes the GCS-guided waypoint when the frame is supported; altitude-only requests are also accepted when X/Y are masked out and GCS navigation is valid. +- `SET_POSITION_TARGET_LOCAL_NED`: accepts altitude-only requests in `MAV_FRAME_LOCAL_OFFSET_NED` when X/Y are zero or ignored and GCS navigation is valid. - `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_port1_radio_type` (also feeds link stats for MAVLink RX receivers). - `ADSB_VEHICLE` populates the internal traffic list when ADS‑B is enabled. @@ -101,7 +102,7 @@ Limited implementation of the Command protocol. - `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_AUTOPILOT_CAPABILITIES`: returns stub `AUTOPILOT_VERSION` (v2 only; v1 returns `UNSUPPORTED`) advertising `SET_POSITION_TARGET_GLOBAL_INT` and `SET_POSITION_TARGET_LOCAL_NED`. - `MAV_CMD_REQUEST_PROTOCOL_VERSION`: returns stub `PROTOCOL_VERSION` (v2 only; v1 returns `UNSUPPORTED`). ## Mode mappings (INAV → MAVLink/ArduPilot) diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 69a74420887..63121588b83 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -3302,9 +3302,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", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 1fd45d97e91..a12fd374756 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -4836,9 +4836,8 @@ |---|---:|---| | `SD_3016` | 0 | | | `HD_5018` | 1 | | -| `HD_3016` | 2 | | -| `HD_6022` | 3 | | -| `HD_5320` | 4 | | +| `HD_6022` | 2 | | +| `HD_5320` | 3 | | --- ## `resourceOwner_e` diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0939ab521b9..b06bf1f07f3 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4224,7 +4224,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; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a3752233149..55d6ccb8af7 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1298,9 +1298,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi 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 + return NAV_FSM_EVENT_SUCCESS; } @@ -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 508eaea4062..e155e4594b4 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -267,29 +267,32 @@ static void mavlinkSetActivePortContext(uint8_t portIndex) mavlinkApplyActivePortOutputVersion(); } -static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum) +static uint8_t mavlinkClampStreamRate(uint8_t rate) +{ + if (rate > TELEMETRY_MAVLINK_MAXRATE) { + return TELEMETRY_MAVLINK_MAXRATE; + } + + return rate; +} + +static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum, timeUs_t currentTimeUs) { if (!mavActivePort || streamNum >= MAXSTREAMS) { return 0; } - uint8_t rate = mavActivePort->mavRates[streamNum]; + const uint8_t rate = mavlinkClampStreamRate(mavActivePort->mavRates[streamNum]); if (rate == 0) { return 0; } - if (mavActivePort->mavTicks[streamNum] == 0) { - // we're triggering now, setup the next trigger point - if (rate > TELEMETRY_MAVLINK_MAXRATE) { - rate = TELEMETRY_MAVLINK_MAXRATE; - } - - mavActivePort->mavTicks[streamNum] = (TELEMETRY_MAVLINK_MAXRATE / rate); + const timeUs_t intervalUs = 1000000UL / rate; + if ((mavActivePort->mavStreamNextDue[streamNum] == 0) || (cmpTimeUs(currentTimeUs, mavActivePort->mavStreamNextDue[streamNum]) >= 0)) { + mavActivePort->mavStreamNextDue[streamNum] = currentTimeUs + intervalUs; return 1; } - // count down at TASK_RATE_HZ - mavActivePort->mavTicks[streamNum]--; return 0; } @@ -298,8 +301,8 @@ static void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate) if (!mavActivePort || streamNum >= MAXSTREAMS) { return; } - mavActivePort->mavRates[streamNum] = rate; - mavActivePort->mavTicks[streamNum] = 0; + mavActivePort->mavRates[streamNum] = mavlinkClampStreamRate(rate); + mavActivePort->mavStreamNextDue[streamNum] = 0; } static int32_t mavlinkStreamIntervalUs(uint8_t streamNum) @@ -308,7 +311,7 @@ static int32_t mavlinkStreamIntervalUs(uint8_t streamNum) return -1; } - uint8_t rate = mavActivePort->mavRates[streamNum]; + uint8_t rate = mavlinkClampStreamRate(mavActivePort->mavRates[streamNum]); if (rate == 0) { return -1; } @@ -336,7 +339,7 @@ static void configureMAVLinkStreamRates(uint8_t portIndex) for (uint8_t stream = 0; stream < MAVLINK_STREAM_COUNT; stream++) { state->mavRates[stream] = selectedRates[stream]; state->mavRatesConfigured[stream] = selectedRates[stream]; - state->mavTicks[stream] = 0; + state->mavStreamNextDue[stream] = 0; } } @@ -357,6 +360,7 @@ static void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; state->txSeq = 0; state->txDroppedFrames = 0; + memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); } @@ -423,6 +427,7 @@ static void configureMAVLinkTelemetryPort(uint8_t portIndex) state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; state->txSeq = 0; state->txDroppedFrames = 0; + memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); } @@ -461,8 +466,22 @@ static void mavlinkSendMessage(void) return; } + uint8_t sendMask = mavSendMask; + if (sendMask == 0) { + if (mavActivePort) { + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if (&mavPortStates[portIndex] == mavActivePort) { + sendMask = MAVLINK_PORT_MASK(portIndex); + break; + } + } + } else if (mavPortCount == 1 && mavPortStates[0].telemetryEnabled && mavPortStates[0].port) { + sendMask = MAVLINK_PORT_MASK(0); + } + } + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - if ((mavSendMask & MAVLINK_PORT_MASK(portIndex)) == 0) { + if ((sendMask & MAVLINK_PORT_MASK(portIndex)) == 0) { continue; } @@ -517,15 +536,21 @@ 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; + 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 - 0, // flight_sw_version. Setting this to actual Ardupilot version makes QGC not display modes anymore but "Unknown", except Guided is "GUIDED". Why? + flightSwVersion, // flight_sw_version 0, // middleware_sw_version 0, // os_sw_version 0, // board_version @@ -609,6 +634,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) { @@ -1128,7 +1216,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) ); @@ -1239,7 +1327,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 ) { @@ -1584,37 +1672,37 @@ 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(); } - if (mavlinkStreamTrigger(MAV_DATA_STREAM_HEARTBEAT)) { + if (mavlinkStreamTrigger(MAV_DATA_STREAM_HEARTBEAT, currentTimeUs)) { 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(); } @@ -1700,7 +1788,9 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u mavlinkTouchIncomingMissionTransaction(); - 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; @@ -1948,36 +2038,15 @@ 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; + 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); @@ -2177,7 +2246,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent } 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); @@ -2200,7 +2269,7 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent 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; } @@ -2433,35 +2502,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); @@ -2572,7 +2621,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; } @@ -2581,7 +2632,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; @@ -2597,13 +2648,45 @@ 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); -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) { 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); mavlinkRxHandleMessage(&msg); return true; } @@ -2904,6 +2987,13 @@ static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) mavlinkExtractTargets(&mavRecvMsg, &targetSystem, &targetComponent); mavlinkForwardMessage(ingressPortIndex, targetSystem, targetComponent); + if (mavRecvMsg.msgid == MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE) { + mavlinkSetActivePortContext(ingressPortIndex); + mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); + handleIncoming_RC_CHANNELS_OVERRIDE(); + return false; + } + const int8_t localPortIndex = mavlinkResolveLocalPortForTarget(targetSystem, targetComponent, ingressPortIndex); if (localPortIndex < 0) { return false; @@ -2970,6 +3060,9 @@ static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) // Don't set that we handled a message, otherwise RC channel packets will block telemetry messages localHandled = false; break; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + localHandled = handleIncoming_SET_POSITION_TARGET_LOCAL_NED(); + break; case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: localHandled = handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(); break; diff --git a/src/main/telemetry/mavlink.h b/src/main/telemetry/mavlink.h index 60b77237d0a..38aface2d5f 100755 --- a/src/main/telemetry/mavlink.h +++ b/src/main/telemetry/mavlink.h @@ -141,7 +141,7 @@ typedef struct mavlinkPortRuntime_s { bool highLatencyEnabled; uint8_t mavRates[MAVLINK_STREAM_COUNT]; uint8_t mavRatesConfigured[MAVLINK_STREAM_COUNT]; - uint8_t mavTicks[MAVLINK_STREAM_COUNT]; + timeUs_t mavStreamNextDue[MAVLINK_STREAM_COUNT]; uint8_t txSeq; uint32_t txDroppedFrames; mavlink_message_t mavRecvMsg; diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index 57035ad7393..641f31432d2 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -52,6 +52,13 @@ extern "C" { #include "io/osd.h" #include "navigation/navigation.h" +#ifdef __cplusplus + #define _Static_assert static_assert +#endif + #include "navigation/navigation_private.h" +#ifdef __cplusplus + #undef _Static_assert +#endif #include "rx/rx.h" @@ -60,19 +67,16 @@ extern "C" { #include "sensors/diagnostics.h" #include "sensors/gyro.h" #include "sensors/pitotmeter.h" + #include "sensors/sensors.h" #include "sensors/temperature.h" #include "telemetry/mavlink.h" #include "telemetry/telemetry.h" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-function" - #define MAVLINK_COMM_NUM_BUFFERS 1 - #include "common/mavlink.h" -#pragma GCC diagnostic pop - void mavlinkSendAttitude(void); + void mavlinkSendHeartbeat(void); void mavlinkSendBatteryTemperatureStatusText(void); + void mavlinkSendPosition(timeUs_t currentTimeUs); PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); @@ -90,6 +94,7 @@ static uint8_t serialTxBuffer[2048]; static size_t serialRxLen; static size_t serialRxPos; static size_t serialTxLen; +static const uint8_t testTargetComponent = MAV_COMP_ID_AUTOPILOT1; const uint32_t baudRates[] = { 0, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 250000, @@ -103,6 +108,14 @@ static int mavlinkRxHandleCalls; static bool gcsValid; static int waypointCount; static navWaypoint_t waypointStore[4]; +static float estimatedPosition[XYZ_AXIS_COUNT]; +static float estimatedVelocity[XYZ_AXIS_COUNT]; +static int altitudeTargetSetCalls; +static bool altitudeTargetSetResult; +static geoAltitudeDatumFlag_e lastAltitudeTargetDatum; +static int32_t lastAltitudeTargetCm; +static flightModeForTelemetry_e testFlightMode; +static uint32_t testSensorsMask; static void resetSerialBuffers(void) { @@ -139,13 +152,24 @@ static void initMavlinkTestState(void) mavlinkRxHandleCalls = 0; gcsValid = true; waypointCount = 0; + memset(estimatedPosition, 0, sizeof(estimatedPosition)); + memset(estimatedVelocity, 0, sizeof(estimatedVelocity)); + altitudeTargetSetCalls = 0; + altitudeTargetSetResult = true; + lastAltitudeTargetDatum = NAV_WP_TAKEOFF_DATUM; + lastAltitudeTargetCm = 0; + testFlightMode = FLM_MANUAL; + testSensorsMask = 0; + memset(&gpsSol, 0, sizeof(gpsSol)); + memset(&GPS_home, 0, sizeof(GPS_home)); 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()->mavlink_common.sysid = 1; + telemetryConfigMutable()->mavlink_common.autopilot_type = MAVLINK_AUTOPILOT_ARDUPILOT; + telemetryConfigMutable()->mavlink_common.version = 2; + telemetryConfigMutable()->mavlink[0].min_txbuff = 0; + telemetryConfigMutable()->mavlink[0].compid = MAV_COMP_ID_AUTOPILOT1; telemetryConfigMutable()->halfDuplex = 0; rxConfigMutable()->receiverType = RX_TYPE_NONE; @@ -158,7 +182,7 @@ static void initMavlinkTestState(void) rxRuntimeConfig.channelCount = 8; initMAVLinkTelemetry(); - configureMAVLinkTelemetryPort(); + checkMAVLinkTelemetryState(); } TEST(MavlinkTelemetryTest, AttitudeUsesRadiansPerSecond) @@ -193,7 +217,7 @@ TEST(MavlinkTelemetryTest, CommandLongRepositionUsesGlobalFrameAndParams) mavlink_message_t cmd; mavlink_msg_command_long_pack( 42, 200, &cmd, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, MAV_CMD_DO_REPOSITION, 0, 0, 0, 0, 123.4f, @@ -226,7 +250,7 @@ TEST(MavlinkTelemetryTest, CommandIntUnsupportedFrameIsRejected) mavlink_message_t cmd; mavlink_msg_command_int_pack( 42, 200, &cmd, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, MAV_FRAME_BODY_NED, MAV_CMD_DO_REPOSITION, 0, 0, @@ -255,7 +279,7 @@ TEST(MavlinkTelemetryTest, CommandIntRepositionScalesCoordinates) mavlink_message_t cmd; mavlink_msg_command_int_pack( 42, 200, &cmd, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_CMD_DO_REPOSITION, 0, 0, @@ -289,7 +313,7 @@ TEST(MavlinkTelemetryTest, MissionClearAllAcksAndResets) mavlink_message_t msg; mavlink_msg_mission_clear_all_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER, MAV_MISSION_TYPE_MISSION); + 1, testTargetComponent, MAV_MISSION_TYPE_MISSION); pushRxMessage(&msg); handleMAVLinkTelemetry(1000); @@ -314,7 +338,7 @@ TEST(MavlinkTelemetryTest, MissionCountRequestsFirstItem) mavlink_message_t msg; mavlink_msg_mission_count_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER, 1, MAV_MISSION_TYPE_MISSION, 0); + 1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0); pushRxMessage(&msg); handleMAVLinkTelemetry(1000); @@ -338,7 +362,7 @@ TEST(MavlinkTelemetryTest, MissionCountWhileArmedIsRejected) mavlink_message_t msg; mavlink_msg_mission_count_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER, 1, MAV_MISSION_TYPE_MISSION, 0); + 1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0); pushRxMessage(&msg); handleMAVLinkTelemetry(1000); @@ -374,7 +398,7 @@ TEST(MavlinkTelemetryTest, MissionItemIntSingleItemAcksAccepted) mavlink_message_t countMsg; mavlink_msg_mission_count_pack( 42, 200, &countMsg, - 1, MAV_COMP_ID_MISSIONPLANNER, 1, MAV_MISSION_TYPE_MISSION, 0); + 1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0); pushRxMessage(&countMsg); handleMAVLinkTelemetry(1000); @@ -384,7 +408,7 @@ TEST(MavlinkTelemetryTest, MissionItemIntSingleItemAcksAccepted) mavlink_message_t itemMsg; mavlink_msg_mission_item_int_pack( 42, 200, &itemMsg, - 1, MAV_COMP_ID_MISSIONPLANNER, 0, + 1, testTargetComponent, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, @@ -408,6 +432,80 @@ TEST(MavlinkTelemetryTest, MissionItemIntSingleItemAcksAccepted) EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f)); } +TEST(MavlinkTelemetryTest, MissionItemIntSingleFinalItemAllowsAutocontinueZero) +{ + initMavlinkTestState(); + + mavlink_message_t countMsg; + mavlink_msg_mission_count_pack( + 42, 200, &countMsg, + 1, testTargetComponent, 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, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 0, 0, + 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); +} + +TEST(MavlinkTelemetryTest, MissionItemIntNonFinalAutocontinueZeroIsRejected) +{ + initMavlinkTestState(); + + mavlink_message_t countMsg; + mavlink_msg_mission_count_pack( + 42, 200, &countMsg, + 1, testTargetComponent, 2, 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, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 0, 0, + 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_UNSUPPORTED); +} + TEST(MavlinkTelemetryTest, MissionItemIntGuidedWhileArmedUpdatesWaypoint) { initMavlinkTestState(); @@ -416,7 +514,7 @@ TEST(MavlinkTelemetryTest, MissionItemIntGuidedWhileArmedUpdatesWaypoint) mavlink_message_t msg; mavlink_msg_mission_item_int_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER, 0, + 1, testTargetComponent, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_CMD_NAV_WAYPOINT, 2, 1, 0, 0, 0, 0, @@ -433,6 +531,37 @@ TEST(MavlinkTelemetryTest, MissionItemIntGuidedWhileArmedUpdatesWaypoint) EXPECT_EQ(lastWaypoint.p3, 0); } +TEST(MavlinkTelemetryTest, MissionItemIntGuidedWhileArmedCurrentThreeChangesAltitude) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t msg; + mavlink_msg_mission_item_int_pack( + 42, 200, &msg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 3, 1, + 0, 0, 0, 0, + 375000000, -1222500000, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + 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(altitudeTargetSetCalls, 1); + EXPECT_EQ(lastAltitudeTargetDatum, NAV_WP_TAKEOFF_DATUM); + EXPECT_EQ(lastAltitudeTargetCm, 1230); +} + TEST(MavlinkTelemetryTest, MissionRequestListSendsCount) { initMavlinkTestState(); @@ -441,7 +570,7 @@ TEST(MavlinkTelemetryTest, MissionRequestListSendsCount) mavlink_message_t msg; mavlink_msg_mission_request_list_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER, MAV_MISSION_TYPE_MISSION); + 1, testTargetComponent, MAV_MISSION_TYPE_MISSION); pushRxMessage(&msg); handleMAVLinkTelemetry(1000); @@ -470,7 +599,7 @@ TEST(MavlinkTelemetryTest, MissionRequestSendsWaypoint) mavlink_message_t msg; mavlink_msg_mission_request_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER, 0, MAV_MISSION_TYPE_MISSION); + 1, testTargetComponent, 0, MAV_MISSION_TYPE_MISSION); pushRxMessage(&msg); handleMAVLinkTelemetry(1000); @@ -498,7 +627,7 @@ TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesAbsoluteAltitude) mavlink_message_t msg; mavlink_msg_mission_item_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER, 0, + 1, testTargetComponent, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT, 2, 1, 0, 0, 0, 0, @@ -519,7 +648,7 @@ TEST(MavlinkTelemetryTest, ParamRequestListRespondsWithEmptyParam) mavlink_message_t msg; mavlink_msg_param_request_list_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER); + 1, testTargetComponent); pushRxMessage(&msg); handleMAVLinkTelemetry(1000); @@ -542,7 +671,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, testTargetComponent, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0, 375000000, -1222500000, 12.3f, 0, 0, 0, 0, 0, 0, 0, 0); @@ -564,7 +693,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, testTargetComponent, MAV_FRAME_GLOBAL_INT, 0, 375000000, -1222500000, 12.3f, 0, 0, 0, 0, 0, 0, 0, 0); @@ -576,6 +705,93 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude) EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE); } +TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntAltitudeOnlyRequiresValidGcs) +{ + initMavlinkTestState(); + gcsValid = false; + + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 0, 0, 12.3f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 0); +} + +TEST(MavlinkTelemetryTest, SetPositionTargetLocalNedAltitudeOnlySetsAltitudeTarget) +{ + initMavlinkTestState(); + estimatedPosition[Z] = 1000.0f; + + mavlink_message_t msg; + mavlink_msg_set_position_target_local_ned_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_LOCAL_OFFSET_NED, + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 0.0f, 0.0f, -2.5f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 1); + EXPECT_EQ(lastAltitudeTargetDatum, NAV_WP_TAKEOFF_DATUM); + EXPECT_EQ(lastAltitudeTargetCm, 1250); +} + +TEST(MavlinkTelemetryTest, SetPositionTargetLocalNedIgnoresXyMotionRequests) +{ + initMavlinkTestState(); + estimatedPosition[Z] = 1000.0f; + + mavlink_message_t msg; + mavlink_msg_set_position_target_local_ned_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_LOCAL_OFFSET_NED, + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 1.0f, 0.0f, -2.5f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 0); +} + TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream) { initMavlinkTestState(); @@ -583,7 +799,7 @@ TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream) mavlink_message_t setMsg; mavlink_msg_request_data_stream_pack( 42, 200, &setMsg, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, MAV_DATA_STREAM_RC_CHANNELS, 10, 1); pushRxMessage(&setMsg); @@ -594,7 +810,7 @@ TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream) mavlink_message_t getMsg; mavlink_msg_command_long_pack( 42, 200, &getMsg, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, MAV_CMD_GET_MESSAGE_INTERVAL, 0, (float)MAVLINK_MSG_ID_RC_CHANNELS, @@ -618,7 +834,7 @@ TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream) mavlink_message_t stopMsg; mavlink_msg_request_data_stream_pack( 42, 200, &stopMsg, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, MAV_DATA_STREAM_RC_CHANNELS, 0, 0); pushRxMessage(&stopMsg); @@ -645,7 +861,7 @@ TEST(MavlinkTelemetryTest, HeartbeatIntervalIsIndependentFromExtra2Stream) mavlink_message_t stopExtra2Msg; mavlink_msg_request_data_stream_pack( 42, 200, &stopExtra2Msg, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, MAV_DATA_STREAM_EXTRA2, 0, 0); pushRxMessage(&stopExtra2Msg); @@ -656,7 +872,7 @@ TEST(MavlinkTelemetryTest, HeartbeatIntervalIsIndependentFromExtra2Stream) mavlink_message_t getMsg; mavlink_msg_command_long_pack( 42, 200, &getMsg, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, MAV_CMD_GET_MESSAGE_INTERVAL, 0, (float)MAVLINK_MSG_ID_HEARTBEAT, @@ -683,7 +899,7 @@ TEST(MavlinkTelemetryTest, RequestProtocolVersionUsesConfiguredVersion) mavlink_message_t msg; mavlink_msg_command_long_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, MAV_CMD_REQUEST_PROTOCOL_VERSION, 0, 0, 0, 0, 0, 0, 0, 0); @@ -703,6 +919,105 @@ TEST(MavlinkTelemetryTest, RequestProtocolVersionUsesConfiguredVersion) EXPECT_EQ(version.max_version, 200); } +TEST(MavlinkTelemetryTest, RequestAutopilotCapabilitiesReportsLocalNedCapabilityAndPackedVersion) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_command_long_pack( + 42, 200, &msg, + 1, testTargetComponent, + MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, + 0, + 1, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + mavlink_message_t outMsg; + bool sawAutopilotVersion = 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_AUTOPILOT_VERSION) { + mavlink_autopilot_version_t version; + mavlink_msg_autopilot_version_decode(&outMsg, &version); + EXPECT_NE((version.capabilities & MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED), 0U); + EXPECT_EQ(version.flight_sw_version, + ((uint32_t)ARDUPILOT_VERSION_MAJOR << 24) | + ((uint32_t)ARDUPILOT_VERSION_MINOR << 16) | + ((uint32_t)ARDUPILOT_VERSION_PATCH << 8)); + EXPECT_EQ(version.middleware_sw_version, 0U); + EXPECT_EQ(version.os_sw_version, 0U); + sawAutopilotVersion = true; + } + } + } + + EXPECT_TRUE(sawAutopilotVersion); +} + +TEST(MavlinkTelemetryTest, HeartbeatGuidedFlagRequiresValidGcsInPoshold) +{ + initMavlinkTestState(); + testFlightMode = FLM_POSITION_HOLD; + gcsValid = false; + + mavlinkSendHeartbeat(); + + mavlink_message_t msg; + ASSERT_TRUE(popTxMessage(&msg)); + ASSERT_EQ(msg.msgid, MAVLINK_MSG_ID_HEARTBEAT); + + mavlink_heartbeat_t heartbeat; + mavlink_msg_heartbeat_decode(&msg, &heartbeat); + EXPECT_EQ((heartbeat.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED), 0); + + serialTxLen = 0; + gcsValid = true; + + mavlinkSendHeartbeat(); + + ASSERT_TRUE(popTxMessage(&msg)); + ASSERT_EQ(msg.msgid, MAVLINK_MSG_ID_HEARTBEAT); + + mavlink_msg_heartbeat_decode(&msg, &heartbeat); + EXPECT_NE((heartbeat.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED), 0); +} + +TEST(MavlinkTelemetryTest, PositionReportsPositiveDownVelocity) +{ + initMavlinkTestState(); + testSensorsMask = SENSOR_GPS; + gpsSol.fixType = GPS_FIX_3D; + gpsSol.llh.lat = 375000000; + gpsSol.llh.lon = -1222500000; + gpsSol.llh.alt = 12345; + estimatedVelocity[Z] = 321.0f; + + mavlinkSendPosition(1000); + + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + mavlink_message_t msg; + bool sawGlobalPosition = 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_GLOBAL_POSITION_INT) { + mavlink_global_position_int_t position; + mavlink_msg_global_position_int_decode(&msg, &position); + EXPECT_EQ(position.vz, -321); + sawGlobalPosition = true; + } + } + } + + EXPECT_TRUE(sawGlobalPosition); +} + TEST(MavlinkTelemetryTest, BatteryStatusDoesNotSendExtendedSysState) { initMavlinkTestState(); @@ -731,7 +1046,7 @@ TEST(MavlinkTelemetryTest, RadioStatusUpdatesRxLinkStats) initMavlinkTestState(); rxConfigMutable()->receiverType = RX_TYPE_SERIAL; rxConfigMutable()->serialrx_provider = SERIALRX_MAVLINK; - telemetryConfigMutable()->mavlink.radio_type = MAVLINK_RADIO_ELRS; + telemetryConfigMutable()->mavlink[0].radio_type = MAVLINK_RADIO_ELRS; mavlink_message_t msg; mavlink_msg_radio_status_pack( @@ -753,7 +1068,24 @@ TEST(MavlinkTelemetryTest, RcChannelsOverrideIsForwarded) mavlink_message_t msg; mavlink_msg_rc_channels_override_pack( 42, 200, &msg, - 1, MAV_COMP_ID_MISSIONPLANNER, + 1, testTargetComponent, + 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); +} + +TEST(MavlinkTelemetryTest, RcChannelsOverrideIgnoresTargetSystemMismatch) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_rc_channels_override_pack( + 42, 200, &msg, + 99, testTargetComponent, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); @@ -775,11 +1107,12 @@ gyro_t gyro; gpsSolutionData_t gpsSol; gpsLocation_t GPS_home; navSystemStatus_t NAV_Status; +navigationPosControl_t posControl; rxRuntimeConfig_t rxRuntimeConfig; rxLinkStatistics_t rxLinkStatistics; uint16_t averageSystemLoadPercent; -uint32_t micros(void) +timeUs_t micros(void) { return 0; } @@ -792,11 +1125,18 @@ uint32_t millis(void) serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) { UNUSED(function); + testPortConfig.functionMask = FUNCTION_TELEMETRY_MAVLINK; testPortConfig.identifier = SERIAL_PORT_USART1; testPortConfig.telemetry_baudrateIndex = BAUD_115200; return &testPortConfig; } +serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function) +{ + UNUSED(function); + return NULL; +} + portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function) { UNUSED(portConfig); @@ -847,6 +1187,23 @@ void serialWrite(serialPort_t *instance, uint8_t ch) serialTxBuffer[serialTxLen++] = ch; } +void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) +{ + UNUSED(instance); + memcpy(&serialTxBuffer[serialTxLen], data, (size_t)count); + serialTxLen += (size_t)count; +} + +void serialBeginWrite(serialPort_t *instance) +{ + UNUSED(instance); +} + +void serialEndWrite(serialPort_t *instance) +{ + UNUSED(instance); +} + void serialSetMode(serialPort_t *instance, portMode_t mode) { UNUSED(instance); @@ -861,8 +1218,7 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) bool sensors(uint32_t mask) { - UNUSED(mask); - return false; + return (testSensorsMask & mask) != 0; } bool isAmperageConfigured(void) @@ -870,6 +1226,11 @@ bool isAmperageConfigured(void) return false; } +bool isBlackboxDeviceFull(void) +{ + return false; +} + bool feature(uint32_t mask) { UNUSED(mask); @@ -896,6 +1257,24 @@ uint8_t getBatteryCellCount(void) return 0; } +batteryState_e getBatteryState(void) +{ + return BATTERY_OK; +} + +bool isEstimatedWindSpeedValid(void) +{ + return false; +} + +float getEstimatedHorizontalWindSpeed(uint16_t *angle) +{ + if (angle) { + *angle = 0; + } + return 0; +} + uint16_t getBatteryAverageCellVoltage(void) { return 0; @@ -919,14 +1298,12 @@ bool osdUsingScaledThrottle(void) float getEstimatedActualPosition(int axis) { - UNUSED(axis); - return 0.0f; + return estimatedPosition[axis]; } float getEstimatedActualVelocity(int axis) { - UNUSED(axis); - return 0.0f; + return estimatedVelocity[axis]; } float getAirspeedEstimate(void) @@ -1001,6 +1378,14 @@ int isGCSValid(void) return gcsValid; } +bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm) +{ + altitudeTargetSetCalls++; + lastAltitudeTargetDatum = datumFlag; + lastAltitudeTargetCm = targetAltitudeCm; + return altitudeTargetSetResult; +} + void setWaypoint(uint8_t wpNumber, const navWaypoint_t *wp) { UNUSED(wpNumber); @@ -1036,7 +1421,7 @@ void resetWaypointList(void) flightModeForTelemetry_e getFlightModeForTelemetry(void) { - return FLM_MANUAL; + return testFlightMode; } bool isModeActivationConditionPresent(boxId_e modeId) diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 4f921e9612e..12b2671167b 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -79,6 +79,7 @@ extern SysTick_Type *SysTick; #define WS2811_DMA_TC_FLAG 1 #define WS2811_DMA_HANDLER_IDENTIFER 0 +#include "target/common.h" #include "target.h" #define FAST_CODE From c0c6fab1f3c08e962847a1514dd79a71c1d8d22e Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Wed, 1 Apr 2026 13:07:40 -0400 Subject: [PATCH 31/42] add mav yaw, fix missing cli yaml --- docs/Mavlink.md | 1 + src/main/fc/settings.yaml | 2 +- src/main/telemetry/mavlink.c | 35 +++++++++++++++++++++++++++++++++++ 3 files changed, 37 insertions(+), 1 deletion(-) diff --git a/docs/Mavlink.md b/docs/Mavlink.md index 78c89e0d648..0474a4a46a6 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -99,6 +99,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_CONDITION_YAW`: changes the current heading target when the active navigation state has yaw control. Accepts absolute heading (`param4=0`) and relative turns (`param4!=0`); turn-rate is ignored. - `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/fc/settings.yaml b/src/main/fc/settings.yaml index 18fda6bc4e6..b9d09980455 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -199,7 +199,7 @@ tables: values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e - name: mavlink_radio_type - values: ["GENERIC", "ELRS", "SIK"] + values: ["GENERIC", "ELRS", "SIK", "NONE"] enum: mavlinkRadio_e - name: mavlink_autopilot_type values: ["GENERIC", "ARDUPILOT"] diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index e155e4594b4..aa0a290d06e 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -2273,6 +2273,34 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); return true; } + case MAV_CMD_CONDITION_YAW: + { + if (!(navGetCurrentStateFlags() & NAV_CTL_YAW)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + int32_t targetHeadingCd = wrap_36000((int32_t)lrintf(param1 * 100.0f)); + + if (param4 != 0.0f) { + const int32_t currentHeadingCd = STATE(AIRPLANE) ? posControl.actualState.cog : posControl.actualState.yaw; + const int32_t headingChangeCd = (int32_t)lrintf(fabsf(param1) * 100.0f); + + if (param3 < 0.0f) { + targetHeadingCd = wrap_36000(currentHeadingCd - headingChangeCd); + } else { + targetHeadingCd = wrap_36000(currentHeadingCd + headingChangeCd); + } + } + + updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(targetHeadingCd)); + posControl.desiredState.yaw = targetHeadingCd; + posControl.cruise.course = targetHeadingCd; + posControl.cruise.previousCourse = targetHeadingCd; + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } case MAV_CMD_SET_MESSAGE_INTERVAL: { uint8_t stream; @@ -2933,6 +2961,13 @@ static int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t tar return -1; } + if (ingressPortIndex < mavPortCount) { + const mavlinkTelemetryPortConfig_t *ingressCfg = mavlinkGetPortConfig(ingressPortIndex); + if (targetComponent <= 0 || ingressCfg->compid == targetComponent) { + return ingressPortIndex; + } + } + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { const mavlinkTelemetryPortConfig_t *cfg = mavlinkGetPortConfig(portIndex); if (targetComponent <= 0 || cfg->compid == targetComponent) { From f2dacc1887cc0b97476760ecf4060766c770a702 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Thu, 2 Apr 2026 14:39:05 -0400 Subject: [PATCH 32/42] Add MSP tunnel over MAVLink --- docs/Mavlink.md | 125 +-- src/main/msp/msp_serial.c | 111 +++ src/main/msp/msp_serial.h | 3 + src/main/telemetry/mavlink.c | 159 ++++ src/test/unit/CMakeLists.txt | 2 +- src/test/unit/mavlink_unittest.cc | 283 +++++- src/utils/mavlink_tests/ROUTING_TEST.md | 33 - .../mavlink_tests/mav_multi_benchmark.py | 880 ------------------ src/utils/mavlink_tests/mav_multi_sweep.py | 164 ---- .../mavlink_routing_compliance_test.py | 499 ---------- src/utils/mavlink_tests/mavlink_test_rc.py | 141 --- src/utils/mavlink_tests/msp_test_rc.py | 145 --- .../mavlink_tests/routing_test_config.yaml | 58 -- ..._multiport4_sweep_rc460800_tele115200.yaml | 51 - 14 files changed, 624 insertions(+), 2030 deletions(-) delete mode 100644 src/utils/mavlink_tests/ROUTING_TEST.md delete mode 100644 src/utils/mavlink_tests/mav_multi_benchmark.py delete mode 100644 src/utils/mavlink_tests/mav_multi_sweep.py delete mode 100644 src/utils/mavlink_tests/mavlink_routing_compliance_test.py delete mode 100644 src/utils/mavlink_tests/mavlink_test_rc.py delete mode 100644 src/utils/mavlink_tests/msp_test_rc.py delete mode 100644 src/utils/mavlink_tests/routing_test_config.yaml delete mode 100644 src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml diff --git a/docs/Mavlink.md b/docs/Mavlink.md index 0474a4a46a6..c42310a30ab 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -2,6 +2,8 @@ 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. +INAV supports up to 4 concurrent MAVLink telemetry ports (`MAX_MAVLINK_PORTS`), one endpoint per serial port configured with `FUNCTION_TELEMETRY_MAVLINK`. + ## 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. @@ -11,54 +13,62 @@ INAV has a partial implementation of MAVLink that is intended primarily for simp - **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_port1_ext_status_rate`, `mavlink_port1_rc_chan_rate`, `mavlink_port1_pos_rate`, `mavlink_port1_extra1_rate`, `mavlink_port1_extra2_rate`, `mavlink_port1_extra3_rate`. Each group is polled up to 50 Hz; a rate of 0 disables the group. -- `mavlink_port1_min_txbuffer` – minimum remote TX buffer level before sending when `RADIO_STATUS` provides flow control. -- `mavlink_port1_radio_type` – scales `RADIO_STATUS` RSSI/SNR for **generic**, **ELRS**, or **SiK** links. +### Usage guidance +- If you rely on RC 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. +- Assign a unique `mavlink_port{1-4}_compid` to each INAV MAVLink port to avoid ambiguous local targeting. +- If a GCS or companion needs telemetry on ports 2..4, explicitly request streams (`REQUEST_DATA_STREAM` or `MAV_CMD_SET_MESSAGE_INTERVAL`) because only heartbeat is enabled by default. +- If you depend on directed forwarding between links, ensure each remote endpoint transmits at least one frame early so route learning is populated. -## Multi-port MAVLink +### Relevant CLI settings + +- `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): Each group is polled up to 50 Hz; a rate of 0 disables the group. + - `mavlink_port{1-4}_ext_status_rate` + - `mavlink_port{1-4}_rc_chan_rate` + - `mavlink_port{1-4}_pos_rate` + - `mavlink_port{1-4}_extra1_rate` + - `mavlink_port{1-4}_extra2_rate` + - `mavlink_port{1-4}_extra3_rate`. +- Port 1 uses configured CLI rates (`mavlink_port1_*_rate`). +- Ports 2..4 start with heartbeat only (1 Hz), all other streams disabled. +- `mavlink_port{1-4}_compid` - MAV_COMPONENT ID of port. Ensure these are different. +- `mavlink_port{1-4}_min_txbuffer` - minimum remote TX buffer level before sending when `RADIO_STATUS` provides flow control. +- `mavlink_port{1-4}_radio_type`- scales `RADIO_STATUS` RSSI/SNR for **generic**, **ELRS**, or **SiK** links. +- `mavlink_port{1-4}_high_latency`- turns on Mavlink HIGH_LATENCY2 mode on this port -INAV supports up to 4 concurrent MAVLink telemetry ports (`MAX_MAVLINK_PORTS`), one endpoint per serial port configured with `FUNCTION_TELEMETRY_MAVLINK`. -### Configuration model +## Datastream groups and defaults -- Shared across all ports: `mavlink_sysid`, `mavlink_version`, `mavlink_autopilot_type`. -- Per-port: `mavlink_portN_compid`, `mavlink_portN_min_txbuffer`, `mavlink_portN_radio_type`, `mavlink_portN_high_latency`. -- Stream defaults at startup: -- Port 1 uses configured CLI rates (`mavlink_port1_*_rate`). -- Ports 2..4 start with heartbeat only (1 Hz), all other streams disabled. +Default rates (Hz) are shown; adjust with the CLI keys above for port 1. +Ports 2..N use a secondary startup profile (heartbeat at 1 Hz, other streams disabled). -### Routing and forwarding behavior +| 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` | 2 Hz | +| `HEARTBEAT` | `HEARTBEAT` | 1 Hz (independent of stream groups) | +| `EXT_SYS_STATE` | `EXTENDED_SYS_STATE` | 1 Hz (defaults to `mavlink_port1_extra3_rate`) | +| `EXTRA3` | `BATTERY_STATUS`, `SCALED_PRESSURE`, `STATUSTEXT` (when present) | 1 Hz | + +### Routing, forwarding and local handling - INAV learns routes from incoming traffic as `(sysid, compid) -> ingress port`. - Broadcast messages are forwarded to all other MAVLink ports (except `RADIO_STATUS`, which is not forwarded). - Targeted messages are forwarded only to ports with a learned route for that target. - Practical caveat: the first targeted message to a never-seen endpoint may not forward until that endpoint has sent at least one MAVLink frame. - -### Local message handling behavior - - Local/system broadcasts (`target_system=0` or local system ID with `target_component=0`) are fanned out to all local ports only for: -- `REQUEST_DATA_STREAM` -- `MAV_CMD_SET_MESSAGE_INTERVAL` -- `MAV_CMD_CONTROL_HIGH_LATENCY` + - `REQUEST_DATA_STREAM` + - `MAV_CMD_SET_MESSAGE_INTERVAL` + - `MAV_CMD_CONTROL_HIGH_LATENCY` - Other incoming commands/messages are handled on one resolved local port, based on local target matching. -### High-latency behavior - -- High-latency mode is per-port (`mavlink_portN_high_latency` or `MAV_CMD_CONTROL_HIGH_LATENCY` on that port). -- Requires MAVLink2; MAVLink1 cannot enable it. -- When enabled, normal stream scheduling for that port is replaced by `HIGH_LATENCY2` at 5-second intervals. - -### Usage guidance - -- Assign a unique `mavlink_portN_compid` to each INAV MAVLink port to avoid ambiguous local targeting. -- If a GCS or companion needs telemetry on ports 2..4, explicitly request streams (`REQUEST_DATA_STREAM` or `MAV_CMD_SET_MESSAGE_INTERVAL`) because only heartbeat is enabled by default. -- If you depend on directed forwarding between links, ensure each remote endpoint transmits at least one frame early so route learning is populated. - ## Supported Outgoing Messages Messages are organized into MAVLink datastream groups. Each group sends **one message per trigger** at the configured rate: @@ -89,9 +99,10 @@ Messages are organized into MAVLink datastream groups. Each group sends **one me - `SET_POSITION_TARGET_GLOBAL_INT`: writes the GCS-guided waypoint when the frame is supported; altitude-only requests are also accepted when X/Y are masked out and GCS navigation is valid. - `SET_POSITION_TARGET_LOCAL_NED`: accepts altitude-only requests in `MAV_FRAME_LOCAL_OFFSET_NED` when X/Y are zero or ignored and GCS navigation is valid. - `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_port1_radio_type` (also feeds link stats for MAVLink RX receivers). +- `RADIO_STATUS` updates remote TX buffer level and scales RSSI/SNR according to `mavlink_port{1-4}_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). +- `TUNNEL` accepts private payload type `0x8001` for MSP-over-MAVLink on MAVLink2 links. ## Supported Commands @@ -134,28 +145,6 @@ Limited implementation of the Command protocol. - 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 for port 1. -Ports 2..N use a secondary startup profile (heartbeat at 1 Hz, other streams disabled). - -| 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` | 2 Hz | -| `HEARTBEAT` | `HEARTBEAT` | 1 Hz (independent of stream groups) | -| `EXT_SYS_STATE` | `EXTENDED_SYS_STATE` | 1 Hz (defaults to `mavlink_port1_extra3_rate`) | -| `EXTRA3` | `BATTERY_STATUS`, `SCALED_PRESSURE`, `STATUSTEXT` (when present) | 1 Hz | - -## Operating tips - -- Set `mavlink_port1_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 @@ -171,3 +160,25 @@ Partial compatibility with MAVLink mission planners such as QGC is implemented, - 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`. + + +## MSP over MAVLink tunnel +This feature uses the MAVLink [Tunnel service](https://mavlink.io/en/services/tunnel.html) to let the INAV Configurator use MSP over an existing MAVLink telemetry link, typically a radio link where there is no separate wireless MSP device. +**It is not intended as a general-purpose serial tunnel, and it is not a replacement for normal MAVLink control/telemetry traffic.** +CLI mode is unavailable in MSP-over-MAVLink. + +- INAV accepts `TUNNEL` messages with private payload type `0x8001` as an MSP byte stream carried over MAVLink2. +- `target_system` must match `mavlink_sysid`. +- `target_component` may be `0` or the local port `mavlink_port{1-4}_compid`. +- `target_component=0` is handled on the ingress MAVLink port only; it is not fanned out to other local MAVLink ports. +- MSP replies are sent back to the requester as one or more `TUNNEL` messages on that same ingress port. +- MSP framing is preserved end-to-end: MSPv1 requests get MSPv1 replies, and MSPv2 requests get MSPv2 replies. +- Reboot (`MSP_REBOOT`) is supported over the tunnel. Serial passthrough and ESC 4way passthrough are rejected before execution. + +## High latency mode +High-latency mode uses the MAVLink [High Latency service](https://mavlink.io/en/services/high_latency.html) to replace normal scheduled telemetry on one port with periodic `HIGH_LATENCY2` summaries for very low-bandwidth or intermittent links. + +- High latency mode is per-port, controlled by `mavlink_port{1-4}_high_latency` or by `MAV_CMD_CONTROL_HIGH_LATENCY` received on that port. +- It requires MAVLink2. MAVLink1 cannot enable or carry `HIGH_LATENCY2`. +- When enabled on a port, normal stream scheduling on that port is replaced by `HIGH_LATENCY2` at 5 second intervals. +- This is intended for slow and high latency telemetry such as cellular, satellite or LoRa, not for normal rich telemetry, mission planning, or configurator use. diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 32aac7a7bd5..02e36004c89 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -259,6 +259,11 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c) return true; } +bool mspSerialProcessReceivedByte(mspPort_t *mspPort, uint8_t c) +{ + return mspSerialProcessReceivedData(mspPort, c); +} + static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len) { while (len-- > 0) { @@ -391,6 +396,98 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e msp return mspSerialSendFrame(msp, hdrBuf, hdrLen, sbufPtr(&packet->buf), dataLen, crcBuf, crcLen); } +int mspSerialEncodePacket(mspPacket_t *packet, mspVersion_e mspVersion, uint8_t *frameBuf, int frameBufSize) +{ + static const uint8_t mspMagic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER; + const int dataLen = sbufBytesRemaining(&packet->buf); + uint8_t hdrBuf[16] = { '$', mspMagic[mspVersion], packet->result == MSP_RESULT_ERROR ? '!' : '>'}; + uint8_t crcBuf[2]; + int hdrLen = 3; + int crcLen = 0; + + if (mspVersion == MSP_V1) { + mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderV1_t); + hdrV1->cmd = packet->cmd; + + if (dataLen >= JUMBO_FRAME_SIZE_LIMIT) { + mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderJUMBO_t); + + hdrV1->size = JUMBO_FRAME_SIZE_LIMIT; + hdrJUMBO->size = dataLen; + } + else { + hdrV1->size = dataLen; + } + + crcBuf[crcLen] = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS); + crcBuf[crcLen] = mspSerialChecksumBuf(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen); + crcLen++; + } + else if (mspVersion == MSP_V2_OVER_V1) { + mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen]; + + hdrLen += sizeof(mspHeaderV1_t); + + mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderV2_t); + + const int v1PayloadSize = sizeof(mspHeaderV2_t) + dataLen + 1; + hdrV1->cmd = MSP_V2_FRAME_ID; + + if (v1PayloadSize >= JUMBO_FRAME_SIZE_LIMIT) { + mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderJUMBO_t); + + hdrV1->size = JUMBO_FRAME_SIZE_LIMIT; + hdrJUMBO->size = v1PayloadSize; + } + else { + hdrV1->size = v1PayloadSize; + } + + hdrV2->flags = packet->flags; + hdrV2->cmd = packet->cmd; + hdrV2->size = dataLen; + + crcBuf[crcLen] = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t)); + crcBuf[crcLen] = crc8_dvb_s2_update(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen); + crcLen++; + + crcBuf[crcLen] = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS); + crcBuf[crcLen] = mspSerialChecksumBuf(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen); + crcBuf[crcLen] = mspSerialChecksumBuf(crcBuf[crcLen], crcBuf, crcLen); + crcLen++; + } + else if (mspVersion == MSP_V2_NATIVE) { + mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderV2_t); + + hdrV2->flags = packet->flags; + hdrV2->cmd = packet->cmd; + hdrV2->size = dataLen; + + crcBuf[crcLen] = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t)); + crcBuf[crcLen] = crc8_dvb_s2_update(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen); + crcLen++; + } + else { + return 0; + } + + const int totalFrameLength = hdrLen + dataLen + crcLen; + if (frameBufSize < totalFrameLength) { + return 0; + } + + memcpy(frameBuf, hdrBuf, hdrLen); + memcpy(frameBuf + hdrLen, sbufPtr(&packet->buf), dataLen); + memcpy(frameBuf + hdrLen + dataLen, crcBuf, crcLen); + + return totalFrameLength; +} + static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn) { uint8_t outBuf[MSP_PORT_OUTBUF_SIZE]; @@ -422,6 +519,20 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr return mspPostProcessFn; } +mspResult_e mspSerialProcessCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +{ + mspPacket_t command = { + .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, }, + .cmd = msp->cmdMSP, + .flags = msp->cmdFlags, + .result = 0, + }; + + const mspResult_e status = mspProcessCommandFn(&command, reply, mspPostProcessFn); + msp->c_state = MSP_IDLE; + return status; +} + static void mspEvaluateNonMspData(mspPort_t * mspPort, uint8_t receivedChar) { if (receivedChar == '#') { diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 67487606da0..86fa6461217 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -101,6 +101,9 @@ typedef struct mspPort_s { void mspSerialInit(void); void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort); +bool mspSerialProcessReceivedByte(mspPort_t *mspPort, uint8_t c); +int mspSerialEncodePacket(mspPacket_t *packet, mspVersion_e mspVersion, uint8_t *frameBuf, int frameBufSize); +mspResult_e mspSerialProcessCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index aa0a290d06e..6b7e5496475 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -48,6 +48,7 @@ #include "fc/config.h" #include "fc/fc_core.h" +#include "fc/fc_msp.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -66,6 +67,9 @@ #include "io/serial.h" #include "io/osd.h" +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -106,6 +110,9 @@ static mavlinkPortRuntime_t mavPortStates[MAX_MAVLINK_PORTS]; static uint8_t mavPortCount = 0; static mavlinkRouteEntry_t mavRouteTable[MAVLINK_MAX_ROUTES]; static uint8_t mavRouteCount = 0; +static mspPort_t mavTunnelMspPorts[MAX_MAVLINK_PORTS]; +static uint8_t mavTunnelRemoteSystemIds[MAX_MAVLINK_PORTS]; +static uint8_t mavTunnelRemoteComponentIds[MAX_MAVLINK_PORTS]; static uint8_t mavSendMask = 0; static mavlinkPortRuntime_t *mavActivePort = NULL; static const mavlinkTelemetryPortConfig_t *mavActiveConfig = NULL; @@ -117,6 +124,13 @@ static uint8_t mavSystemId = 1; static uint8_t mavAutopilotType; static uint8_t mavComponentId = MAV_COMP_ID_AUTOPILOT1; +#define MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP 0x8001 +#define MAVLINK_TUNNEL_MSP_TIMEOUT_MS 1000 +#define MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE (MSP_PORT_OUTBUF_SIZE + 16) + +static uint8_t mavTunnelReplyPayloadBuf[MSP_PORT_OUTBUF_SIZE]; +static uint8_t mavTunnelFrameBuf[MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE]; + static uint8_t mavlinkGetVehicleType(void) { switch (mixerConfig()->platformType) @@ -363,6 +377,9 @@ static void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); + resetMspPort(&mavTunnelMspPorts[portIndex], NULL); + mavTunnelRemoteSystemIds[portIndex] = 0; + mavTunnelRemoteComponentIds[portIndex] = 0; } void freeMAVLinkTelemetryPort(void) @@ -379,6 +396,9 @@ void initMAVLinkTelemetry(void) { memset(mavPortStates, 0, sizeof(mavPortStates)); memset(mavRouteTable, 0, sizeof(mavRouteTable)); + memset(mavTunnelMspPorts, 0, sizeof(mavTunnelMspPorts)); + memset(mavTunnelRemoteSystemIds, 0, sizeof(mavTunnelRemoteSystemIds)); + memset(mavTunnelRemoteComponentIds, 0, sizeof(mavTunnelRemoteComponentIds)); mavPortCount = 0; mavRouteCount = 0; mavSendMask = 0; @@ -430,6 +450,9 @@ static void configureMAVLinkTelemetryPort(uint8_t portIndex) memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); + resetMspPort(&mavTunnelMspPorts[portIndex], NULL); + mavTunnelRemoteSystemIds[portIndex] = 0; + mavTunnelRemoteComponentIds[portIndex] = 0; } void checkMAVLinkTelemetryState(void) @@ -526,6 +549,139 @@ static void mavlinkSendMessage(void) } } +static void mavlinkResetTunnelState(uint8_t ingressPortIndex) +{ + resetMspPort(&mavTunnelMspPorts[ingressPortIndex], NULL); + mavTunnelRemoteSystemIds[ingressPortIndex] = 0; + mavTunnelRemoteComponentIds[ingressPortIndex] = 0; +} + +static void mavlinkSendTunnelReply(uint8_t targetSystem, uint8_t targetComponent, const uint8_t *payload, uint8_t payloadLength) +{ + uint8_t tunnelPayload[MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN] = { 0 }; + memcpy(tunnelPayload, payload, payloadLength); + + mavlink_msg_tunnel_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + targetSystem, + targetComponent, + MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP, + payloadLength, + tunnelPayload); + mavlinkSendMessage(); +} + +static void mavlinkSendTunnelMspReply(uint8_t targetSystem, uint8_t targetComponent, mspPacket_t *reply, uint8_t *replyPayloadHead, mspVersion_e mspVersion) +{ + sbufSwitchToReader(&reply->buf, replyPayloadHead); + + const int frameLength = mspSerialEncodePacket(reply, mspVersion, mavTunnelFrameBuf, sizeof(mavTunnelFrameBuf)); + for (int offset = 0; offset < frameLength; offset += MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { + const uint8_t chunkLength = MIN(MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN, frameLength - offset); + mavlinkSendTunnelReply( + targetSystem, + targetComponent, + mavTunnelFrameBuf + offset, + chunkLength); + } +} + +static bool handleIncoming_TUNNEL(uint8_t ingressPortIndex) +{ + if (mavlinkGetProtocolVersion() == 1) { + return false; + } + + mavlink_tunnel_t msg; + mavlink_msg_tunnel_decode(&mavRecvMsg, &msg); + + if (msg.payload_type != MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP) { + return false; + } + + if (msg.target_system != mavSystemId) { + return false; + } + + if (msg.target_component != 0 && msg.target_component != mavComponentId) { + return false; + } + + mspPort_t *mspPort = &mavTunnelMspPorts[ingressPortIndex]; + const timeMs_t now = millis(); + if (msg.payload_length > MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { + mavlinkResetTunnelState(ingressPortIndex); + return false; + } + + if (mspPort->c_state != MSP_IDLE && + ((now - mspPort->lastActivityMs) > MAVLINK_TUNNEL_MSP_TIMEOUT_MS || + mavTunnelRemoteSystemIds[ingressPortIndex] != mavRecvMsg.sysid || + mavTunnelRemoteComponentIds[ingressPortIndex] != mavRecvMsg.compid)) { + mavlinkResetTunnelState(ingressPortIndex); + } + + mavTunnelRemoteSystemIds[ingressPortIndex] = mavRecvMsg.sysid; + mavTunnelRemoteComponentIds[ingressPortIndex] = mavRecvMsg.compid; + mspPort->lastActivityMs = now; + + bool handled = false; + for (uint8_t payloadIndex = 0; payloadIndex < msg.payload_length; payloadIndex++) { + if (!mspSerialProcessReceivedByte(mspPort, msg.payload[payloadIndex])) { + continue; + } + + handled = true; + if (mspPort->c_state != MSP_COMMAND_RECEIVED) { + continue; + } + + mspPacket_t reply = { + .buf = { .ptr = mavTunnelReplyPayloadBuf, .end = ARRAYEND(mavTunnelReplyPayloadBuf), }, + .cmd = -1, + .flags = 0, + .result = 0, + }; + uint8_t *replyPayloadHead = reply.buf.ptr; + + if (mspPort->cmdMSP == MSP_SET_PASSTHROUGH) { + reply.cmd = MSP_SET_PASSTHROUGH; + reply.result = MSP_RESULT_ERROR; + mavlinkSendTunnelMspReply(mavRecvMsg.sysid, mavRecvMsg.compid, &reply, replyPayloadHead, mspPort->mspVersion); + mavlinkResetTunnelState(ingressPortIndex); + break; + } + + mspPostProcessFnPtr mspPostProcessFn = NULL; + const uint16_t command = mspPort->cmdMSP; + mspResult_e status = mspSerialProcessCommand(mspPort, mspFcProcessCommand, &reply, &mspPostProcessFn); + + if (mspPostProcessFn && command != MSP_REBOOT) { + sbufInit(&reply.buf, mavTunnelReplyPayloadBuf, ARRAYEND(mavTunnelReplyPayloadBuf)); + reply.result = MSP_RESULT_ERROR; + mspPostProcessFn = NULL; + status = MSP_RESULT_ERROR; + } + + if (status != MSP_RESULT_NO_REPLY) { + mavlinkSendTunnelMspReply(mavRecvMsg.sysid, mavRecvMsg.compid, &reply, replyPayloadHead, mspPort->mspVersion); + } + + mavlinkResetTunnelState(ingressPortIndex); + + if (mspPostProcessFn) { + waitForSerialPortToFinishTransmitting(mavPortStates[ingressPortIndex].port); + mspPostProcessFn(mavPortStates[ingressPortIndex].port); + } + + break; + } + + return handled; +} + static void mavlinkSendAutopilotVersion(void) { if (mavlinkGetProtocolVersion() == 1) return; @@ -3111,6 +3267,9 @@ static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) // Don't set that we handled a message, otherwise radio status packets will block telemetry messages. localHandled = false; break; + case MAVLINK_MSG_ID_TUNNEL: + localHandled = handleIncoming_TUNNEL(ingressPortIndex); + break; default: localHandled = false; break; diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index f377b24a80e..7debcc97758 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -44,7 +44,7 @@ set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial 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") + "telemetry/mavlink.c" "common/crc.c" "common/maths.c" "common/streambuf.c" "msp/msp_serial.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") diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index 641f31432d2..a60b72357b2 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -20,6 +20,7 @@ #include #include +#include extern "C" { #include "platform.h" @@ -28,6 +29,7 @@ extern "C" { #include "common/axis.h" #include "common/maths.h" + #include "common/streambuf.h" #include "common/time.h" #include "config/parameter_group.h" @@ -51,6 +53,10 @@ extern "C" { #include "io/gps.h" #include "io/osd.h" + #include "msp/msp.h" + #include "msp/msp_protocol.h" + #include "msp/msp_serial.h" + #include "navigation/navigation.h" #ifdef __cplusplus #define _Static_assert static_assert @@ -95,6 +101,17 @@ static size_t serialRxLen; static size_t serialRxPos; static size_t serialTxLen; static const uint8_t testTargetComponent = MAV_COMP_ID_AUTOPILOT1; +static const uint8_t testTunnelSourceSystem = 42; +static const uint8_t testTunnelSourceComponent = 200; +static const uint8_t testSimpleMspCommand = 90; +static const uint8_t testLargeReplyMspCommand = 91; +static const size_t testMspFrameBufSize = MSP_PORT_OUTBUF_SIZE + 16; +static timeMs_t fakeMillis; +static int mspCommandCallCount; +static int mspPassthroughDispatchCount; +static int mspRebootPostProcessCount; +static int waitForSerialPortToFinishTransmittingCalls; +static serialPort_t *lastPostProcessPort; const uint32_t baudRates[] = { 0, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 250000, @@ -124,6 +141,42 @@ static void resetSerialBuffers(void) serialTxLen = 0; } +static std::vector makeMspV1Request(uint8_t cmd, const std::vector &payload = {}) +{ + std::vector frame = { + '$', 'M', '<', + static_cast(payload.size()), + cmd + }; + uint8_t checksum = frame[3] ^ frame[4]; + for (const uint8_t byte : payload) { + frame.push_back(byte); + checksum ^= byte; + } + frame.push_back(checksum); + return frame; +} + +static std::vector encodeMspV1Reply(uint8_t cmd, int16_t result, const std::vector &payload = {}) +{ + uint8_t payloadBuf[MSP_PORT_OUTBUF_SIZE]; + mspPacket_t reply = { + .buf = { .ptr = payloadBuf, .end = ARRAYEND(payloadBuf), }, + .cmd = cmd, + .flags = 0, + .result = result, + }; + uint8_t *payloadHead = reply.buf.ptr; + if (!payload.empty()) { + sbufWriteData(&reply.buf, payload.data(), (int)payload.size()); + } + sbufSwitchToReader(&reply.buf, payloadHead); + + uint8_t frameBuf[testMspFrameBufSize]; + const int frameLength = mspSerialEncodePacket(&reply, MSP_V1, frameBuf, sizeof(frameBuf)); + return std::vector(frameBuf, frameBuf + frameLength); +} + static void pushRxMessage(const mavlink_message_t *msg) { uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; @@ -132,6 +185,30 @@ static void pushRxMessage(const mavlink_message_t *msg) serialRxLen += (size_t)length; } +static void pushTunnelPayload(uint8_t payloadLength, const std::vector &payload, uint8_t targetComponent = testTargetComponent) +{ + uint8_t tunnelPayload[MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN] = { 0 }; + size_t copyLength = payload.size(); + if (copyLength > MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { + copyLength = MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN; + } + if (copyLength > 0) { + memcpy(tunnelPayload, payload.data(), copyLength); + } + + mavlink_message_t msg; + mavlink_msg_tunnel_pack( + testTunnelSourceSystem, + testTunnelSourceComponent, + &msg, + telemetryConfig()->mavlink_common.sysid, + targetComponent, + 0x8001, + payloadLength, + tunnelPayload); + pushRxMessage(&msg); +} + static bool popTxMessage(mavlink_message_t *msg) { mavlink_status_t status; @@ -144,12 +221,54 @@ static bool popTxMessage(mavlink_message_t *msg) return false; } +static std::vector parseTxMessages(void) +{ + std::vector messages; + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + + mavlink_message_t msg; + for (size_t i = 0; i < serialTxLen; i++) { + if (mavlink_parse_char(0, serialTxBuffer[i], &msg, &status) == MAVLINK_FRAMING_OK) { + messages.push_back(msg); + } + } + + return messages; +} + +static std::vector collectTunnelPayload(const std::vector &messages) +{ + std::vector payload; + + for (const mavlink_message_t &msg : messages) { + EXPECT_EQ(msg.msgid, MAVLINK_MSG_ID_TUNNEL); + + mavlink_tunnel_t tunnel; + mavlink_msg_tunnel_decode(&msg, &tunnel); + + EXPECT_EQ(tunnel.payload_type, 0x8001); + EXPECT_EQ(tunnel.target_system, testTunnelSourceSystem); + EXPECT_EQ(tunnel.target_component, testTunnelSourceComponent); + + payload.insert(payload.end(), tunnel.payload, tunnel.payload + tunnel.payload_length); + } + + return payload; +} + static void initMavlinkTestState(void) { resetSerialBuffers(); + fakeMillis = 0; setWaypointCalls = 0; resetWaypointCalls = 0; mavlinkRxHandleCalls = 0; + mspCommandCallCount = 0; + mspPassthroughDispatchCount = 0; + mspRebootPostProcessCount = 0; + waitForSerialPortToFinishTransmittingCalls = 0; + lastPostProcessPort = NULL; gcsValid = true; waypointCount = 0; memset(estimatedPosition, 0, sizeof(estimatedPosition)); @@ -185,6 +304,99 @@ static void initMavlinkTestState(void) checkMAVLinkTelemetryState(); } +TEST(MavlinkTelemetryTest, TunnelMalformedPayloadLengthIsDroppedAndDoesNotPoisonState) +{ + initMavlinkTestState(); + + std::vector request = makeMspV1Request(testSimpleMspCommand); + request.resize(MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN, 0); + + pushTunnelPayload(MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN + 1, request); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 0); + EXPECT_TRUE(parseTxMessages().empty()); + + resetSerialBuffers(); + pushTunnelPayload((uint8_t)makeMspV1Request(testSimpleMspCommand).size(), makeMspV1Request(testSimpleMspCommand)); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 1); + EXPECT_EQ(collectTunnelPayload(parseTxMessages()), encodeMspV1Reply(testSimpleMspCommand, MSP_RESULT_ACK)); +} + +TEST(MavlinkTelemetryTest, TunnelRejectsPassthroughBeforeDispatch) +{ + initMavlinkTestState(); + + const std::vector request = makeMspV1Request((uint8_t)MSP_SET_PASSTHROUGH); + + pushTunnelPayload((uint8_t)request.size(), request); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 0); + EXPECT_EQ(mspPassthroughDispatchCount, 0); + EXPECT_EQ(waitForSerialPortToFinishTransmittingCalls, 0); + EXPECT_EQ(mspRebootPostProcessCount, 0); + EXPECT_EQ(collectTunnelPayload(parseTxMessages()), encodeMspV1Reply((uint8_t)MSP_SET_PASSTHROUGH, MSP_RESULT_ERROR)); +} + +TEST(MavlinkTelemetryTest, TunnelRebootUsesIngressPortForPostProcess) +{ + initMavlinkTestState(); + + const std::vector request = makeMspV1Request((uint8_t)MSP_REBOOT); + + pushTunnelPayload((uint8_t)request.size(), request); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 1); + EXPECT_EQ(waitForSerialPortToFinishTransmittingCalls, 1); + EXPECT_EQ(mspRebootPostProcessCount, 1); + EXPECT_EQ(lastPostProcessPort, &testSerialPort); + EXPECT_EQ(collectTunnelPayload(parseTxMessages()), encodeMspV1Reply((uint8_t)MSP_REBOOT, MSP_RESULT_ACK)); +} + +TEST(MavlinkTelemetryTest, TunnelStalePartialFrameResetsBeforeNextRequest) +{ + initMavlinkTestState(); + + pushTunnelPayload(3, {'$', 'M', '<'}); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 0); + EXPECT_TRUE(parseTxMessages().empty()); + + resetSerialBuffers(); + fakeMillis = 2000; + + const std::vector request = makeMspV1Request(testSimpleMspCommand); + pushTunnelPayload((uint8_t)request.size(), request); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 1); + EXPECT_EQ(collectTunnelPayload(parseTxMessages()), encodeMspV1Reply(testSimpleMspCommand, MSP_RESULT_ACK)); +} + +TEST(MavlinkTelemetryTest, TunnelLargeReplyFragmentsAcrossMultipleMessages) +{ + initMavlinkTestState(); + + const std::vector request = makeMspV1Request(testLargeReplyMspCommand); + pushTunnelPayload((uint8_t)request.size(), request); + handleMAVLinkTelemetry(1000); + + const std::vector messages = parseTxMessages(); + ASSERT_EQ(messages.size(), 3U); + + std::vector expectedPayload(300); + for (size_t i = 0; i < expectedPayload.size(); i++) { + expectedPayload[i] = (uint8_t)i; + } + + EXPECT_EQ(collectTunnelPayload(messages), encodeMspV1Reply(testLargeReplyMspCommand, MSP_RESULT_ACK, expectedPayload)); +} + TEST(MavlinkTelemetryTest, AttitudeUsesRadiansPerSecond) { initMavlinkTestState(); @@ -1101,6 +1313,7 @@ int32_t debug[DEBUG32_VALUE_COUNT]; uint32_t armingFlags; uint32_t stateFlags; +bool cliMode; attitudeEulerAngles_t attitude; gyro_t gyro; @@ -1119,7 +1332,7 @@ timeUs_t micros(void) uint32_t millis(void) { - return 0; + return fakeMillis; } serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) @@ -1216,6 +1429,29 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) return true; } +bool serialIsConnected(const serialPort_t *instance) +{ + UNUSED(instance); + return true; +} + +bool isSerialTransmitBufferEmpty(const serialPort_t *instance) +{ + UNUSED(instance); + return true; +} + +void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort) +{ + waitForSerialPortToFinishTransmittingCalls++; + lastPostProcessPort = serialPort; +} + +void cliEnter(serialPort_t *serialPort) +{ + UNUSED(serialPort); +} + bool sensors(uint32_t mask) { return (testSensorsMask & mask) != 0; @@ -1386,6 +1622,16 @@ bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int3 return altitudeTargetSetResult; } +navigationFSMStateFlags_t navGetCurrentStateFlags(void) +{ + return (navigationFSMStateFlags_t)0; +} + +void updateHeadingHoldTarget(int16_t heading) +{ + UNUSED(heading); +} + void setWaypoint(uint8_t wpNumber, const navWaypoint_t *wp) { UNUSED(wpNumber); @@ -1445,6 +1691,41 @@ void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg) mavlinkRxHandleCalls++; } +static void testMspRebootPostProcess(serialPort_t *serialPort) +{ + mspRebootPostProcessCount++; + lastPostProcessPort = serialPort; +} + +mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +{ + mspCommandCallCount++; + reply->cmd = cmd->cmd; + reply->flags = 0; + reply->result = 0; + + switch (cmd->cmd) { + case MSP_SET_PASSTHROUGH: + mspPassthroughDispatchCount++; + if (mspPostProcessFn) { + *mspPostProcessFn = testMspRebootPostProcess; + } + return MSP_RESULT_ACK; + case MSP_REBOOT: + if (mspPostProcessFn) { + *mspPostProcessFn = testMspRebootPostProcess; + } + return MSP_RESULT_ACK; + case testLargeReplyMspCommand: + for (uint16_t i = 0; i < 300; i++) { + sbufWriteU8(&reply->buf, (uint8_t)i); + } + return MSP_RESULT_ACK; + default: + return MSP_RESULT_ACK; + } +} + adsbVehicleValues_t* getVehicleForFill(void) { return NULL; diff --git a/src/utils/mavlink_tests/ROUTING_TEST.md b/src/utils/mavlink_tests/ROUTING_TEST.md deleted file mode 100644 index 7ed080083d4..00000000000 --- a/src/utils/mavlink_tests/ROUTING_TEST.md +++ /dev/null @@ -1,33 +0,0 @@ -# MAVLink Routing Compliance Test - -- fc_system_id: `1` -- links: `['link_camera', 'link_companion', 'link_gimbal', 'link_radio']` -- gcs_link: `link_radio` -- components: `[('mav2_companion_computer', 1, 191, 'link_companion'), ('mav1_elrs_receiver', 1, 68, 'link_radio'), ('mav3_camera', 1, 100, 'link_camera'), ('mav4_gimbal', 1, 154, 'link_gimbal')]` - -## Rule References - -- broadcast_forward: `routing.md:25-27,48-49` -- known_target_forward: `routing.md:49-50; MAVLink_routing.cpp:66-80` -- unknown_target_blocked: `routing.md:26-27,53` -- no_ingress_loop: `MAVLink_routing.cpp:197-209` -- no_repack: `routing.md:29-31` - -## Results - -| Case | Source(link) | Target(sys,comp) | Expected Links | Observed Links | Routing | No Ingress Loop | Payload Intact | Pass | -| --- | --- | --- | --- | --- | --- | --- | --- | --- | -| `gcs_broadcast` | `gcs (link_radio)` | `(0,0)` | `['link_camera', 'link_companion', 'link_gimbal']` | `['link_camera', 'link_companion', 'link_gimbal']` | `True` | `True` | `True` | `True` | -| `gcs_target_local_system_component_broadcast` | `gcs (link_radio)` | `(1,0)` | `['link_camera', 'link_companion', 'link_gimbal']` | `['link_camera', 'link_companion', 'link_gimbal']` | `True` | `True` | `True` | `True` | -| `gcs_target_mav2_companion_computer` | `gcs (link_radio)` | `(1,191)` | `['link_companion']` | `['link_companion']` | `True` | `True` | `True` | `True` | -| `gcs_target_mav1_elrs_receiver` | `gcs (link_radio)` | `(1,68)` | `[]` | `[]` | `True` | `True` | `True` | `True` | -| `gcs_target_mav3_camera` | `gcs (link_radio)` | `(1,100)` | `['link_camera']` | `['link_camera']` | `True` | `True` | `True` | `True` | -| `gcs_target_mav4_gimbal` | `gcs (link_radio)` | `(1,154)` | `['link_gimbal']` | `['link_gimbal']` | `True` | `True` | `True` | `True` | -| `gcs_target_unknown_component` | `gcs (link_radio)` | `(1,199)` | `[]` | `[]` | `True` | `True` | `True` | `True` | -| `gcs_target_unknown_system` | `gcs (link_radio)` | `(42,1)` | `[]` | `[]` | `True` | `True` | `True` | `True` | -| `mav2_companion_computer_broadcast` | `mav2_companion_computer (link_companion)` | `(0,0)` | `['link_camera', 'link_gimbal', 'link_radio']` | `['link_camera', 'link_gimbal', 'link_radio']` | `True` | `True` | `True` | `True` | -| `mav2_companion_computer_to_mav1_elrs_receiver` | `mav2_companion_computer (link_companion)` | `(1,68)` | `['link_radio']` | `['link_radio']` | `True` | `True` | `True` | `True` | -| `mav2_companion_computer_to_gcs` | `mav2_companion_computer (link_companion)` | `(1,68)` | `['link_radio']` | `['link_radio']` | `True` | `True` | `True` | `True` | - -summary pass_count=11 fail_count=0 total=11 - diff --git a/src/utils/mavlink_tests/mav_multi_benchmark.py b/src/utils/mavlink_tests/mav_multi_benchmark.py deleted file mode 100644 index bb74da85779..00000000000 --- a/src/utils/mavlink_tests/mav_multi_benchmark.py +++ /dev/null @@ -1,880 +0,0 @@ -#!/usr/bin/env python3 -""" -Usage: - conda run -n drone python mydev/branch/mav_multi/mav_multi_benchmark.py - conda run -n drone python mydev/branch/mav_multi/mav_multi_benchmark.py --config mydev/branch/mav_multi/test_config.yaml -""" - -from __future__ import annotations - -import argparse -import asyncio -import socket -import subprocess -import time -from datetime import datetime, timezone -from pathlib import Path -from typing import Any, Dict, List, Tuple - -import yaml -from mavsdk import System -from pymavlink import mavutil -from serial.serialutil import SerialException - -try: - from mspapi2.msp_api import MSPApi - from mspapi2.lib import InavMSP -except ModuleNotFoundError: - repo_root_guess = Path(__file__).resolve().parents[3] - mspapi2_repo = repo_root_guess.parent / "mspapi2" - if mspapi2_repo.exists(): - import sys - - sys.path.insert(0, str(mspapi2_repo)) - from mspapi2.msp_api import MSPApi - from mspapi2.lib import InavMSP - - -DEFAULT_CONFIG_PATH = Path("mydev/branch/mav_multi/test_config.yaml") -CHANNELS = [1500, 1500, 900, 1500] + [900] * 14 -MAV_CMD_REQUEST_MESSAGE = int(mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE) -MAV_CMD_SET_MESSAGE_INTERVAL = int(mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL) -MAVLINK_MSG_ID_HEARTBEAT = int(mavutil.mavlink.MAVLINK_MSG_ID_HEARTBEAT) -MAV_AUTOPILOT_INVALID = int(mavutil.mavlink.MAV_AUTOPILOT_INVALID) -MAV_TYPE_GCS = int(mavutil.mavlink.MAV_TYPE_GCS) - - -def load_config(config_path: Path) -> Dict[str, Any]: - return yaml.safe_load(config_path.read_text(encoding="utf-8")) - - -def wait_for_tcp_port(host: str, port: int, timeout_s: float) -> None: - deadline = time.monotonic() + timeout_s - while time.monotonic() < deadline: - try: - with socket.create_connection((host, port), timeout=1.0): - return - except ConnectionRefusedError: - time.sleep(0.2) - continue - except socket.timeout: - time.sleep(0.2) - continue - raise TimeoutError(f"TCP port {host}:{port} did not become available within {timeout_s}s") - - -def wait_for_tcp_port_cycle(host: str, port: int, timeout_s: float) -> None: - deadline = time.monotonic() + timeout_s - saw_closed = False - while time.monotonic() < deadline: - try: - with socket.create_connection((host, port), timeout=1.0): - if saw_closed: - return - except (ConnectionRefusedError, socket.timeout, OSError): - saw_closed = True - time.sleep(0.2) - - if saw_closed: - raise TimeoutError(f"TCP port {host}:{port} did not return after reboot within {timeout_s}s") - - # If no close window was observed, treat it as already rebooted and just ensure the port is reachable. - wait_for_tcp_port(host, port, timeout_s) - - -def cli_read_until_prompt(cli_socket: socket.socket) -> str: - data = b"" - while b"\n# " not in data: - chunk = cli_socket.recv(65536) - if not chunk: - raise ConnectionError("CLI socket closed before prompt") - data += chunk - return data.decode("utf-8", errors="replace") - - -def wait_for_cli_ready(host: str, port: int, timeout_s: float) -> None: - deadline = time.monotonic() + timeout_s - while time.monotonic() < deadline: - try: - with socket.create_connection((host, port), timeout=1.0) as cli_socket: - cli_socket.settimeout(1.0) - cli_socket.sendall(b"#\n") - cli_read_until_prompt(cli_socket) - return - except (ConnectionRefusedError, ConnectionError, socket.timeout, OSError): - time.sleep(0.2) - raise TimeoutError(f"CLI prompt did not become available on {host}:{port} within {timeout_s}s") - - -def run_cli_commands(host: str, port: int, commands: List[str], reboot_timeout_s: float) -> bool: - sent_save = False - with socket.create_connection((host, port), timeout=5.0) as cli_socket: - cli_socket.settimeout(3.0) - cli_socket.sendall(b"#\n") - cli_read_until_prompt(cli_socket) - for command in commands: - cli_socket.sendall(command.encode("utf-8") + b"\n") - if command == "save": - sent_save = True - break - cli_read_until_prompt(cli_socket) - if sent_save: - # Save triggers reboot; do not send '#' again because that re-enters CLI mode. - time.sleep(min(reboot_timeout_s, 1.0)) - return sent_save - - -def build_serial_command(index: int, function_mask: int, baud: int) -> str: - return f"serial {index} {function_mask} {baud} {baud} 0 {baud}" - - -def build_cli_config_commands(config: Dict[str, Any], mavlink_port_count: int) -> List[str]: - cli_cfg = config["cli"] - rc_baud = int(cli_cfg["rc_baud"]) - telemetry_baud = int(cli_cfg["telemetry_baud"]) - uart2_mask = 320 # FUNCTION_RX_SERIAL + FUNCTION_TELEMETRY_MAVLINK - uart3_mask = 256 if mavlink_port_count >= 2 else 0 # FUNCTION_TELEMETRY_MAVLINK - uart4_mask = 256 if mavlink_port_count >= 3 else 0 # FUNCTION_TELEMETRY_MAVLINK - uart5_mask = 256 if mavlink_port_count >= 4 else 0 # FUNCTION_TELEMETRY_MAVLINK - commands = [ - "feature TELEMETRY", - "set receiver_type = SERIAL", - "set serialrx_provider = MAVLINK", - build_serial_command(0, 1, 115200), # MSP on UART1, always. - build_serial_command(1, uart2_mask, rc_baud), - build_serial_command(2, uart3_mask, telemetry_baud), - build_serial_command(3, uart4_mask, telemetry_baud), - build_serial_command(4, uart5_mask, telemetry_baud), - "set mavlink_version = 2", - "set mavlink_port1_compid = 1", - "set mavlink_port2_compid = 2", - "set mavlink_port3_compid = 3", - "set mavlink_port4_compid = 4", - "set mavlink_port1_high_latency = OFF", - "set mavlink_port2_high_latency = OFF", - "set mavlink_port3_high_latency = OFF", - "set mavlink_port4_high_latency = OFF", - "save", - ] - return commands - - -def apply_config_and_wait(config: Dict[str, Any], mavlink_port_count: int) -> None: - ports = config["ports"] - commands = build_cli_config_commands(config, mavlink_port_count) - sent_save = run_cli_commands( - "127.0.0.1", - int(ports["msp"]), - commands, - reboot_timeout_s=float(config["tests"]["save_reboot_timeout_s"]), - ) - if sent_save: - wait_for_tcp_port_cycle("127.0.0.1", int(ports["msp"]), float(config["tests"]["save_reboot_timeout_s"])) - wait_for_tcp_port("127.0.0.1", int(ports["msp"]), float(config["tests"]["port_ready_timeout_s"])) - wait_for_tcp_port("127.0.0.1", int(ports["rc"]), float(config["tests"]["port_ready_timeout_s"])) - if mavlink_port_count >= 2: - wait_for_tcp_port("127.0.0.1", int(ports["telemetry"][0]), float(config["tests"]["port_ready_timeout_s"])) - if mavlink_port_count >= 3: - wait_for_tcp_port("127.0.0.1", int(ports["telemetry"][1]), float(config["tests"]["port_ready_timeout_s"])) - if mavlink_port_count >= 4: - wait_for_tcp_port("127.0.0.1", int(ports["telemetry"][2]), float(config["tests"]["port_ready_timeout_s"])) - - -async def mavsdk_probe(address: str, timeout_s: float) -> Dict[str, Any]: - drone = System() - result: Dict[str, Any] = { - "address": address, - "connected": False, - "flight_mode": None, - } - await drone.connect(system_address=address) - async with asyncio.timeout(timeout_s): - async for state in drone.core.connection_state(): - if state.is_connected: - result["connected"] = True - break - async with asyncio.timeout(timeout_s): - async for flight_mode in drone.telemetry.flight_mode(): - result["flight_mode"] = str(flight_mode) - break - return result - - -def probe_mavsdk_sync(address: str, timeout_s: float) -> Dict[str, Any]: - return asyncio.run(mavsdk_probe(address, timeout_s)) - - -def open_mavlink_tcp_connection(port: int, source_system: int, source_component: int, timeout_s: float) -> Any: - deadline = time.monotonic() + timeout_s - while True: - try: - return mavutil.mavlink_connection( - f"tcp:127.0.0.1:{port}", - source_system=source_system, - source_component=source_component, - autoreconnect=True, - ) - except OSError: - if time.monotonic() >= deadline: - raise - time.sleep(0.2) - - -def is_fc_heartbeat(msg: Any) -> bool: - return int(msg.autopilot) != MAV_AUTOPILOT_INVALID and int(msg.type) != MAV_TYPE_GCS - - -def run_workload( - config: Dict[str, Any], - sitl_pid: int, - telemetry_ports: List[int], - rc_port: int, - load_rates_hz: Dict[int, float], - duration_s: float, - rc_tx_hz_override: float | None = None, -) -> Dict[str, Any]: - print( - f"run_workload_start duration_s={duration_s} telemetry_ports={telemetry_ports} rc_port={rc_port} " - f"load_rates_hz={load_rates_hz}", - flush=True, - ) - tests_cfg = config["tests"] - rc_tx_hz = float(tests_cfg["rc_tx_hz"]) if rc_tx_hz_override is None else float(rc_tx_hz_override) - msp_poll_hz = float(tests_cfg["msp_poll_hz"]) - inav_status_hz = float(tests_cfg.get("inav_status_hz", 1.0)) - msp_request_timeout_s = float(tests_cfg["msp_request_timeout_s"]) - command_message_id = int(tests_cfg["stress_command_message_id"]) - rc_target_system = int(tests_cfg["rc_target_system"]) - rc_target_component = int(tests_cfg["rc_target_component"]) - heartbeat_expected_hz = float(tests_cfg["heartbeat_expected_hz"]) - stream_cfg = config["cli"] - port_ready_timeout_s = float(tests_cfg["port_ready_timeout_s"]) - warmup_s = float(tests_cfg.get("warmup_s", 3.0)) - warmup_heartbeat_count = int(tests_cfg.get("warmup_heartbeat_count", 3)) - warmup_timeout_s = float(tests_cfg.get("warmup_timeout_s", 30.0)) - - listeners: Dict[int, Any] = {} - targets: Dict[int, Tuple[int, int]] = {} - port_stats: Dict[int, Dict[str, Any]] = {} - load_senders: Dict[int, Any] = {} - load_sent: Dict[int, int] = {} - load_next_send: Dict[int, float] = {} - msp_success = 0 - msp_fail = 0 - rc_sent = 0 - fc_cpu_load_samples: List[float] = [] - fc_cycle_time_samples: List[float] = [] - fc_status_fail = 0 - fc_status_last_error = "" - - source_base = 170 - ports_to_open = set(telemetry_ports) - ports_to_open.update(load_rates_hz.keys()) - ports_to_open.add(rc_port) - for port in sorted(ports_to_open): - wait_for_tcp_port("127.0.0.1", int(port), port_ready_timeout_s) - - for idx, port in enumerate(telemetry_ports): - conn = open_mavlink_tcp_connection( - port=port, - source_system=source_base + idx, - source_component=190, - timeout_s=port_ready_timeout_s, - ) - listeners[port] = conn - targets[port] = (rc_target_system, rc_target_component) - port_stats[port] = { - "heartbeat_count": 0, - "rc_count": 0, - "command_ack_total": 0, - "command_ack_ok": 0, - "mav_msg_count": 0, - "mav_seq_lost": 0, - "last_seq_by_source": {}, - "seq_track_source": None, - } - - for idx, port in enumerate(sorted(load_rates_hz.keys())): - if port in listeners: - sender = listeners[port] - else: - sender = open_mavlink_tcp_connection( - port=port, - source_system=210 + idx, - source_component=191, - timeout_s=port_ready_timeout_s, - ) - load_senders[port] = sender - load_sent[port] = 0 - load_next_send[port] = time.monotonic() - if port not in targets: - targets[port] = (rc_target_system, rc_target_component) - - if rc_port in listeners: - rc_sender = listeners[rc_port] - else: - rc_sender = open_mavlink_tcp_connection( - port=rc_port, - source_system=239, - source_component=191, - timeout_s=port_ready_timeout_s, - ) - if telemetry_ports: - telemetry_target = targets[telemetry_ports[0]] - rc_target_system, rc_target_component = telemetry_target - - rc_period_s = 1.0 / rc_tx_hz - next_rc_send_t = time.monotonic() - heartbeat_period_s = 1.0 - next_heartbeat_send_t = next_rc_send_t - - warmup_hb_seen: Dict[int, int] = {port: 0 for port in telemetry_ports} - warmup_start_t = time.monotonic() - warmup_deadline_t = warmup_start_t + warmup_timeout_s - - while True: - now = time.monotonic() - - if now >= next_heartbeat_send_t: - heartbeat_conns: Dict[int, Any] = {} - for conn in listeners.values(): - heartbeat_conns[id(conn)] = conn - for conn in load_senders.values(): - heartbeat_conns[id(conn)] = conn - heartbeat_conns[id(rc_sender)] = rc_sender - for conn in heartbeat_conns.values(): - conn.mav.heartbeat_send( - mavutil.mavlink.MAV_TYPE_GCS, - mavutil.mavlink.MAV_AUTOPILOT_INVALID, - 0, - 0, - 0, - ) - next_heartbeat_send_t += heartbeat_period_s - if next_heartbeat_send_t < now: - next_heartbeat_send_t = now + heartbeat_period_s - - if now >= next_rc_send_t: - rc_target = targets.get(rc_port) - if rc_target is not None: - rc_target_system, rc_target_component = rc_target - rc_sender.mav.rc_channels_override_send(rc_target_system, rc_target_component, *CHANNELS[:8]) - next_rc_send_t += rc_period_s - if next_rc_send_t < now: - next_rc_send_t = now + rc_period_s - - for port, conn in listeners.items(): - while True: - msg = conn.recv_match(blocking=False) - if msg is None: - break - msg_type = msg.get_type() - if msg_type == "HEARTBEAT": - src_system = int(msg.get_srcSystem()) - src_component = int(msg.get_srcComponent()) - if is_fc_heartbeat(msg): - targets[port] = (src_system, src_component) - warmup_hb_seen[port] += 1 - - if telemetry_ports and all(count >= warmup_heartbeat_count for count in warmup_hb_seen.values()) and (now - warmup_start_t) >= warmup_s: - break - if (not telemetry_ports) and (now - warmup_start_t) >= warmup_s: - break - if now >= warmup_deadline_t: - raise TimeoutError( - f"Warmup failed: heartbeat_counts={warmup_hb_seen} required={warmup_heartbeat_count} timeout_s={warmup_timeout_s}" - ) - - time.sleep(0.001) - - for port, conn in listeners.items(): - target_system, target_component = targets[port] - port_stats[port]["seq_track_source"] = (target_system, target_component) - conn.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS), - int(stream_cfg["ext_status_rate_hz"]), - 1 if int(stream_cfg["ext_status_rate_hz"]) > 0 else 0, - ) - conn.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS), - int(stream_cfg["rc_chan_rate_hz"]), - 1 if int(stream_cfg["rc_chan_rate_hz"]) > 0 else 0, - ) - conn.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_POSITION), - int(stream_cfg["pos_rate_hz"]), - 1 if int(stream_cfg["pos_rate_hz"]) > 0 else 0, - ) - conn.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA1), - int(stream_cfg["extra1_rate_hz"]), - 1 if int(stream_cfg["extra1_rate_hz"]) > 0 else 0, - ) - conn.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA2), - int(stream_cfg["extra2_rate_hz"]), - 1 if int(stream_cfg["extra2_rate_hz"]) > 0 else 0, - ) - conn.mav.request_data_stream_send( - target_system, - target_component, - int(mavutil.mavlink.MAV_DATA_STREAM_EXTRA3), - int(stream_cfg["extra3_rate_hz"]), - 1 if int(stream_cfg["extra3_rate_hz"]) > 0 else 0, - ) - conn.mav.command_long_send( - target_system, - target_component, - MAV_CMD_SET_MESSAGE_INTERVAL, - 0, - float(MAVLINK_MSG_ID_HEARTBEAT), - float(int(1_000_000.0 / heartbeat_expected_hz)), - 0, - 0, - 0, - 0, - 0, - ) - time.sleep(0.1) - - for stats in port_stats.values(): - stats["heartbeat_count"] = 0 - stats["rc_count"] = 0 - stats["command_ack_total"] = 0 - stats["command_ack_ok"] = 0 - stats["mav_msg_count"] = 0 - stats["mav_seq_lost"] = 0 - stats["last_seq_by_source"] = {} - - rc_sent = 0 - measurement_start_t = time.monotonic() - next_rc_send_t = measurement_start_t - next_heartbeat_send_t = measurement_start_t - for port in load_next_send: - load_next_send[port] = measurement_start_t - next_resource_sample_t = measurement_start_t + (1.0 / inav_status_hz) - next_msp_poll_t = measurement_start_t + (1.0 / msp_poll_hz) - workload_end_t = measurement_start_t + duration_s - - with MSPApi(tcp_endpoint=f"127.0.0.1:{int(config['ports']['msp'])}") as msp_api: - msp_api._serial._max_retries = 1 - msp_api._serial._reconnect_delay = 0.05 - while time.monotonic() < workload_end_t: - now = time.monotonic() - - if now >= next_heartbeat_send_t: - heartbeat_conns: Dict[int, Any] = {} - for conn in listeners.values(): - heartbeat_conns[id(conn)] = conn - for conn in load_senders.values(): - heartbeat_conns[id(conn)] = conn - heartbeat_conns[id(rc_sender)] = rc_sender - for conn in heartbeat_conns.values(): - conn.mav.heartbeat_send( - mavutil.mavlink.MAV_TYPE_GCS, - mavutil.mavlink.MAV_AUTOPILOT_INVALID, - 0, - 0, - 0, - ) - next_heartbeat_send_t += heartbeat_period_s - if next_heartbeat_send_t < now: - next_heartbeat_send_t = now + heartbeat_period_s - - if now >= next_rc_send_t: - rc_target = targets.get(rc_port) - if rc_target is not None: - rc_target_system, rc_target_component = rc_target - rc_sender.mav.rc_channels_override_send(rc_target_system, rc_target_component, *CHANNELS[:8]) - rc_sent += 1 - next_rc_send_t += rc_period_s - if next_rc_send_t < now: - next_rc_send_t = now + rc_period_s - - for port, rate_hz in load_rates_hz.items(): - next_send_t = load_next_send[port] - if now >= next_send_t: - target_system, target_component = targets[port] - load_senders[port].mav.command_long_send( - target_system, - target_component, - MAV_CMD_REQUEST_MESSAGE, - 0, - float(command_message_id), - 0, - 0, - 0, - 0, - 0, - 0, - ) - load_sent[port] += 1 - next_send_t += 1.0 / rate_hz - if next_send_t < now: - next_send_t = now + (1.0 / rate_hz) - load_next_send[port] = next_send_t - - for port, conn in listeners.items(): - while True: - msg = conn.recv_match(blocking=False) - if msg is None: - break - msg_type = msg.get_type() - stats = port_stats[port] - seq = None - if hasattr(msg, "get_seq"): - try: - seq = int(msg.get_seq()) - except (TypeError, ValueError): - seq = None - if seq is None: - header = getattr(msg, "_header", None) - if header is not None and hasattr(header, "seq"): - seq = int(header.seq) - if seq is not None: - src_key = (int(msg.get_srcSystem()), int(msg.get_srcComponent())) - if src_key == stats["seq_track_source"]: - last_seq = stats["last_seq_by_source"].get(src_key) - if last_seq is not None: - seq_delta = (seq - int(last_seq)) & 0xFF - if seq_delta > 0: - stats["mav_seq_lost"] += max(seq_delta - 1, 0) - stats["last_seq_by_source"][src_key] = seq - stats["mav_msg_count"] += 1 - - if msg_type == "HEARTBEAT": - src_system = int(msg.get_srcSystem()) - src_component = int(msg.get_srcComponent()) - if is_fc_heartbeat(msg): - targets[port] = (src_system, src_component) - stats["heartbeat_count"] += 1 - elif msg_type == "RC_CHANNELS": - stats["rc_count"] += 1 - elif msg_type == "COMMAND_ACK": - if int(msg.command) == MAV_CMD_REQUEST_MESSAGE: - stats["command_ack_total"] += 1 - if int(msg.result) == int(mavutil.mavlink.MAV_RESULT_ACCEPTED): - stats["command_ack_ok"] += 1 - - if now >= next_msp_poll_t: - try: - msp_api._request_raw(InavMSP.MSP_RC, timeout=msp_request_timeout_s) - msp_success += 1 - except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError, OSError): - msp_fail += 1 - next_msp_poll_t += 1.0 / msp_poll_hz - - if now >= next_resource_sample_t: - try: - status = msp_api.get_inav_status() - fc_cpu_load_samples.append(float(status["cpuLoad"])) - fc_cycle_time_samples.append(float(status["cycleTime"])) - except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError, OSError, KeyError, ValueError) as exc: - fc_status_fail += 1 - fc_status_last_error = str(exc) - next_resource_sample_t += 1.0 / inav_status_hz - - time.sleep(0.001) - - all_connections: Dict[int, Any] = {} - for conn in listeners.values(): - all_connections[id(conn)] = conn - for conn in load_senders.values(): - all_connections[id(conn)] = conn - all_connections[id(rc_sender)] = rc_sender - for conn in all_connections.values(): - conn.close() - - if not fc_cpu_load_samples: - raise RuntimeError(f"No MSP2_INAV_STATUS samples collected; last_error={fc_status_last_error} fail_count={fc_status_fail}") - - summarized_ports: Dict[int, Dict[str, Any]] = {} - for port in telemetry_ports: - stats = port_stats[port] - mav_total = int(stats["mav_msg_count"]) - mav_lost = int(stats["mav_seq_lost"]) - mav_expected = mav_total + mav_lost - sent = load_sent.get(port, 0) - ack_total = stats["command_ack_total"] - ack_ok = stats["command_ack_ok"] - ack_missing = max(sent - ack_total, 0) - command_failed_total = max(sent - ack_ok, 0) - summarized_ports[port] = { - "heartbeat_count": stats["heartbeat_count"], - "rc_count": stats["rc_count"], - "mav_msg_count": mav_total, - "mav_seq_lost": mav_lost, - "mav_seq_loss_rate": (mav_lost / mav_expected) if mav_expected > 0 else 0.0, - "command_sent": sent, - "command_ack_total": ack_total, - "command_ack_ok": ack_ok, - "command_failed_total": command_failed_total, - "command_ack_missing": ack_missing, - "command_ack_failure_rate": (command_failed_total / sent) if sent > 0 else 0.0, - } - - result = { - "duration_s": duration_s, - "ports": summarized_ports, - "msp_success": msp_success, - "msp_fail": msp_fail, - "rc_sent": rc_sent, - "rc_sent_hz": (rc_sent / max(duration_s, 1e-6)), - "msp_failure_rate": (msp_fail / max(msp_success + msp_fail, 1)), - "fc_cpu_load_avg_pct": (sum(fc_cpu_load_samples) / len(fc_cpu_load_samples)) if fc_cpu_load_samples else 0.0, - "fc_cpu_load_max_pct": max(fc_cpu_load_samples) if fc_cpu_load_samples else 0.0, - "fc_cycle_time_avg_us": (sum(fc_cycle_time_samples) / len(fc_cycle_time_samples)) if fc_cycle_time_samples else 0.0, - "fc_cycle_time_max_us": max(fc_cycle_time_samples) if fc_cycle_time_samples else 0.0, - "fc_status_samples": len(fc_cpu_load_samples), - } - print( - f"run_workload_done duration_s={duration_s} fc_cpu_load_avg_pct={result['fc_cpu_load_avg_pct']:.2f} " - f"msp_fail_rate={result['msp_failure_rate']:.4f}", - flush=True, - ) - return result - - -def format_port_table(port_result: Dict[int, Dict[str, Any]]) -> str: - if not port_result: - return "_No MAVLink telemetry ports active in this scenario._" - - lines = [ - "| Port | MAV Msg Count | MAV Seq Lost | MAV Seq Loss % | HB Count | RC_CHANNELS Count | Cmd Sent | Cmd Ack OK | Cmd Failed Total | Cmd Missing Ack |", - "| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |", - ] - for port in sorted(port_result.keys()): - data = port_result[port] - lines.append( - "| " - f"{port} | {data['mav_msg_count']} | {data['mav_seq_lost']} | {(data['mav_seq_loss_rate'] * 100.0):.2f}% | " - f"{data['heartbeat_count']} | {data['rc_count']} | {data['command_sent']} | " - f"{data['command_ack_ok']} | {data['command_failed_total']} | {data['command_ack_missing']} |" - ) - return "\n".join(lines) - - -def summarize_command_failures(port_result: Dict[int, Dict[str, Any]]) -> Dict[str, float]: - sent_total = sum(int(item["command_sent"]) for item in port_result.values()) - failed_total = sum(int(item["command_failed_total"]) for item in port_result.values()) - missing_ack_total = sum(int(item["command_ack_missing"]) for item in port_result.values()) - mav_seq_lost_total = sum(int(item["mav_seq_lost"]) for item in port_result.values()) - mav_msg_total = sum(int(item["mav_msg_count"]) for item in port_result.values()) - mav_seq_loss_pct = (mav_seq_lost_total / max(mav_seq_lost_total + mav_msg_total, 1)) * 100.0 - return { - "sent_total": float(sent_total), - "failed_total": float(failed_total), - "failed_pct": (failed_total / max(sent_total, 1)) * 100.0, - "missing_ack_total": float(missing_ack_total), - "mav_seq_loss_pct": mav_seq_loss_pct, - } - - -def write_testing_report(config: Dict[str, Any], report: Dict[str, Any]) -> None: - out_path = Path(config["output"]["testing_md"]) - timestamp = datetime.now(timezone.utc).isoformat() - lines: List[str] = [] - lines.append("# MAVLink Multiport Testing") - lines.append("") - lines.append(f"- Generated: `{timestamp}`") - lines.append(f"- Conda env: `drone`") - lines.append(f"- SITL binary: `{config['sitl']['binary']}`") - lines.append(f"- EEPROM: `{config['sitl']['eeprom_path']}`") - lines.append("- MSP kept on UART1 by design (`serial 0 ... functionMask=1`).") - lines.append("- CLI under test sets `receiver_type=SERIAL` and `serialrx_provider=MAVLINK`.") - lines.append("- Stream rates are configured at runtime via MAVLink `REQUEST_DATA_STREAM` and `MAV_CMD_SET_MESSAGE_INTERVAL`.") - lines.append("- `RC_CHANNELS` metrics below are MAVLink telemetry stream metrics, not RX input ownership.") - lines.append("") - lines.append("## MAVSDK Probe") - lines.append("") - lines.append("| Address | Connected | Flight Mode |") - lines.append("| --- | --- | --- |") - if report["mavsdk_probes"]: - for probe in report["mavsdk_probes"]: - lines.append(f"| {probe['address']} | {probe['connected']} | {probe['flight_mode']} |") - else: - lines.append("| _skipped_ | _skipped_ | _skipped_ |") - lines.append("") - lines.append("## Port Count Baseline (1 RC, then +Telemetry ports)") - lines.append("") - for label, result in report["baseline"].items(): - cmd_summary = summarize_command_failures(result["ports"]) - lines.append(f"### {label}") - lines.append("") - lines.append( - f"- Duration: `{result['duration_s']}s` | FC cpuLoad avg/max: `{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}%` | " - f"FC cycleTime avg/max: `{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f}` us | " - f"Status samples: `{result['fc_status_samples']}`" - ) - lines.append( - f"- MSP success/fail: `{result['msp_success']}/{result['msp_fail']}` | MSP failure rate: `{(result['msp_failure_rate'] * 100.0):.2f}%`" - ) - lines.append( - f"- Commands sent/failed: `{int(cmd_summary['sent_total'])}/{int(cmd_summary['failed_total'])}` (`{cmd_summary['failed_pct']:.2f}%`) | " - f"Missing ACK: `{int(cmd_summary['missing_ack_total'])}` | MAVLink seq loss: `{cmd_summary['mav_seq_loss_pct']:.2f}%`" - ) - lines.append(f"- RC override send rate: `{result['rc_sent_hz']:.2f} Hz` (`{result['rc_sent']}` packets)") - lines.append("") - lines.append(format_port_table(result["ports"])) - lines.append("") - lines.append("## Stress Test: Progressive Overload On One MAVLink Port") - lines.append("") - for item in report["single_port_stress"]: - rate = item["rate_hz"] - result = item["result"] - cmd_summary = summarize_command_failures(result["ports"]) - lines.append(f"### Load Rate `{rate} req/s` on telemetry port") - lines.append("") - lines.append( - f"- Duration: `{result['duration_s']}s` | FC cpuLoad avg/max: `{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}%` | " - f"FC cycleTime avg/max: `{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f}` us | " - f"Status samples: `{result['fc_status_samples']}`" - ) - lines.append( - f"- MSP success/fail: `{result['msp_success']}/{result['msp_fail']}` | MSP failure rate: `{(result['msp_failure_rate'] * 100.0):.2f}%`" - ) - lines.append( - f"- Commands sent/failed: `{int(cmd_summary['sent_total'])}/{int(cmd_summary['failed_total'])}` (`{cmd_summary['failed_pct']:.2f}%`) | " - f"Missing ACK: `{int(cmd_summary['missing_ack_total'])}` | MAVLink seq loss: `{cmd_summary['mav_seq_loss_pct']:.2f}%`" - ) - lines.append(f"- RC override send rate: `{result['rc_sent_hz']:.2f} Hz` (`{result['rc_sent']}` packets)") - lines.append("") - lines.append(format_port_table(result["ports"])) - lines.append("") - max_result = report["all_ports_max"] - cmd_summary = summarize_command_failures(max_result["ports"]) - max_ports = len(max_result["ports"]) - lines.append(f"## Stress Test: All {max_ports} MAVLink Ports At Max Load") - lines.append("") - lines.append( - f"- Duration: `{max_result['duration_s']}s` | FC cpuLoad avg/max: `{max_result['fc_cpu_load_avg_pct']:.2f}% / {max_result['fc_cpu_load_max_pct']:.2f}%` | " - f"FC cycleTime avg/max: `{max_result['fc_cycle_time_avg_us']:.1f} / {max_result['fc_cycle_time_max_us']:.1f}` us | " - f"Status samples: `{max_result['fc_status_samples']}`" - ) - lines.append( - f"- MSP success/fail: `{max_result['msp_success']}/{max_result['msp_fail']}` | MSP failure rate: `{(max_result['msp_failure_rate'] * 100.0):.2f}%`" - ) - lines.append( - f"- Commands sent/failed: `{int(cmd_summary['sent_total'])}/{int(cmd_summary['failed_total'])}` (`{cmd_summary['failed_pct']:.2f}%`) | " - f"Missing ACK: `{int(cmd_summary['missing_ack_total'])}` | MAVLink seq loss: `{cmd_summary['mav_seq_loss_pct']:.2f}%`" - ) - lines.append(f"- RC override send rate: `{max_result['rc_sent_hz']:.2f} Hz` (`{max_result['rc_sent']}` packets)") - lines.append("") - lines.append(format_port_table(max_result["ports"])) - lines.append("") - out_path.write_text("\n".join(lines) + "\n", encoding="utf-8") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Run MAVLink multiport RC/telemetry/stress benchmark and write TESTING.md") - parser.add_argument("--config", default=str(DEFAULT_CONFIG_PATH), help="YAML configuration path") - args = parser.parse_args() - - config_path = Path(args.config) - config = load_config(config_path) - - sitl_bin = Path(config["sitl"]["binary"]) - sitl_workdir = Path(config["sitl"]["workdir"]) - sitl_eeprom = str(config["sitl"]["eeprom_path"]) - sitl_log = Path(config["sitl"]["runtime_log"]) - sitl_log.parent.mkdir(parents=True, exist_ok=True) - log_handle = sitl_log.open("w", encoding="utf-8") - sitl_proc = subprocess.Popen( - [str(sitl_bin), f"--path={sitl_eeprom}"], - cwd=str(sitl_workdir), - stdout=log_handle, - stderr=subprocess.STDOUT, - text=True, - ) - - report: Dict[str, Any] = { - "mavsdk_probes": [], - "baseline": {}, - "single_port_stress": [], - "all_ports_max": {}, - } - - try: - wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(config["tests"]["port_ready_timeout_s"])) - - configured_port_count = 1 + len(config["ports"]["telemetry"]) - print(f"single_config_start count={configured_port_count}", flush=True) - apply_config_and_wait(config, configured_port_count) - print(f"single_config_done count={configured_port_count}", flush=True) - - baseline_scenarios: List[Tuple[str, List[int]]] = [] - rc_port = int(config["ports"]["rc"]) - telemetry_ports = [int(port) for port in config["ports"]["telemetry"]] - for count in range(1, configured_port_count + 1): - baseline_scenarios.append((f"{count} MAVLink port(s)", [rc_port] + telemetry_ports[: max(0, count - 1)])) - for label, active_telemetry_ports in baseline_scenarios: - print(f"baseline_run_start label={label} ports={active_telemetry_ports}", flush=True) - result = run_workload( - config=config, - sitl_pid=sitl_proc.pid, - telemetry_ports=active_telemetry_ports, - rc_port=int(config["ports"]["rc"]), - load_rates_hz={}, - duration_s=float(config["tests"]["baseline_duration_s"]), - ) - report["baseline"][label] = result - print(f"baseline_run_done label={label}", flush=True) - - if bool(config["tests"].get("run_mavsdk_probe", True)): - print("mavsdk_probe_start", flush=True) - mavsdk_probe_ports = [int(config["ports"]["rc"])] + [int(p) for p in config["ports"]["telemetry"]] - for port in mavsdk_probe_ports: - probe = probe_mavsdk_sync(f"tcp://127.0.0.1:{port}", float(config["tests"]["mavsdk_probe_timeout_s"])) - report["mavsdk_probes"].append(probe) - print("mavsdk_probe_done", flush=True) - else: - print("mavsdk_probe_skipped", flush=True) - - stress_port = int(config["ports"]["telemetry"][0]) - active_telemetry_ports = [int(config["ports"]["rc"])] + [int(port) for port in config["ports"]["telemetry"]] - for rate in config["tests"]["progressive_rates_hz"]: - print(f"single_port_stress_start rate={rate}", flush=True) - result = run_workload( - config=config, - sitl_pid=sitl_proc.pid, - telemetry_ports=active_telemetry_ports, - rc_port=int(config["ports"]["rc"]), - load_rates_hz={stress_port: float(rate)}, - duration_s=float(config["tests"]["stress_duration_s"]), - ) - report["single_port_stress"].append({"rate_hz": rate, "result": result}) - - print("all_ports_max_start", flush=True) - max_rate = float(config["tests"]["max_rate_hz"]) - max_loads = {port: max_rate for port in active_telemetry_ports} - result = run_workload( - config=config, - sitl_pid=sitl_proc.pid, - telemetry_ports=active_telemetry_ports, - rc_port=int(config["ports"]["rc"]), - load_rates_hz=max_loads, - duration_s=float(config["tests"]["all_ports_max_duration_s"]), - ) - report["all_ports_max"] = result - print("all_ports_max_done", flush=True) - - write_testing_report(config, report) - print(f"report_written={config['output']['testing_md']}") - finally: - sitl_proc.terminate() - try: - sitl_proc.wait(timeout=5.0) - except subprocess.TimeoutExpired: - sitl_proc.kill() - sitl_proc.wait(timeout=5.0) - log_handle.close() diff --git a/src/utils/mavlink_tests/mav_multi_sweep.py b/src/utils/mavlink_tests/mav_multi_sweep.py deleted file mode 100644 index bb7a38b741e..00000000000 --- a/src/utils/mavlink_tests/mav_multi_sweep.py +++ /dev/null @@ -1,164 +0,0 @@ -#!/usr/bin/env python3 -""" -Usage: - conda run -n drone python src/utils/mavlink_tests/mav_multi_sweep.py - conda run -n drone python src/utils/mavlink_tests/mav_multi_sweep.py --config src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml -""" - -from __future__ import annotations - -import argparse -import importlib.util -import subprocess -from datetime import datetime, timezone -from pathlib import Path -from typing import Any, Dict, List, Tuple - -import yaml - - -TESTS_DIR = Path(__file__).resolve().parent -DEFAULT_CONFIG_PATH = TESTS_DIR / "test_config_multiport4_sweep_rc460800_tele115200.yaml" -BENCHMARK_MODULE_PATH = TESTS_DIR / "mav_multi_benchmark.py" - -spec = importlib.util.spec_from_file_location("mav_multi_benchmark", BENCHMARK_MODULE_PATH) -benchmark = importlib.util.module_from_spec(spec) -assert spec is not None -assert spec.loader is not None -spec.loader.exec_module(benchmark) - - -parser = argparse.ArgumentParser(description="Run MAVLink total-load sweep table for 1..4 ports and [50,100,200,max] req/s total") -parser.add_argument("--config", default=str(DEFAULT_CONFIG_PATH), help="YAML configuration path") -args = parser.parse_args() - -config_path = Path(args.config) -config: Dict[str, Any] = yaml.safe_load(config_path.read_text(encoding="utf-8")) - -sitl_bin = Path(config["sitl"]["binary"]) -sitl_workdir = Path(config["sitl"]["workdir"]) -sitl_eeprom = str(config["sitl"]["eeprom_path"]) -sitl_log = Path(config["sitl"]["runtime_log"]) -sitl_log.parent.mkdir(parents=True, exist_ok=True) - -rate_labels: List[Tuple[str, float]] = [ - ("50", 50.0), - ("100", 100.0), - ("200", 200.0), - ("max", float(config["tests"]["max_rate_hz"])), -] - -results: Dict[str, Dict[int, Dict[str, Any]]] = {} - -rc_port = int(config["ports"]["rc"]) -telemetry_ports = [int(port) for port in config["ports"]["telemetry"]] -sweep_duration_s = float(config["tests"]["sweep_duration_s"]) - -rate_cache: Dict[float, Dict[int, Dict[str, Any]]] = {} - -for label, total_rate_hz in rate_labels: - if total_rate_hz not in rate_cache: - rate_cache[total_rate_hz] = {} - for port_count in range(1, 5): - scenario_log = sitl_log.with_name(f"{sitl_log.stem}_{label}_{port_count}{sitl_log.suffix}") - log_handle = scenario_log.open("w", encoding="utf-8") - sitl_proc = subprocess.Popen( - [str(sitl_bin), f"--path={sitl_eeprom}"], - cwd=str(sitl_workdir), - stdout=log_handle, - stderr=subprocess.STDOUT, - text=True, - ) - try: - benchmark.wait_for_tcp_port("127.0.0.1", int(config["ports"]["msp"]), float(config["tests"]["port_ready_timeout_s"])) - benchmark.apply_config_and_wait(config, 4) - active_ports = [rc_port] + telemetry_ports[: max(0, port_count - 1)] - per_port_rate_hz = total_rate_hz / float(port_count) - load_rates_hz = {port: per_port_rate_hz for port in active_ports} - print( - f"sweep_run_start label={label} total_rate={total_rate_hz} per_port_rate={per_port_rate_hz} ports={port_count}", - flush=True, - ) - result = benchmark.run_workload( - config=config, - sitl_pid=sitl_proc.pid, - telemetry_ports=active_ports, - rc_port=rc_port, - load_rates_hz=load_rates_hz, - duration_s=sweep_duration_s, - ) - rate_cache[total_rate_hz][port_count] = result - print( - f"sweep_run_done label={label} total_rate={total_rate_hz} per_port_rate={per_port_rate_hz} ports={port_count}", - flush=True, - ) - finally: - sitl_proc.terminate() - try: - sitl_proc.wait(timeout=5.0) - except subprocess.TimeoutExpired: - sitl_proc.kill() - sitl_proc.wait(timeout=5.0) - log_handle.close() - scenario_log.unlink(missing_ok=True) - results[label] = rate_cache[total_rate_hz] - -output_path = Path(config["output"]["testing_md"]) -lines: List[str] = [ - "# MAVLink Port Load Sweep", - "", - f"- Generated: `{datetime.now(timezone.utc).isoformat()}`", - f"- SITL binary: `{config['sitl']['binary']}`", - f"- EEPROM: `{config['sitl']['eeprom_path']}`", - "- UART1 MSP only.", - "- UART2 is MAVLink RC (460800), RC override target is 100 Hz.", - "- Additional MAVLink telemetry ports are 115200 baud.", - "- Total command rate is held constant per load label and split evenly across active MAVLink ports.", - "", - "## Comparison Table", - "", - "| Load Label | Total Msg/s | Active MAVLink ports | Msg/s per active port | FC cpuLoad avg/max | FC cycleTime avg/max (us) | MAV seq loss max | Cmd failed total | Cmd failed % | MSP fail % |", - "| --- | ---: | ---: | ---: | --- | --- | ---: | ---: | ---: | ---: |", -] - -for label, total_rate_hz in rate_labels: - rate_results = results[label] - for port_count in range(1, 5): - result = rate_results[port_count] - per_port = list(result["ports"].values()) - mav_seq_loss_max_pct = max(item["mav_seq_loss_rate"] * 100.0 for item in per_port) - cmd_sent_total = sum(int(item["command_sent"]) for item in per_port) - cmd_failed_total = sum(int(item["command_failed_total"]) for item in per_port) - cmd_failed_pct = (cmd_failed_total / max(cmd_sent_total, 1)) * 100.0 - per_port_rate_hz = total_rate_hz / float(port_count) - lines.append( - f"| {label} | {total_rate_hz:.1f} | {port_count} | {per_port_rate_hz:.2f} | " - f"{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}% | " - f"{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f} | " - f"{mav_seq_loss_max_pct:.2f}% | {cmd_failed_total} | {cmd_failed_pct:.2f}% | {(result['msp_failure_rate'] * 100.0):.2f}% |" - ) - -lines.append("") -lines.append("## Raw Scenario Details") -lines.append("") -for label, total_rate_hz in rate_labels: - lines.append(f"### {label} ({total_rate_hz:.1f} total msg/s)") - lines.append("") - for port_count in range(1, 5): - result = results[label][port_count] - per_port_rate_hz = total_rate_hz / float(port_count) - lines.append(f"#### {port_count} active MAVLink port(s)") - lines.append("") - lines.append(f"- Total command rate: `{total_rate_hz:.1f} msg/s`") - lines.append(f"- Per-port command rate: `{per_port_rate_hz:.2f} msg/s`") - lines.append( - f"- FC cpuLoad avg/max: `{result['fc_cpu_load_avg_pct']:.2f}% / {result['fc_cpu_load_max_pct']:.2f}%` | " - f"FC cycleTime avg/max: `{result['fc_cycle_time_avg_us']:.1f} / {result['fc_cycle_time_max_us']:.1f}` us | " - f"MSP fail rate: `{(result['msp_failure_rate'] * 100.0):.2f}%`" - ) - lines.append("") - lines.append(benchmark.format_port_table(result["ports"])) - lines.append("") - -output_path.write_text("\n".join(lines) + "\n", encoding="utf-8") -print(f"report_written={output_path}", flush=True) diff --git a/src/utils/mavlink_tests/mavlink_routing_compliance_test.py b/src/utils/mavlink_tests/mavlink_routing_compliance_test.py deleted file mode 100644 index 0ea996ec1b5..00000000000 --- a/src/utils/mavlink_tests/mavlink_routing_compliance_test.py +++ /dev/null @@ -1,499 +0,0 @@ -#!/usr/bin/env python3 -""" -Usage: - conda run -n drone python src/utils/mavlink_tests/mavlink_routing_compliance_test.py --config src/utils/mavlink_tests/routing_test_config.yaml -""" - -from __future__ import annotations - -import argparse -import math -import time -from pathlib import Path -from typing import Any, Dict, List - -import yaml -from pymavlink import mavutil - - -MAV_AUTOPILOT_INVALID = int(mavutil.mavlink.MAV_AUTOPILOT_INVALID) -MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = int(mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) -MAV_STATE_ACTIVE = int(mavutil.mavlink.MAV_STATE_ACTIVE) -MAV_TYPE_GCS = int(mavutil.mavlink.MAV_TYPE_GCS) - -ROUTING_RULE_REFERENCES = { - "broadcast_forward": "routing.md:25-27,48-49", - "known_target_forward": "routing.md:49-50; MAVLink_routing.cpp:66-80", - "unknown_target_blocked": "routing.md:26-27,53", - "no_ingress_loop": "MAVLink_routing.cpp:197-209", - "no_repack": "routing.md:29-31", -} - - -def load_config(config_path: Path) -> Dict[str, Any]: - return yaml.safe_load(config_path.read_text(encoding="utf-8")) - - -def open_mavlink_connection(endpoint: str, source_system: int, source_component: int, timeout_s: float) -> Any: - deadline = time.monotonic() + timeout_s - while True: - try: - return mavutil.mavlink_connection( - endpoint, - source_system=source_system, - source_component=source_component, - autoreconnect=True, - ) - except OSError as error: - if time.monotonic() >= deadline: - raise TimeoutError( - f"endpoint={endpoint} source_system={source_system} source_component={source_component} timeout_s={timeout_s}" - ) from error - time.sleep(0.2) - - -def set_source_identity(connection: Any, system_id: int, component_id: int) -> None: - connection.mav.srcSystem = system_id - connection.mav.srcComponent = component_id - - -def send_heartbeat(connection: Any, system_id: int, component_id: int, mav_type: int) -> None: - set_source_identity(connection, system_id, component_id) - connection.mav.heartbeat_send( - mav_type, - MAV_AUTOPILOT_INVALID, - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - 0, - MAV_STATE_ACTIVE, - ) - - -def drain_link(connection: Any, duration_s: float) -> None: - deadline = time.monotonic() + duration_s - while time.monotonic() < deadline: - message = connection.recv_match(blocking=False) - if message is None: - time.sleep(0.002) - - -def drain_all_links(connections: Dict[str, Any], duration_s: float) -> None: - for connection in connections.values(): - drain_link(connection, duration_s) - - -def collect_command_observations(connections: Dict[str, Any], command_id: int, observation_window_s: float) -> Dict[str, List[Dict[str, Any]]]: - observations: Dict[str, List[Dict[str, Any]]] = {link_name: [] for link_name in connections} - deadline = time.monotonic() + observation_window_s - while time.monotonic() < deadline: - saw_message = False - for link_name, connection in connections.items(): - while True: - message = connection.recv_match(type=["COMMAND_LONG"], blocking=False) - if message is None: - break - if int(message.command) != command_id: - continue - observations[link_name].append( - { - "src_system": int(message.get_srcSystem()), - "src_component": int(message.get_srcComponent()), - "target_system": int(message.target_system), - "target_component": int(message.target_component), - "command": int(message.command), - "confirmation": int(message.confirmation), - "params": [ - float(message.param1), - float(message.param2), - float(message.param3), - float(message.param4), - float(message.param5), - float(message.param6), - float(message.param7), - ], - } - ) - saw_message = True - if not saw_message: - time.sleep(0.002) - return observations - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Run MAVLink routing compliance checks against INAV multiport routing.") - parser.add_argument("--config", required=True, type=Path, help="YAML config path") - args = parser.parse_args() - - config = load_config(args.config) - timing = config["timing"] - network = config["network"] - tests = config["tests"] - components_cfg = config["components"] - vehicle = config["vehicle"] - - gcs_link_name = network["gcs_link"] - fc_system_id = int(vehicle["fc_system_id"]) - unknown_component_id = int(tests["unknown_component_id"]) - unknown_system_id = int(tests["unknown_system_id"]) - marker_command_base = int(tests["marker_command_base"]) - report_path = Path(tests["report_markdown"]) - - link_cfgs = network["links"] - if gcs_link_name not in link_cfgs: - raise ValueError(f"gcs_link={gcs_link_name} missing from network.links") - - components: List[Dict[str, Any]] = [] - for component in components_cfg: - if component["link"] not in link_cfgs: - raise ValueError(f"component={component['name']} link={component['link']} missing from network.links") - components.append( - { - "name": component["name"], - "link": component["link"], - "system_id": int(component["system_id"]), - "component_id": int(component["component_id"]), - "mav_type": int(component["mav_type"]), - } - ) - - component_ids = {component["component_id"] for component in components} - component_system_ids = {component["system_id"] for component in components} - if unknown_component_id in component_ids: - raise ValueError(f"unknown_component_id={unknown_component_id} collides with configured component IDs") - if unknown_system_id in component_system_ids or unknown_system_id == fc_system_id: - raise ValueError(f"unknown_system_id={unknown_system_id} collides with configured systems") - - gcs_actor = { - "name": "gcs", - "link": gcs_link_name, - "system_id": int(link_cfgs[gcs_link_name]["source_system"]), - "component_id": int(link_cfgs[gcs_link_name]["source_component"]), - "mav_type": MAV_TYPE_GCS, - } - - inter_source = components[0] - inter_target = None - for component in components: - if component["link"] != inter_source["link"]: - inter_target = component - break - if inter_target is None: - raise ValueError("Need at least one component on a different link for inter-component routing test") - - connections: Dict[str, Any] = {} - fc_heartbeats: Dict[str, Dict[str, int]] = {} - - try: - for link_name, link_cfg in link_cfgs.items(): - connection = open_mavlink_connection( - endpoint=link_cfg["endpoint"], - source_system=int(link_cfg["source_system"]), - source_component=int(link_cfg["source_component"]), - timeout_s=float(timing["connect_timeout_s"]), - ) - connections[link_name] = connection - - for link_name, connection in connections.items(): - heartbeat = connection.wait_heartbeat(timeout=float(timing["connect_timeout_s"])) - if heartbeat is None: - raise TimeoutError(f"link={link_name} wait_heartbeat timed out") - fc_heartbeats[link_name] = { - "system_id": int(heartbeat.get_srcSystem()), - "component_id": int(heartbeat.get_srcComponent()), - } - print( - f"fc_heartbeat link={link_name} system_id={fc_heartbeats[link_name]['system_id']} " - f"component_id={fc_heartbeats[link_name]['component_id']}", - flush=True, - ) - - time.sleep(float(timing["settle_after_connect_s"])) - drain_all_links(connections, float(timing["drain_window_s"])) - - send_heartbeat( - connections[gcs_actor["link"]], - gcs_actor["system_id"], - gcs_actor["component_id"], - gcs_actor["mav_type"], - ) - for component in components: - send_heartbeat( - connections[component["link"]], - component["system_id"], - component["component_id"], - component["mav_type"], - ) - print( - f"route_learn_heartbeat component={component['name']} link={component['link']} " - f"system_id={component['system_id']} component_id={component['component_id']} mav_type={component['mav_type']}", - flush=True, - ) - time.sleep(float(timing["route_learn_settle_s"])) - drain_all_links(connections, float(timing["drain_window_s"])) - - cases: List[Dict[str, Any]] = [] - all_other_than_gcs = sorted([link_name for link_name in connections if link_name != gcs_link_name]) - - cases.append( - { - "name": "gcs_broadcast", - "source": gcs_actor, - "target_system": 0, - "target_component": 0, - "expected_links": all_other_than_gcs, - "rules": [ROUTING_RULE_REFERENCES["broadcast_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], - } - ) - - cases.append( - { - "name": "gcs_target_local_system_component_broadcast", - "source": gcs_actor, - "target_system": fc_system_id, - "target_component": 0, - "expected_links": all_other_than_gcs, - "rules": [ROUTING_RULE_REFERENCES["broadcast_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], - } - ) - - for component in components: - expected_links = [component["link"]] - if component["link"] == gcs_link_name: - expected_links = [] - cases.append( - { - "name": f"gcs_target_{component['name']}", - "source": gcs_actor, - "target_system": component["system_id"], - "target_component": component["component_id"], - "expected_links": sorted(expected_links), - "rules": [ROUTING_RULE_REFERENCES["known_target_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], - } - ) - - cases.append( - { - "name": "gcs_target_unknown_component", - "source": gcs_actor, - "target_system": fc_system_id, - "target_component": unknown_component_id, - "expected_links": [], - "rules": [ROUTING_RULE_REFERENCES["unknown_target_blocked"]], - } - ) - - cases.append( - { - "name": "gcs_target_unknown_system", - "source": gcs_actor, - "target_system": unknown_system_id, - "target_component": 1, - "expected_links": [], - "rules": [ROUTING_RULE_REFERENCES["unknown_target_blocked"]], - } - ) - - component_broadcast_expected = sorted([link_name for link_name in connections if link_name != inter_source["link"]]) - cases.append( - { - "name": f"{inter_source['name']}_broadcast", - "source": inter_source, - "target_system": 0, - "target_component": 0, - "expected_links": component_broadcast_expected, - "rules": [ROUTING_RULE_REFERENCES["broadcast_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], - } - ) - - cases.append( - { - "name": f"{inter_source['name']}_to_{inter_target['name']}", - "source": inter_source, - "target_system": inter_target["system_id"], - "target_component": inter_target["component_id"], - "expected_links": [inter_target["link"]], - "rules": [ROUTING_RULE_REFERENCES["known_target_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], - } - ) - - cases.append( - { - "name": f"{inter_source['name']}_to_gcs", - "source": inter_source, - "target_system": gcs_actor["system_id"], - "target_component": gcs_actor["component_id"], - "expected_links": [gcs_actor["link"]], - "rules": [ROUTING_RULE_REFERENCES["known_target_forward"], ROUTING_RULE_REFERENCES["no_ingress_loop"]], - } - ) - - if marker_command_base + len(cases) > 65535: - raise ValueError(f"marker_command_base={marker_command_base} too high for case_count={len(cases)}") - - results: List[Dict[str, Any]] = [] - for case_index, case in enumerate(cases, start=1): - drain_all_links(connections, float(timing["drain_window_s"])) - command_id = marker_command_base + case_index - confirmation = case_index % 256 - params = [ - float(1000 + case_index), - float(2000 + case_index), - float(3000 + case_index), - float(4000 + case_index), - float(5000 + case_index), - float(6000 + case_index), - float(7000 + case_index), - ] - - source = case["source"] - source_connection = connections[source["link"]] - set_source_identity(source_connection, source["system_id"], source["component_id"]) - source_connection.mav.command_long_send( - int(case["target_system"]), - int(case["target_component"]), - command_id, - confirmation, - params[0], - params[1], - params[2], - params[3], - params[4], - params[5], - params[6], - ) - - observations = collect_command_observations( - connections=connections, - command_id=command_id, - observation_window_s=float(timing["observation_window_s"]), - ) - observed_links = sorted([link_name for link_name, msgs in observations.items() if len(msgs) > 0]) - expected_links = sorted(case["expected_links"]) - no_ingress_loop = source["link"] not in observed_links - routing_match = observed_links == expected_links - - payload_intact = True - payload_issues: List[str] = [] - for link_name, messages in observations.items(): - for message in messages: - if message["src_system"] != int(source["system_id"]) or message["src_component"] != int(source["component_id"]): - payload_intact = False - payload_issues.append( - f"link={link_name} src_mismatch expected=({source['system_id']},{source['component_id']}) " - f"observed=({message['src_system']},{message['src_component']})" - ) - if message["target_system"] != int(case["target_system"]) or message["target_component"] != int(case["target_component"]): - payload_intact = False - payload_issues.append( - f"link={link_name} target_mismatch expected=({case['target_system']},{case['target_component']}) " - f"observed=({message['target_system']},{message['target_component']})" - ) - if message["command"] != command_id or message["confirmation"] != confirmation: - payload_intact = False - payload_issues.append( - f"link={link_name} cmd_mismatch expected=(command={command_id},confirmation={confirmation}) " - f"observed=(command={message['command']},confirmation={message['confirmation']})" - ) - for index, observed_value in enumerate(message["params"]): - expected_value = params[index] - if not math.isclose(observed_value, expected_value, rel_tol=0.0, abs_tol=1e-3): - payload_intact = False - payload_issues.append( - f"link={link_name} param{index + 1}_mismatch expected={expected_value} observed={observed_value}" - ) - - case_pass = routing_match and no_ingress_loop and payload_intact - results.append( - { - "name": case["name"], - "source": source["name"], - "source_link": source["link"], - "target_system": int(case["target_system"]), - "target_component": int(case["target_component"]), - "command_id": command_id, - "expected_links": expected_links, - "observed_links": observed_links, - "routing_match": routing_match, - "no_ingress_loop": no_ingress_loop, - "payload_intact": payload_intact, - "pass": case_pass, - "rules": case["rules"], - "payload_issues": payload_issues, - } - ) - - print( - f"case={case['name']} command_id={command_id} source={source['name']} source_link={source['link']} " - f"target=({case['target_system']},{case['target_component']}) expected_links={expected_links} observed_links={observed_links} " - f"routing_match={routing_match} no_ingress_loop={no_ingress_loop} payload_intact={payload_intact} pass={case_pass}", - flush=True, - ) - time.sleep(float(timing["inter_case_pause_s"])) - - finally: - for connection in connections.values(): - connection.close() - - pass_count = sum(1 for result in results if result["pass"]) - fail_count = len(results) - pass_count - - markdown_lines: List[str] = [] - markdown_lines.append("# MAVLink Routing Compliance Test") - markdown_lines.append("") - markdown_lines.append( - f"- fc_system_id: `{fc_system_id}`" - ) - markdown_lines.append( - f"- links: `{sorted(list(link_cfgs.keys()))}`" - ) - markdown_lines.append( - f"- gcs_link: `{gcs_link_name}`" - ) - markdown_lines.append( - f"- components: `{[(component['name'], component['system_id'], component['component_id'], component['link']) for component in components]}`" - ) - markdown_lines.append("") - markdown_lines.append("## Rule References") - markdown_lines.append("") - markdown_lines.append(f"- broadcast_forward: `{ROUTING_RULE_REFERENCES['broadcast_forward']}`") - markdown_lines.append(f"- known_target_forward: `{ROUTING_RULE_REFERENCES['known_target_forward']}`") - markdown_lines.append(f"- unknown_target_blocked: `{ROUTING_RULE_REFERENCES['unknown_target_blocked']}`") - markdown_lines.append(f"- no_ingress_loop: `{ROUTING_RULE_REFERENCES['no_ingress_loop']}`") - markdown_lines.append(f"- no_repack: `{ROUTING_RULE_REFERENCES['no_repack']}`") - markdown_lines.append("") - markdown_lines.append("## Results") - markdown_lines.append("") - markdown_lines.append("| Case | Source(link) | Target(sys,comp) | Expected Links | Observed Links | Routing | No Ingress Loop | Payload Intact | Pass |") - markdown_lines.append("| --- | --- | --- | --- | --- | --- | --- | --- | --- |") - for result in results: - markdown_lines.append( - f"| `{result['name']}` | `{result['source']} ({result['source_link']})` | " - f"`({result['target_system']},{result['target_component']})` | " - f"`{result['expected_links']}` | `{result['observed_links']}` | " - f"`{result['routing_match']}` | `{result['no_ingress_loop']}` | `{result['payload_intact']}` | `{result['pass']}` |" - ) - markdown_lines.append("") - markdown_lines.append(f"summary pass_count={pass_count} fail_count={fail_count} total={len(results)}") - markdown_lines.append("") - - failures = [result for result in results if not result["pass"]] - if failures: - markdown_lines.append("## Failure Details") - markdown_lines.append("") - for failure in failures: - markdown_lines.append(f"### {failure['name']}") - markdown_lines.append("") - markdown_lines.append( - f"- expected_links: `{failure['expected_links']}` observed_links: `{failure['observed_links']}` " - f"routing_match: `{failure['routing_match']}` no_ingress_loop: `{failure['no_ingress_loop']}` " - f"payload_intact: `{failure['payload_intact']}`" - ) - if failure["payload_issues"]: - markdown_lines.append(f"- payload_issues: `{failure['payload_issues']}`") - markdown_lines.append(f"- rule_refs: `{failure['rules']}`") - markdown_lines.append("") - - report_path.write_text("\n".join(markdown_lines) + "\n", encoding="utf-8") - print(f"report_path={report_path} pass_count={pass_count} fail_count={fail_count} total={len(results)}", flush=True) - - if fail_count > 0: - raise SystemExit(1) diff --git a/src/utils/mavlink_tests/mavlink_test_rc.py b/src/utils/mavlink_tests/mavlink_test_rc.py deleted file mode 100644 index 4862ec7aad2..00000000000 --- a/src/utils/mavlink_tests/mavlink_test_rc.py +++ /dev/null @@ -1,141 +0,0 @@ -#!/usr/bin/env python3 -""" -Usage: - conda run -n drone python mydev/branch/mav_multi/mavlink_test_rc.py --master tcp:127.0.0.1:5761 - conda run -n drone python mydev/branch/mav_multi/mavlink_test_rc.py --master tcp:127.0.0.1:5761 --duration 20 --tx-hz 100 --roll MID --pitch MID --yaw MID --throttle LOW -""" - -from __future__ import annotations - -import argparse -import time -from collections import Counter -from typing import List - -from pymavlink import mavutil - - -CHANNEL_ORDER = [ - "roll", - "pitch", - "throttle", - "yaw", - "ch5", - "ch6", - "ch7", - "ch8", - "ch9", - "ch10", - "ch11", - "ch12", - "ch13", - "ch14", - "ch15", - "ch16", - "ch17", - "ch18", -] -DEFAULT_CHANNELS = [900] * 18 -DEFAULT_CHANNELS[0] = 1500 -DEFAULT_CHANNELS[1] = 1500 -DEFAULT_CHANNELS[3] = 1500 -LEVEL_TO_PWM = {"LOW": 900, "MID": 1500, "HIGH": 2100} - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Send MAVLink RC_CHANNELS_OVERRIDE stream and monitor RC echo.") - parser.add_argument("--master", required=True, help='pymavlink master endpoint, e.g. "tcp:127.0.0.1:5761"') - parser.add_argument("--tx-hz", type=float, default=100.0, help="RC override transmit rate in Hz") - parser.add_argument("--duration", type=float, default=0.0, help="Test duration in seconds, 0 means run forever") - parser.add_argument("--source-system", type=int, default=240, help="MAVLink source system ID") - parser.add_argument("--source-component", type=int, default=191, help="MAVLink source component ID") - parser.add_argument("--echo-tolerance", type=int, default=20, help="Allowed absolute PWM error for RC_CHANNELS echo checks") - for channel_name in CHANNEL_ORDER: - parser.add_argument(f"--{channel_name}", default=None, help="LOW/MID/HIGH or integer PWM") - args = parser.parse_args() - - channels: List[int] = list(DEFAULT_CHANNELS) - for idx, channel_name in enumerate(CHANNEL_ORDER): - raw_value = getattr(args, channel_name) - if raw_value is None: - continue - level_name = raw_value.upper() - if level_name in LEVEL_TO_PWM: - channels[idx] = LEVEL_TO_PWM[level_name] - else: - channels[idx] = int(raw_value) - - print( - f"master={args.master} tx_hz={args.tx_hz} duration={args.duration} " - f"source_system={args.source_system} source_component={args.source_component} channels={channels}", - flush=True, - ) - - master = mavutil.mavlink_connection( - args.master, - source_system=args.source_system, - source_component=args.source_component, - autoreconnect=True, - ) - heartbeat = master.wait_heartbeat(timeout=10) - if heartbeat is None: - raise TimeoutError("No heartbeat received from FC") - target_system = heartbeat.get_srcSystem() - target_component = heartbeat.get_srcComponent() - print(f"target_system={target_system} target_component={target_component}", flush=True) - - period_s = 1.0 / args.tx_hz - tx_count = 0 - rx_count = 0 - mismatch_count = 0 - decode_errors = 0 - last_rx_time = 0.0 - loop_start = time.monotonic() - next_tx_time = loop_start - next_report_time = loop_start + 1.0 - mismatch_hist = Counter() - - while True: - now = time.monotonic() - if args.duration > 0 and (now - loop_start) >= args.duration: - break - - if now >= next_tx_time: - master.mav.rc_channels_override_send(target_system, target_component, *channels[:8]) - tx_count += 1 - next_tx_time += period_s - if next_tx_time < now: - next_tx_time = now + period_s - - message = master.recv_match(type=["RC_CHANNELS", "RC_CHANNELS_RAW"], blocking=False) - if message is not None: - rx_count += 1 - last_rx_time = now - if message.get_type() == "RC_CHANNELS": - for i in range(4): - key = f"chan{i + 1}_raw" - observed = int(getattr(message, key)) - err = abs(observed - channels[i]) - if err > args.echo_tolerance: - mismatch_count += 1 - mismatch_hist[i + 1] += 1 - else: - decode_errors += 1 - - if now >= next_report_time: - rx_age = -1.0 if last_rx_time == 0.0 else (now - last_rx_time) - print( - f"status_s={now - loop_start:.1f} tx={tx_count} rx={rx_count} mismatches={mismatch_count} " - f"decode_errors={decode_errors} rx_age_s={rx_age:.3f} mismatch_hist={dict(mismatch_hist)}", - flush=True, - ) - next_report_time += 1.0 - - time.sleep(0.001) - - elapsed = max(time.monotonic() - loop_start, 1e-6) - print( - f"summary elapsed_s={elapsed:.2f} tx={tx_count} rx={rx_count} mismatch_count={mismatch_count} " - f"mismatch_rate={mismatch_count / max(rx_count, 1):.6f} tx_hz={tx_count / elapsed:.2f} rx_hz={rx_count / elapsed:.2f}", - flush=True, - ) diff --git a/src/utils/mavlink_tests/msp_test_rc.py b/src/utils/mavlink_tests/msp_test_rc.py deleted file mode 100644 index 9cd41cb6c5d..00000000000 --- a/src/utils/mavlink_tests/msp_test_rc.py +++ /dev/null @@ -1,145 +0,0 @@ -#!/usr/bin/env python3 -""" -Usage: - conda run -n drone python mydev/branch/mav_multi/msp_test_rc.py --msp-endpoint 127.0.0.1:5760 --duration 20 - conda run -n drone python mydev/branch/mav_multi/msp_test_rc.py --msp-endpoint 127.0.0.1:5760 --poll-hz 25 --roll MID --pitch MID --yaw MID --throttle LOW -""" - -from __future__ import annotations - -import argparse -import struct -import time -from collections import Counter -from pathlib import Path -from typing import List - -from serial.serialutil import SerialException - -try: - from mspapi2.msp_api import MSPApi - from mspapi2.lib import InavMSP -except ModuleNotFoundError: - repo_root_guess = Path(__file__).resolve().parents[3] - mspapi2_repo = repo_root_guess.parent / "mspapi2" - if mspapi2_repo.exists(): - import sys - - sys.path.insert(0, str(mspapi2_repo)) - from mspapi2.msp_api import MSPApi - from mspapi2.lib import InavMSP - - -CHANNEL_ORDER = [ - "roll", - "pitch", - "throttle", - "yaw", - "ch5", - "ch6", - "ch7", - "ch8", - "ch9", - "ch10", - "ch11", - "ch12", - "ch13", - "ch14", - "ch15", - "ch16", - "ch17", - "ch18", -] -DEFAULT_CHANNELS = [900] * 18 -DEFAULT_CHANNELS[0] = 1500 -DEFAULT_CHANNELS[1] = 1500 -DEFAULT_CHANNELS[3] = 1500 -LEVEL_TO_PWM = {"LOW": 900, "MID": 1500, "HIGH": 2100} - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Poll MSP_RC and monitor RC values against expected channels.") - parser.add_argument("--msp-endpoint", required=True, help='MSP endpoint, e.g. "127.0.0.1:5760"') - parser.add_argument("--poll-hz", type=float, default=25.0, help="MSP_RC poll rate in Hz") - parser.add_argument("--timeout-s", type=float, default=0.2, help="Per MSP request timeout") - parser.add_argument("--duration", type=float, default=0.0, help="Test duration in seconds, 0 means run forever") - parser.add_argument("--echo-tolerance", type=int, default=40, help="Allowed absolute PWM error per channel") - for channel_name in CHANNEL_ORDER: - parser.add_argument(f"--{channel_name}", default=None, help="LOW/MID/HIGH or integer PWM") - args = parser.parse_args() - - expected_channels: List[int] = list(DEFAULT_CHANNELS) - for idx, channel_name in enumerate(CHANNEL_ORDER): - raw_value = getattr(args, channel_name) - if raw_value is None: - continue - level_name = raw_value.upper() - if level_name in LEVEL_TO_PWM: - expected_channels[idx] = LEVEL_TO_PWM[level_name] - else: - expected_channels[idx] = int(raw_value) - - print( - f"msp_endpoint={args.msp_endpoint} poll_hz={args.poll_hz} duration={args.duration} " - f"timeout_s={args.timeout_s} expected_channels={expected_channels}", - flush=True, - ) - - poll_period_s = 1.0 / args.poll_hz - success_count = 0 - fail_count = 0 - mismatch_count = 0 - mismatch_hist = Counter() - last_success_time = 0.0 - start_t = time.monotonic() - next_poll_t = start_t - next_report_t = start_t + 1.0 - - with MSPApi(tcp_endpoint=args.msp_endpoint) as msp_api: - msp_api._serial._max_retries = 1 - msp_api._serial._reconnect_delay = 0.05 - while True: - now = time.monotonic() - if args.duration > 0 and (now - start_t) >= args.duration: - break - - if now >= next_poll_t: - try: - _, payload = msp_api._request_raw(InavMSP.MSP_RC, timeout=args.timeout_s) - if len(payload) % 2: - raise ValueError("MSP RC payload length must be even") - channel_count = len(payload) // 2 - observed_channels = list(struct.unpack(f"<{channel_count}H", payload)) - success_count += 1 - last_success_time = now - for i in range(min(channel_count, len(expected_channels))): - error = abs(observed_channels[i] - expected_channels[i]) - if error > args.echo_tolerance: - mismatch_count += 1 - mismatch_hist[i + 1] += 1 - except (SerialException, RuntimeError, TimeoutError, ConnectionRefusedError): - fail_count += 1 - next_poll_t += poll_period_s - if next_poll_t < now: - next_poll_t = now + poll_period_s - - if now >= next_report_t: - last_ok_age_s = -1.0 if last_success_time == 0.0 else (now - last_success_time) - total = success_count + fail_count - print( - f"status_s={now - start_t:.1f} ok={success_count} fail={fail_count} fail_rate={fail_count / max(total, 1):.6f} " - f"mismatch={mismatch_count} mismatch_rate={mismatch_count / max(success_count, 1):.6f} " - f"last_ok_age_s={last_ok_age_s:.3f} mismatch_hist={dict(mismatch_hist)}", - flush=True, - ) - next_report_t += 1.0 - - time.sleep(0.001) - - elapsed = max(time.monotonic() - start_t, 1e-6) - total = success_count + fail_count - print( - f"summary elapsed_s={elapsed:.2f} ok={success_count} fail={fail_count} fail_rate={fail_count / max(total, 1):.6f} " - f"mismatch={mismatch_count} mismatch_rate={mismatch_count / max(success_count, 1):.6f} poll_hz={success_count / elapsed:.2f}", - flush=True, - ) diff --git a/src/utils/mavlink_tests/routing_test_config.yaml b/src/utils/mavlink_tests/routing_test_config.yaml deleted file mode 100644 index d4677ba4ebf..00000000000 --- a/src/utils/mavlink_tests/routing_test_config.yaml +++ /dev/null @@ -1,58 +0,0 @@ -timing: - connect_timeout_s: 10.0 - settle_after_connect_s: 0.5 - route_learn_settle_s: 0.5 - drain_window_s: 0.15 - observation_window_s: 0.8 - inter_case_pause_s: 0.15 - -network: - gcs_link: link_radio - links: - link_radio: - endpoint: tcp:127.0.0.1:5761 - source_system: 1 - source_component: 68 - link_companion: - endpoint: tcp:127.0.0.1:5762 - source_system: 1 - source_component: 191 - link_camera: - endpoint: tcp:127.0.0.1:5763 - source_system: 1 - source_component: 100 - link_gimbal: - endpoint: tcp:127.0.0.1:5764 - source_system: 1 - source_component: 154 - -vehicle: - fc_system_id: 1 - -tests: - marker_command_base: 31000 - unknown_component_id: 199 - unknown_system_id: 42 - report_markdown: src/utils/mavlink_tests/ROUTING_TEST.md - -components: - - name: mav2_companion_computer - link: link_companion - system_id: 1 - component_id: 191 - mav_type: 18 - - name: mav1_elrs_receiver - link: link_radio - system_id: 1 - component_id: 68 - mav_type: 49 - - name: mav3_camera - link: link_camera - system_id: 1 - component_id: 100 - mav_type: 30 - - name: mav4_gimbal - link: link_gimbal - system_id: 1 - component_id: 154 - mav_type: 26 diff --git a/src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml b/src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml deleted file mode 100644 index a73523f0698..00000000000 --- a/src/utils/mavlink_tests/test_config_multiport4_sweep_rc460800_tele115200.yaml +++ /dev/null @@ -1,51 +0,0 @@ -sitl: - binary: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/cmake/build_SITL/inav_9.0.0_SITL - workdir: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/cmake - eeprom_path: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/mydev/branch/mav_multi/eeprom_multi4_sweep.bin - runtime_log: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/mydev/branch/mav_multi/sitl_runtime_multi4_sweep.log - -ports: - msp: 5760 - rc: 5761 - telemetry: - - 5762 - - 5763 - - 5764 - -cli: - rc_baud: 460800 - telemetry_baud: 115200 - ext_status_rate_hz: 10 - rc_chan_rate_hz: 25 - pos_rate_hz: 10 - extra1_rate_hz: 10 - extra2_rate_hz: 10 - extra3_rate_hz: 5 - -tests: - port_ready_timeout_s: 30 - save_reboot_timeout_s: 45 - mavsdk_probe_timeout_s: 10 - heartbeat_expected_hz: 1.0 - rc_expected_hz: 25.0 - rc_target_system: 1 - rc_target_component: 1 - rc_tx_hz: 100.0 - rc_tx_hz_max: 400.0 - msp_poll_hz: 50.0 - inav_status_hz: 5.0 - msp_request_timeout_s: 0.2 - stress_command_message_id: 33 - baseline_duration_s: 30 - progressive_rates_hz: - - 50 - - 100 - - 200 - - 400 - stress_duration_s: 20 - max_rate_hz: 400 - all_ports_max_duration_s: 30 - sweep_duration_s: 8 - -output: - testing_md: /media/anon/WD2TB/DataVault/TechProjects/Software/GitRepos/inav/mydev/branch/mav_multi/TESTING_multiport4_sweep_rc460800_tele115200.md From b2194921d2829991923dfde16a77bf6eaf77ecf9 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Fri, 3 Apr 2026 13:53:22 -0400 Subject: [PATCH 33/42] Fix MAVLink routing and intervals --- docs/Mavlink.md | 17 +- docs/Settings.md | 40 --- src/main/fc/settings.yaml | 28 -- src/main/io/serial.c | 2 + src/main/telemetry/mavlink.c | 391 +++++++++++++++++++--------- src/main/telemetry/mavlink.h | 18 ++ src/main/telemetry/telemetry.c | 4 - src/main/telemetry/telemetry.h | 1 - src/test/unit/mavlink_unittest.cc | 412 +++++++++++++++++++++++++++++- 9 files changed, 716 insertions(+), 197 deletions(-) diff --git a/docs/Mavlink.md b/docs/Mavlink.md index c42310a30ab..5194881c29f 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -17,7 +17,6 @@ INAV supports up to 4 concurrent MAVLink telemetry ports (`MAX_MAVLINK_PORTS`), ### Usage guidance - If you rely on RC 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. -- Assign a unique `mavlink_port{1-4}_compid` to each INAV MAVLink port to avoid ambiguous local targeting. - If a GCS or companion needs telemetry on ports 2..4, explicitly request streams (`REQUEST_DATA_STREAM` or `MAV_CMD_SET_MESSAGE_INTERVAL`) because only heartbeat is enabled by default. - If you depend on directed forwarding between links, ensure each remote endpoint transmits at least one frame early so route learning is populated. @@ -35,11 +34,12 @@ INAV supports up to 4 concurrent MAVLink telemetry ports (`MAX_MAVLINK_PORTS`), - `mavlink_port{1-4}_extra3_rate`. - Port 1 uses configured CLI rates (`mavlink_port1_*_rate`). - Ports 2..4 start with heartbeat only (1 Hz), all other streams disabled. -- `mavlink_port{1-4}_compid` - MAV_COMPONENT ID of port. Ensure these are different. - `mavlink_port{1-4}_min_txbuffer` - minimum remote TX buffer level before sending when `RADIO_STATUS` provides flow control. - `mavlink_port{1-4}_radio_type`- scales `RADIO_STATUS` RSSI/SNR for **generic**, **ELRS**, or **SiK** links. - `mavlink_port{1-4}_high_latency`- turns on Mavlink HIGH_LATENCY2 mode on this port +INAV always transmits as MAVLink component `MAV_COMP_ID_AUTOPILOT1`. Attached MAVLink devices are remote components learned from incoming traffic; they are not modeled as local per-port FC identities. + ## Datastream groups and defaults @@ -55,7 +55,7 @@ Ports 2..N use a secondary startup profile (heartbeat at 1 Hz, other streams dis | `EXTRA2` | `VFR_HUD` | 2 Hz | | `HEARTBEAT` | `HEARTBEAT` | 1 Hz (independent of stream groups) | | `EXT_SYS_STATE` | `EXTENDED_SYS_STATE` | 1 Hz (defaults to `mavlink_port1_extra3_rate`) | -| `EXTRA3` | `BATTERY_STATUS`, `SCALED_PRESSURE`, `STATUSTEXT` (when present) | 1 Hz | +| `EXTRA3` | `BATTERY_STATUS`, `SCALED_PRESSURE`, `SYSTEM_TIME`, `STATUSTEXT` (when present) | 1 Hz | ### Routing, forwarding and local handling @@ -63,11 +63,13 @@ Ports 2..N use a secondary startup profile (heartbeat at 1 Hz, other streams dis - Broadcast messages are forwarded to all other MAVLink ports (except `RADIO_STATUS`, which is not forwarded). - Targeted messages are forwarded only to ports with a learned route for that target. - Practical caveat: the first targeted message to a never-seen endpoint may not forward until that endpoint has sent at least one MAVLink frame. +- INAV’s local FC identity is always `(mavlink_sysid, MAV_COMP_ID_AUTOPILOT1)`. +- Traffic from the local system ID but a different component ID is treated as a remote component and can be learned into the route table. - Local/system broadcasts (`target_system=0` or local system ID with `target_component=0`) are fanned out to all local ports only for: - `REQUEST_DATA_STREAM` - `MAV_CMD_SET_MESSAGE_INTERVAL` - `MAV_CMD_CONTROL_HIGH_LATENCY` -- Other incoming commands/messages are handled on one resolved local port, based on local target matching. +- Other incoming commands/messages are handled once on the resolved local ingress path, but broadcast-targeted control requests still execute locally. ## Supported Outgoing Messages @@ -84,6 +86,7 @@ Messages are organized into MAVLink datastream groups. Each group sends **one me - `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. +- `SYSTEM_TIME`: with `time_boot_ms = millis()` and `time_unix_usec = 0`. - `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`. @@ -111,9 +114,9 @@ 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_CONDITION_YAW`: changes the current heading target when the active navigation state has yaw control. Accepts absolute heading (`param4=0`) and relative turns (`param4!=0`); turn-rate is ignored. -- `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_SET_MESSAGE_INTERVAL` / `MAV_CMD_GET_MESSAGE_INTERVAL`: adjust or query per-message periodic output for `HEARTBEAT`, `SYS_STATUS`, `EXTENDED_SYS_STATE`, RC channels, `GPS_RAW_INT`, `GLOBAL_POSITION_INT`, `GPS_GLOBAL_ORIGIN`, `ATTITUDE`, `VFR_HUD`, `BATTERY_STATUS`, `SCALED_PRESSURE`, and `SYSTEM_TIME`. `REQUEST_DATA_STREAM` still controls the legacy base stream groups; `SET_MESSAGE_INTERVAL` overrides individual messages on top. - `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_MESSAGE`: emits one selected message (`HEARTBEAT`, `SYS_STATUS`, `ATTITUDE`, `VFR_HUD`, `AVAILABLE_MODES`, `CURRENT_MODE`, `EXTENDED_SYS_STATE`, RC channels, `GPS_RAW_INT`, `GLOBAL_POSITION_INT`, `GPS_GLOBAL_ORIGIN`, `BATTERY_STATUS`, `SCALED_PRESSURE`, `SYSTEM_TIME`, and `HOME_POSITION` when available) or `UNSUPPORTED`. - `MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES`: returns stub `AUTOPILOT_VERSION` (v2 only; v1 returns `UNSUPPORTED`) advertising `SET_POSITION_TARGET_GLOBAL_INT` and `SET_POSITION_TARGET_LOCAL_NED`. - `MAV_CMD_REQUEST_PROTOCOL_VERSION`: returns stub `PROTOCOL_VERSION` (v2 only; v1 returns `UNSUPPORTED`). @@ -169,7 +172,7 @@ CLI mode is unavailable in MSP-over-MAVLink. - INAV accepts `TUNNEL` messages with private payload type `0x8001` as an MSP byte stream carried over MAVLink2. - `target_system` must match `mavlink_sysid`. -- `target_component` may be `0` or the local port `mavlink_port{1-4}_compid`. +- `target_component` may be `0` or `MAV_COMP_ID_AUTOPILOT1`. - `target_component=0` is handled on the ingress MAVLink port only; it is not fanned out to other local MAVLink ports. - MSP replies are sent back to the requester as one or more `TUNNEL` messages on that same ingress port. - MSP framing is preserved end-to-end: MSPv1 requests get MSPv1 replies, and MSPv2 requests get MSPv2 replies. diff --git a/docs/Settings.md b/docs/Settings.md index 31e8095d34e..23c5da33457 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2652,16 +2652,6 @@ Autopilot type to advertise for MAVLink telemetry --- -### mavlink_port1_compid - -MAVLink Component ID for MAVLink port 1 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 1 | 255 | - ---- - ### mavlink_port1_ext_status_rate Rate of the extended status message for MAVLink telemetry on port 1 @@ -2752,16 +2742,6 @@ Rate of the RC channels message for MAVLink telemetry on port 1 --- -### mavlink_port2_compid - -MAVLink Component ID for MAVLink port 2 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 1 | 255 | - ---- - ### mavlink_port2_high_latency Enable MAVLink high-latency mode on port 2 @@ -2792,16 +2772,6 @@ MAVLink radio type for port 2. Affects RSSI and LQ reporting on OSD. --- -### mavlink_port3_compid - -MAVLink Component ID for MAVLink port 3 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 1 | 255 | - ---- - ### mavlink_port3_high_latency Enable MAVLink high-latency mode on port 3 @@ -2832,16 +2802,6 @@ MAVLink radio type for port 3. Affects RSSI and LQ reporting on OSD. --- -### mavlink_port4_compid - -MAVLink Component ID for MAVLink port 4 - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 1 | 255 | - ---- - ### mavlink_port4_high_latency Enable MAVLink high-latency mode on port 4 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b9d09980455..118582e8071 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3298,13 +3298,6 @@ groups: min: 1 max: 255 default_value: 1 - - name: mavlink_port1_compid - field: mavlink[0].compid - description: "MAVLink Component ID for MAVLink port 1" - type: uint8_t - min: 1 - max: 255 - default_value: 1 - name: mavlink_port1_high_latency field: mavlink[0].high_latency description: "Enable MAVLink high-latency mode on port 1" @@ -3322,13 +3315,6 @@ groups: table: mavlink_radio_type default_value: "GENERIC" type: uint8_t - - name: mavlink_port2_compid - field: mavlink[1].compid - description: "MAVLink Component ID for MAVLink port 2" - type: uint8_t - min: 1 - max: 255 - default_value: 1 - name: mavlink_port2_high_latency field: mavlink[1].high_latency description: "Enable MAVLink high-latency mode on port 2" @@ -3346,13 +3332,6 @@ groups: table: mavlink_radio_type default_value: "GENERIC" type: uint8_t - - name: mavlink_port3_compid - field: mavlink[2].compid - description: "MAVLink Component ID for MAVLink port 3" - type: uint8_t - min: 1 - max: 255 - default_value: 1 - name: mavlink_port3_high_latency field: mavlink[2].high_latency description: "Enable MAVLink high-latency mode on port 3" @@ -3370,13 +3349,6 @@ groups: table: mavlink_radio_type default_value: "GENERIC" type: uint8_t - - name: mavlink_port4_compid - field: mavlink[3].compid - description: "MAVLink Component ID for MAVLink port 4" - type: uint8_t - min: 1 - max: 255 - default_value: 1 - name: mavlink_port4_high_latency field: mavlink[3].high_latency description: "Enable MAVLink high-latency mode on port 4" diff --git a/src/main/io/serial.c b/src/main/io/serial.c index e7865f3fac2..d6b82da02b8 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -296,8 +296,10 @@ bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) if ((portConfig->functionMask & FUNCTION_MSP) && (portConfig->functionMask & ALL_FUNCTIONS_SHARABLE_WITH_MSP)) { // MSP & telemetry +#ifdef USE_TELEMETRY } else if (telemetryCheckRxPortShared(portConfig)) { // serial RX & telemetry +#endif } else { // some other combination return false; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 6b7e5496475..29fe90f58f9 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -119,7 +119,7 @@ static const mavlinkTelemetryPortConfig_t *mavActiveConfig = NULL; static mavlink_message_t mavSendMsg; static mavlink_message_t mavRecvMsg; -// Set active MAV identity from active port settings. +// Set active MAV identity from global MAVLink settings. static uint8_t mavSystemId = 1; static uint8_t mavAutopilotType; static uint8_t mavComponentId = MAV_COMP_ID_AUTOPILOT1; @@ -277,7 +277,7 @@ static void mavlinkSetActivePortContext(uint8_t portIndex) const mavlinkTelemetryCommonConfig_t *commonConfig = mavlinkGetCommonConfig(); mavAutopilotType = commonConfig->autopilot_type; mavSystemId = commonConfig->sysid; - mavComponentId = mavActiveConfig->compid; + mavComponentId = MAV_COMP_ID_AUTOPILOT1; mavlinkApplyActivePortOutputVersion(); } @@ -290,6 +290,145 @@ static uint8_t mavlinkClampStreamRate(uint8_t rate) return rate; } +static int32_t mavlinkRateToIntervalUs(uint8_t rate) +{ + rate = mavlinkClampStreamRate(rate); + if (rate == 0) { + return -1; + } + + return 1000000 / rate; +} + +static bool mavlinkPeriodicMessageFromMessageId(uint16_t messageId, mavlinkPeriodicMessage_e *periodicMessage) +{ + switch (messageId) { + case MAVLINK_MSG_ID_HEARTBEAT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_HEARTBEAT; + return true; + case MAVLINK_MSG_ID_SYS_STATUS: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SYS_STATUS; + return true; + case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_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: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS; + return true; + case MAVLINK_MSG_ID_GPS_RAW_INT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT; + return true; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT; + return true; + case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN; + return true; + case MAVLINK_MSG_ID_ATTITUDE: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_ATTITUDE; + return true; + case MAVLINK_MSG_ID_VFR_HUD: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_VFR_HUD; + return true; + case MAVLINK_MSG_ID_BATTERY_STATUS: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS; + return true; + case MAVLINK_MSG_ID_SCALED_PRESSURE: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE; + return true; + case MAVLINK_MSG_ID_SYSTEM_TIME: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME; + return true; + default: + return false; + } +} + +static uint8_t mavlinkPeriodicMessageBaseStream(mavlinkPeriodicMessage_e periodicMessage) +{ + switch (periodicMessage) { + case MAVLINK_PERIODIC_MESSAGE_HEARTBEAT: + return MAV_DATA_STREAM_HEARTBEAT; + case MAVLINK_PERIODIC_MESSAGE_SYS_STATUS: + return MAV_DATA_STREAM_EXTENDED_STATUS; + case MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE: + return MAV_DATA_STREAM_EXTENDED_SYS_STATE; + case MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS: + return MAV_DATA_STREAM_RC_CHANNELS; + case MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT: + case MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT: + case MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN: + return MAV_DATA_STREAM_POSITION; + case MAVLINK_PERIODIC_MESSAGE_ATTITUDE: + return MAV_DATA_STREAM_EXTRA1; + case MAVLINK_PERIODIC_MESSAGE_VFR_HUD: + return MAV_DATA_STREAM_EXTRA2; + case MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS: + case MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE: + case MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME: + return MAV_DATA_STREAM_EXTRA3; + default: + return MAV_DATA_STREAM_ALL; + } +} + +static void mavlinkResetMessageSchedule(mavlinkPeriodicMessage_e periodicMessage) +{ + if (!mavActivePort) { + return; + } + + mavActivePort->mavMessageNextDue[periodicMessage] = 0; +} + +static void mavlinkResetMessagesForStream(uint8_t streamNum) +{ + if (!mavActivePort) { + return; + } + + for (uint8_t messageIndex = 0; messageIndex < MAVLINK_PERIODIC_MESSAGE_COUNT; messageIndex++) { + if (mavlinkPeriodicMessageBaseStream((mavlinkPeriodicMessage_e)messageIndex) == streamNum) { + mavActivePort->mavMessageNextDue[messageIndex] = 0; + } + } +} + +static int32_t mavlinkMessageBaseIntervalUs(mavlinkPeriodicMessage_e periodicMessage) +{ + if (!mavActivePort) { + return -1; + } + + return mavlinkRateToIntervalUs(mavActivePort->mavRates[mavlinkPeriodicMessageBaseStream(periodicMessage)]); +} + +static int32_t mavlinkMessageIntervalUs(mavlinkPeriodicMessage_e periodicMessage) +{ + if (!mavActivePort) { + return -1; + } + + const int32_t overrideIntervalUs = mavActivePort->mavMessageOverrideIntervalsUs[periodicMessage]; + if (overrideIntervalUs != 0) { + return overrideIntervalUs; + } + + return mavlinkMessageBaseIntervalUs(periodicMessage); +} + +static void mavlinkSetMessageOverrideIntervalUs(mavlinkPeriodicMessage_e periodicMessage, int32_t intervalUs) +{ + if (!mavActivePort) { + return; + } + + mavActivePort->mavMessageOverrideIntervalsUs[periodicMessage] = intervalUs; + mavlinkResetMessageSchedule(periodicMessage); +} + static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum, timeUs_t currentTimeUs) { if (!mavActivePort || streamNum >= MAXSTREAMS) { @@ -317,20 +456,26 @@ static void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate) } mavActivePort->mavRates[streamNum] = mavlinkClampStreamRate(rate); mavActivePort->mavStreamNextDue[streamNum] = 0; + mavlinkResetMessagesForStream(streamNum); } -static int32_t mavlinkStreamIntervalUs(uint8_t streamNum) +static int mavlinkMessageTrigger(mavlinkPeriodicMessage_e periodicMessage, timeUs_t currentTimeUs) { - if (!mavActivePort || streamNum >= MAXSTREAMS) { - return -1; + if (!mavActivePort) { + return 0; } - uint8_t rate = mavlinkClampStreamRate(mavActivePort->mavRates[streamNum]); - if (rate == 0) { - return -1; + const int32_t intervalUs = mavlinkMessageIntervalUs(periodicMessage); + if (intervalUs <= 0) { + return 0; } - return 1000000 / rate; + if ((mavActivePort->mavMessageNextDue[periodicMessage] == 0) || (cmpTimeUs(currentTimeUs, mavActivePort->mavMessageNextDue[periodicMessage]) >= 0)) { + mavActivePort->mavMessageNextDue[periodicMessage] = currentTimeUs + intervalUs; + return 1; + } + + return 0; } static void configureMAVLinkStreamRates(uint8_t portIndex) @@ -375,6 +520,8 @@ static void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) state->txSeq = 0; state->txDroppedFrames = 0; memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); + memset(state->mavMessageOverrideIntervalsUs, 0, sizeof(state->mavMessageOverrideIntervalsUs)); + memset(state->mavMessageNextDue, 0, sizeof(state->mavMessageNextDue)); memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); resetMspPort(&mavTunnelMspPorts[portIndex], NULL); @@ -448,6 +595,8 @@ static void configureMAVLinkTelemetryPort(uint8_t portIndex) state->txSeq = 0; state->txDroppedFrames = 0; memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); + memset(state->mavMessageOverrideIntervalsUs, 0, sizeof(state->mavMessageOverrideIntervalsUs)); + memset(state->mavMessageNextDue, 0, sizeof(state->mavMessageNextDue)); memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); resetMspPort(&mavTunnelMspPorts[portIndex], NULL); @@ -1301,7 +1450,7 @@ static void mavlinkSendHomePosition(void) mavlinkSendMessage(); } -void mavlinkSendPosition(timeUs_t currentTimeUs) +static void mavlinkSendGpsRawInt(timeUs_t currentTimeUs) { uint8_t gpsFixType = 0; @@ -1354,8 +1503,18 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) 0); mavlinkSendMessage(); +} + +static void mavlinkSendGlobalPositionInt(timeUs_t currentTimeUs) +{ + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { + return; + } - // Global position mavlink_msg_global_position_int_pack(mavSystemId, mavComponentId, &mavSendMsg, // time_boot_ms Timestamp (milliseconds since system boot) currentTimeUs / 1000, @@ -1378,7 +1537,10 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) ); mavlinkSendMessage(); +} +static void mavlinkSendGpsGlobalOrigin(void) +{ mavlink_msg_gps_global_origin_pack(mavSystemId, mavComponentId, &mavSendMsg, // latitude Latitude (WGS84), expressed as * 1E7 GPS_home.lat, @@ -1392,6 +1554,13 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) mavlinkSendMessage(); } + +void mavlinkSendPosition(timeUs_t currentTimeUs) +{ + mavlinkSendGpsRawInt(currentTimeUs); + mavlinkSendGlobalPositionInt(currentTimeUs); + mavlinkSendGpsGlobalOrigin(); +} #endif void mavlinkSendAttitude(void) @@ -1599,6 +1768,18 @@ static void mavlinkSendScaledPressure(void) mavlinkSendMessage(); } +static void mavlinkSendSystemTime(void) +{ + mavlink_msg_system_time_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + 0, + millis()); + + mavlinkSendMessage(); +} + static bool mavlinkSendStatusText(void) { // FIXME - Status text is limited to boards with USE_OSD @@ -1828,38 +2009,58 @@ void processMAVLinkTelemetry(timeUs_t currentTimeUs) } // is executed @ TELEMETRY_MAVLINK_MAXRATE rate - if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS, currentTimeUs)) { + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SYS_STATUS, currentTimeUs)) { mavlinkSendSystemStatus(); } - if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS, currentTimeUs)) { + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS, currentTimeUs)) { mavlinkSendRCChannelsAndRSSI(); } #ifdef USE_GPS - if (mavlinkStreamTrigger(MAV_DATA_STREAM_POSITION, currentTimeUs)) { - mavlinkSendPosition(currentTimeUs); + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT, currentTimeUs)) { + mavlinkSendGpsRawInt(currentTimeUs); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT, currentTimeUs)) { + mavlinkSendGlobalPositionInt(currentTimeUs); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN, currentTimeUs)) { + mavlinkSendGpsGlobalOrigin(); } #endif - if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA1, currentTimeUs)) { + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_ATTITUDE, currentTimeUs)) { mavlinkSendAttitude(); } - if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2, currentTimeUs)) { + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_VFR_HUD, currentTimeUs)) { mavlinkSendVfrHud(); } - if (mavlinkStreamTrigger(MAV_DATA_STREAM_HEARTBEAT, currentTimeUs)) { + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_HEARTBEAT, currentTimeUs)) { mavlinkSendHeartbeat(); } - if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_SYS_STATE, currentTimeUs)) { + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE, currentTimeUs)) { mavlinkSendExtendedSysState(); } + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS, currentTimeUs)) { + mavlinkSendBatteryStatus(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE, currentTimeUs)) { + mavlinkSendScaledPressure(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME, currentTimeUs)) { + mavlinkSendSystemTime(); + } + if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3, currentTimeUs)) { - mavlinkSendBatteryTemperatureStatusText(); + mavlinkSendStatusText(); } } @@ -2325,43 +2526,18 @@ static bool handleIncoming_MISSION_REQUEST(void) return true; } -static bool mavlinkMessageToStream(uint16_t messageId, uint8_t *streamNum) +static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) { - switch (messageId) { - case MAVLINK_MSG_ID_HEARTBEAT: - *streamNum = MAV_DATA_STREAM_HEARTBEAT; - return true; - 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 (targetSystem != 0 && targetSystem != mavSystemId) { + return false; + } + + if (targetComponent != 0 && targetComponent != mavComponentId) { + return false; } -} + return true; +} static void mavlinkSendCommandAck(uint16_t command, MAV_RESULT result, uint8_t ackTargetSystem, uint8_t ackTargetComponent) { @@ -2377,10 +2553,7 @@ static void mavlinkSendCommandAck(uint16_t command, MAV_RESULT result, uint8_t a static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent, 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; - } - if (targetComponent != 0 && targetComponent != mavComponentId) { + if (!mavlinkIsLocalTarget(targetSystem, targetComponent)) { return false; } UNUSED(param3); @@ -2459,27 +2632,26 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent } case MAV_CMD_SET_MESSAGE_INTERVAL: { - uint8_t stream; + mavlinkPeriodicMessage_e periodicMessage; MAV_RESULT result = MAV_RESULT_UNSUPPORTED; - if (mavlinkMessageToStream((uint16_t)param1, &stream)) { + if (mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { if (param2 == 0.0f) { - mavlinkSetStreamRate(stream, mavActivePort->mavRatesConfigured[stream]); + mavlinkSetMessageOverrideIntervalUs(periodicMessage, 0); result = MAV_RESULT_ACCEPTED; } else if (param2 < 0.0f) { - mavlinkSetStreamRate(stream, 0); + mavlinkSetMessageOverrideIntervalUs(periodicMessage, -1); 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; + const uint32_t minIntervalUs = 1000000UL / TELEMETRY_MAVLINK_MAXRATE; + if (intervalUs < minIntervalUs) { + intervalUs = minIntervalUs; } + + mavlinkSetMessageOverrideIntervalUs(periodicMessage, (int32_t)intervalUs); + result = MAV_RESULT_ACCEPTED; } } } @@ -2489,19 +2661,19 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent } case MAV_CMD_GET_MESSAGE_INTERVAL: { - uint8_t stream; - if (!mavlinkMessageToStream((uint16_t)param1, &stream)) { + mavlinkPeriodicMessage_e periodicMessage; + if (!mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); return true; } - mavlink_msg_message_interval_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - (uint16_t)param1, - mavlinkStreamIntervalUs(stream)); - mavlinkSendMessage(); + mavlink_msg_message_interval_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + (uint16_t)param1, + mavlinkMessageIntervalUs(periodicMessage)); + mavlinkSendMessage(); mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); return true; @@ -2610,10 +2782,20 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent sent = true; break; case MAVLINK_MSG_ID_GPS_RAW_INT: + #ifdef USE_GPS + mavlinkSendGpsRawInt(micros()); + sent = true; + #endif + break; case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + #ifdef USE_GPS + mavlinkSendGlobalPositionInt(micros()); + sent = true; + #endif + break; case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: #ifdef USE_GPS - mavlinkSendPosition(micros()); + mavlinkSendGpsGlobalOrigin(); sent = true; #endif break; @@ -2625,6 +2807,10 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent mavlinkSendScaledPressure(); sent = true; break; + case MAVLINK_MSG_ID_SYSTEM_TIME: + mavlinkSendSystemTime(); + sent = true; + break; case MAVLINK_MSG_ID_STATUSTEXT: sent = mavlinkSendStatusText(); break; @@ -2747,10 +2933,7 @@ 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.target_component != 0 && msg.target_component != mavComponentId) { + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { return false; } @@ -2782,10 +2965,7 @@ static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void) mavlink_set_position_target_global_int_t msg; mavlink_msg_set_position_target_global_int_decode(&mavRecvMsg, &msg); - if (msg.target_system != mavSystemId) { - return false; - } - if (msg.target_component != 0 && msg.target_component != mavComponentId) { + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { return false; } @@ -2837,10 +3017,7 @@ 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) { + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { return false; } @@ -2880,7 +3057,7 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { mavlink_msg_param_request_list_decode(&mavRecvMsg, &msg); // Respond that we don't have any parameters to force Mission Planner to give up quickly - if (msg.target_system == mavSystemId) { + if (mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { // mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); mavlink_msg_param_value_pack(mavSystemId, mavComponentId, &mavSendMsg, 0, 0, 0, 0, 0); mavlinkSendMessage(); @@ -2975,19 +3152,7 @@ static bool handleIncoming_ADSB_VEHICLE(void) { static bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid) { - const uint8_t localSystemId = mavlinkGetCommonConfig()->sysid; - if (sysid != localSystemId) { - return false; - } - - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - const mavlinkTelemetryPortConfig_t *cfg = mavlinkGetPortConfig(portIndex); - if (cfg->compid == compid) { - return true; - } - } - - return false; + return sysid == mavlinkGetCommonConfig()->sysid && compid == MAV_COMP_ID_AUTOPILOT1; } static void mavlinkLearnRoute(uint8_t ingressPortIndex) @@ -3117,21 +3282,15 @@ static int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t tar return -1; } - if (ingressPortIndex < mavPortCount) { - const mavlinkTelemetryPortConfig_t *ingressCfg = mavlinkGetPortConfig(ingressPortIndex); - if (targetComponent <= 0 || ingressCfg->compid == targetComponent) { - return ingressPortIndex; - } + if (targetComponent > 0 && (uint8_t)targetComponent != MAV_COMP_ID_AUTOPILOT1) { + return -1; } - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - const mavlinkTelemetryPortConfig_t *cfg = mavlinkGetPortConfig(portIndex); - if (targetComponent <= 0 || cfg->compid == targetComponent) { - return portIndex; - } + if (ingressPortIndex < mavPortCount) { + return ingressPortIndex; } - return -1; + return mavPortCount > 0 ? 0 : -1; } static bool mavlinkShouldFanOutLocalBroadcast(const mavlink_message_t *msg) diff --git a/src/main/telemetry/mavlink.h b/src/main/telemetry/mavlink.h index 38aface2d5f..c732f6c3fff 100755 --- a/src/main/telemetry/mavlink.h +++ b/src/main/telemetry/mavlink.h @@ -45,6 +45,22 @@ #define MAVLINK_PORT_MASK(portIndex) (1U << (portIndex)) #define MAXSTREAMS MAVLINK_STREAM_COUNT +typedef enum { + MAVLINK_PERIODIC_MESSAGE_HEARTBEAT, + MAVLINK_PERIODIC_MESSAGE_SYS_STATUS, + MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE, + MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS, + MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT, + MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT, + MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN, + MAVLINK_PERIODIC_MESSAGE_ATTITUDE, + MAVLINK_PERIODIC_MESSAGE_VFR_HUD, + MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS, + MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE, + MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME, + MAVLINK_PERIODIC_MESSAGE_COUNT +} mavlinkPeriodicMessage_e; + typedef enum { MAV_FRAME_SUPPORTED_NONE = 0, MAV_FRAME_SUPPORTED_GLOBAL = (1 << 0), @@ -142,6 +158,8 @@ typedef struct mavlinkPortRuntime_s { uint8_t mavRates[MAVLINK_STREAM_COUNT]; uint8_t mavRatesConfigured[MAVLINK_STREAM_COUNT]; timeUs_t mavStreamNextDue[MAVLINK_STREAM_COUNT]; + int32_t mavMessageOverrideIntervalsUs[MAVLINK_PERIODIC_MESSAGE_COUNT]; + timeUs_t mavMessageNextDue[MAVLINK_PERIODIC_MESSAGE_COUNT]; uint8_t txSeq; uint32_t txDroppedFrames; mavlink_message_t mavRecvMsg; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index dba8709e6e9..4288b213bc1 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -102,7 +102,6 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra3_rate = SETTING_MAVLINK_PORT1_EXTRA3_RATE_DEFAULT, .min_txbuff = SETTING_MAVLINK_PORT1_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_PORT1_RADIO_TYPE_DEFAULT, - .compid = SETTING_MAVLINK_PORT1_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT1_HIGH_LATENCY_DEFAULT }, { @@ -114,7 +113,6 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra3_rate = 0, .min_txbuff = SETTING_MAVLINK_PORT2_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_PORT2_RADIO_TYPE_DEFAULT, - .compid = SETTING_MAVLINK_PORT2_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT2_HIGH_LATENCY_DEFAULT }, { @@ -126,7 +124,6 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra3_rate = 0, .min_txbuff = SETTING_MAVLINK_PORT3_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_PORT3_RADIO_TYPE_DEFAULT, - .compid = SETTING_MAVLINK_PORT3_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT3_HIGH_LATENCY_DEFAULT }, { @@ -138,7 +135,6 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra3_rate = 0, .min_txbuff = SETTING_MAVLINK_PORT4_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_PORT4_RADIO_TYPE_DEFAULT, - .compid = SETTING_MAVLINK_PORT4_COMPID_DEFAULT, .high_latency = SETTING_MAVLINK_PORT4_HIGH_LATENCY_DEFAULT } } diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index f2686c5dcac..9b1f0a6bca0 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -70,7 +70,6 @@ typedef struct mavlinkTelemetryPortConfig_s { uint8_t extra3_rate; uint8_t min_txbuff; uint8_t radio_type; - uint8_t compid; bool high_latency; } mavlinkTelemetryPortConfig_t; diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index a60b72357b2..7b579dcfa9f 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -221,6 +221,22 @@ static bool popTxMessage(mavlink_message_t *msg) return false; } +static bool findTxMessageById(uint32_t msgid, mavlink_message_t *match) +{ + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + + mavlink_message_t msg; + for (size_t i = 0; i < serialTxLen; i++) { + if (mavlink_parse_char(0, serialTxBuffer[i], &msg, &status) == MAVLINK_FRAMING_OK && msg.msgid == msgid) { + *match = msg; + return true; + } + } + + return false; +} + static std::vector parseTxMessages(void) { std::vector messages; @@ -287,8 +303,13 @@ static void initMavlinkTestState(void) telemetryConfigMutable()->mavlink_common.sysid = 1; telemetryConfigMutable()->mavlink_common.autopilot_type = MAVLINK_AUTOPILOT_ARDUPILOT; telemetryConfigMutable()->mavlink_common.version = 2; + telemetryConfigMutable()->mavlink[0].extended_status_rate = 2; + telemetryConfigMutable()->mavlink[0].rc_channels_rate = 1; + telemetryConfigMutable()->mavlink[0].position_rate = 2; + telemetryConfigMutable()->mavlink[0].extra1_rate = 2; + telemetryConfigMutable()->mavlink[0].extra2_rate = 2; + telemetryConfigMutable()->mavlink[0].extra3_rate = 1; telemetryConfigMutable()->mavlink[0].min_txbuff = 0; - telemetryConfigMutable()->mavlink[0].compid = MAV_COMP_ID_AUTOPILOT1; telemetryConfigMutable()->halfDuplex = 0; rxConfigMutable()->receiverType = RX_TYPE_NONE; @@ -455,6 +476,60 @@ TEST(MavlinkTelemetryTest, CommandLongRepositionUsesGlobalFrameAndParams) EXPECT_EQ(lastWaypoint.p1, 123); } +TEST(MavlinkTelemetryTest, BroadcastCommandLongRepositionExecutesLocally) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 0, 0, + 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(findTxMessageById(MAVLINK_MSG_ID_COMMAND_ACK, &ackMsg)); + + 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); +} + +TEST(MavlinkTelemetryTest, SameSystemDifferentComponentIsNotDroppedAsLocalIdentity) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 1, 42, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_REPOSITION, + 0, + 0, 0, 0, 0, + 37.5f, -122.25f, 12.3f); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_COMMAND_ACK, &ackMsg)); + + 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); +} + TEST(MavlinkTelemetryTest, CommandIntUnsupportedFrameIsRejected) { initMavlinkTestState(); @@ -876,6 +951,28 @@ TEST(MavlinkTelemetryTest, ParamRequestListRespondsWithEmptyParam) EXPECT_EQ(param.param_index, 0); } +TEST(MavlinkTelemetryTest, BroadcastParamRequestListRespondsWithEmptyParam) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_param_request_list_pack( + 42, 200, &msg, + 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t paramMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_PARAM_VALUE, ¶mMsg)); + + 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(); @@ -898,6 +995,26 @@ TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntSetsWaypoint) EXPECT_EQ(lastWaypoint.p3, 0); } +TEST(MavlinkTelemetryTest, BroadcastSetPositionTargetGlobalIntSetsWaypoint) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 0, 0, + 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); +} + TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude) { initMavlinkTestState(); @@ -977,6 +1094,36 @@ TEST(MavlinkTelemetryTest, SetPositionTargetLocalNedAltitudeOnlySetsAltitudeTarg EXPECT_EQ(lastAltitudeTargetCm, 1250); } +TEST(MavlinkTelemetryTest, BroadcastSetPositionTargetLocalNedAltitudeOnlySetsAltitudeTarget) +{ + initMavlinkTestState(); + estimatedPosition[Z] = 1000.0f; + + mavlink_message_t msg; + mavlink_msg_set_position_target_local_ned_pack( + 42, 200, &msg, + 0, 0, 0, + MAV_FRAME_LOCAL_OFFSET_NED, + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 0.0f, 0.0f, -2.5f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 1); + EXPECT_EQ(lastAltitudeTargetCm, 1250); +} + TEST(MavlinkTelemetryTest, SetPositionTargetLocalNedIgnoresXyMotionRequests) { initMavlinkTestState(); @@ -1066,6 +1213,212 @@ TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream) EXPECT_EQ(interval.interval_us, -1); } +TEST(MavlinkTelemetryTest, BroadcastRequestDataStreamAdjustsBaseInterval) +{ + initMavlinkTestState(); + + mavlink_message_t setMsg; + mavlink_msg_request_data_stream_pack( + 42, 200, &setMsg, + 0, 0, + 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, testTargetComponent, + 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(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + 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); +} + +TEST(MavlinkTelemetryTest, SetMessageIntervalOverridesOnlyRequestedPositionMessage) +{ + initMavlinkTestState(); + + mavlink_message_t setMsg; + mavlink_msg_command_long_pack( + 42, 200, &setMsg, + 1, testTargetComponent, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + 200000.0f, 0, 0, 0, 0, 0); + + pushRxMessage(&setMsg); + handleMAVLinkTelemetry(1000); + + const uint16_t messageIds[] = { + MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + MAVLINK_MSG_ID_GPS_RAW_INT, + MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN + }; + const int32_t expectedIntervals[] = { + 200000, + 500000, + 500000 + }; + + for (size_t i = 0; i < ARRAYLEN(messageIds); i++) { + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)messageIds[i], + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, messageIds[i]); + EXPECT_EQ(interval.interval_us, expectedIntervals[i]); + } +} + +TEST(MavlinkTelemetryTest, SetMessageIntervalCanDisableBatteryStatusWithoutAffectingScaledPressure) +{ + initMavlinkTestState(); + + mavlink_message_t setMsg; + mavlink_msg_command_long_pack( + 42, 200, &setMsg, + 1, testTargetComponent, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_BATTERY_STATUS, + -1.0f, 0, 0, 0, 0, 0); + + pushRxMessage(&setMsg); + handleMAVLinkTelemetry(1000); + + const uint16_t messageIds[] = { + MAVLINK_MSG_ID_BATTERY_STATUS, + MAVLINK_MSG_ID_SCALED_PRESSURE + }; + const int32_t expectedIntervals[] = { + -1, + 1000000 + }; + + for (size_t i = 0; i < ARRAYLEN(messageIds); i++) { + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)messageIds[i], + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, messageIds[i]); + EXPECT_EQ(interval.interval_us, expectedIntervals[i]); + } +} + +TEST(MavlinkTelemetryTest, SetMessageIntervalResetRevertsToCurrentGroupInterval) +{ + initMavlinkTestState(); + + mavlink_message_t streamMsg; + mavlink_msg_request_data_stream_pack( + 42, 200, &streamMsg, + 1, testTargetComponent, + MAV_DATA_STREAM_POSITION, 10, 1); + + pushRxMessage(&streamMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t setMsg; + mavlink_msg_command_long_pack( + 42, 200, &setMsg, + 1, testTargetComponent, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + 250000.0f, 0, 0, 0, 0, 0); + + pushRxMessage(&setMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t clearMsg; + mavlink_msg_command_long_pack( + 42, 200, &clearMsg, + 1, testTargetComponent, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + 0.0f, 0, 0, 0, 0, 0); + + pushRxMessage(&clearMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT); + EXPECT_EQ(interval.interval_us, 100000); +} + TEST(MavlinkTelemetryTest, HeartbeatIntervalIsIndependentFromExtra2Stream) { initMavlinkTestState(); @@ -1131,6 +1484,63 @@ TEST(MavlinkTelemetryTest, RequestProtocolVersionUsesConfiguredVersion) EXPECT_EQ(version.max_version, 200); } +TEST(MavlinkTelemetryTest, SystemTimeSupportsRequestPeriodicOutputAndIntervalQuery) +{ + initMavlinkTestState(); + fakeMillis = 1234; + + mavlink_message_t requestMsg; + mavlink_msg_command_long_pack( + 42, 200, &requestMsg, + 1, testTargetComponent, + MAV_CMD_REQUEST_MESSAGE, + 0, + (float)MAVLINK_MSG_ID_SYSTEM_TIME, + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&requestMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t systemTimeMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_SYSTEM_TIME, &systemTimeMsg)); + + mavlink_system_time_t systemTime; + mavlink_msg_system_time_decode(&systemTimeMsg, &systemTime); + + EXPECT_EQ(systemTime.time_unix_usec, 0U); + EXPECT_EQ(systemTime.time_boot_ms, 1234U); + + serialTxLen = 0; + handleMAVLinkTelemetry(1000000); + + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_SYSTEM_TIME, &systemTimeMsg)); + mavlink_msg_system_time_decode(&systemTimeMsg, &systemTime); + EXPECT_EQ(systemTime.time_boot_ms, 1234U); + + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_SYSTEM_TIME, + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_SYSTEM_TIME); + EXPECT_EQ(interval.interval_us, 1000000); +} + TEST(MavlinkTelemetryTest, RequestAutopilotCapabilitiesReportsLocalNedCapabilityAndPackedVersion) { initMavlinkTestState(); From 64c268048e1074bba371c07e2e971fbdbce78a53 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Fri, 3 Apr 2026 17:38:21 -0400 Subject: [PATCH 34/42] Refactor MAVLink runtime split --- src/main/CMakeLists.txt | 12 + src/main/fc/fc_mavlink.c | 2553 ++++++++++++++++++++ src/main/fc/fc_mavlink.h | 29 + src/main/mavlink/mavlink_internal.h | 131 + src/main/mavlink/mavlink_ports.c | 73 + src/main/mavlink/mavlink_ports.h | 4 + src/main/mavlink/mavlink_routing.c | 172 ++ src/main/mavlink/mavlink_routing.h | 13 + src/main/mavlink/mavlink_runtime.c | 281 +++ src/main/mavlink/mavlink_runtime.h | 16 + src/main/mavlink/mavlink_streams.c | 309 +++ src/main/mavlink/mavlink_streams.h | 20 + src/main/telemetry/mavlink.c | 3452 +-------------------------- src/test/unit/CMakeLists.txt | 3 +- src/test/unit/mavlink_unittest.cc | 2 + 15 files changed, 3624 insertions(+), 3446 deletions(-) create mode 100644 src/main/fc/fc_mavlink.c create mode 100644 src/main/fc/fc_mavlink.h create mode 100644 src/main/mavlink/mavlink_internal.h create mode 100644 src/main/mavlink/mavlink_ports.c create mode 100644 src/main/mavlink/mavlink_ports.h create mode 100644 src/main/mavlink/mavlink_routing.c create mode 100644 src/main/mavlink/mavlink_routing.h create mode 100644 src/main/mavlink/mavlink_runtime.c create mode 100644 src/main/mavlink/mavlink_runtime.h create mode 100644 src/main/mavlink/mavlink_streams.c create mode 100644 src/main/mavlink/mavlink_streams.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 101c7223372..26ca5b952c6 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -290,6 +290,8 @@ main_sources(COMMON_SRC fc/fc_tasks.c fc/fc_tasks.h fc/fc_hardfaults.c + fc/fc_mavlink.c + fc/fc_mavlink.h fc/fc_msp.c fc/fc_msp.h fc/fc_msp_box.c @@ -391,6 +393,16 @@ main_sources(COMMON_SRC io/osd/custom_elements.c + mavlink/mavlink_internal.h + mavlink/mavlink_ports.c + mavlink/mavlink_ports.h + mavlink/mavlink_routing.c + mavlink/mavlink_routing.h + mavlink/mavlink_runtime.c + mavlink/mavlink_runtime.h + mavlink/mavlink_streams.c + mavlink/mavlink_streams.h + msp/msp_serial.c msp/msp_serial.h diff --git a/src/main/fc/fc_mavlink.c b/src/main/fc/fc_mavlink.c new file mode 100644 index 00000000000..bd7f31eed5e --- /dev/null +++ b/src/main/fc/fc_mavlink.c @@ -0,0 +1,2553 @@ +#include "mavlink/mavlink_internal.h" + +#include "fc/fc_mavlink.h" + +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +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) + { + case FLM_ACRO: return COPTER_MODE_ACRO; + case FLM_ACRO_AIR: return COPTER_MODE_ACRO; + case FLM_ANGLE: return COPTER_MODE_STABILIZE; + case FLM_HORIZON: return COPTER_MODE_STABILIZE; + case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; + case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return COPTER_MODE_GUIDED; + } else { + return COPTER_MODE_POSHOLD; + } + } + case FLM_RTH: return COPTER_MODE_RTL; + case FLM_MISSION: return COPTER_MODE_AUTO; + case FLM_LAUNCH: return COPTER_MODE_THROW; + case FLM_FAILSAFE: + { + if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { + return COPTER_MODE_RTL; + } else if (failsafePhase() == FAILSAFE_LANDING) { + return COPTER_MODE_LAND; + } else { + return COPTER_MODE_RTL; + } + } + default: return COPTER_MODE_STABILIZE; + } +} + +static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) +{ + switch (flightMode) + { + case FLM_MANUAL: return PLANE_MODE_MANUAL; + case FLM_ACRO: return PLANE_MODE_ACRO; + case FLM_ACRO_AIR: return PLANE_MODE_ACRO; + case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; + case FLM_HORIZON: return PLANE_MODE_STABILIZE; + case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; + case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return PLANE_MODE_GUIDED; + } else { + return PLANE_MODE_LOITER; + } + } + case FLM_RTH: return PLANE_MODE_RTL; + case FLM_MISSION: return PLANE_MODE_AUTO; + case FLM_CRUISE: return PLANE_MODE_CRUISE; + case FLM_LAUNCH: return PLANE_MODE_TAKEOFF; + case FLM_FAILSAFE: //failsafePhase_e + { + if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { + return PLANE_MODE_RTL; + } + else if (failsafePhase() == FAILSAFE_LANDING) { + return PLANE_MODE_AUTOLAND; + } + else { + return PLANE_MODE_RTL; + } + } + 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 void mavlinkResetTunnelState(uint8_t ingressPortIndex) +{ + resetMspPort(&mavTunnelMspPorts[ingressPortIndex], NULL); + mavTunnelRemoteSystemIds[ingressPortIndex] = 0; + mavTunnelRemoteComponentIds[ingressPortIndex] = 0; +} + +static void mavlinkSendTunnelReply(uint8_t targetSystem, uint8_t targetComponent, const uint8_t *payload, uint8_t payloadLength) +{ + uint8_t tunnelPayload[MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN] = { 0 }; + memcpy(tunnelPayload, payload, payloadLength); + + mavlink_msg_tunnel_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + targetSystem, + targetComponent, + MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP, + payloadLength, + tunnelPayload); + mavlinkSendMessage(); +} + +static void mavlinkSendTunnelMspReply(uint8_t targetSystem, uint8_t targetComponent, mspPacket_t *reply, uint8_t *replyPayloadHead, mspVersion_e mspVersion) +{ + sbufSwitchToReader(&reply->buf, replyPayloadHead); + + const int frameLength = mspSerialEncodePacket(reply, mspVersion, mavTunnelFrameBuf, sizeof(mavTunnelFrameBuf)); + for (int offset = 0; offset < frameLength; offset += MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { + const uint8_t chunkLength = MIN(MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN, frameLength - offset); + mavlinkSendTunnelReply( + targetSystem, + targetComponent, + mavTunnelFrameBuf + offset, + chunkLength); + } +} + +static bool handleIncoming_TUNNEL(uint8_t ingressPortIndex) +{ + if (mavlinkGetProtocolVersion() == 1) { + return false; + } + + mavlink_tunnel_t msg; + mavlink_msg_tunnel_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.payload_type != MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP) { + return false; + } + + if (msg.target_system != mavSystemId) { + return false; + } + + if (msg.target_component != 0 && msg.target_component != mavComponentId) { + return false; + } + + mspPort_t *mspPort = &mavTunnelMspPorts[ingressPortIndex]; + const timeMs_t now = millis(); + if (msg.payload_length > MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { + mavlinkResetTunnelState(ingressPortIndex); + return false; + } + + if (mspPort->c_state != MSP_IDLE && + ((now - mspPort->lastActivityMs) > MAVLINK_TUNNEL_MSP_TIMEOUT_MS || + mavTunnelRemoteSystemIds[ingressPortIndex] != mavlinkContext.recvMsg.sysid || + mavTunnelRemoteComponentIds[ingressPortIndex] != mavlinkContext.recvMsg.compid)) { + mavlinkResetTunnelState(ingressPortIndex); + } + + mavTunnelRemoteSystemIds[ingressPortIndex] = mavlinkContext.recvMsg.sysid; + mavTunnelRemoteComponentIds[ingressPortIndex] = mavlinkContext.recvMsg.compid; + mspPort->lastActivityMs = now; + + bool handled = false; + for (uint8_t payloadIndex = 0; payloadIndex < msg.payload_length; payloadIndex++) { + if (!mspSerialProcessReceivedByte(mspPort, msg.payload[payloadIndex])) { + continue; + } + + handled = true; + if (mspPort->c_state != MSP_COMMAND_RECEIVED) { + continue; + } + + mspPacket_t reply = { + .buf = { .ptr = mavTunnelReplyPayloadBuf, .end = ARRAYEND(mavTunnelReplyPayloadBuf), }, + .cmd = -1, + .flags = 0, + .result = 0, + }; + uint8_t *replyPayloadHead = reply.buf.ptr; + + if (mspPort->cmdMSP == MSP_SET_PASSTHROUGH) { + reply.cmd = MSP_SET_PASSTHROUGH; + reply.result = MSP_RESULT_ERROR; + mavlinkSendTunnelMspReply(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, &reply, replyPayloadHead, mspPort->mspVersion); + mavlinkResetTunnelState(ingressPortIndex); + break; + } + + mspPostProcessFnPtr mspPostProcessFn = NULL; + const uint16_t command = mspPort->cmdMSP; + mspResult_e status = mspSerialProcessCommand(mspPort, mspFcProcessCommand, &reply, &mspPostProcessFn); + + if (mspPostProcessFn && command != MSP_REBOOT) { + sbufInit(&reply.buf, mavTunnelReplyPayloadBuf, ARRAYEND(mavTunnelReplyPayloadBuf)); + reply.result = MSP_RESULT_ERROR; + mspPostProcessFn = NULL; + status = MSP_RESULT_ERROR; + } + + if (status != MSP_RESULT_NO_REPLY) { + mavlinkSendTunnelMspReply(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, &reply, replyPayloadHead, mspPort->mspVersion); + } + + mavlinkResetTunnelState(ingressPortIndex); + + if (mspPostProcessFn) { + waitForSerialPortToFinishTransmitting(mavPortStates[ingressPortIndex].port); + mspPostProcessFn(mavPortStates[ingressPortIndex].port); + } + + break; + } + + return handled; +} + +static void mavlinkSendAutopilotVersion(void) +{ + if (mavlinkGetProtocolVersion() == 1) return; + + // Capabilities aligned with what we actually support. + uint64_t capabilities = 0; + capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; + capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; + capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT; + capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT; + capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; + capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT; + + 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 + flightSwVersion, // 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 (mavlinkGetProtocolVersion() == 1) return; + + uint8_t specHash[8] = {0}; + uint8_t libHash[8] = {0}; + const uint16_t protocolVersion = (uint16_t)mavlinkGetProtocolVersion() * 100; + + mavlink_msg_protocol_version_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + protocolVersion, + protocolVersion, + protocolVersion, + specHash, + libHash); + + mavlinkSendMessage(); +} + +static bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask) +{ + switch (frame) { + case MAV_FRAME_GLOBAL: + return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL; + case MAV_FRAME_GLOBAL_INT: + return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_INT; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT; + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT; + default: + return false; + } +} + +static bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame) +{ + return frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_INT; +} + +static MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters) +{ +#ifdef USE_BARO + geoAltitudeDatumFlag_e datum; + + switch (frame) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_INT: + datum = NAV_WP_MSL_DATUM; + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + datum = NAV_WP_TAKEOFF_DATUM; + break; + default: + return MAV_RESULT_UNSUPPORTED; + } + + const int32_t targetAltitudeCm = (int32_t)lrintf(altitudeMeters * 100.0f); + return navigationSetAltitudeTargetWithDatum(datum, targetAltitudeCm) ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED; +#else + UNUSED(frame); + UNUSED(altitudeMeters); + return MAV_RESULT_UNSUPPORTED; +#endif +} + +static MAV_MISSION_RESULT mavlinkMissionResultFromCommandResult(MAV_RESULT result) +{ + switch (result) { + case MAV_RESULT_ACCEPTED: + return MAV_MISSION_ACCEPTED; + case MAV_RESULT_UNSUPPORTED: + return MAV_MISSION_UNSUPPORTED; + default: + return MAV_MISSION_ERROR; + } +} + +static bool mavlinkHandleArmedGuidedMissionItem(uint8_t current, uint8_t frame, mavFrameSupportMask_e allowedFrames, int32_t latitudeE7, int32_t longitudeE7, float altitudeMeters) +{ + if (!isGCSValid()) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, + MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + if (!mavlinkFrameIsSupported(frame, allowedFrames)) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, + mavlinkMissionResultFromCommandResult(result), MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, + MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; +} + +static uint8_t navWaypointFrame(const navWaypoint_t *wp, bool useIntMessages) +{ + switch (wp->action) { + case NAV_WP_ACTION_RTH: + case NAV_WP_ACTION_JUMP: + case NAV_WP_ACTION_SET_HEAD: + return MAV_FRAME_MISSION; + default: + break; + } + + const bool absoluteAltitude = (wp->p3 & NAV_WP_ALTMODE) == NAV_WP_ALTMODE; + + if (absoluteAltitude) { + return useIntMessages ? MAV_FRAME_GLOBAL_INT : MAV_FRAME_GLOBAL; + } + + return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT; +} + +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" }, +}; +const uint8_t planeModesCount = (uint8_t)ARRAYLEN(planeModes); + +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" }, +}; +const uint8_t copterModesCount = (uint8_t)ARRAYLEN(copterModes); + +static bool mavlinkPlaneModeIsConfigured(uint8_t customMode) +{ + switch ((APM_PLANE_MODE)customMode) { + case PLANE_MODE_MANUAL: + return isModeActivationConditionPresent(BOXMANUAL); + case PLANE_MODE_ACRO: + return true; + case PLANE_MODE_STABILIZE: + return isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXANGLEHOLD); + case PLANE_MODE_FLY_BY_WIRE_A: + return isModeActivationConditionPresent(BOXANGLE); + case PLANE_MODE_FLY_BY_WIRE_B: + return isModeActivationConditionPresent(BOXNAVALTHOLD); + case PLANE_MODE_CRUISE: + return isModeActivationConditionPresent(BOXNAVCRUISE) || + (isModeActivationConditionPresent(BOXNAVCOURSEHOLD) && + isModeActivationConditionPresent(BOXNAVALTHOLD)); + case PLANE_MODE_AUTO: + return isModeActivationConditionPresent(BOXNAVWP); + case PLANE_MODE_RTL: + return isModeActivationConditionPresent(BOXNAVRTH); + case PLANE_MODE_GUIDED: + return isModeActivationConditionPresent(BOXNAVPOSHOLD) && + isModeActivationConditionPresent(BOXGCSNAV); + case PLANE_MODE_LOITER: + return isModeActivationConditionPresent(BOXNAVPOSHOLD); + case PLANE_MODE_TAKEOFF: + return isModeActivationConditionPresent(BOXNAVLAUNCH); + default: + return false; + } +} + +static bool mavlinkCopterModeIsConfigured(uint8_t customMode) +{ + switch ((APM_COPTER_MODE)customMode) { + case COPTER_MODE_ACRO: + return true; + case COPTER_MODE_STABILIZE: + return isModeActivationConditionPresent(BOXANGLE) || + isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXANGLEHOLD); + case COPTER_MODE_ALT_HOLD: + return isModeActivationConditionPresent(BOXNAVALTHOLD); + case COPTER_MODE_GUIDED: + return isModeActivationConditionPresent(BOXNAVPOSHOLD) && + isModeActivationConditionPresent(BOXGCSNAV); + case COPTER_MODE_POSHOLD: + return isModeActivationConditionPresent(BOXNAVPOSHOLD); + case COPTER_MODE_RTL: + return isModeActivationConditionPresent(BOXNAVRTH); + case COPTER_MODE_AUTO: + return isModeActivationConditionPresent(BOXNAVWP); + case COPTER_MODE_THROW: + return isModeActivationConditionPresent(BOXNAVLAUNCH); + case COPTER_MODE_BRAKE: + return isModeActivationConditionPresent(BOXBRAKING); + default: + return false; + } +} + +static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom, + bool (*isModeConfigured)(uint8_t customMode)) +{ + uint8_t availableCount = 0; + for (uint8_t i = 0; i < count; i++) { + if (isModeConfigured(modes[i].customMode)) { + availableCount++; + } + } + + if (availableCount == 0) { + return; + } + + uint8_t modeIndex = 0; + for (uint8_t i = 0; i < count; i++) { + if (!isModeConfigured(modes[i].customMode)) { + continue; + } + + modeIndex++; + const uint8_t stdMode = MAV_STANDARD_MODE_NON_STANDARD; + const uint32_t properties = 0; + + mavlink_msg_available_modes_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + availableCount, + modeIndex, + stdMode, + modes[i].customMode, + properties, + modes[i].name); + + mavlinkSendMessage(); + + if (modes[i].customMode == currentCustom) { + mavlink_msg_current_mode_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + stdMode, + currentCustom, + currentCustom); + mavlinkSendMessage(); + } + } +} + +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 + uint32_t onboard_control_sensors_present = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER); + // GYRO and RC are assumed as minimum requirements + uint32_t onboard_control_sensors_enabled = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER); + uint32_t onboard_control_sensors_health = 0; + + if (getHwGyroStatus() == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO; + // Missing presence will report as sensor unhealthy + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO; + } + + hardwareSensorStatus_e accStatus = getHwAccelerometerStatus(); + if (accStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } else if (accStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } else if (accStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } + + hardwareSensorStatus_e compassStatus = getHwCompassStatus(); + if (compassStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } else if (compassStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } else if (compassStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } + + hardwareSensorStatus_e baroStatus = getHwBarometerStatus(); + if (baroStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } else if (baroStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } else if (baroStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } + + hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus(); + if (pitotStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } else if (pitotStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } else if (pitotStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } + + hardwareSensorStatus_e gpsStatus = getHwGPSStatus(); + if (gpsStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; + } else if (gpsStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; + } else if (gpsStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; + } + + hardwareSensorStatus_e opFlowStatus = getHwOpticalFlowStatus(); + if (opFlowStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } else if (opFlowStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } else if (opFlowStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } + + hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus(); + if (rangefinderStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } else if (rangefinderStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } else if (rangefinderStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + + if (rxIsReceivingSignal() && rxAreFlightChannelsValid()) { + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + } + +#ifdef USE_BLACKBOX + // BLACKBOX is assumed enabled and present for boards with capability + onboard_control_sensors_present |= MAV_SYS_STATUS_LOGGING; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; + // Unhealthy only for cases with not enough space to record + if (!isBlackboxDeviceFull()) { + onboard_control_sensors_health |= MAV_SYS_STATUS_LOGGING; + } +#endif + + mavlink_msg_sys_status_pack(mavSystemId, mavComponentId, &mavSendMsg, + // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. + //Value of 0: not present. Value of 1: present. Indices according MAV_SYS_STATUS_SENSOR + onboard_control_sensors_present, + // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled + onboard_control_sensors_enabled, + // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error. + onboard_control_sensors_health, + // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + constrain(averageSystemLoadPercent*10, 0, 1000), + // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) + feature(FEATURE_VBAT) ? getBatteryVoltage() * 10 : 0, + // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + isAmperageConfigured() ? getAmperage() : -1, + // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery + feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100, + // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + 0, + // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + 0, + // errors_count1 Autopilot-specific errors + 0, + // errors_count2 Autopilot-specific errors + 0, + // errors_count3 Autopilot-specific errors + 0, + // errors_count4 Autopilot-specific errors + 0, 0, 0, 0); + + mavlinkSendMessage(); +} + +void mavlinkSendRCChannelsAndRSSI(void) +{ +#define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) + if (mavlinkGetProtocolVersion() == 1) { + mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, + // time_boot_ms Timestamp (milliseconds since system boot) + millis(), + // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + 0, + // chan1_raw RC channel 1 value, in microseconds + GET_CHANNEL_VALUE(0), + // chan2_raw RC channel 2 value, in microseconds + GET_CHANNEL_VALUE(1), + // chan3_raw RC channel 3 value, in microseconds + GET_CHANNEL_VALUE(2), + // chan4_raw RC channel 4 value, in microseconds + GET_CHANNEL_VALUE(3), + // chan5_raw RC channel 5 value, in microseconds + GET_CHANNEL_VALUE(4), + // chan6_raw RC channel 6 value, in microseconds + GET_CHANNEL_VALUE(5), + // chan7_raw RC channel 7 value, in microseconds + GET_CHANNEL_VALUE(6), + // chan8_raw RC channel 8 value, in microseconds + GET_CHANNEL_VALUE(7), + // rssi Receive signal strength indicator, 0: 0%, 254: 100% + //https://github.com/mavlink/mavlink/issues/1027 + scaleRange(getRSSI(), 0, 1023, 0, 254)); + } + else { + mavlink_msg_rc_channels_pack(mavSystemId, mavComponentId, &mavSendMsg, + // time_boot_ms Timestamp (milliseconds since system boot) + millis(), + // Total number of RC channels being received. + rxRuntimeConfig.channelCount, + // chan1_raw RC channel 1 value, in microseconds + GET_CHANNEL_VALUE(0), + // chan2_raw RC channel 2 value, in microseconds + GET_CHANNEL_VALUE(1), + // chan3_raw RC channel 3 value, in microseconds + GET_CHANNEL_VALUE(2), + // chan4_raw RC channel 4 value, in microseconds + GET_CHANNEL_VALUE(3), + // chan5_raw RC channel 5 value, in microseconds + GET_CHANNEL_VALUE(4), + // chan6_raw RC channel 6 value, in microseconds + GET_CHANNEL_VALUE(5), + // chan7_raw RC channel 7 value, in microseconds + GET_CHANNEL_VALUE(6), + // chan8_raw RC channel 8 value, in microseconds + GET_CHANNEL_VALUE(7), + // chan9_raw RC channel 9 value, in microseconds + GET_CHANNEL_VALUE(8), + // chan10_raw RC channel 10 value, in microseconds + GET_CHANNEL_VALUE(9), + // chan11_raw RC channel 11 value, in microseconds + GET_CHANNEL_VALUE(10), + // chan12_raw RC channel 12 value, in microseconds + GET_CHANNEL_VALUE(11), + // chan13_raw RC channel 13 value, in microseconds + GET_CHANNEL_VALUE(12), + // chan14_raw RC channel 14 value, in microseconds + GET_CHANNEL_VALUE(13), + // chan15_raw RC channel 15 value, in microseconds + GET_CHANNEL_VALUE(14), + // chan16_raw RC channel 16 value, in microseconds + GET_CHANNEL_VALUE(15), + // chan17_raw RC channel 17 value, in microseconds + GET_CHANNEL_VALUE(16), + // chan18_raw RC channel 18 value, in microseconds + GET_CHANNEL_VALUE(17), + // rssi Receive signal strength indicator, 0: 0%, 254: 100% + //https://github.com/mavlink/mavlink/issues/1027 + scaleRange(getRSSI(), 0, 1023, 0, 254)); + } +#undef GET_CHANNEL_VALUE + + mavlinkSendMessage(); +} + +#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 mavlinkSendGpsRawInt(timeUs_t currentTimeUs) +{ + uint8_t gpsFixType = 0; + + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) + return; + + if (gpsSol.fixType == GPS_NO_FIX) + gpsFixType = 1; + else if (gpsSol.fixType == GPS_FIX_2D) + gpsFixType = 2; + else if (gpsSol.fixType == GPS_FIX_3D) + gpsFixType = 3; + + mavlink_msg_gps_raw_int_pack(mavSystemId, mavComponentId, &mavSendMsg, + // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + currentTimeUs, + // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + gpsFixType, + // lat Latitude in 1E7 degrees + gpsSol.llh.lat, + // lon Longitude in 1E7 degrees + gpsSol.llh.lon, + // alt Altitude in 1E3 meters (millimeters) above MSL + gpsSol.llh.alt * 10, + // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + gpsSol.eph, + // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + gpsSol.epv, + // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + gpsSol.groundSpeed, + // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + gpsSol.groundCourse * 10, + // satellites_visible Number of satellites visible. If unknown, set to 255 + gpsSol.numSat, + // alt_ellipsoid Altitude (above WGS84, EGM96 ellipsoid). Positive for up + 0, + // h_acc Position uncertainty in mm, + gpsSol.eph * 10, + // v_acc Altitude uncertainty in mm, + gpsSol.epv * 10, + // vel_acc Speed uncertainty in mm (??) + 0, + // hdg_acc Heading uncertainty in degE5 + 0, + // yaw Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + 0); + + mavlinkSendMessage(); +} + +void mavlinkSendGlobalPositionInt(timeUs_t currentTimeUs) +{ + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { + return; + } + + mavlink_msg_global_position_int_pack(mavSystemId, mavComponentId, &mavSendMsg, + // time_boot_ms Timestamp (milliseconds since system boot) + currentTimeUs / 1000, + // lat Latitude in 1E7 degrees + gpsSol.llh.lat, + // lon Longitude in 1E7 degrees + gpsSol.llh.lon, + // alt Altitude in 1E3 meters (millimeters) above MSL + gpsSol.llh.alt * 10, + // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) + getEstimatedActualPosition(Z) * 10, + // [cm/s] Ground X Speed (Latitude, positive north) + getEstimatedActualVelocity(X), + // [cm/s] Ground Y Speed (Longitude, positive east) + getEstimatedActualVelocity(Y), + // [cm/s] Ground Z Speed (Altitude, positive down) + -getEstimatedActualVelocity(Z), + // [cdeg] Vehicle heading (yaw angle) (0.0..359.99 degrees, 0=north) + DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw) + ); + + mavlinkSendMessage(); +} + +void mavlinkSendGpsGlobalOrigin(void) +{ + mavlink_msg_gps_global_origin_pack(mavSystemId, mavComponentId, &mavSendMsg, + // latitude Latitude (WGS84), expressed as * 1E7 + GPS_home.lat, + // longitude Longitude (WGS84), expressed as * 1E7 + GPS_home.lon, + // altitude Altitude(WGS84), expressed as * 1000 + GPS_home.alt * 10, // FIXME + // time_usec Timestamp (microseconds since system boot) + // Use millis() * 1000 as micros() will overflow after 1.19 hours. + ((uint64_t) millis()) * 1000); + + mavlinkSendMessage(); +} + +void mavlinkSendPosition(timeUs_t currentTimeUs) +{ + mavlinkSendGpsRawInt(currentTimeUs); + mavlinkSendGlobalPositionInt(currentTimeUs); + mavlinkSendGpsGlobalOrigin(); +} +#endif + +void mavlinkSendAttitude(void) +{ + mavlink_msg_attitude_pack(mavSystemId, mavComponentId, &mavSendMsg, + // time_boot_ms Timestamp (milliseconds since system boot) + millis(), + // roll Roll angle (rad) + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.roll)), + // pitch Pitch angle (rad) + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(-attitude.values.pitch)), + // yaw Yaw angle (rad) + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)), + // rollspeed Roll angular speed (rad/s) + DEGREES_TO_RADIANS(gyro.gyroADCf[FD_ROLL]), + // pitchspeed Pitch angular speed (rad/s) + DEGREES_TO_RADIANS(gyro.gyroADCf[FD_PITCH]), + // yawspeed Yaw angular speed (rad/s) + DEGREES_TO_RADIANS(gyro.gyroADCf[FD_YAW])); + + mavlinkSendMessage(); +} + +void mavlinkSendVfrHud(void) +{ + float mavAltitude = 0; + float mavGroundSpeed = 0; + float mavAirSpeed = 0; + float mavClimbRate = 0; + +#if defined(USE_GPS) + // use ground speed if source available + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + mavGroundSpeed = gpsSol.groundSpeed / 100.0f; + } +#endif + +#if defined(USE_PITOT) + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + mavAirSpeed = getAirspeedEstimate() / 100.0f; + } +#endif + + // select best source for altitude + mavAltitude = getEstimatedActualPosition(Z) / 100.0f; + mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; + + int16_t thr = getThrottlePercent(osdUsingScaledThrottle()); + mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg, + // airspeed Current airspeed in m/s + mavAirSpeed, + // groundspeed Current ground speed in m/s + mavGroundSpeed, + // heading Current heading in degrees, in compass units (0..360, 0=north) + DECIDEGREES_TO_DEGREES(attitude.values.yaw), + // throttle Current throttle setting in integer percent, 0 to 100 + thr, + // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) + mavAltitude, + // climb Current climb rate in meters/second + mavClimbRate); + + mavlinkSendMessage(); +} + +static uint8_t mavlinkGetAutopilotEnum(void); + +void mavlinkSendHeartbeat(void) +{ + uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + 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) { + mavSystemType = MAV_TYPE_FIXED_WING; + } + else { + mavSystemType = mavlinkGetVehicleType(); + } + + 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 && isGCSValid()) { + mavModes |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + else if (flm == FLM_MISSION || flm == FLM_RTH ) { + mavModes |= MAV_MODE_FLAG_AUTO_ENABLED; + } + else if (flm != FLM_MANUAL && flm!= FLM_ACRO && flm!=FLM_ACRO_AIR) { + mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + + if (ARMING_FLAG(ARMED)) + mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; + + uint8_t mavSystemState = 0; + if (ARMING_FLAG(ARMED)) { + if (failsafeIsActive()) { + mavSystemState = MAV_STATE_CRITICAL; + } + else { + mavSystemState = MAV_STATE_ACTIVE; + } + } + else if (areSensorsCalibrating()) { + mavSystemState = MAV_STATE_CALIBRATING; + } + else { + mavSystemState = MAV_STATE_STANDBY; + } + + mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg, + // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + mavSystemType, + // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM + mavlinkGetAutopilotEnum(), + // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + mavModes, + // custom_mode A bitfield for use for autopilot-specific flags. + mavCustomMode, + // system_status System status flag, see MAV_STATE ENUM + mavSystemState); + + mavlinkSendMessage(); +} + +void mavlinkSendBatteryStatus(void) +{ + uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; + uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; + memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages)); + memset(batteryVoltagesExt, 0, sizeof(batteryVoltagesExt)); + if (feature(FEATURE_VBAT)) { + uint8_t batteryCellCount = getBatteryCellCount(); + if (batteryCellCount > 0) { + for (int cell=0; cell < batteryCellCount && cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN + MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; cell++) { + if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { + batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; + } else { + batteryVoltagesExt[cell-MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN] = getBatteryAverageCellVoltage() * 10; + } + } + } + else { + batteryVoltages[0] = getBatteryVoltage() * 10; + } + } + else { + batteryVoltages[0] = 0; + } + + mavlink_msg_battery_status_pack(mavSystemId, mavComponentId, &mavSendMsg, + // id Battery ID + 0, + // battery_function Function of the battery + MAV_BATTERY_FUNCTION_UNKNOWN, + // type Type (chemistry) of the battery + MAV_BATTERY_TYPE_UNKNOWN, + // temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature + INT16_MAX, + // voltages Battery voltage of cells, in millivolts (1 = 1 millivolt). Cells above the valid cell count for this battery should have the UINT16_MAX value. + batteryVoltages, + // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + isAmperageConfigured() ? getAmperage() : -1, + // current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate + isAmperageConfigured() ? getMAhDrawn() : -1, + // energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate + isAmperageConfigured() ? getMWhDrawn()*36 : -1, + // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery); + feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1, + // time_remaining Remaining battery time, 0: autopilot does not provide remaining battery time estimate + 0, // TODO this could easily be implemented + // charge_state State for extent of discharge, provided by autopilot for warning or external reactions + 0, + // voltages_ext Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + batteryVoltagesExt, + // mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + 0, + // fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). + 0); + + mavlinkSendMessage(); +} + +void mavlinkSendScaledPressure(void) +{ + int16_t temperature; + sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); + mavlink_msg_scaled_pressure_pack(mavSystemId, mavComponentId, &mavSendMsg, + millis(), + 0, + 0, + temperature * 10, + 0); + + mavlinkSendMessage(); +} + +void mavlinkSendSystemTime(void) +{ + mavlink_msg_system_time_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + 0, + millis()); + + mavlinkSendMessage(); +} + +bool mavlinkSendStatusText(void) +{ +// FIXME - Status text is limited to boards with USE_OSD +#ifdef USE_OSD + char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "}; + textAttributes_t elemAttr = osdGetSystemMessage(buff, sizeof(buff), false); + if (buff[0] != SYM_BLANK) { + MAV_SEVERITY severity = MAV_SEVERITY_NOTICE; + if (TEXT_ATTRIBUTES_HAVE_BLINK(elemAttr)) { + severity = MAV_SEVERITY_CRITICAL; + } else if TEXT_ATTRIBUTES_HAVE_INVERTED(elemAttr) { + severity = MAV_SEVERITY_WARNING; + } + + mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg, + (uint8_t)severity, + buff, + 0, + 0); + + mavlinkSendMessage(); + return true; + } +#endif + return false; +} + +void mavlinkSendBatteryTemperatureStatusText(void) +{ + mavlinkSendBatteryStatus(); + mavlinkSendScaledPressure(); + mavlinkSendStatusText(); +} + +static uint8_t mavlinkGetAutopilotEnum(void) +{ + if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { + return MAV_AUTOPILOT_ARDUPILOTMEGA; + } + + return MAV_AUTOPILOT_GENERIC; +} + +static uint8_t mavlinkHeadingDeg2FromCd(int32_t headingCd) +{ + return (uint8_t)(wrap_36000(headingCd) / 200); +} + +static uint16_t mavlinkComputeHighLatencyFailures(void) +{ + uint16_t flags = 0; + +#if defined(USE_GPS) + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) || gpsSol.fixType == GPS_NO_FIX) { + flags |= HL_FAILURE_FLAG_GPS; + } +#endif + +#ifdef USE_PITOT + if (getHwPitotmeterStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; + } +#endif + + if (getHwBarometerStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; + } + + if (getHwAccelerometerStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_ACCEL; + } + + if (getHwGyroStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_GYRO; + } + + if (getHwCompassStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_MAG; + } + + if (getHwRangefinderStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_TERRAIN; + } + + const batteryState_e batteryState = getBatteryState(); + if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { + flags |= HL_FAILURE_FLAG_BATTERY; + } + + if (!rxIsReceivingSignal() || !rxAreFlightChannelsValid()) { + flags |= HL_FAILURE_FLAG_RC_RECEIVER; + } + + return flags; +} + +void mavlinkSendHighLatency2(timeUs_t currentTimeUs) +{ + const bool isPlane = (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE); + const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); + + int32_t latitude = 0; + int32_t longitude = 0; + int16_t altitude = (int16_t)constrain((int32_t)(getEstimatedActualPosition(Z) / 100), INT16_MIN, INT16_MAX); + int16_t targetAltitude = (int16_t)constrain((int32_t)(posControl.desiredState.pos.z / 100), INT16_MIN, INT16_MAX); + uint16_t targetDistance = 0; + uint16_t wpNum = 0; + uint8_t heading = mavlinkHeadingDeg2FromCd(DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)); + uint8_t targetHeading = mavlinkHeadingDeg2FromCd(posControl.desiredState.yaw); + uint8_t groundspeed = 0; + uint8_t airspeed = 0; + uint8_t airspeedSp = 0; + uint8_t windspeed = 0; + uint8_t windHeading = 0; + uint8_t eph = UINT8_MAX; + uint8_t epv = UINT8_MAX; + int8_t temperatureAir = 0; + int8_t climbRate = (int8_t)constrain(lrintf(getEstimatedActualVelocity(Z) / 10.0f), INT8_MIN, INT8_MAX); + int8_t battery = feature(FEATURE_VBAT) ? (int8_t)calculateBatteryPercentage() : -1; + +#if defined(USE_GPS) + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + latitude = gpsSol.llh.lat; + longitude = gpsSol.llh.lon; + altitude = (int16_t)constrain((int32_t)(gpsSol.llh.alt / 100), INT16_MIN, INT16_MAX); + + const int32_t desiredAltCm = lrintf(posControl.desiredState.pos.z); + const int32_t targetAltCm = GPS_home.alt + desiredAltCm; + targetAltitude = (int16_t)constrain(targetAltCm / 100, INT16_MIN, INT16_MAX); + + groundspeed = (uint8_t)constrain(lrintf(gpsSol.groundSpeed / 20.0f), 0, UINT8_MAX); + + if (gpsSol.flags.validEPE) { + eph = (uint8_t)constrain((gpsSol.eph + 5) / 10, 0, UINT8_MAX); + epv = (uint8_t)constrain((gpsSol.epv + 5) / 10, 0, UINT8_MAX); + } + + if (posControl.activeWaypointIndex >= 0) { + wpNum = (uint16_t)posControl.activeWaypointIndex; + targetDistance = (uint16_t)constrain(lrintf(posControl.wpDistance / 1000.0f), 0, UINT16_MAX); + } + } +#endif + +#if defined(USE_PITOT) + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + airspeed = (uint8_t)constrain(lrintf(getAirspeedEstimate() / 20.0f), 0, UINT8_MAX); + airspeedSp = airspeed; + } +#endif + + if (airspeedSp == 0) { + const float desiredVelXY = calc_length_pythagorean_2D(posControl.desiredState.vel.x, posControl.desiredState.vel.y); + airspeedSp = (uint8_t)constrain(lrintf(desiredVelXY / 20.0f), 0, UINT8_MAX); + } + + if (airspeed == 0) { + airspeed = groundspeed; + } + +#ifdef USE_WIND_ESTIMATOR + if (isEstimatedWindSpeedValid()) { + uint16_t windAngleCd = 0; + const float windHoriz = getEstimatedHorizontalWindSpeed(&windAngleCd); + windspeed = (uint8_t)constrain(lrintf(windHoriz / 20.0f), 0, UINT8_MAX); + windHeading = mavlinkHeadingDeg2FromCd(windAngleCd); + } +#endif + + int16_t temperature; + sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); + temperatureAir = (int8_t)constrain(temperature, INT8_MIN, INT8_MAX); + + const uint16_t failureFlags = mavlinkComputeHighLatencyFailures(); + + mavlink_msg_high_latency2_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + (uint32_t)(currentTimeUs / 1000), + mavlinkGetVehicleType(), + mavlinkGetAutopilotEnum(), + modeSelection.customMode, + latitude, + longitude, + altitude, + targetAltitude, + heading, + targetHeading, + targetDistance, + (uint8_t)constrain(getThrottlePercent(osdUsingScaledThrottle()), 0, 100), + airspeed, + airspeedSp, + groundspeed, + windspeed, + windHeading, + eph, + epv, + temperatureAir, + climbRate, + battery, + wpNum, + failureFlags, + 0, + 0, + 0); + + mavlinkSendMessage(); +} + + +static void mavlinkResetIncomingMissionTransaction(void); + +static bool handleIncoming_MISSION_CLEAR_ALL(void) +{ + mavlink_mission_clear_all_t msg; + mavlink_msg_mission_clear_all_decode(&mavlinkContext.recvMsg, &msg); + + // Check if this message is for us + if (msg.target_system == mavSystemId) { + resetWaypointList(); + mavlinkResetIncomingMissionTransaction(); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + return false; +} + +static void mavlinkResetIncomingMissionTransaction(void) +{ + incomingMissionWpCount = 0; + incomingMissionWpSequence = 0; + incomingMissionSourceSystem = 0; + incomingMissionSourceComponent = 0; + incomingMissionLastActivityMs = 0; +} + +static void mavlinkStartIncomingMissionTransaction(int missionCount) +{ + incomingMissionWpCount = missionCount; + incomingMissionWpSequence = 0; + incomingMissionSourceSystem = mavlinkContext.recvMsg.sysid; + incomingMissionSourceComponent = mavlinkContext.recvMsg.compid; + incomingMissionLastActivityMs = millis(); +} + +static void mavlinkTouchIncomingMissionTransaction(void) +{ + incomingMissionLastActivityMs = millis(); +} + +static bool mavlinkIsIncomingMissionTransactionActive(void) +{ + if (incomingMissionWpCount <= 0) { + return false; + } + + if ((timeMs_t)(millis() - incomingMissionLastActivityMs) > MAVLINK_MISSION_UPLOAD_TIMEOUT_MS) { + mavlinkResetIncomingMissionTransaction(); + return false; + } + + return true; +} + +static bool mavlinkIsIncomingMissionTransactionOwner(void) +{ + return mavlinkContext.recvMsg.sysid == incomingMissionSourceSystem && + mavlinkContext.recvMsg.compid == incomingMissionSourceComponent; +} + +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 (!mavlinkIsIncomingMissionTransactionActive() || !mavlinkIsIncomingMissionTransactionOwner()) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + mavlinkTouchIncomingMissionTransaction(); + + const bool lastMissionItem = incomingMissionWpCount > 0 && ((int)seq + 1 >= incomingMissionWpCount); + + if (autocontinue == 0 && !lastMissionItem) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = lat; + wp.lon = lon; + wp.alt = (int32_t)(altMeters * 100.0f); + wp.p1 = 0; + wp.p2 = 0; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + break; + + case MAV_CMD_NAV_LOITER_TIME: + if (!mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + wp.action = NAV_WP_ACTION_HOLD_TIME; + wp.lat = lat; + wp.lon = lon; + wp.alt = (int32_t)(altMeters * 100.0f); + wp.p1 = (int16_t)lrintf(param1); + wp.p2 = 0; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + { + const bool coordinateFrame = mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT); + + if (!coordinateFrame && frame != MAV_FRAME_MISSION) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + wp.action = NAV_WP_ACTION_RTH; + wp.lat = 0; + wp.lon = 0; + wp.alt = coordinateFrame ? (int32_t)(altMeters * 100.0f) : 0; + wp.p1 = 0; // Land if non-zero + wp.p2 = 0; + wp.p3 = 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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.p1 = 0; // Speed (cm/s) + wp.p2 = 0; // Elevation Adjustment (m): P2 defines the ground elevation (in metres) for the LAND WP. If the altitude mode is absolute, this is also absolute; for relative altitude, then it is the difference between the assumed home location and the LAND WP. + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; // Altitude Mode & Actions, P3 defines the altitude mode. 0 (default, legacy) = Relative to Home, 1 = Absolute (AMSL). + break; + + case MAV_CMD_DO_JUMP: + if (frame != MAV_FRAME_MISSION) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + wp.lat = 0; + wp.lon = 0; + wp.alt = 0; + wp.action = NAV_WP_ACTION_JUMP; + wp.p1 = (int16_t)lrintf(param1 + 1.0f); + wp.p2 = (int16_t)lrintf(param2); + wp.p3 = 0; + break; + + case MAV_CMD_DO_SET_ROI: + if (param1 != MAV_ROI_LOCATION || + !mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + wp.action = NAV_WP_ACTION_SET_POI; + wp.lat = lat; + wp.lon = lon; + wp.alt = (int32_t)(altMeters * 100.0f); + wp.p1 = 0; + wp.p2 = 0; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + break; + + case MAV_CMD_CONDITION_YAW: + if (frame != MAV_FRAME_MISSION) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + wp.lat = 0; + wp.lon = 0; + wp.alt = 0; + wp.action = NAV_WP_ACTION_SET_HEAD; + wp.p1 = (int16_t)lrintf(param1); + wp.p2 = 0; + wp.p3 = 0; + break; + + default: + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + if (seq == incomingMissionWpSequence) { + incomingMissionWpSequence++; + + 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + mavlinkResetIncomingMissionTransaction(); + } + else { + if (useIntMessages) { + mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + } else { + mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + } + mavlinkSendMessage(); + } + } + else { + // If we get a duplicate of the last accepted item, re-request the next one instead of aborting. + if (seq + 1 == incomingMissionWpSequence) { + mavlinkTouchIncomingMissionTransaction(); + if (useIntMessages) { + mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + } else { + mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + } + mavlinkSendMessage(); + } else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + } + + return true; +} + +static bool handleIncoming_MISSION_COUNT(void) +{ + mavlink_mission_count_t msg; + mavlink_msg_mission_count_decode(&mavlinkContext.recvMsg, &msg); + + // Check if this message is for us + if (msg.target_system == mavSystemId) { + mavlinkResetIncomingMissionTransaction(); + if (ARMING_FLAG(ARMED)) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + if (msg.count == 0) { + resetWaypointList(); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + if (msg.count <= NAV_MAX_WAYPOINTS) { + mavlinkStartIncomingMissionTransaction(msg.count); + mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + mavlinkSendMessage(); + return true; + } else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + } + + return false; +} + +static bool handleIncoming_MISSION_ITEM(void) +{ + mavlink_mission_item_t msg; + mavlink_msg_mission_item_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + if (ARMING_FLAG(ARMED)) { + if (msg.command == MAV_CMD_NAV_WAYPOINT) { + return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, + MAV_FRAME_SUPPORTED_GLOBAL | 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, (int32_t)(msg.x * 1e7f), (int32_t)(msg.y * 1e7f), msg.z); +} + +static bool handleIncoming_MISSION_REQUEST_LIST(void) +{ + mavlink_mission_request_list_t msg; + mavlink_msg_mission_request_list_decode(&mavlinkContext.recvMsg, &msg); + + // Check if this message is for us + if (msg.target_system == mavSystemId) { + mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + return false; +} + +static bool fillMavlinkMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item) +{ + mavlinkMissionItemData_t data = {0}; + + data.frame = navWaypointFrame(wp, useIntMessages); + + switch (wp->action) { + case NAV_WP_ACTION_WAYPOINT: + data.command = MAV_CMD_NAV_WAYPOINT; + data.lat = wp->lat; + data.lon = wp->lon; + data.alt = wp->alt / 100.0f; + break; + + case NAV_WP_ACTION_HOLD_TIME: + data.command = MAV_CMD_NAV_LOITER_TIME; + data.param1 = wp->p1; + data.lat = wp->lat; + data.lon = wp->lon; + data.alt = wp->alt / 100.0f; + break; + + case NAV_WP_ACTION_RTH: + data.command = MAV_CMD_NAV_RETURN_TO_LAUNCH; + break; + + case NAV_WP_ACTION_LAND: + data.command = MAV_CMD_NAV_LAND; + data.lat = wp->lat; + data.lon = wp->lon; + data.alt = wp->alt / 100.0f; + break; + + case NAV_WP_ACTION_JUMP: + data.command = MAV_CMD_DO_JUMP; + data.param1 = (wp->p1 > 0) ? (float)(wp->p1 - 1) : 0.0f; + data.param2 = wp->p2; + break; + + case NAV_WP_ACTION_SET_POI: + data.command = MAV_CMD_DO_SET_ROI; + data.param1 = MAV_ROI_LOCATION; + data.lat = wp->lat; + data.lon = wp->lon; + data.alt = wp->alt / 100.0f; + break; + + case NAV_WP_ACTION_SET_HEAD: + data.command = MAV_CMD_CONDITION_YAW; + data.param1 = wp->p1; + break; + + default: + return false; + } + + *item = data; + return true; +} + +static bool handleIncoming_MISSION_REQUEST(void) +{ + mavlink_mission_request_t msg; + mavlink_msg_mission_request_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + int wpCount = getWaypointCount(); + + if (msg.seq < wpCount) { + navWaypoint_t wp; + getWaypoint(msg.seq + 1, &wp); + + mavlinkMissionItemData_t item; + if (fillMavlinkMissionItemFromWaypoint(&wp, false, &item)) { + mavlink_msg_mission_item_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + } + else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + + return true; +} + +static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) +{ + if (targetSystem != 0 && targetSystem != mavSystemId) { + return false; + } + + if (targetComponent != 0 && targetComponent != mavComponentId) { + return false; + } + + 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 targetComponent, 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 (!mavlinkIsLocalTarget(targetSystem, targetComponent)) { + 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 = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + 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; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + wp.flag = 0; + + setWaypoint(255, &wp); + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_DO_CHANGE_ALTITUDE: + { + const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame((uint8_t)lrintf(param2), param1); + mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_CONDITION_YAW: + { + if (!(navGetCurrentStateFlags() & NAV_CTL_YAW)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + int32_t targetHeadingCd = wrap_36000((int32_t)lrintf(param1 * 100.0f)); + + if (param4 != 0.0f) { + const int32_t currentHeadingCd = STATE(AIRPLANE) ? posControl.actualState.cog : posControl.actualState.yaw; + const int32_t headingChangeCd = (int32_t)lrintf(fabsf(param1) * 100.0f); + + if (param3 < 0.0f) { + targetHeadingCd = wrap_36000(currentHeadingCd - headingChangeCd); + } else { + targetHeadingCd = wrap_36000(currentHeadingCd + headingChangeCd); + } + } + + updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(targetHeadingCd)); + posControl.desiredState.yaw = targetHeadingCd; + posControl.cruise.course = targetHeadingCd; + posControl.cruise.previousCourse = targetHeadingCd; + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_SET_MESSAGE_INTERVAL: + { + mavlinkPeriodicMessage_e periodicMessage; + MAV_RESULT result = MAV_RESULT_UNSUPPORTED; + + if (mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { + if (param2 == 0.0f) { + mavlinkSetMessageOverrideIntervalUs(periodicMessage, 0); + result = MAV_RESULT_ACCEPTED; + } else if (param2 < 0.0f) { + mavlinkSetMessageOverrideIntervalUs(periodicMessage, -1); + result = MAV_RESULT_ACCEPTED; + } else { + uint32_t intervalUs = (uint32_t)param2; + if (intervalUs > 0) { + const uint32_t minIntervalUs = 1000000UL / TELEMETRY_MAVLINK_MAXRATE; + if (intervalUs < minIntervalUs) { + intervalUs = minIntervalUs; + } + + mavlinkSetMessageOverrideIntervalUs(periodicMessage, (int32_t)intervalUs); + result = MAV_RESULT_ACCEPTED; + } + } + } + + mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_GET_MESSAGE_INTERVAL: + { + mavlinkPeriodicMessage_e periodicMessage; + if (!mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + mavlink_msg_message_interval_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + (uint16_t)param1, + mavlinkMessageIntervalUs(periodicMessage)); + mavlinkSendMessage(); + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_CONTROL_HIGH_LATENCY: + if (param1 == 0.0f || param1 == 1.0f) { + if (mavlinkGetProtocolVersion() == 1 && param1 > 0.5f) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + mavActivePort->highLatencyEnabled = param1 > 0.5f; + if (mavActivePort->highLatencyEnabled) { + mavActivePort->lastHighLatencyMessageUs = 0; + } + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_REQUEST_PROTOCOL_VERSION: + if (mavlinkGetProtocolVersion() == 1) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendProtocolVersion(); + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: + if (mavlinkGetProtocolVersion() == 1) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendAutopilotVersion(); + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_REQUEST_MESSAGE: + { + bool sent = false; + uint16_t messageId = (uint16_t)param1; + + switch (messageId) { + case MAVLINK_MSG_ID_HEARTBEAT: + mavlinkSendHeartbeat(); + sent = true; + break; + case MAVLINK_MSG_ID_AUTOPILOT_VERSION: + if (mavlinkGetProtocolVersion() != 1) { + mavlinkSendAutopilotVersion(); + sent = true; + } + break; + case MAVLINK_MSG_ID_PROTOCOL_VERSION: + if (mavlinkGetProtocolVersion() != 1) { + mavlinkSendProtocolVersion(); + sent = true; + } + break; + case MAVLINK_MSG_ID_SYS_STATUS: + mavlinkSendSystemStatus(); + sent = true; + break; + case MAVLINK_MSG_ID_ATTITUDE: + mavlinkSendAttitude(); + sent = true; + break; + case MAVLINK_MSG_ID_VFR_HUD: + mavlinkSendVfrHud(); + sent = true; + break; + case MAVLINK_MSG_ID_AVAILABLE_MODES: + { + const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); + const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); + if (isPlane) { + mavlinkSendAvailableModes(planeModes, planeModesCount, modeSelection.customMode, mavlinkPlaneModeIsConfigured); + } else { + mavlinkSendAvailableModes(copterModes, copterModesCount, modeSelection.customMode, mavlinkCopterModeIsConfigured); + } + sent = true; + } + break; + case MAVLINK_MSG_ID_CURRENT_MODE: + { + const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); + const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); + mavlink_msg_current_mode_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + MAV_STANDARD_MODE_NON_STANDARD, + modeSelection.customMode, + modeSelection.customMode); + mavlinkSendMessage(); + sent = true; + } + break; + case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: + mavlinkSendExtendedSysState(); + sent = true; + break; + case MAVLINK_MSG_ID_RC_CHANNELS: + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: + mavlinkSendRCChannelsAndRSSI(); + sent = true; + break; + case MAVLINK_MSG_ID_GPS_RAW_INT: + #ifdef USE_GPS + mavlinkSendGpsRawInt(micros()); + sent = true; + #endif + break; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + #ifdef USE_GPS + mavlinkSendGlobalPositionInt(micros()); + sent = true; + #endif + break; + case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: + #ifdef USE_GPS + mavlinkSendGpsGlobalOrigin(); + sent = true; + #endif + break; + case MAVLINK_MSG_ID_BATTERY_STATUS: + mavlinkSendBatteryStatus(); + sent = true; + break; + case MAVLINK_MSG_ID_SCALED_PRESSURE: + mavlinkSendScaledPressure(); + sent = true; + break; + case MAVLINK_MSG_ID_SYSTEM_TIME: + mavlinkSendSystemTime(); + sent = true; + break; + case MAVLINK_MSG_ID_STATUSTEXT: + sent = mavlinkSendStatusText(); + 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(&mavlinkContext.recvMsg, &msg); + + return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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(&mavlinkContext.recvMsg, &msg); + + // COMMAND_LONG has no frame field; location commands are WGS84 global by definition. + return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, msg.command, MAV_FRAME_GLOBAL, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7); +} + +static bool handleIncoming_MISSION_ITEM_INT(void) +{ + mavlink_mission_item_int_t msg; + mavlink_msg_mission_item_int_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + if (ARMING_FLAG(ARMED)) { + if (msg.command == MAV_CMD_NAV_WAYPOINT) { + return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, + MAV_FRAME_SUPPORTED_GLOBAL_INT | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT, + msg.x, msg.y, msg.z); + } + + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + return mavlinkHandleMissionItemCommon(true, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, msg.x, msg.y, msg.z); +} + +static bool handleIncoming_MISSION_REQUEST_INT(void) +{ + mavlink_mission_request_int_t msg; + mavlink_msg_mission_request_int_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + int wpCount = getWaypointCount(); + + if (msg.seq < wpCount) { + navWaypoint_t wp; + getWaypoint(msg.seq + 1, &wp); + + mavlinkMissionItemData_t item; + if (fillMavlinkMissionItemFromWaypoint(&wp, true, &item)) { + mavlink_msg_mission_item_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + } + else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + return false; + } + + uint8_t rate = 0; + if (msg.start_stop != 0) { + rate = (uint8_t)msg.req_message_rate; + if (rate > TELEMETRY_MAVLINK_MAXRATE) { + rate = TELEMETRY_MAVLINK_MAXRATE; + } + } + + if (msg.req_stream_id == MAV_DATA_STREAM_ALL) { + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_STATUS, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_RC_CHANNELS, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_POSITION, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA1, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA2, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA3, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_SYS_STATE, rate); + return true; + } + + mavlinkSetStreamRate(msg.req_stream_id, rate); + return true; +} + +static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void) +{ + mavlink_set_position_target_global_int_t msg; + mavlink_msg_set_position_target_global_int_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + return false; + } + + uint8_t frame = msg.coordinate_frame; + if (!mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + return true; + } + + const uint16_t typeMask = msg.type_mask; + const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0; + const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0; + const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0; + + // Altitude-only SET_POSITION_TARGET_GLOBAL_INT mirrors MAV_CMD_DO_CHANGE_ALTITUDE semantics. + if (xIgnored && yIgnored && !zIgnored) { + if (isGCSValid()) { + mavlinkSetAltitudeTargetFromFrame(frame, msg.alt); + } + return true; + } + + if (xIgnored || yIgnored) { + return true; + } + + if (isGCSValid()) { + navWaypoint_t wp = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = msg.lat_int; + wp.lon = msg.lon_int; + wp.alt = zIgnored ? 0 : (int32_t)(msg.alt * 100.0f); + wp.p1 = 0; + wp.p2 = 0; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + wp.flag = 0; + + setWaypoint(255, &wp); + } + + return true; +} + +static bool handleIncoming_SET_POSITION_TARGET_LOCAL_NED(void) +{ + mavlink_set_position_target_local_ned_t msg; + mavlink_msg_set_position_target_local_ned_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + 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(&mavlinkContext.recvMsg, &msg); + mavlinkRxHandleMessage(&msg); + return true; +} + +static bool handleIncoming_PARAM_REQUEST_LIST(void) { + mavlink_param_request_list_t msg; + mavlink_msg_param_request_list_decode(&mavlinkContext.recvMsg, &msg); + + // Respond that we don't have any parameters to force Mission Planner to give up quickly + if (mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + // mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); + mavlink_msg_param_value_pack(mavSystemId, mavComponentId, &mavSendMsg, 0, 0, 0, 0, 0); + mavlinkSendMessage(); + } + return true; +} + +static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { + switch(mavActiveConfig->radio_type) { + case MAVLINK_RADIO_NONE: + break; + case MAVLINK_RADIO_SIK: + // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html + rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; + rxLinkStatistics.uplinkSNR = msg->noise / 1.9; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + case MAVLINK_RADIO_ELRS: + rxLinkStatistics.uplinkRSSI = -msg->remrssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); + break; + case MAVLINK_RADIO_GENERIC: + default: + rxLinkStatistics.uplinkRSSI = msg->rssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + } +} + +static bool handleIncoming_RADIO_STATUS(void) { + mavlink_radio_status_t msg; + mavlink_msg_radio_status_decode(&mavlinkContext.recvMsg, &msg); + if (msg.txbuf > 0) { + mavActivePort->txbuffValid = true; + mavActivePort->txbuffFree = msg.txbuf; + } else { + mavActivePort->txbuffValid = false; + mavActivePort->txbuffFree = 100; + } + + if (rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { + mavlinkParseRxStats(&msg); + } + + return true; +} + +static bool handleIncoming_HEARTBEAT(void) { + mavlink_heartbeat_t msg; + mavlink_msg_heartbeat_decode(&mavlinkContext.recvMsg, &msg); + + switch (msg.type) { +#ifdef USE_ADSB + case MAV_TYPE_ADSB: + return adsbHeartbeat(); +#endif + default: + break; + } + + return false; +} + +#ifdef USE_ADSB +static bool handleIncoming_ADSB_VEHICLE(void) { + mavlink_adsb_vehicle_t msg; + mavlink_msg_adsb_vehicle_decode(&mavlinkContext.recvMsg, &msg); + + adsbVehicleValues_t* vehicle = getVehicleForFill(); + if(vehicle != NULL){ + vehicle->icao = msg.ICAO_address; + vehicle->gps.lat = msg.lat; + vehicle->gps.lon = msg.lon; + vehicle->alt = (int32_t)(msg.altitude / 10); + vehicle->horVelocity = msg.hor_velocity; + vehicle->heading = msg.heading; + vehicle->flags = msg.flags; + vehicle->altitudeType = msg.altitude_type; + memcpy(&(vehicle->callsign), msg.callsign, sizeof(vehicle->callsign)); + vehicle->emitterType = msg.emitter_type; + vehicle->tslc = msg.tslc; + + adsbNewVehicle(vehicle); + } + + return true; +} +#endif + +mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIndex) +{ + UNUSED(ingressPortIndex); + + switch (mavlinkContext.recvMsg.msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: + return handleIncoming_HEARTBEAT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + return handleIncoming_PARAM_REQUEST_LIST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + return handleIncoming_MISSION_CLEAR_ALL() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_COUNT: + return handleIncoming_MISSION_COUNT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_ITEM: + return handleIncoming_MISSION_ITEM() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_ITEM_INT: + return handleIncoming_MISSION_ITEM_INT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + return handleIncoming_MISSION_REQUEST_LIST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_COMMAND_LONG: + return handleIncoming_COMMAND_LONG() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_COMMAND_INT: + return handleIncoming_COMMAND_INT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_REQUEST: + return handleIncoming_MISSION_REQUEST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_REQUEST_INT: + return handleIncoming_MISSION_REQUEST_INT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + return handleIncoming_REQUEST_DATA_STREAM() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + handleIncoming_RC_CHANNELS_OVERRIDE(); + return MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + return handleIncoming_SET_POSITION_TARGET_LOCAL_NED() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: + return handleIncoming_SET_POSITION_TARGET_GLOBAL_INT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; +#ifdef USE_ADSB + case MAVLINK_MSG_ID_ADSB_VEHICLE: + return handleIncoming_ADSB_VEHICLE() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; +#endif + case MAVLINK_MSG_ID_RADIO_STATUS: + handleIncoming_RADIO_STATUS(); + return MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY; + case MAVLINK_MSG_ID_TUNNEL: + return handleIncoming_TUNNEL(ingressPortIndex) ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + default: + return MAVLINK_FC_DISPATCH_NOT_HANDLED; + } +} + + +#endif diff --git a/src/main/fc/fc_mavlink.h b/src/main/fc/fc_mavlink.h new file mode 100644 index 00000000000..29be4363157 --- /dev/null +++ b/src/main/fc/fc_mavlink.h @@ -0,0 +1,29 @@ +#pragma once + +#include "common/time.h" + +#include "telemetry/mavlink.h" + +typedef enum { + MAVLINK_FC_DISPATCH_NOT_HANDLED = 0, + MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY, + MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY, +} mavlinkFcDispatchResult_e; + +void mavlinkSendSystemStatus(void); +void mavlinkSendRCChannelsAndRSSI(void); +void mavlinkSendPosition(timeUs_t currentTimeUs); +void mavlinkSendGpsRawInt(timeUs_t currentTimeUs); +void mavlinkSendGlobalPositionInt(timeUs_t currentTimeUs); +void mavlinkSendGpsGlobalOrigin(void); +void mavlinkSendAttitude(void); +void mavlinkSendVfrHud(void); +void mavlinkSendHeartbeat(void); +void mavlinkSendBatteryTemperatureStatusText(void); +void mavlinkSendExtendedSysState(void); +void mavlinkSendBatteryStatus(void); +void mavlinkSendScaledPressure(void); +void mavlinkSendSystemTime(void); +bool mavlinkSendStatusText(void); +void mavlinkSendHighLatency2(timeUs_t currentTimeUs); +mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIndex); diff --git a/src/main/mavlink/mavlink_internal.h b/src/main/mavlink/mavlink_internal.h new file mode 100644 index 00000000000..4c365f364db --- /dev/null +++ b/src/main/mavlink/mavlink_internal.h @@ -0,0 +1,131 @@ +#pragma once + +#include "platform.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +#include +#include +#include +#include + +#include "build/build_config.h" +#include "build/debug.h" +#include "build/version.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/maths.h" +#include "common/utils.h" +#include "common/string_light.h" + +#include "config/feature.h" + +#include "drivers/serial.h" +#include "drivers/time.h" +#include "drivers/display.h" +#include "drivers/osd_symbols.h" + +#include "fc/config.h" +#include "fc/fc_core.h" +#include "fc/fc_msp.h" +#include "fc/rc_controls.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_profile.h" +#include "flight/pid.h" +#include "flight/servos.h" +#include "flight/wind_estimator.h" + +#include "io/adsb.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/serial.h" +#include "io/osd.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "rx/rx.h" +#include "rx/mavlink.h" + +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/gyro.h" +#include "sensors/pitotmeter.h" +#include "sensors/diagnostics.h" +#include "sensors/sensors.h" +#include "sensors/temperature.h" +#include "sensors/esc_sensor.h" + +#include "telemetry/mavlink.h" +#include "telemetry/telemetry.h" + +#include "blackbox/blackbox_io.h" + +#include "scheduler/scheduler.h" + +#define MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP 0x8001 +#define MAVLINK_TUNNEL_MSP_TIMEOUT_MS 1000 +#define MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE (MSP_PORT_OUTBUF_SIZE + 16) +#define MAVLINK_MISSION_UPLOAD_TIMEOUT_MS 10000 + +typedef struct mavlinkContext_s { + mavlinkPortRuntime_t portStates[MAX_MAVLINK_PORTS]; + uint8_t portCount; + mavlinkRouteEntry_t routeTable[MAVLINK_MAX_ROUTES]; + uint8_t routeCount; + mspPort_t tunnelMspPorts[MAX_MAVLINK_PORTS]; + uint8_t tunnelRemoteSystemIds[MAX_MAVLINK_PORTS]; + uint8_t tunnelRemoteComponentIds[MAX_MAVLINK_PORTS]; + uint8_t sendMask; + mavlinkPortRuntime_t *activePort; + const mavlinkTelemetryPortConfig_t *activeConfig; + mavlink_message_t sendMsg; + mavlink_message_t recvMsg; + uint8_t systemId; + uint8_t autopilotType; + uint8_t componentId; + uint8_t tunnelReplyPayloadBuf[MSP_PORT_OUTBUF_SIZE]; + uint8_t tunnelFrameBuf[MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE]; + int incomingMissionWpCount; + int incomingMissionWpSequence; + uint8_t incomingMissionSourceSystem; + uint8_t incomingMissionSourceComponent; + timeMs_t incomingMissionLastActivityMs; +} mavlinkContext_t; + +extern mavlinkContext_t mavlinkContext; + +#define mavPortStates (mavlinkContext.portStates) +#define mavPortCount (mavlinkContext.portCount) +#define mavRouteTable (mavlinkContext.routeTable) +#define mavRouteCount (mavlinkContext.routeCount) +#define mavTunnelMspPorts (mavlinkContext.tunnelMspPorts) +#define mavTunnelRemoteSystemIds (mavlinkContext.tunnelRemoteSystemIds) +#define mavTunnelRemoteComponentIds (mavlinkContext.tunnelRemoteComponentIds) +#define mavSendMask (mavlinkContext.sendMask) +#define mavActivePort (mavlinkContext.activePort) +#define mavActiveConfig (mavlinkContext.activeConfig) +#define mavSendMsg (mavlinkContext.sendMsg) +#define mavSystemId (mavlinkContext.systemId) +#define mavAutopilotType (mavlinkContext.autopilotType) +#define mavComponentId (mavlinkContext.componentId) +#define mavTunnelReplyPayloadBuf (mavlinkContext.tunnelReplyPayloadBuf) +#define mavTunnelFrameBuf (mavlinkContext.tunnelFrameBuf) +#define incomingMissionWpCount (mavlinkContext.incomingMissionWpCount) +#define incomingMissionWpSequence (mavlinkContext.incomingMissionWpSequence) +#define incomingMissionSourceSystem (mavlinkContext.incomingMissionSourceSystem) +#define incomingMissionSourceComponent (mavlinkContext.incomingMissionSourceComponent) +#define incomingMissionLastActivityMs (mavlinkContext.incomingMissionLastActivityMs) + +#endif diff --git a/src/main/mavlink/mavlink_ports.c b/src/main/mavlink/mavlink_ports.c new file mode 100644 index 00000000000..9d499071a51 --- /dev/null +++ b/src/main/mavlink/mavlink_ports.c @@ -0,0 +1,73 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_ports.h" +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) +{ + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + if (state->port) { + closeSerialPort(state->port); + } + + state->port = NULL; + state->telemetryEnabled = false; + state->txbuffValid = false; + state->txbuffFree = 100; + state->lastMavlinkMessageUs = 0; + state->lastHighLatencyMessageUs = 0; + state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; + state->txSeq = 0; + state->txDroppedFrames = 0; + memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); + memset(state->mavMessageOverrideIntervalsUs, 0, sizeof(state->mavMessageOverrideIntervalsUs)); + memset(state->mavMessageNextDue, 0, sizeof(state->mavMessageNextDue)); + memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); + memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); + resetMspPort(&mavTunnelMspPorts[portIndex], NULL); + mavTunnelRemoteSystemIds[portIndex] = 0; + mavTunnelRemoteComponentIds[portIndex] = 0; +} + +void configureMAVLinkTelemetryPort(uint8_t portIndex) +{ + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + if (!state->portConfig) { + return; + } + + baudRate_e baudRateIndex = state->portConfig->telemetry_baudrateIndex; + if (baudRateIndex == BAUD_AUTO) { + // default rate for minimOSD + baudRateIndex = BAUD_57600; + } + + state->port = openSerialPort(state->portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_PORT_MODE, SERIAL_NOT_INVERTED); + if (!state->port) { + return; + } + + state->telemetryEnabled = true; + state->txbuffValid = false; + state->txbuffFree = 100; + state->lastMavlinkMessageUs = 0; + state->lastHighLatencyMessageUs = 0; + state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; + state->txSeq = 0; + state->txDroppedFrames = 0; + memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); + memset(state->mavMessageOverrideIntervalsUs, 0, sizeof(state->mavMessageOverrideIntervalsUs)); + memset(state->mavMessageNextDue, 0, sizeof(state->mavMessageNextDue)); + memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); + memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); + resetMspPort(&mavTunnelMspPorts[portIndex], NULL); + mavTunnelRemoteSystemIds[portIndex] = 0; + mavTunnelRemoteComponentIds[portIndex] = 0; +} + +#endif diff --git a/src/main/mavlink/mavlink_ports.h b/src/main/mavlink/mavlink_ports.h new file mode 100644 index 00000000000..c0b325a354c --- /dev/null +++ b/src/main/mavlink/mavlink_ports.h @@ -0,0 +1,4 @@ +#pragma once + +void configureMAVLinkTelemetryPort(uint8_t portIndex); +void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex); diff --git a/src/main/mavlink/mavlink_routing.c b/src/main/mavlink/mavlink_routing.c new file mode 100644 index 00000000000..f0b0b373d11 --- /dev/null +++ b/src/main/mavlink/mavlink_routing.c @@ -0,0 +1,172 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_routing.h" +#include "mavlink/mavlink_runtime.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid) +{ + return sysid == mavlinkGetCommonConfig()->sysid && compid == MAV_COMP_ID_AUTOPILOT1; +} + +void mavlinkLearnRoute(uint8_t ingressPortIndex) +{ + if (mavlinkContext.recvMsg.sysid == 0 || mavlinkIsFromLocalIdentity(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid)) { + return; + } + + for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { + mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; + if (route->sysid == mavlinkContext.recvMsg.sysid && route->compid == mavlinkContext.recvMsg.compid) { + route->ingressPortIndex = ingressPortIndex; + return; + } + } + + if (mavRouteCount >= MAVLINK_MAX_ROUTES) { + return; + } + + mavRouteTable[mavRouteCount].sysid = mavlinkContext.recvMsg.sysid; + mavRouteTable[mavRouteCount].compid = mavlinkContext.recvMsg.compid; + mavRouteTable[mavRouteCount].ingressPortIndex = ingressPortIndex; + mavRouteCount++; +} + +void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent) +{ + *targetSystem = -1; + *targetComponent = -1; + + const mavlink_msg_entry_t *msgEntry = mavlink_get_msg_entry(msg->msgid); + if (!msgEntry) { + return; + } + + if (msgEntry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { + *targetSystem = _MAV_RETURN_uint8_t(msg, msgEntry->target_system_ofs); + } + + if (msgEntry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { + *targetComponent = _MAV_RETURN_uint8_t(msg, msgEntry->target_component_ofs); + } +} + +static uint8_t mavlinkComputeForwardMask(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent) +{ + uint8_t mask = 0; + + if (targetSystem <= 0) { + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if (portIndex == ingressPortIndex) { + continue; + } + + const mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + mask |= MAVLINK_PORT_MASK(portIndex); + } + return mask; + } + + for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { + const mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; + + if (route->sysid != targetSystem) { + continue; + } + if (targetComponent > 0 && route->compid != targetComponent) { + continue; + } + if (route->ingressPortIndex == ingressPortIndex || route->ingressPortIndex >= mavPortCount) { + continue; + } + + const mavlinkPortRuntime_t *state = &mavPortStates[route->ingressPortIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + mask |= MAVLINK_PORT_MASK(route->ingressPortIndex); + } + + return mask; +} + +void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent) +{ + if (mavlinkContext.recvMsg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + return; + } + + uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; + const int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavlinkContext.recvMsg); + if (msgLength <= 0) { + return; + } + + const uint8_t forwardMask = mavlinkComputeForwardMask(ingressPortIndex, targetSystem, targetComponent); + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if ((forwardMask & MAVLINK_PORT_MASK(portIndex)) == 0) { + continue; + } + + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (serialTxBytesFree(state->port) < (uint32_t)msgLength) { + state->txDroppedFrames++; + continue; + } + + serialBeginWrite(state->port); + serialWriteBuf(state->port, mavBuffer, msgLength); + serialEndWrite(state->port); + } +} + +int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t targetComponent, uint8_t ingressPortIndex) +{ + if (targetSystem <= 0) { + return ingressPortIndex; + } + + if ((uint8_t)targetSystem != mavlinkGetCommonConfig()->sysid) { + return -1; + } + + if (targetComponent > 0 && (uint8_t)targetComponent != MAV_COMP_ID_AUTOPILOT1) { + return -1; + } + + if (ingressPortIndex < mavPortCount) { + return ingressPortIndex; + } + + return mavPortCount > 0 ? 0 : -1; +} + +bool mavlinkShouldFanOutLocalBroadcast(const mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_REQUEST_DATA_STREAM) { + return true; + } + + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + mavlink_command_long_t cmd; + mavlink_msg_command_long_decode(msg, &cmd); + return cmd.command == MAV_CMD_SET_MESSAGE_INTERVAL || cmd.command == MAV_CMD_CONTROL_HIGH_LATENCY; + } + + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_INT) { + mavlink_command_int_t cmd; + mavlink_msg_command_int_decode(msg, &cmd); + return cmd.command == MAV_CMD_SET_MESSAGE_INTERVAL || cmd.command == MAV_CMD_CONTROL_HIGH_LATENCY; + } + + return false; +} + +#endif diff --git a/src/main/mavlink/mavlink_routing.h b/src/main/mavlink/mavlink_routing.h new file mode 100644 index 00000000000..1aa472e0f8c --- /dev/null +++ b/src/main/mavlink/mavlink_routing.h @@ -0,0 +1,13 @@ +#pragma once + +#include +#include + +#include "telemetry/mavlink.h" + +bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid); +void mavlinkLearnRoute(uint8_t ingressPortIndex); +void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent); +void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent); +int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t targetComponent, uint8_t ingressPortIndex); +bool mavlinkShouldFanOutLocalBroadcast(const mavlink_message_t *msg); diff --git a/src/main/mavlink/mavlink_runtime.c b/src/main/mavlink/mavlink_runtime.c new file mode 100644 index 00000000000..0c780e0f7f3 --- /dev/null +++ b/src/main/mavlink/mavlink_runtime.c @@ -0,0 +1,281 @@ +#include "mavlink/mavlink_internal.h" + +#include "fc/fc_mavlink.h" + +#include "mavlink/mavlink_ports.h" +#include "mavlink/mavlink_routing.h" +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +mavlinkContext_t mavlinkContext; + +const mavlinkTelemetryPortConfig_t *mavlinkGetPortConfig(uint8_t portIndex) +{ + return &telemetryConfig()->mavlink[portIndex]; +} + +const mavlinkTelemetryCommonConfig_t *mavlinkGetCommonConfig(void) +{ + return &telemetryConfig()->mavlink_common; +} + +uint8_t mavlinkGetProtocolVersion(void) +{ + return mavlinkGetCommonConfig()->version; +} + +static void mavlinkApplyActivePortOutputVersion(void) +{ + mavlink_status_t *chanState = mavlink_get_channel_status(MAVLINK_COMM_0); + if (mavlinkGetProtocolVersion() == 1) { + chanState->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } else { + chanState->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } +} + +void mavlinkSetActivePortContext(uint8_t portIndex) +{ + mavActivePort = &mavPortStates[portIndex]; + mavActiveConfig = mavlinkGetPortConfig(portIndex); + const mavlinkTelemetryCommonConfig_t *commonConfig = mavlinkGetCommonConfig(); + mavAutopilotType = commonConfig->autopilot_type; + mavSystemId = commonConfig->sysid; + mavComponentId = MAV_COMP_ID_AUTOPILOT1; + mavlinkApplyActivePortOutputVersion(); +} + +void mavlinkRuntimeFreePorts(void) +{ + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + freeMAVLinkTelemetryPortByIndex(portIndex); + } + + mavSendMask = 0; + mavRouteCount = 0; +} + +void mavlinkRuntimeInit(void) +{ + memset(&mavlinkContext, 0, sizeof(mavlinkContext)); + mavSystemId = 1; + mavComponentId = MAV_COMP_ID_AUTOPILOT1; + + const serialPortConfig_t *serialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); + while (serialPortConfig && mavPortCount < MAX_MAVLINK_PORTS) { + mavlinkPortRuntime_t *state = &mavPortStates[mavPortCount]; + state->portConfig = serialPortConfig; + state->portSharing = determinePortSharing(serialPortConfig, FUNCTION_TELEMETRY_MAVLINK); + state->txbuffFree = 100; + state->highLatencyEnabled = mavlinkGetPortConfig(mavPortCount)->high_latency; + configureMAVLinkStreamRates(mavPortCount); + + mavPortCount++; + serialPortConfig = findNextSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); + } +} + +void mavlinkRuntimeCheckState(void) +{ + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(state->portSharing); + if ((state->portConfig->functionMask & FUNCTION_RX_SERIAL) && + rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { + newTelemetryEnabledValue = true; + } + + if (newTelemetryEnabledValue == state->telemetryEnabled) { + continue; + } + + if (newTelemetryEnabledValue) { + configureMAVLinkTelemetryPort(portIndex); + if (state->telemetryEnabled) { + configureMAVLinkStreamRates(portIndex); + } + } else { + freeMAVLinkTelemetryPortByIndex(portIndex); + } + } +} + +void mavlinkSendMessage(void) +{ + const mavlink_msg_entry_t *msgEntry = mavlink_get_msg_entry(mavSendMsg.msgid); + if (!msgEntry) { + return; + } + + uint8_t sendMask = mavSendMask; + if (sendMask == 0) { + if (mavActivePort) { + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if (&mavPortStates[portIndex] == mavActivePort) { + sendMask = MAVLINK_PORT_MASK(portIndex); + break; + } + } + } else if (mavPortCount == 1 && mavPortStates[0].telemetryEnabled && mavPortStates[0].port) { + sendMask = MAVLINK_PORT_MASK(0); + } + } + + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if ((sendMask & MAVLINK_PORT_MASK(portIndex)) == 0) { + continue; + } + + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + mavlink_status_t txStatus = { 0 }; + txStatus.current_tx_seq = state->txSeq; + if (mavlinkGetProtocolVersion() == 1) { + txStatus.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } + + mavlink_message_t txMsg = mavSendMsg; + mavlink_finalize_message_buffer( + &txMsg, + txMsg.sysid, + txMsg.compid, + &txStatus, + msgEntry->min_msg_len, + txMsg.len, + msgEntry->crc_extra + ); + state->txSeq = txStatus.current_tx_seq; + + uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; + const int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &txMsg); + if (msgLength <= 0) { + continue; + } + + // Drop the frame on this port if there is no room; do not block telemetry task. + if (serialTxBytesFree(state->port) < (uint32_t)msgLength) { + state->txDroppedFrames++; + continue; + } + + serialBeginWrite(state->port); + serialWriteBuf(state->port, mavBuffer, msgLength); + serialEndWrite(state->port); + } +} + +static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) +{ + mavlinkPortRuntime_t *state = &mavPortStates[ingressPortIndex]; + + while (serialRxBytesWaiting(state->port) > 0) { + // Limit handling to one message per cycle + const char c = serialRead(state->port); + const uint8_t result = mavlink_parse_char(ingressPortIndex, c, &state->mavRecvMsg, &state->mavRecvStatus); + if (result == MAVLINK_FRAMING_OK) { + mavlinkContext.recvMsg = state->mavRecvMsg; + + if (mavlinkIsFromLocalIdentity(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid)) { + return false; + } + + mavlinkLearnRoute(ingressPortIndex); + + int16_t targetSystem; + int16_t targetComponent; + mavlinkExtractTargets(&mavlinkContext.recvMsg, &targetSystem, &targetComponent); + mavlinkForwardMessage(ingressPortIndex, targetSystem, targetComponent); + + if (mavlinkContext.recvMsg.msgid == MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE) { + mavlinkSetActivePortContext(ingressPortIndex); + mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); + const mavlinkFcDispatchResult_e dispatchResult = mavlinkFcDispatchIncomingMessage(ingressPortIndex); + return dispatchResult == MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY; + } + + const int8_t localPortIndex = mavlinkResolveLocalPortForTarget(targetSystem, targetComponent, ingressPortIndex); + if (localPortIndex < 0) { + return false; + } + + uint8_t localPortMask = MAVLINK_PORT_MASK((uint8_t)localPortIndex); + const bool isLocalOrSystemBroadcast = targetSystem == 0 || ((targetSystem > 0) && ((uint8_t)targetSystem == mavlinkGetCommonConfig()->sysid)); + if (targetComponent == 0 && isLocalOrSystemBroadcast && mavlinkShouldFanOutLocalBroadcast(&mavlinkContext.recvMsg)) { + localPortMask = 0; + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + localPortMask |= MAVLINK_PORT_MASK(portIndex); + } + } + + bool handled = false; + for (uint8_t localIndex = 0; localIndex < mavPortCount; localIndex++) { + if (!(localPortMask & MAVLINK_PORT_MASK(localIndex))) { + continue; + } + + mavlinkSetActivePortContext(localIndex); + mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); + + const mavlinkFcDispatchResult_e dispatchResult = mavlinkFcDispatchIncomingMessage(ingressPortIndex); + handled = handled || dispatchResult == MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY; + } + + return handled; + } + } + + return false; +} + +static bool isMAVLinkTelemetryHalfDuplex(void) +{ + return telemetryConfig()->halfDuplex || + (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK && tristateWithDefaultOffIsActive(rxConfig()->halfDuplex)); +} + +void mavlinkRuntimeHandle(timeUs_t currentTimeUs) +{ + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + mavlinkSetActivePortContext(portIndex); + + // Process incoming MAVLink on this port and forward when needed. + const bool receivedMessage = processMAVLinkIncomingTelemetry(portIndex); + + // Restore context back to this port before periodic send decisions. + mavlinkSetActivePortContext(portIndex); + bool shouldSendTelemetry = false; + + if (state->txbuffValid) { + // Use flow control if available. + shouldSendTelemetry = state->txbuffFree >= mavActiveConfig->min_txbuff; + } else { + // If not, use blind frame pacing and half-duplex backoff after RX activity. + const bool halfDuplexBackoff = isMAVLinkTelemetryHalfDuplex() && receivedMessage; + shouldSendTelemetry = ((currentTimeUs - state->lastMavlinkMessageUs) >= TELEMETRY_MAVLINK_DELAY) && !halfDuplexBackoff; + } + + if (!shouldSendTelemetry) { + continue; + } + + mavSendMask = MAVLINK_PORT_MASK(portIndex); + processMAVLinkTelemetry(currentTimeUs); + state->lastMavlinkMessageUs = currentTimeUs; + } + + mavSendMask = 0; +} + +#endif diff --git a/src/main/mavlink/mavlink_runtime.h b/src/main/mavlink/mavlink_runtime.h new file mode 100644 index 00000000000..da5e1394eac --- /dev/null +++ b/src/main/mavlink/mavlink_runtime.h @@ -0,0 +1,16 @@ +#pragma once + +#include "common/time.h" + +#include "telemetry/telemetry.h" + +void mavlinkRuntimeInit(void); +void mavlinkRuntimeHandle(timeUs_t currentTimeUs); +void mavlinkRuntimeCheckState(void); +void mavlinkRuntimeFreePorts(void); + +const mavlinkTelemetryPortConfig_t *mavlinkGetPortConfig(uint8_t portIndex); +const mavlinkTelemetryCommonConfig_t *mavlinkGetCommonConfig(void); +uint8_t mavlinkGetProtocolVersion(void); +void mavlinkSetActivePortContext(uint8_t portIndex); +void mavlinkSendMessage(void); diff --git a/src/main/mavlink/mavlink_streams.c b/src/main/mavlink/mavlink_streams.c new file mode 100644 index 00000000000..a774f2ef00e --- /dev/null +++ b/src/main/mavlink/mavlink_streams.c @@ -0,0 +1,309 @@ +#include "mavlink/mavlink_internal.h" + +#include "fc/fc_mavlink.h" + +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +/* Secondary profile for ports 2..N: heartbeat only. */ +const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT] = { + [MAV_DATA_STREAM_EXTENDED_STATUS] = 0, + [MAV_DATA_STREAM_RC_CHANNELS] = 0, + [MAV_DATA_STREAM_POSITION] = 0, + [MAV_DATA_STREAM_EXTRA1] = 0, + [MAV_DATA_STREAM_EXTRA2] = 0, + [MAV_DATA_STREAM_EXTRA3] = 0, + [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = 0, + [MAV_DATA_STREAM_HEARTBEAT] = 1 +}; + +uint8_t mavlinkClampStreamRate(uint8_t rate) +{ + if (rate > TELEMETRY_MAVLINK_MAXRATE) { + return TELEMETRY_MAVLINK_MAXRATE; + } + + return rate; +} + +int32_t mavlinkRateToIntervalUs(uint8_t rate) +{ + rate = mavlinkClampStreamRate(rate); + if (rate == 0) { + return -1; + } + + return 1000000 / rate; +} + +bool mavlinkPeriodicMessageFromMessageId(uint16_t messageId, mavlinkPeriodicMessage_e *periodicMessage) +{ + switch (messageId) { + case MAVLINK_MSG_ID_HEARTBEAT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_HEARTBEAT; + return true; + case MAVLINK_MSG_ID_SYS_STATUS: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SYS_STATUS; + return true; + case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_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: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS; + return true; + case MAVLINK_MSG_ID_GPS_RAW_INT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT; + return true; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT; + return true; + case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN; + return true; + case MAVLINK_MSG_ID_ATTITUDE: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_ATTITUDE; + return true; + case MAVLINK_MSG_ID_VFR_HUD: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_VFR_HUD; + return true; + case MAVLINK_MSG_ID_BATTERY_STATUS: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS; + return true; + case MAVLINK_MSG_ID_SCALED_PRESSURE: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE; + return true; + case MAVLINK_MSG_ID_SYSTEM_TIME: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME; + return true; + default: + return false; + } +} + +uint8_t mavlinkPeriodicMessageBaseStream(mavlinkPeriodicMessage_e periodicMessage) +{ + switch (periodicMessage) { + case MAVLINK_PERIODIC_MESSAGE_HEARTBEAT: + return MAV_DATA_STREAM_HEARTBEAT; + case MAVLINK_PERIODIC_MESSAGE_SYS_STATUS: + return MAV_DATA_STREAM_EXTENDED_STATUS; + case MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE: + return MAV_DATA_STREAM_EXTENDED_SYS_STATE; + case MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS: + return MAV_DATA_STREAM_RC_CHANNELS; + case MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT: + case MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT: + case MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN: + return MAV_DATA_STREAM_POSITION; + case MAVLINK_PERIODIC_MESSAGE_ATTITUDE: + return MAV_DATA_STREAM_EXTRA1; + case MAVLINK_PERIODIC_MESSAGE_VFR_HUD: + return MAV_DATA_STREAM_EXTRA2; + case MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS: + case MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE: + case MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME: + return MAV_DATA_STREAM_EXTRA3; + default: + return MAV_DATA_STREAM_ALL; + } +} + +static void mavlinkResetMessageSchedule(mavlinkPeriodicMessage_e periodicMessage) +{ + if (!mavActivePort) { + return; + } + + mavActivePort->mavMessageNextDue[periodicMessage] = 0; +} + +static void mavlinkResetMessagesForStream(uint8_t streamNum) +{ + if (!mavActivePort) { + return; + } + + for (uint8_t messageIndex = 0; messageIndex < MAVLINK_PERIODIC_MESSAGE_COUNT; messageIndex++) { + if (mavlinkPeriodicMessageBaseStream((mavlinkPeriodicMessage_e)messageIndex) == streamNum) { + mavActivePort->mavMessageNextDue[messageIndex] = 0; + } + } +} + +int32_t mavlinkMessageBaseIntervalUs(mavlinkPeriodicMessage_e periodicMessage) +{ + if (!mavActivePort) { + return -1; + } + + return mavlinkRateToIntervalUs(mavActivePort->mavRates[mavlinkPeriodicMessageBaseStream(periodicMessage)]); +} + +int32_t mavlinkMessageIntervalUs(mavlinkPeriodicMessage_e periodicMessage) +{ + if (!mavActivePort) { + return -1; + } + + const int32_t overrideIntervalUs = mavActivePort->mavMessageOverrideIntervalsUs[periodicMessage]; + if (overrideIntervalUs != 0) { + return overrideIntervalUs; + } + + return mavlinkMessageBaseIntervalUs(periodicMessage); +} + +void mavlinkSetMessageOverrideIntervalUs(mavlinkPeriodicMessage_e periodicMessage, int32_t intervalUs) +{ + if (!mavActivePort) { + return; + } + + mavActivePort->mavMessageOverrideIntervalsUs[periodicMessage] = intervalUs; + mavlinkResetMessageSchedule(periodicMessage); +} + +int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum, timeUs_t currentTimeUs) +{ + if (!mavActivePort || streamNum >= MAXSTREAMS) { + return 0; + } + + const uint8_t rate = mavlinkClampStreamRate(mavActivePort->mavRates[streamNum]); + if (rate == 0) { + return 0; + } + + const timeUs_t intervalUs = 1000000UL / rate; + if ((mavActivePort->mavStreamNextDue[streamNum] == 0) || (cmpTimeUs(currentTimeUs, mavActivePort->mavStreamNextDue[streamNum]) >= 0)) { + mavActivePort->mavStreamNextDue[streamNum] = currentTimeUs + intervalUs; + return 1; + } + + return 0; +} + +void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate) +{ + if (!mavActivePort || streamNum >= MAXSTREAMS) { + return; + } + mavActivePort->mavRates[streamNum] = mavlinkClampStreamRate(rate); + mavActivePort->mavStreamNextDue[streamNum] = 0; + mavlinkResetMessagesForStream(streamNum); +} + +int mavlinkMessageTrigger(mavlinkPeriodicMessage_e periodicMessage, timeUs_t currentTimeUs) +{ + if (!mavActivePort) { + return 0; + } + + const int32_t intervalUs = mavlinkMessageIntervalUs(periodicMessage); + if (intervalUs <= 0) { + return 0; + } + + if ((mavActivePort->mavMessageNextDue[periodicMessage] == 0) || (cmpTimeUs(currentTimeUs, mavActivePort->mavMessageNextDue[periodicMessage]) >= 0)) { + mavActivePort->mavMessageNextDue[periodicMessage] = currentTimeUs + intervalUs; + return 1; + } + + return 0; +} + +void configureMAVLinkStreamRates(uint8_t portIndex) +{ + const mavlinkTelemetryPortConfig_t *primaryConfig = &telemetryConfig()->mavlink[0]; + const uint8_t mavPrimaryRates[MAVLINK_STREAM_COUNT] = { + [MAV_DATA_STREAM_EXTENDED_STATUS] = primaryConfig->extended_status_rate, + [MAV_DATA_STREAM_RC_CHANNELS] = primaryConfig->rc_channels_rate, + [MAV_DATA_STREAM_POSITION] = primaryConfig->position_rate, + [MAV_DATA_STREAM_EXTRA1] = primaryConfig->extra1_rate, + [MAV_DATA_STREAM_EXTRA2] = primaryConfig->extra2_rate, + [MAV_DATA_STREAM_EXTRA3] = primaryConfig->extra3_rate, + [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = primaryConfig->extra3_rate, + [MAV_DATA_STREAM_HEARTBEAT] = 1 + }; + + const uint8_t *selectedRates = (portIndex == 0) ? mavPrimaryRates : mavSecondaryRates; + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + for (uint8_t stream = 0; stream < MAVLINK_STREAM_COUNT; stream++) { + state->mavRates[stream] = selectedRates[stream]; + state->mavRatesConfigured[stream] = selectedRates[stream]; + state->mavStreamNextDue[stream] = 0; + } +} + +void processMAVLinkTelemetry(timeUs_t currentTimeUs) +{ + if (mavActivePort->highLatencyEnabled && mavlinkGetProtocolVersion() != 1) { + if ((currentTimeUs - mavActivePort->lastHighLatencyMessageUs) >= TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US) { + mavlinkSendHighLatency2(currentTimeUs); + mavActivePort->lastHighLatencyMessageUs = currentTimeUs; + } + return; + } + + // is executed @ TELEMETRY_MAVLINK_MAXRATE rate + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SYS_STATUS, currentTimeUs)) { + mavlinkSendSystemStatus(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS, currentTimeUs)) { + mavlinkSendRCChannelsAndRSSI(); + } + +#ifdef USE_GPS + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT, currentTimeUs)) { + mavlinkSendGpsRawInt(currentTimeUs); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT, currentTimeUs)) { + mavlinkSendGlobalPositionInt(currentTimeUs); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN, currentTimeUs)) { + mavlinkSendGpsGlobalOrigin(); + } +#endif + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_ATTITUDE, currentTimeUs)) { + mavlinkSendAttitude(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_VFR_HUD, currentTimeUs)) { + mavlinkSendVfrHud(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_HEARTBEAT, currentTimeUs)) { + mavlinkSendHeartbeat(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE, currentTimeUs)) { + mavlinkSendExtendedSysState(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS, currentTimeUs)) { + mavlinkSendBatteryStatus(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE, currentTimeUs)) { + mavlinkSendScaledPressure(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME, currentTimeUs)) { + mavlinkSendSystemTime(); + } + + if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3, currentTimeUs)) { + mavlinkSendStatusText(); + } +} + +#endif diff --git a/src/main/mavlink/mavlink_streams.h b/src/main/mavlink/mavlink_streams.h new file mode 100644 index 00000000000..2084518a546 --- /dev/null +++ b/src/main/mavlink/mavlink_streams.h @@ -0,0 +1,20 @@ +#pragma once + +#include "common/time.h" + +#include "telemetry/mavlink.h" + +extern const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT]; + +uint8_t mavlinkClampStreamRate(uint8_t rate); +int32_t mavlinkRateToIntervalUs(uint8_t rate); +bool mavlinkPeriodicMessageFromMessageId(uint16_t messageId, mavlinkPeriodicMessage_e *periodicMessage); +uint8_t mavlinkPeriodicMessageBaseStream(mavlinkPeriodicMessage_e periodicMessage); +int32_t mavlinkMessageBaseIntervalUs(mavlinkPeriodicMessage_e periodicMessage); +int32_t mavlinkMessageIntervalUs(mavlinkPeriodicMessage_e periodicMessage); +void mavlinkSetMessageOverrideIntervalUs(mavlinkPeriodicMessage_e periodicMessage, int32_t intervalUs); +int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum, timeUs_t currentTimeUs); +void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate); +int mavlinkMessageTrigger(mavlinkPeriodicMessage_e periodicMessage, timeUs_t currentTimeUs); +void configureMAVLinkStreamRates(uint8_t portIndex); +void processMAVLinkTelemetry(timeUs_t currentTimeUs); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 29fe90f58f9..fe019c6ec87 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -20,3471 +20,33 @@ * * Author: Konstantin Sharlaimov */ -#include -#include -#include -#include #include "platform.h" #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) -#include "build/build_config.h" -#include "build/debug.h" -#include "build/version.h" - -#include "common/axis.h" -#include "common/color.h" -#include "common/maths.h" -#include "common/utils.h" -#include "common/string_light.h" - -#include "config/feature.h" - -#include "drivers/serial.h" -#include "drivers/time.h" -#include "drivers/display.h" -#include "drivers/osd_symbols.h" - -#include "fc/config.h" -#include "fc/fc_core.h" -#include "fc/fc_msp.h" -#include "fc/rc_controls.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_profile.h" -#include "flight/pid.h" -#include "flight/servos.h" -#include "flight/wind_estimator.h" - -#include "io/adsb.h" -#include "io/gps.h" -#include "io/ledstrip.h" -#include "io/serial.h" -#include "io/osd.h" - -#include "msp/msp_protocol.h" -#include "msp/msp_serial.h" - -#include "navigation/navigation.h" -#include "navigation/navigation_private.h" - -#include "rx/rx.h" -#include "rx/mavlink.h" - -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/battery.h" -#include "sensors/boardalignment.h" -#include "sensors/gyro.h" -#include "sensors/pitotmeter.h" -#include "sensors/diagnostics.h" -#include "sensors/sensors.h" -#include "sensors/temperature.h" -#include "sensors/esc_sensor.h" +#include "mavlink/mavlink_runtime.h" #include "telemetry/mavlink.h" -#include "telemetry/telemetry.h" - -#include "blackbox/blackbox_io.h" - -#include "scheduler/scheduler.h" - -/* Secondary profile for ports 2..N: heartbeat only. */ -static const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT] = { - [MAV_DATA_STREAM_EXTENDED_STATUS] = 0, - [MAV_DATA_STREAM_RC_CHANNELS] = 0, - [MAV_DATA_STREAM_POSITION] = 0, - [MAV_DATA_STREAM_EXTRA1] = 0, - [MAV_DATA_STREAM_EXTRA2] = 0, - [MAV_DATA_STREAM_EXTRA3] = 0, - [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = 0, - [MAV_DATA_STREAM_HEARTBEAT] = 1 -}; - -static mavlinkPortRuntime_t mavPortStates[MAX_MAVLINK_PORTS]; -static uint8_t mavPortCount = 0; -static mavlinkRouteEntry_t mavRouteTable[MAVLINK_MAX_ROUTES]; -static uint8_t mavRouteCount = 0; -static mspPort_t mavTunnelMspPorts[MAX_MAVLINK_PORTS]; -static uint8_t mavTunnelRemoteSystemIds[MAX_MAVLINK_PORTS]; -static uint8_t mavTunnelRemoteComponentIds[MAX_MAVLINK_PORTS]; -static uint8_t mavSendMask = 0; -static mavlinkPortRuntime_t *mavActivePort = NULL; -static const mavlinkTelemetryPortConfig_t *mavActiveConfig = NULL; -static mavlink_message_t mavSendMsg; -static mavlink_message_t mavRecvMsg; - -// Set active MAV identity from global MAVLink settings. -static uint8_t mavSystemId = 1; -static uint8_t mavAutopilotType; -static uint8_t mavComponentId = MAV_COMP_ID_AUTOPILOT1; - -#define MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP 0x8001 -#define MAVLINK_TUNNEL_MSP_TIMEOUT_MS 1000 -#define MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE (MSP_PORT_OUTBUF_SIZE + 16) - -static uint8_t mavTunnelReplyPayloadBuf[MSP_PORT_OUTBUF_SIZE]; -static uint8_t mavTunnelFrameBuf[MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE]; - -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) - { - case FLM_ACRO: return COPTER_MODE_ACRO; - case FLM_ACRO_AIR: return COPTER_MODE_ACRO; - case FLM_ANGLE: return COPTER_MODE_STABILIZE; - case FLM_HORIZON: return COPTER_MODE_STABILIZE; - case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; - case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; - case FLM_POSITION_HOLD: - { - if (isGCSValid()) { - return COPTER_MODE_GUIDED; - } else { - return COPTER_MODE_POSHOLD; - } - } - case FLM_RTH: return COPTER_MODE_RTL; - case FLM_MISSION: return COPTER_MODE_AUTO; - case FLM_LAUNCH: return COPTER_MODE_THROW; - case FLM_FAILSAFE: - { - if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { - return COPTER_MODE_RTL; - } else if (failsafePhase() == FAILSAFE_LANDING) { - return COPTER_MODE_LAND; - } else { - return COPTER_MODE_RTL; - } - } - default: return COPTER_MODE_STABILIZE; - } -} - -static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) -{ - switch (flightMode) - { - case FLM_MANUAL: return PLANE_MODE_MANUAL; - case FLM_ACRO: return PLANE_MODE_ACRO; - case FLM_ACRO_AIR: return PLANE_MODE_ACRO; - case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; - case FLM_HORIZON: return PLANE_MODE_STABILIZE; - case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; - case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; - case FLM_POSITION_HOLD: - { - if (isGCSValid()) { - return PLANE_MODE_GUIDED; - } else { - return PLANE_MODE_LOITER; - } - } - case FLM_RTH: return PLANE_MODE_RTL; - case FLM_MISSION: return PLANE_MODE_AUTO; - case FLM_CRUISE: return PLANE_MODE_CRUISE; - case FLM_LAUNCH: return PLANE_MODE_TAKEOFF; - case FLM_FAILSAFE: //failsafePhase_e - { - if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { - return PLANE_MODE_RTL; - } - else if (failsafePhase() == FAILSAFE_LANDING) { - return PLANE_MODE_AUTOLAND; - } - else { - return PLANE_MODE_RTL; - } - } - 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 const mavlinkTelemetryPortConfig_t *mavlinkGetPortConfig(uint8_t portIndex) -{ - return &telemetryConfig()->mavlink[portIndex]; -} - -static const mavlinkTelemetryCommonConfig_t *mavlinkGetCommonConfig(void) -{ - return &telemetryConfig()->mavlink_common; -} - -static uint8_t mavlinkGetProtocolVersion(void) -{ - return mavlinkGetCommonConfig()->version; -} - -static void mavlinkApplyActivePortOutputVersion(void) -{ - mavlink_status_t *chanState = mavlink_get_channel_status(MAVLINK_COMM_0); - if (mavlinkGetProtocolVersion() == 1) { - chanState->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - } else { - chanState->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - } -} - -static void mavlinkSetActivePortContext(uint8_t portIndex) -{ - mavActivePort = &mavPortStates[portIndex]; - mavActiveConfig = mavlinkGetPortConfig(portIndex); - const mavlinkTelemetryCommonConfig_t *commonConfig = mavlinkGetCommonConfig(); - mavAutopilotType = commonConfig->autopilot_type; - mavSystemId = commonConfig->sysid; - mavComponentId = MAV_COMP_ID_AUTOPILOT1; - mavlinkApplyActivePortOutputVersion(); -} - -static uint8_t mavlinkClampStreamRate(uint8_t rate) -{ - if (rate > TELEMETRY_MAVLINK_MAXRATE) { - return TELEMETRY_MAVLINK_MAXRATE; - } - - return rate; -} - -static int32_t mavlinkRateToIntervalUs(uint8_t rate) -{ - rate = mavlinkClampStreamRate(rate); - if (rate == 0) { - return -1; - } - - return 1000000 / rate; -} - -static bool mavlinkPeriodicMessageFromMessageId(uint16_t messageId, mavlinkPeriodicMessage_e *periodicMessage) -{ - switch (messageId) { - case MAVLINK_MSG_ID_HEARTBEAT: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_HEARTBEAT; - return true; - case MAVLINK_MSG_ID_SYS_STATUS: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SYS_STATUS; - return true; - case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_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: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS; - return true; - case MAVLINK_MSG_ID_GPS_RAW_INT: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT; - return true; - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT; - return true; - case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN; - return true; - case MAVLINK_MSG_ID_ATTITUDE: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_ATTITUDE; - return true; - case MAVLINK_MSG_ID_VFR_HUD: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_VFR_HUD; - return true; - case MAVLINK_MSG_ID_BATTERY_STATUS: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS; - return true; - case MAVLINK_MSG_ID_SCALED_PRESSURE: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE; - return true; - case MAVLINK_MSG_ID_SYSTEM_TIME: - *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME; - return true; - default: - return false; - } -} - -static uint8_t mavlinkPeriodicMessageBaseStream(mavlinkPeriodicMessage_e periodicMessage) -{ - switch (periodicMessage) { - case MAVLINK_PERIODIC_MESSAGE_HEARTBEAT: - return MAV_DATA_STREAM_HEARTBEAT; - case MAVLINK_PERIODIC_MESSAGE_SYS_STATUS: - return MAV_DATA_STREAM_EXTENDED_STATUS; - case MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE: - return MAV_DATA_STREAM_EXTENDED_SYS_STATE; - case MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS: - return MAV_DATA_STREAM_RC_CHANNELS; - case MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT: - case MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT: - case MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN: - return MAV_DATA_STREAM_POSITION; - case MAVLINK_PERIODIC_MESSAGE_ATTITUDE: - return MAV_DATA_STREAM_EXTRA1; - case MAVLINK_PERIODIC_MESSAGE_VFR_HUD: - return MAV_DATA_STREAM_EXTRA2; - case MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS: - case MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE: - case MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME: - return MAV_DATA_STREAM_EXTRA3; - default: - return MAV_DATA_STREAM_ALL; - } -} - -static void mavlinkResetMessageSchedule(mavlinkPeriodicMessage_e periodicMessage) -{ - if (!mavActivePort) { - return; - } - - mavActivePort->mavMessageNextDue[periodicMessage] = 0; -} - -static void mavlinkResetMessagesForStream(uint8_t streamNum) -{ - if (!mavActivePort) { - return; - } - - for (uint8_t messageIndex = 0; messageIndex < MAVLINK_PERIODIC_MESSAGE_COUNT; messageIndex++) { - if (mavlinkPeriodicMessageBaseStream((mavlinkPeriodicMessage_e)messageIndex) == streamNum) { - mavActivePort->mavMessageNextDue[messageIndex] = 0; - } - } -} - -static int32_t mavlinkMessageBaseIntervalUs(mavlinkPeriodicMessage_e periodicMessage) -{ - if (!mavActivePort) { - return -1; - } - - return mavlinkRateToIntervalUs(mavActivePort->mavRates[mavlinkPeriodicMessageBaseStream(periodicMessage)]); -} - -static int32_t mavlinkMessageIntervalUs(mavlinkPeriodicMessage_e periodicMessage) -{ - if (!mavActivePort) { - return -1; - } - - const int32_t overrideIntervalUs = mavActivePort->mavMessageOverrideIntervalsUs[periodicMessage]; - if (overrideIntervalUs != 0) { - return overrideIntervalUs; - } - - return mavlinkMessageBaseIntervalUs(periodicMessage); -} - -static void mavlinkSetMessageOverrideIntervalUs(mavlinkPeriodicMessage_e periodicMessage, int32_t intervalUs) -{ - if (!mavActivePort) { - return; - } - - mavActivePort->mavMessageOverrideIntervalsUs[periodicMessage] = intervalUs; - mavlinkResetMessageSchedule(periodicMessage); -} - -static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum, timeUs_t currentTimeUs) -{ - if (!mavActivePort || streamNum >= MAXSTREAMS) { - return 0; - } - - const uint8_t rate = mavlinkClampStreamRate(mavActivePort->mavRates[streamNum]); - if (rate == 0) { - return 0; - } - - const timeUs_t intervalUs = 1000000UL / rate; - if ((mavActivePort->mavStreamNextDue[streamNum] == 0) || (cmpTimeUs(currentTimeUs, mavActivePort->mavStreamNextDue[streamNum]) >= 0)) { - mavActivePort->mavStreamNextDue[streamNum] = currentTimeUs + intervalUs; - return 1; - } - - return 0; -} - -static void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate) -{ - if (!mavActivePort || streamNum >= MAXSTREAMS) { - return; - } - mavActivePort->mavRates[streamNum] = mavlinkClampStreamRate(rate); - mavActivePort->mavStreamNextDue[streamNum] = 0; - mavlinkResetMessagesForStream(streamNum); -} - -static int mavlinkMessageTrigger(mavlinkPeriodicMessage_e periodicMessage, timeUs_t currentTimeUs) -{ - if (!mavActivePort) { - return 0; - } - - const int32_t intervalUs = mavlinkMessageIntervalUs(periodicMessage); - if (intervalUs <= 0) { - return 0; - } - - if ((mavActivePort->mavMessageNextDue[periodicMessage] == 0) || (cmpTimeUs(currentTimeUs, mavActivePort->mavMessageNextDue[periodicMessage]) >= 0)) { - mavActivePort->mavMessageNextDue[periodicMessage] = currentTimeUs + intervalUs; - return 1; - } - - return 0; -} - -static void configureMAVLinkStreamRates(uint8_t portIndex) -{ - const mavlinkTelemetryPortConfig_t *primaryConfig = &telemetryConfig()->mavlink[0]; - const uint8_t mavPrimaryRates[MAVLINK_STREAM_COUNT] = { - [MAV_DATA_STREAM_EXTENDED_STATUS] = primaryConfig->extended_status_rate, - [MAV_DATA_STREAM_RC_CHANNELS] = primaryConfig->rc_channels_rate, - [MAV_DATA_STREAM_POSITION] = primaryConfig->position_rate, - [MAV_DATA_STREAM_EXTRA1] = primaryConfig->extra1_rate, - [MAV_DATA_STREAM_EXTRA2] = primaryConfig->extra2_rate, - [MAV_DATA_STREAM_EXTRA3] = primaryConfig->extra3_rate, - [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = primaryConfig->extra3_rate, - [MAV_DATA_STREAM_HEARTBEAT] = 1 - }; - - const uint8_t *selectedRates = (portIndex == 0) ? mavPrimaryRates : mavSecondaryRates; - mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; - - for (uint8_t stream = 0; stream < MAVLINK_STREAM_COUNT; stream++) { - state->mavRates[stream] = selectedRates[stream]; - state->mavRatesConfigured[stream] = selectedRates[stream]; - state->mavStreamNextDue[stream] = 0; - } -} - -static void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) -{ - mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; - - if (state->port) { - closeSerialPort(state->port); - } - - state->port = NULL; - state->telemetryEnabled = false; - state->txbuffValid = false; - state->txbuffFree = 100; - state->lastMavlinkMessageUs = 0; - state->lastHighLatencyMessageUs = 0; - state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; - state->txSeq = 0; - state->txDroppedFrames = 0; - memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); - memset(state->mavMessageOverrideIntervalsUs, 0, sizeof(state->mavMessageOverrideIntervalsUs)); - memset(state->mavMessageNextDue, 0, sizeof(state->mavMessageNextDue)); - memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); - memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); - resetMspPort(&mavTunnelMspPorts[portIndex], NULL); - mavTunnelRemoteSystemIds[portIndex] = 0; - mavTunnelRemoteComponentIds[portIndex] = 0; -} - -void freeMAVLinkTelemetryPort(void) -{ - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - freeMAVLinkTelemetryPortByIndex(portIndex); - } - - mavSendMask = 0; - mavRouteCount = 0; -} void initMAVLinkTelemetry(void) { - memset(mavPortStates, 0, sizeof(mavPortStates)); - memset(mavRouteTable, 0, sizeof(mavRouteTable)); - memset(mavTunnelMspPorts, 0, sizeof(mavTunnelMspPorts)); - memset(mavTunnelRemoteSystemIds, 0, sizeof(mavTunnelRemoteSystemIds)); - memset(mavTunnelRemoteComponentIds, 0, sizeof(mavTunnelRemoteComponentIds)); - mavPortCount = 0; - mavRouteCount = 0; - mavSendMask = 0; - - const serialPortConfig_t *serialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); - while (serialPortConfig && mavPortCount < MAX_MAVLINK_PORTS) { - mavlinkPortRuntime_t *state = &mavPortStates[mavPortCount]; - state->portConfig = serialPortConfig; - state->portSharing = determinePortSharing(serialPortConfig, FUNCTION_TELEMETRY_MAVLINK); - state->txbuffFree = 100; - state->highLatencyEnabled = mavlinkGetPortConfig(mavPortCount)->high_latency; - configureMAVLinkStreamRates(mavPortCount); - - mavPortCount++; - serialPortConfig = findNextSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); - } - - mavActivePort = NULL; - mavActiveConfig = NULL; + mavlinkRuntimeInit(); } -static void configureMAVLinkTelemetryPort(uint8_t portIndex) +void handleMAVLinkTelemetry(timeUs_t currentTimeUs) { - mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; - - if (!state->portConfig) { - return; - } - - baudRate_e baudRateIndex = state->portConfig->telemetry_baudrateIndex; - if (baudRateIndex == BAUD_AUTO) { - // default rate for minimOSD - baudRateIndex = BAUD_57600; - } - - state->port = openSerialPort(state->portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_PORT_MODE, SERIAL_NOT_INVERTED); - if (!state->port) { - return; - } - - state->telemetryEnabled = true; - state->txbuffValid = false; - state->txbuffFree = 100; - state->lastMavlinkMessageUs = 0; - state->lastHighLatencyMessageUs = 0; - state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; - state->txSeq = 0; - state->txDroppedFrames = 0; - memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); - memset(state->mavMessageOverrideIntervalsUs, 0, sizeof(state->mavMessageOverrideIntervalsUs)); - memset(state->mavMessageNextDue, 0, sizeof(state->mavMessageNextDue)); - memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); - memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); - resetMspPort(&mavTunnelMspPorts[portIndex], NULL); - mavTunnelRemoteSystemIds[portIndex] = 0; - mavTunnelRemoteComponentIds[portIndex] = 0; + mavlinkRuntimeHandle(currentTimeUs); } void checkMAVLinkTelemetryState(void) { - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; - - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(state->portSharing); - if ((state->portConfig->functionMask & FUNCTION_RX_SERIAL) && - rxConfig()->receiverType == RX_TYPE_SERIAL && - rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { - newTelemetryEnabledValue = true; - } - - if (newTelemetryEnabledValue == state->telemetryEnabled) { - continue; - } - - if (newTelemetryEnabledValue) { - configureMAVLinkTelemetryPort(portIndex); - if (state->telemetryEnabled) { - configureMAVLinkStreamRates(portIndex); - } - } else { - freeMAVLinkTelemetryPortByIndex(portIndex); - } - } -} - -static void mavlinkSendMessage(void) -{ - const mavlink_msg_entry_t *msgEntry = mavlink_get_msg_entry(mavSendMsg.msgid); - if (!msgEntry) { - return; - } - - uint8_t sendMask = mavSendMask; - if (sendMask == 0) { - if (mavActivePort) { - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - if (&mavPortStates[portIndex] == mavActivePort) { - sendMask = MAVLINK_PORT_MASK(portIndex); - break; - } - } - } else if (mavPortCount == 1 && mavPortStates[0].telemetryEnabled && mavPortStates[0].port) { - sendMask = MAVLINK_PORT_MASK(0); - } - } - - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - if ((sendMask & MAVLINK_PORT_MASK(portIndex)) == 0) { - continue; - } - - mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; - if (!state->telemetryEnabled || !state->port) { - continue; - } - - mavlink_status_t txStatus = { 0 }; - txStatus.current_tx_seq = state->txSeq; - if (mavlinkGetProtocolVersion() == 1) { - txStatus.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - } - - mavlink_message_t txMsg = mavSendMsg; - mavlink_finalize_message_buffer( - &txMsg, - txMsg.sysid, - txMsg.compid, - &txStatus, - msgEntry->min_msg_len, - txMsg.len, - msgEntry->crc_extra - ); - state->txSeq = txStatus.current_tx_seq; - - uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; - const int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &txMsg); - if (msgLength <= 0) { - continue; - } - - // Drop the frame on this port if there is no room; do not block telemetry task. - if (serialTxBytesFree(state->port) < (uint32_t)msgLength) { - state->txDroppedFrames++; - continue; - } - - serialBeginWrite(state->port); - serialWriteBuf(state->port, mavBuffer, msgLength); - serialEndWrite(state->port); - } -} - -static void mavlinkResetTunnelState(uint8_t ingressPortIndex) -{ - resetMspPort(&mavTunnelMspPorts[ingressPortIndex], NULL); - mavTunnelRemoteSystemIds[ingressPortIndex] = 0; - mavTunnelRemoteComponentIds[ingressPortIndex] = 0; -} - -static void mavlinkSendTunnelReply(uint8_t targetSystem, uint8_t targetComponent, const uint8_t *payload, uint8_t payloadLength) -{ - uint8_t tunnelPayload[MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN] = { 0 }; - memcpy(tunnelPayload, payload, payloadLength); - - mavlink_msg_tunnel_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - targetSystem, - targetComponent, - MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP, - payloadLength, - tunnelPayload); - mavlinkSendMessage(); -} - -static void mavlinkSendTunnelMspReply(uint8_t targetSystem, uint8_t targetComponent, mspPacket_t *reply, uint8_t *replyPayloadHead, mspVersion_e mspVersion) -{ - sbufSwitchToReader(&reply->buf, replyPayloadHead); - - const int frameLength = mspSerialEncodePacket(reply, mspVersion, mavTunnelFrameBuf, sizeof(mavTunnelFrameBuf)); - for (int offset = 0; offset < frameLength; offset += MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { - const uint8_t chunkLength = MIN(MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN, frameLength - offset); - mavlinkSendTunnelReply( - targetSystem, - targetComponent, - mavTunnelFrameBuf + offset, - chunkLength); - } -} - -static bool handleIncoming_TUNNEL(uint8_t ingressPortIndex) -{ - if (mavlinkGetProtocolVersion() == 1) { - return false; - } - - mavlink_tunnel_t msg; - mavlink_msg_tunnel_decode(&mavRecvMsg, &msg); - - if (msg.payload_type != MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP) { - return false; - } - - if (msg.target_system != mavSystemId) { - return false; - } - - if (msg.target_component != 0 && msg.target_component != mavComponentId) { - return false; - } - - mspPort_t *mspPort = &mavTunnelMspPorts[ingressPortIndex]; - const timeMs_t now = millis(); - if (msg.payload_length > MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { - mavlinkResetTunnelState(ingressPortIndex); - return false; - } - - if (mspPort->c_state != MSP_IDLE && - ((now - mspPort->lastActivityMs) > MAVLINK_TUNNEL_MSP_TIMEOUT_MS || - mavTunnelRemoteSystemIds[ingressPortIndex] != mavRecvMsg.sysid || - mavTunnelRemoteComponentIds[ingressPortIndex] != mavRecvMsg.compid)) { - mavlinkResetTunnelState(ingressPortIndex); - } - - mavTunnelRemoteSystemIds[ingressPortIndex] = mavRecvMsg.sysid; - mavTunnelRemoteComponentIds[ingressPortIndex] = mavRecvMsg.compid; - mspPort->lastActivityMs = now; - - bool handled = false; - for (uint8_t payloadIndex = 0; payloadIndex < msg.payload_length; payloadIndex++) { - if (!mspSerialProcessReceivedByte(mspPort, msg.payload[payloadIndex])) { - continue; - } - - handled = true; - if (mspPort->c_state != MSP_COMMAND_RECEIVED) { - continue; - } - - mspPacket_t reply = { - .buf = { .ptr = mavTunnelReplyPayloadBuf, .end = ARRAYEND(mavTunnelReplyPayloadBuf), }, - .cmd = -1, - .flags = 0, - .result = 0, - }; - uint8_t *replyPayloadHead = reply.buf.ptr; - - if (mspPort->cmdMSP == MSP_SET_PASSTHROUGH) { - reply.cmd = MSP_SET_PASSTHROUGH; - reply.result = MSP_RESULT_ERROR; - mavlinkSendTunnelMspReply(mavRecvMsg.sysid, mavRecvMsg.compid, &reply, replyPayloadHead, mspPort->mspVersion); - mavlinkResetTunnelState(ingressPortIndex); - break; - } - - mspPostProcessFnPtr mspPostProcessFn = NULL; - const uint16_t command = mspPort->cmdMSP; - mspResult_e status = mspSerialProcessCommand(mspPort, mspFcProcessCommand, &reply, &mspPostProcessFn); - - if (mspPostProcessFn && command != MSP_REBOOT) { - sbufInit(&reply.buf, mavTunnelReplyPayloadBuf, ARRAYEND(mavTunnelReplyPayloadBuf)); - reply.result = MSP_RESULT_ERROR; - mspPostProcessFn = NULL; - status = MSP_RESULT_ERROR; - } - - if (status != MSP_RESULT_NO_REPLY) { - mavlinkSendTunnelMspReply(mavRecvMsg.sysid, mavRecvMsg.compid, &reply, replyPayloadHead, mspPort->mspVersion); - } - - mavlinkResetTunnelState(ingressPortIndex); - - if (mspPostProcessFn) { - waitForSerialPortToFinishTransmitting(mavPortStates[ingressPortIndex].port); - mspPostProcessFn(mavPortStates[ingressPortIndex].port); - } - - break; - } - - return handled; -} - -static void mavlinkSendAutopilotVersion(void) -{ - if (mavlinkGetProtocolVersion() == 1) return; - - // Capabilities aligned with what we actually support. - uint64_t capabilities = 0; - capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; - capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; - capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT; - capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT; - capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; - capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT; - - 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 - flightSwVersion, // 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 (mavlinkGetProtocolVersion() == 1) return; - - uint8_t specHash[8] = {0}; - uint8_t libHash[8] = {0}; - const uint16_t protocolVersion = (uint16_t)mavlinkGetProtocolVersion() * 100; - - mavlink_msg_protocol_version_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - protocolVersion, - protocolVersion, - protocolVersion, - specHash, - libHash); - - mavlinkSendMessage(); -} - -static bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask) -{ - switch (frame) { - case MAV_FRAME_GLOBAL: - return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL; - case MAV_FRAME_GLOBAL_INT: - return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_INT; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT; - case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: - return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT; - default: - return false; - } -} - -static bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame) -{ - return frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_INT; -} - -static MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters) -{ -#ifdef USE_BARO - geoAltitudeDatumFlag_e datum; - - switch (frame) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_INT: - datum = NAV_WP_MSL_DATUM; - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: - datum = NAV_WP_TAKEOFF_DATUM; - break; - default: - return MAV_RESULT_UNSUPPORTED; - } - - const int32_t targetAltitudeCm = (int32_t)lrintf(altitudeMeters * 100.0f); - return navigationSetAltitudeTargetWithDatum(datum, targetAltitudeCm) ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED; -#else - UNUSED(frame); - UNUSED(altitudeMeters); - return MAV_RESULT_UNSUPPORTED; -#endif -} - -static MAV_MISSION_RESULT mavlinkMissionResultFromCommandResult(MAV_RESULT result) -{ - switch (result) { - case MAV_RESULT_ACCEPTED: - return MAV_MISSION_ACCEPTED; - case MAV_RESULT_UNSUPPORTED: - return MAV_MISSION_UNSUPPORTED; - default: - return MAV_MISSION_ERROR; - } -} - -static bool mavlinkHandleArmedGuidedMissionItem(uint8_t current, uint8_t frame, mavFrameSupportMask_e allowedFrames, int32_t latitudeE7, int32_t longitudeE7, float altitudeMeters) -{ - if (!isGCSValid()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavRecvMsg.sysid, mavRecvMsg.compid, - MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - if (!mavlinkFrameIsSupported(frame, allowedFrames)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavRecvMsg.sysid, mavRecvMsg.compid, - MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - if (current == 2) { - navWaypoint_t wp = {0}; - wp.action = NAV_WP_ACTION_WAYPOINT; - wp.lat = latitudeE7; - wp.lon = longitudeE7; - wp.alt = (int32_t)lrintf(altitudeMeters * 100.0f); - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - - setWaypoint(255, &wp); - - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavRecvMsg.sysid, mavRecvMsg.compid, - MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - if (current == 3) { - const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame(frame, altitudeMeters); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavRecvMsg.sysid, mavRecvMsg.compid, - mavlinkMissionResultFromCommandResult(result), MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavRecvMsg.sysid, mavRecvMsg.compid, - MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; -} - -static uint8_t navWaypointFrame(const navWaypoint_t *wp, bool useIntMessages) -{ - switch (wp->action) { - case NAV_WP_ACTION_RTH: - case NAV_WP_ACTION_JUMP: - case NAV_WP_ACTION_SET_HEAD: - return MAV_FRAME_MISSION; - default: - break; - } - - const bool absoluteAltitude = (wp->p3 & NAV_WP_ALTMODE) == NAV_WP_ALTMODE; - - if (absoluteAltitude) { - return useIntMessages ? MAV_FRAME_GLOBAL_INT : MAV_FRAME_GLOBAL; - } - - return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT; -} - -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" }, -}; -const uint8_t planeModesCount = (uint8_t)ARRAYLEN(planeModes); - -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" }, -}; -const uint8_t copterModesCount = (uint8_t)ARRAYLEN(copterModes); - -static bool mavlinkPlaneModeIsConfigured(uint8_t customMode) -{ - switch ((APM_PLANE_MODE)customMode) { - case PLANE_MODE_MANUAL: - return isModeActivationConditionPresent(BOXMANUAL); - case PLANE_MODE_ACRO: - return true; - case PLANE_MODE_STABILIZE: - return isModeActivationConditionPresent(BOXHORIZON) || - isModeActivationConditionPresent(BOXANGLEHOLD); - case PLANE_MODE_FLY_BY_WIRE_A: - return isModeActivationConditionPresent(BOXANGLE); - case PLANE_MODE_FLY_BY_WIRE_B: - return isModeActivationConditionPresent(BOXNAVALTHOLD); - case PLANE_MODE_CRUISE: - return isModeActivationConditionPresent(BOXNAVCRUISE) || - (isModeActivationConditionPresent(BOXNAVCOURSEHOLD) && - isModeActivationConditionPresent(BOXNAVALTHOLD)); - case PLANE_MODE_AUTO: - return isModeActivationConditionPresent(BOXNAVWP); - case PLANE_MODE_RTL: - return isModeActivationConditionPresent(BOXNAVRTH); - case PLANE_MODE_GUIDED: - return isModeActivationConditionPresent(BOXNAVPOSHOLD) && - isModeActivationConditionPresent(BOXGCSNAV); - case PLANE_MODE_LOITER: - return isModeActivationConditionPresent(BOXNAVPOSHOLD); - case PLANE_MODE_TAKEOFF: - return isModeActivationConditionPresent(BOXNAVLAUNCH); - default: - return false; - } -} - -static bool mavlinkCopterModeIsConfigured(uint8_t customMode) -{ - switch ((APM_COPTER_MODE)customMode) { - case COPTER_MODE_ACRO: - return true; - case COPTER_MODE_STABILIZE: - return isModeActivationConditionPresent(BOXANGLE) || - isModeActivationConditionPresent(BOXHORIZON) || - isModeActivationConditionPresent(BOXANGLEHOLD); - case COPTER_MODE_ALT_HOLD: - return isModeActivationConditionPresent(BOXNAVALTHOLD); - case COPTER_MODE_GUIDED: - return isModeActivationConditionPresent(BOXNAVPOSHOLD) && - isModeActivationConditionPresent(BOXGCSNAV); - case COPTER_MODE_POSHOLD: - return isModeActivationConditionPresent(BOXNAVPOSHOLD); - case COPTER_MODE_RTL: - return isModeActivationConditionPresent(BOXNAVRTH); - case COPTER_MODE_AUTO: - return isModeActivationConditionPresent(BOXNAVWP); - case COPTER_MODE_THROW: - return isModeActivationConditionPresent(BOXNAVLAUNCH); - case COPTER_MODE_BRAKE: - return isModeActivationConditionPresent(BOXBRAKING); - default: - return false; - } -} - -static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom, - bool (*isModeConfigured)(uint8_t customMode)) -{ - uint8_t availableCount = 0; - for (uint8_t i = 0; i < count; i++) { - if (isModeConfigured(modes[i].customMode)) { - availableCount++; - } - } - - if (availableCount == 0) { - return; - } - - uint8_t modeIndex = 0; - for (uint8_t i = 0; i < count; i++) { - if (!isModeConfigured(modes[i].customMode)) { - continue; - } - - modeIndex++; - const uint8_t stdMode = MAV_STANDARD_MODE_NON_STANDARD; - const uint32_t properties = 0; - - mavlink_msg_available_modes_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - availableCount, - modeIndex, - stdMode, - modes[i].customMode, - properties, - modes[i].name); - - mavlinkSendMessage(); - - if (modes[i].customMode == currentCustom) { - mavlink_msg_current_mode_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - stdMode, - currentCustom, - currentCustom); - mavlinkSendMessage(); - } - } -} - -static void mavlinkSendExtendedSysState(void) -{ - uint8_t vtolState = MAV_VTOL_STATE_UNDEFINED; - uint8_t landedState; - - switch (NAV_Status.state) { - case MW_NAV_STATE_LAND_START: - case MW_NAV_STATE_LAND_IN_PROGRESS: - case MW_NAV_STATE_LAND_SETTLE: - case MW_NAV_STATE_LAND_START_DESCENT: - case MW_NAV_STATE_EMERGENCY_LANDING: - landedState = MAV_LANDED_STATE_LANDING; - break; - case MW_NAV_STATE_LANDED: - landedState = MAV_LANDED_STATE_ON_GROUND; - break; - default: - if (!ARMING_FLAG(ARMED) || STATE(LANDING_DETECTED)) { - landedState = MAV_LANDED_STATE_ON_GROUND; - } else { - landedState = MAV_LANDED_STATE_IN_AIR; - } - break; - } - - mavlink_msg_extended_sys_state_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - vtolState, - landedState); - - mavlinkSendMessage(); -} - -void mavlinkSendSystemStatus(void) -{ - // Receiver is assumed to be always present - uint32_t onboard_control_sensors_present = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER); - // GYRO and RC are assumed as minimum requirements - uint32_t onboard_control_sensors_enabled = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER); - uint32_t onboard_control_sensors_health = 0; - - if (getHwGyroStatus() == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO; - // Missing presence will report as sensor unhealthy - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO; - } - - hardwareSensorStatus_e accStatus = getHwAccelerometerStatus(); - if (accStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } else if (accStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } else if (accStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } - - hardwareSensorStatus_e compassStatus = getHwCompassStatus(); - if (compassStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } else if (compassStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } else if (compassStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } - - hardwareSensorStatus_e baroStatus = getHwBarometerStatus(); - if (baroStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } else if (baroStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } else if (baroStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } - - hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus(); - if (pitotStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } else if (pitotStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } else if (pitotStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } - - hardwareSensorStatus_e gpsStatus = getHwGPSStatus(); - if (gpsStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; - } else if (gpsStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; - } else if (gpsStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; - } - - hardwareSensorStatus_e opFlowStatus = getHwOpticalFlowStatus(); - if (opFlowStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } else if (opFlowStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } else if (opFlowStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } - - hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus(); - if (rangefinderStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } else if (rangefinderStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } else if (rangefinderStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - - if (rxIsReceivingSignal() && rxAreFlightChannelsValid()) { - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } - -#ifdef USE_BLACKBOX - // BLACKBOX is assumed enabled and present for boards with capability - onboard_control_sensors_present |= MAV_SYS_STATUS_LOGGING; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; - // Unhealthy only for cases with not enough space to record - if (!isBlackboxDeviceFull()) { - onboard_control_sensors_health |= MAV_SYS_STATUS_LOGGING; - } -#endif - - mavlink_msg_sys_status_pack(mavSystemId, mavComponentId, &mavSendMsg, - // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. - //Value of 0: not present. Value of 1: present. Indices according MAV_SYS_STATUS_SENSOR - onboard_control_sensors_present, - // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled - onboard_control_sensors_enabled, - // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error. - onboard_control_sensors_health, - // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - constrain(averageSystemLoadPercent*10, 0, 1000), - // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) - feature(FEATURE_VBAT) ? getBatteryVoltage() * 10 : 0, - // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - isAmperageConfigured() ? getAmperage() : -1, - // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery - feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100, - // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - 0, - // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - 0, - // errors_count1 Autopilot-specific errors - 0, - // errors_count2 Autopilot-specific errors - 0, - // errors_count3 Autopilot-specific errors - 0, - // errors_count4 Autopilot-specific errors - 0, 0, 0, 0); - - mavlinkSendMessage(); -} - -void mavlinkSendRCChannelsAndRSSI(void) -{ -#define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) - if (mavlinkGetProtocolVersion() == 1) { - mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - millis(), - // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - 0, - // chan1_raw RC channel 1 value, in microseconds - GET_CHANNEL_VALUE(0), - // chan2_raw RC channel 2 value, in microseconds - GET_CHANNEL_VALUE(1), - // chan3_raw RC channel 3 value, in microseconds - GET_CHANNEL_VALUE(2), - // chan4_raw RC channel 4 value, in microseconds - GET_CHANNEL_VALUE(3), - // chan5_raw RC channel 5 value, in microseconds - GET_CHANNEL_VALUE(4), - // chan6_raw RC channel 6 value, in microseconds - GET_CHANNEL_VALUE(5), - // chan7_raw RC channel 7 value, in microseconds - GET_CHANNEL_VALUE(6), - // chan8_raw RC channel 8 value, in microseconds - GET_CHANNEL_VALUE(7), - // rssi Receive signal strength indicator, 0: 0%, 254: 100% - //https://github.com/mavlink/mavlink/issues/1027 - scaleRange(getRSSI(), 0, 1023, 0, 254)); - } - else { - mavlink_msg_rc_channels_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - millis(), - // Total number of RC channels being received. - rxRuntimeConfig.channelCount, - // chan1_raw RC channel 1 value, in microseconds - GET_CHANNEL_VALUE(0), - // chan2_raw RC channel 2 value, in microseconds - GET_CHANNEL_VALUE(1), - // chan3_raw RC channel 3 value, in microseconds - GET_CHANNEL_VALUE(2), - // chan4_raw RC channel 4 value, in microseconds - GET_CHANNEL_VALUE(3), - // chan5_raw RC channel 5 value, in microseconds - GET_CHANNEL_VALUE(4), - // chan6_raw RC channel 6 value, in microseconds - GET_CHANNEL_VALUE(5), - // chan7_raw RC channel 7 value, in microseconds - GET_CHANNEL_VALUE(6), - // chan8_raw RC channel 8 value, in microseconds - GET_CHANNEL_VALUE(7), - // chan9_raw RC channel 9 value, in microseconds - GET_CHANNEL_VALUE(8), - // chan10_raw RC channel 10 value, in microseconds - GET_CHANNEL_VALUE(9), - // chan11_raw RC channel 11 value, in microseconds - GET_CHANNEL_VALUE(10), - // chan12_raw RC channel 12 value, in microseconds - GET_CHANNEL_VALUE(11), - // chan13_raw RC channel 13 value, in microseconds - GET_CHANNEL_VALUE(12), - // chan14_raw RC channel 14 value, in microseconds - GET_CHANNEL_VALUE(13), - // chan15_raw RC channel 15 value, in microseconds - GET_CHANNEL_VALUE(14), - // chan16_raw RC channel 16 value, in microseconds - GET_CHANNEL_VALUE(15), - // chan17_raw RC channel 17 value, in microseconds - GET_CHANNEL_VALUE(16), - // chan18_raw RC channel 18 value, in microseconds - GET_CHANNEL_VALUE(17), - // rssi Receive signal strength indicator, 0: 0%, 254: 100% - //https://github.com/mavlink/mavlink/issues/1027 - scaleRange(getRSSI(), 0, 1023, 0, 254)); - } -#undef GET_CHANNEL_VALUE - - mavlinkSendMessage(); + mavlinkRuntimeCheckState(); } -#if defined(USE_GPS) -static void mavlinkSendHomePosition(void) +void freeMAVLinkTelemetryPort(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(); -} - -static void mavlinkSendGpsRawInt(timeUs_t currentTimeUs) -{ - uint8_t gpsFixType = 0; - - if (!(sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - )) - return; - - if (gpsSol.fixType == GPS_NO_FIX) - gpsFixType = 1; - else if (gpsSol.fixType == GPS_FIX_2D) - gpsFixType = 2; - else if (gpsSol.fixType == GPS_FIX_3D) - gpsFixType = 3; - - mavlink_msg_gps_raw_int_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - currentTimeUs, - // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - gpsFixType, - // lat Latitude in 1E7 degrees - gpsSol.llh.lat, - // lon Longitude in 1E7 degrees - gpsSol.llh.lon, - // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 10, - // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - gpsSol.eph, - // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - gpsSol.epv, - // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - gpsSol.groundSpeed, - // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - gpsSol.groundCourse * 10, - // satellites_visible Number of satellites visible. If unknown, set to 255 - gpsSol.numSat, - // alt_ellipsoid Altitude (above WGS84, EGM96 ellipsoid). Positive for up - 0, - // h_acc Position uncertainty in mm, - gpsSol.eph * 10, - // v_acc Altitude uncertainty in mm, - gpsSol.epv * 10, - // vel_acc Speed uncertainty in mm (??) - 0, - // hdg_acc Heading uncertainty in degE5 - 0, - // yaw Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. - 0); - - mavlinkSendMessage(); -} - -static void mavlinkSendGlobalPositionInt(timeUs_t currentTimeUs) -{ - if (!(sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - )) { - return; - } - - mavlink_msg_global_position_int_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - currentTimeUs / 1000, - // lat Latitude in 1E7 degrees - gpsSol.llh.lat, - // lon Longitude in 1E7 degrees - gpsSol.llh.lon, - // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 10, - // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) - getEstimatedActualPosition(Z) * 10, - // [cm/s] Ground X Speed (Latitude, positive north) - getEstimatedActualVelocity(X), - // [cm/s] Ground Y Speed (Longitude, positive east) - getEstimatedActualVelocity(Y), - // [cm/s] Ground Z Speed (Altitude, positive down) - -getEstimatedActualVelocity(Z), - // [cdeg] Vehicle heading (yaw angle) (0.0..359.99 degrees, 0=north) - DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw) - ); - - mavlinkSendMessage(); -} - -static void mavlinkSendGpsGlobalOrigin(void) -{ - mavlink_msg_gps_global_origin_pack(mavSystemId, mavComponentId, &mavSendMsg, - // latitude Latitude (WGS84), expressed as * 1E7 - GPS_home.lat, - // longitude Longitude (WGS84), expressed as * 1E7 - GPS_home.lon, - // altitude Altitude(WGS84), expressed as * 1000 - GPS_home.alt * 10, // FIXME - // time_usec Timestamp (microseconds since system boot) - // Use millis() * 1000 as micros() will overflow after 1.19 hours. - ((uint64_t) millis()) * 1000); - - mavlinkSendMessage(); -} - -void mavlinkSendPosition(timeUs_t currentTimeUs) -{ - mavlinkSendGpsRawInt(currentTimeUs); - mavlinkSendGlobalPositionInt(currentTimeUs); - mavlinkSendGpsGlobalOrigin(); -} -#endif - -void mavlinkSendAttitude(void) -{ - mavlink_msg_attitude_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - millis(), - // roll Roll angle (rad) - RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.roll)), - // pitch Pitch angle (rad) - RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(-attitude.values.pitch)), - // yaw Yaw angle (rad) - RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)), - // rollspeed Roll angular speed (rad/s) - DEGREES_TO_RADIANS(gyro.gyroADCf[FD_ROLL]), - // pitchspeed Pitch angular speed (rad/s) - DEGREES_TO_RADIANS(gyro.gyroADCf[FD_PITCH]), - // yawspeed Yaw angular speed (rad/s) - DEGREES_TO_RADIANS(gyro.gyroADCf[FD_YAW])); - - mavlinkSendMessage(); -} - -void mavlinkSendVfrHud(void) -{ - float mavAltitude = 0; - float mavGroundSpeed = 0; - float mavAirSpeed = 0; - float mavClimbRate = 0; - -#if defined(USE_GPS) - // use ground speed if source available - if (sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - ) { - mavGroundSpeed = gpsSol.groundSpeed / 100.0f; - } -#endif - -#if defined(USE_PITOT) - if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - mavAirSpeed = getAirspeedEstimate() / 100.0f; - } -#endif - - // select best source for altitude - mavAltitude = getEstimatedActualPosition(Z) / 100.0f; - mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; - - int16_t thr = getThrottlePercent(osdUsingScaledThrottle()); - mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg, - // airspeed Current airspeed in m/s - mavAirSpeed, - // groundspeed Current ground speed in m/s - mavGroundSpeed, - // heading Current heading in degrees, in compass units (0..360, 0=north) - DECIDEGREES_TO_DEGREES(attitude.values.yaw), - // throttle Current throttle setting in integer percent, 0 to 100 - thr, - // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) - mavAltitude, - // climb Current climb rate in meters/second - mavClimbRate); - - mavlinkSendMessage(); -} - -static uint8_t mavlinkGetAutopilotEnum(void); - -void mavlinkSendHeartbeat(void) -{ - uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - - 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) { - mavSystemType = MAV_TYPE_FIXED_WING; - } - else { - mavSystemType = mavlinkGetVehicleType(); - } - - 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 && isGCSValid()) { - mavModes |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - else if (flm == FLM_MISSION || flm == FLM_RTH ) { - mavModes |= MAV_MODE_FLAG_AUTO_ENABLED; - } - else if (flm != FLM_MANUAL && flm!= FLM_ACRO && flm!=FLM_ACRO_AIR) { - mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - if (ARMING_FLAG(ARMED)) - mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; - - uint8_t mavSystemState = 0; - if (ARMING_FLAG(ARMED)) { - if (failsafeIsActive()) { - mavSystemState = MAV_STATE_CRITICAL; - } - else { - mavSystemState = MAV_STATE_ACTIVE; - } - } - else if (areSensorsCalibrating()) { - mavSystemState = MAV_STATE_CALIBRATING; - } - else { - mavSystemState = MAV_STATE_STANDBY; - } - - mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg, - // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - mavSystemType, - // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - mavlinkGetAutopilotEnum(), - // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - mavModes, - // custom_mode A bitfield for use for autopilot-specific flags. - mavCustomMode, - // system_status System status flag, see MAV_STATE ENUM - mavSystemState); - - mavlinkSendMessage(); -} - -static void mavlinkSendBatteryStatus(void) -{ - uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; - uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; - memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages)); - memset(batteryVoltagesExt, 0, sizeof(batteryVoltagesExt)); - if (feature(FEATURE_VBAT)) { - uint8_t batteryCellCount = getBatteryCellCount(); - if (batteryCellCount > 0) { - for (int cell=0; cell < batteryCellCount && cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN + MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; cell++) { - if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { - batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; - } else { - batteryVoltagesExt[cell-MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN] = getBatteryAverageCellVoltage() * 10; - } - } - } - else { - batteryVoltages[0] = getBatteryVoltage() * 10; - } - } - else { - batteryVoltages[0] = 0; - } - - mavlink_msg_battery_status_pack(mavSystemId, mavComponentId, &mavSendMsg, - // id Battery ID - 0, - // battery_function Function of the battery - MAV_BATTERY_FUNCTION_UNKNOWN, - // type Type (chemistry) of the battery - MAV_BATTERY_TYPE_UNKNOWN, - // temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature - INT16_MAX, - // voltages Battery voltage of cells, in millivolts (1 = 1 millivolt). Cells above the valid cell count for this battery should have the UINT16_MAX value. - batteryVoltages, - // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - isAmperageConfigured() ? getAmperage() : -1, - // current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate - isAmperageConfigured() ? getMAhDrawn() : -1, - // energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate - isAmperageConfigured() ? getMWhDrawn()*36 : -1, - // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery); - feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1, - // time_remaining Remaining battery time, 0: autopilot does not provide remaining battery time estimate - 0, // TODO this could easily be implemented - // charge_state State for extent of discharge, provided by autopilot for warning or external reactions - 0, - // voltages_ext Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. - batteryVoltagesExt, - // mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. - 0, - // fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). - 0); - - mavlinkSendMessage(); -} - -static void mavlinkSendScaledPressure(void) -{ - int16_t temperature; - sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); - mavlink_msg_scaled_pressure_pack(mavSystemId, mavComponentId, &mavSendMsg, - millis(), - 0, - 0, - temperature * 10, - 0); - - mavlinkSendMessage(); -} - -static void mavlinkSendSystemTime(void) -{ - mavlink_msg_system_time_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - 0, - millis()); - - mavlinkSendMessage(); -} - -static bool mavlinkSendStatusText(void) -{ -// FIXME - Status text is limited to boards with USE_OSD -#ifdef USE_OSD - char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "}; - textAttributes_t elemAttr = osdGetSystemMessage(buff, sizeof(buff), false); - if (buff[0] != SYM_BLANK) { - MAV_SEVERITY severity = MAV_SEVERITY_NOTICE; - if (TEXT_ATTRIBUTES_HAVE_BLINK(elemAttr)) { - severity = MAV_SEVERITY_CRITICAL; - } else if TEXT_ATTRIBUTES_HAVE_INVERTED(elemAttr) { - severity = MAV_SEVERITY_WARNING; - } - - mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg, - (uint8_t)severity, - buff, - 0, - 0); - - mavlinkSendMessage(); - return true; - } -#endif - return false; -} - -void mavlinkSendBatteryTemperatureStatusText(void) -{ - mavlinkSendBatteryStatus(); - mavlinkSendScaledPressure(); - mavlinkSendStatusText(); -} - -static uint8_t mavlinkGetAutopilotEnum(void) -{ - if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { - return MAV_AUTOPILOT_ARDUPILOTMEGA; - } - - return MAV_AUTOPILOT_GENERIC; -} - -static uint8_t mavlinkHeadingDeg2FromCd(int32_t headingCd) -{ - return (uint8_t)(wrap_36000(headingCd) / 200); -} - -static uint16_t mavlinkComputeHighLatencyFailures(void) -{ - uint16_t flags = 0; - -#if defined(USE_GPS) - if (!(sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - ) || gpsSol.fixType == GPS_NO_FIX) { - flags |= HL_FAILURE_FLAG_GPS; - } -#endif - -#ifdef USE_PITOT - if (getHwPitotmeterStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; - } -#endif - - if (getHwBarometerStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; - } - - if (getHwAccelerometerStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_3D_ACCEL; - } - - if (getHwGyroStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_3D_GYRO; - } - - if (getHwCompassStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_3D_MAG; - } - - if (getHwRangefinderStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_TERRAIN; - } - - const batteryState_e batteryState = getBatteryState(); - if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { - flags |= HL_FAILURE_FLAG_BATTERY; - } - - if (!rxIsReceivingSignal() || !rxAreFlightChannelsValid()) { - flags |= HL_FAILURE_FLAG_RC_RECEIVER; - } - - return flags; -} - -static void mavlinkSendHighLatency2(timeUs_t currentTimeUs) -{ - const bool isPlane = (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); - - int32_t latitude = 0; - int32_t longitude = 0; - int16_t altitude = (int16_t)constrain((int32_t)(getEstimatedActualPosition(Z) / 100), INT16_MIN, INT16_MAX); - int16_t targetAltitude = (int16_t)constrain((int32_t)(posControl.desiredState.pos.z / 100), INT16_MIN, INT16_MAX); - uint16_t targetDistance = 0; - uint16_t wpNum = 0; - uint8_t heading = mavlinkHeadingDeg2FromCd(DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)); - uint8_t targetHeading = mavlinkHeadingDeg2FromCd(posControl.desiredState.yaw); - uint8_t groundspeed = 0; - uint8_t airspeed = 0; - uint8_t airspeedSp = 0; - uint8_t windspeed = 0; - uint8_t windHeading = 0; - uint8_t eph = UINT8_MAX; - uint8_t epv = UINT8_MAX; - int8_t temperatureAir = 0; - int8_t climbRate = (int8_t)constrain(lrintf(getEstimatedActualVelocity(Z) / 10.0f), INT8_MIN, INT8_MAX); - int8_t battery = feature(FEATURE_VBAT) ? (int8_t)calculateBatteryPercentage() : -1; - -#if defined(USE_GPS) - if (sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - ) { - latitude = gpsSol.llh.lat; - longitude = gpsSol.llh.lon; - altitude = (int16_t)constrain((int32_t)(gpsSol.llh.alt / 100), INT16_MIN, INT16_MAX); - - const int32_t desiredAltCm = lrintf(posControl.desiredState.pos.z); - const int32_t targetAltCm = GPS_home.alt + desiredAltCm; - targetAltitude = (int16_t)constrain(targetAltCm / 100, INT16_MIN, INT16_MAX); - - groundspeed = (uint8_t)constrain(lrintf(gpsSol.groundSpeed / 20.0f), 0, UINT8_MAX); - - if (gpsSol.flags.validEPE) { - eph = (uint8_t)constrain((gpsSol.eph + 5) / 10, 0, UINT8_MAX); - epv = (uint8_t)constrain((gpsSol.epv + 5) / 10, 0, UINT8_MAX); - } - - if (posControl.activeWaypointIndex >= 0) { - wpNum = (uint16_t)posControl.activeWaypointIndex; - targetDistance = (uint16_t)constrain(lrintf(posControl.wpDistance / 1000.0f), 0, UINT16_MAX); - } - } -#endif - -#if defined(USE_PITOT) - if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - airspeed = (uint8_t)constrain(lrintf(getAirspeedEstimate() / 20.0f), 0, UINT8_MAX); - airspeedSp = airspeed; - } -#endif - - if (airspeedSp == 0) { - const float desiredVelXY = calc_length_pythagorean_2D(posControl.desiredState.vel.x, posControl.desiredState.vel.y); - airspeedSp = (uint8_t)constrain(lrintf(desiredVelXY / 20.0f), 0, UINT8_MAX); - } - - if (airspeed == 0) { - airspeed = groundspeed; - } - -#ifdef USE_WIND_ESTIMATOR - if (isEstimatedWindSpeedValid()) { - uint16_t windAngleCd = 0; - const float windHoriz = getEstimatedHorizontalWindSpeed(&windAngleCd); - windspeed = (uint8_t)constrain(lrintf(windHoriz / 20.0f), 0, UINT8_MAX); - windHeading = mavlinkHeadingDeg2FromCd(windAngleCd); - } -#endif - - int16_t temperature; - sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); - temperatureAir = (int8_t)constrain(temperature, INT8_MIN, INT8_MAX); - - const uint16_t failureFlags = mavlinkComputeHighLatencyFailures(); - - mavlink_msg_high_latency2_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - (uint32_t)(currentTimeUs / 1000), - mavlinkGetVehicleType(), - mavlinkGetAutopilotEnum(), - modeSelection.customMode, - latitude, - longitude, - altitude, - targetAltitude, - heading, - targetHeading, - targetDistance, - (uint8_t)constrain(getThrottlePercent(osdUsingScaledThrottle()), 0, 100), - airspeed, - airspeedSp, - groundspeed, - windspeed, - windHeading, - eph, - epv, - temperatureAir, - climbRate, - battery, - wpNum, - failureFlags, - 0, - 0, - 0); - - mavlinkSendMessage(); -} - -void processMAVLinkTelemetry(timeUs_t currentTimeUs) -{ - if (mavActivePort->highLatencyEnabled && mavlinkGetProtocolVersion() != 1) { - if ((currentTimeUs - mavActivePort->lastHighLatencyMessageUs) >= TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US) { - mavlinkSendHighLatency2(currentTimeUs); - mavActivePort->lastHighLatencyMessageUs = currentTimeUs; - } - return; - } - - // is executed @ TELEMETRY_MAVLINK_MAXRATE rate - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SYS_STATUS, currentTimeUs)) { - mavlinkSendSystemStatus(); - } - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS, currentTimeUs)) { - mavlinkSendRCChannelsAndRSSI(); - } - -#ifdef USE_GPS - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT, currentTimeUs)) { - mavlinkSendGpsRawInt(currentTimeUs); - } - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT, currentTimeUs)) { - mavlinkSendGlobalPositionInt(currentTimeUs); - } - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN, currentTimeUs)) { - mavlinkSendGpsGlobalOrigin(); - } -#endif - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_ATTITUDE, currentTimeUs)) { - mavlinkSendAttitude(); - } - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_VFR_HUD, currentTimeUs)) { - mavlinkSendVfrHud(); - } - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_HEARTBEAT, currentTimeUs)) { - mavlinkSendHeartbeat(); - } - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE, currentTimeUs)) { - mavlinkSendExtendedSysState(); - } - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS, currentTimeUs)) { - mavlinkSendBatteryStatus(); - } - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE, currentTimeUs)) { - mavlinkSendScaledPressure(); - } - - if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME, currentTimeUs)) { - mavlinkSendSystemTime(); - } - - if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3, currentTimeUs)) { - mavlinkSendStatusText(); - } - -} - -static void mavlinkResetIncomingMissionTransaction(void); - -static bool handleIncoming_MISSION_CLEAR_ALL(void) -{ - mavlink_mission_clear_all_t msg; - mavlink_msg_mission_clear_all_decode(&mavRecvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - resetWaypointList(); - mavlinkResetIncomingMissionTransaction(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return false; -} - -// Static state for MISSION UPLOAD transaction (starting with MISSION_COUNT) -#define MAVLINK_MISSION_UPLOAD_TIMEOUT_MS 10000 -static int incomingMissionWpCount = 0; -static int incomingMissionWpSequence = 0; -static uint8_t incomingMissionSourceSystem = 0; -static uint8_t incomingMissionSourceComponent = 0; -static timeMs_t incomingMissionLastActivityMs = 0; - -static void mavlinkResetIncomingMissionTransaction(void) -{ - incomingMissionWpCount = 0; - incomingMissionWpSequence = 0; - incomingMissionSourceSystem = 0; - incomingMissionSourceComponent = 0; - incomingMissionLastActivityMs = 0; -} - -static void mavlinkStartIncomingMissionTransaction(int missionCount) -{ - incomingMissionWpCount = missionCount; - incomingMissionWpSequence = 0; - incomingMissionSourceSystem = mavRecvMsg.sysid; - incomingMissionSourceComponent = mavRecvMsg.compid; - incomingMissionLastActivityMs = millis(); -} - -static void mavlinkTouchIncomingMissionTransaction(void) -{ - incomingMissionLastActivityMs = millis(); -} - -static bool mavlinkIsIncomingMissionTransactionActive(void) -{ - if (incomingMissionWpCount <= 0) { - return false; - } - - if ((timeMs_t)(millis() - incomingMissionLastActivityMs) > MAVLINK_MISSION_UPLOAD_TIMEOUT_MS) { - mavlinkResetIncomingMissionTransaction(); - return false; - } - - return true; -} - -static bool mavlinkIsIncomingMissionTransactionOwner(void) -{ - return mavRecvMsg.sysid == incomingMissionSourceSystem && - mavRecvMsg.compid == incomingMissionSourceComponent; -} - -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 (!mavlinkIsIncomingMissionTransactionActive() || !mavlinkIsIncomingMissionTransactionOwner()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - mavlinkTouchIncomingMissionTransaction(); - - 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; - } - - 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.p1 = 0; - wp.p2 = 0; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - break; - - case MAV_CMD_NAV_LOITER_TIME: - if (!mavlinkFrameIsSupported(frame, - MAV_FRAME_SUPPORTED_GLOBAL | - MAV_FRAME_SUPPORTED_GLOBAL_INT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.action = NAV_WP_ACTION_HOLD_TIME; - wp.lat = lat; - wp.lon = lon; - wp.alt = (int32_t)(altMeters * 100.0f); - wp.p1 = (int16_t)lrintf(param1); - wp.p2 = 0; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - { - const bool coordinateFrame = mavlinkFrameIsSupported(frame, - MAV_FRAME_SUPPORTED_GLOBAL | - MAV_FRAME_SUPPORTED_GLOBAL_INT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT); - - if (!coordinateFrame && frame != MAV_FRAME_MISSION) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.action = NAV_WP_ACTION_RTH; - wp.lat = 0; - wp.lon = 0; - wp.alt = coordinateFrame ? (int32_t)(altMeters * 100.0f) : 0; - wp.p1 = 0; // Land if non-zero - wp.p2 = 0; - wp.p3 = 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.p1 = 0; // Speed (cm/s) - wp.p2 = 0; // Elevation Adjustment (m): P2 defines the ground elevation (in metres) for the LAND WP. If the altitude mode is absolute, this is also absolute; for relative altitude, then it is the difference between the assumed home location and the LAND WP. - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; // Altitude Mode & Actions, P3 defines the altitude mode. 0 (default, legacy) = Relative to Home, 1 = Absolute (AMSL). - break; - - case MAV_CMD_DO_JUMP: - if (frame != MAV_FRAME_MISSION) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - if (param1 < 0.0f) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.lat = 0; - wp.lon = 0; - wp.alt = 0; - wp.action = NAV_WP_ACTION_JUMP; - wp.p1 = (int16_t)lrintf(param1 + 1.0f); - wp.p2 = (int16_t)lrintf(param2); - wp.p3 = 0; - break; - - case MAV_CMD_DO_SET_ROI: - if (param1 != MAV_ROI_LOCATION || - !mavlinkFrameIsSupported(frame, - MAV_FRAME_SUPPORTED_GLOBAL | - MAV_FRAME_SUPPORTED_GLOBAL_INT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.action = NAV_WP_ACTION_SET_POI; - wp.lat = lat; - wp.lon = lon; - wp.alt = (int32_t)(altMeters * 100.0f); - wp.p1 = 0; - wp.p2 = 0; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - break; - - case MAV_CMD_CONDITION_YAW: - if (frame != MAV_FRAME_MISSION) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - if (param4 != 0.0f) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.lat = 0; - wp.lon = 0; - wp.alt = 0; - wp.action = NAV_WP_ACTION_SET_HEAD; - wp.p1 = (int16_t)lrintf(param1); - wp.p2 = 0; - wp.p3 = 0; - break; - - 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++; - - 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(); - } - mavlinkResetIncomingMissionTransaction(); - } - 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 { - // If we get a duplicate of the last accepted item, re-request the next one instead of aborting. - if (seq + 1 == incomingMissionWpSequence) { - mavlinkTouchIncomingMissionTransaction(); - 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; - mavlink_msg_mission_count_decode(&mavRecvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - mavlinkResetIncomingMissionTransaction(); - 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 == 0) { - resetWaypointList(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - if (msg.count <= NAV_MAX_WAYPOINTS) { - mavlinkStartIncomingMissionTransaction(msg.count); - mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); - mavlinkSendMessage(); - return true; - } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - } - - return false; -} - -static bool handleIncoming_MISSION_ITEM(void) -{ - mavlink_mission_item_t msg; - mavlink_msg_mission_item_decode(&mavRecvMsg, &msg); - - if (msg.target_system != mavSystemId) { - return false; - } - - if (ARMING_FLAG(ARMED)) { - if (msg.command == MAV_CMD_NAV_WAYPOINT) { - return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, - MAV_FRAME_SUPPORTED_GLOBAL | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT, - (int32_t)lrintf(msg.x * 1e7f), (int32_t)lrintf(msg.y * 1e7f), msg.z); - } - - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, (int32_t)(msg.x * 1e7f), (int32_t)(msg.y * 1e7f), msg.z); -} - -static bool handleIncoming_MISSION_REQUEST_LIST(void) -{ - mavlink_mission_request_list_t msg; - mavlink_msg_mission_request_list_decode(&mavRecvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return false; -} - -static bool fillMavlinkMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item) -{ - mavlinkMissionItemData_t data = {0}; - - data.frame = navWaypointFrame(wp, useIntMessages); - - switch (wp->action) { - case NAV_WP_ACTION_WAYPOINT: - data.command = MAV_CMD_NAV_WAYPOINT; - data.lat = wp->lat; - data.lon = wp->lon; - data.alt = wp->alt / 100.0f; - break; - - case NAV_WP_ACTION_HOLD_TIME: - data.command = MAV_CMD_NAV_LOITER_TIME; - data.param1 = wp->p1; - data.lat = wp->lat; - data.lon = wp->lon; - data.alt = wp->alt / 100.0f; - break; - - case NAV_WP_ACTION_RTH: - data.command = MAV_CMD_NAV_RETURN_TO_LAUNCH; - break; - - case NAV_WP_ACTION_LAND: - data.command = MAV_CMD_NAV_LAND; - data.lat = wp->lat; - data.lon = wp->lon; - data.alt = wp->alt / 100.0f; - break; - - case NAV_WP_ACTION_JUMP: - data.command = MAV_CMD_DO_JUMP; - data.param1 = (wp->p1 > 0) ? (float)(wp->p1 - 1) : 0.0f; - data.param2 = wp->p2; - break; - - case NAV_WP_ACTION_SET_POI: - data.command = MAV_CMD_DO_SET_ROI; - data.param1 = MAV_ROI_LOCATION; - data.lat = wp->lat; - data.lon = wp->lon; - data.alt = wp->alt / 100.0f; - break; - - case NAV_WP_ACTION_SET_HEAD: - data.command = MAV_CMD_CONDITION_YAW; - data.param1 = wp->p1; - break; - - default: - return false; - } - - *item = data; - return true; -} - -static bool handleIncoming_MISSION_REQUEST(void) -{ - mavlink_mission_request_t msg; - mavlink_msg_mission_request_decode(&mavRecvMsg, &msg); - - if (msg.target_system != mavSystemId) { - return false; - } - - int wpCount = getWaypointCount(); - - if (msg.seq < wpCount) { - navWaypoint_t wp; - getWaypoint(msg.seq + 1, &wp); - - mavlinkMissionItemData_t item; - if (fillMavlinkMissionItemFromWaypoint(&wp, 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); - mavlinkSendMessage(); - } - - return true; -} - -static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) -{ - if (targetSystem != 0 && targetSystem != mavSystemId) { - return false; - } - - if (targetComponent != 0 && targetComponent != mavComponentId) { - return false; - } - - 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 targetComponent, 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 (!mavlinkIsLocalTarget(targetSystem, targetComponent)) { - 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 = {0}; - wp.action = NAV_WP_ACTION_WAYPOINT; - 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; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - wp.flag = 0; - - setWaypoint(255, &wp); - - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - } else { - mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); - } - return true; - case MAV_CMD_DO_CHANGE_ALTITUDE: - { - const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame((uint8_t)lrintf(param2), param1); - mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); - return true; - } - case MAV_CMD_CONDITION_YAW: - { - if (!(navGetCurrentStateFlags() & NAV_CTL_YAW)) { - mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); - return true; - } - - int32_t targetHeadingCd = wrap_36000((int32_t)lrintf(param1 * 100.0f)); - - if (param4 != 0.0f) { - const int32_t currentHeadingCd = STATE(AIRPLANE) ? posControl.actualState.cog : posControl.actualState.yaw; - const int32_t headingChangeCd = (int32_t)lrintf(fabsf(param1) * 100.0f); - - if (param3 < 0.0f) { - targetHeadingCd = wrap_36000(currentHeadingCd - headingChangeCd); - } else { - targetHeadingCd = wrap_36000(currentHeadingCd + headingChangeCd); - } - } - - updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(targetHeadingCd)); - posControl.desiredState.yaw = targetHeadingCd; - posControl.cruise.course = targetHeadingCd; - posControl.cruise.previousCourse = targetHeadingCd; - - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - return true; - } - case MAV_CMD_SET_MESSAGE_INTERVAL: - { - mavlinkPeriodicMessage_e periodicMessage; - MAV_RESULT result = MAV_RESULT_UNSUPPORTED; - - if (mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { - if (param2 == 0.0f) { - mavlinkSetMessageOverrideIntervalUs(periodicMessage, 0); - result = MAV_RESULT_ACCEPTED; - } else if (param2 < 0.0f) { - mavlinkSetMessageOverrideIntervalUs(periodicMessage, -1); - result = MAV_RESULT_ACCEPTED; - } else { - uint32_t intervalUs = (uint32_t)param2; - if (intervalUs > 0) { - const uint32_t minIntervalUs = 1000000UL / TELEMETRY_MAVLINK_MAXRATE; - if (intervalUs < minIntervalUs) { - intervalUs = minIntervalUs; - } - - mavlinkSetMessageOverrideIntervalUs(periodicMessage, (int32_t)intervalUs); - result = MAV_RESULT_ACCEPTED; - } - } - } - - mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); - return true; - } - case MAV_CMD_GET_MESSAGE_INTERVAL: - { - mavlinkPeriodicMessage_e periodicMessage; - if (!mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { - mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); - return true; - } - - mavlink_msg_message_interval_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - (uint16_t)param1, - mavlinkMessageIntervalUs(periodicMessage)); - mavlinkSendMessage(); - - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - return true; - } - case MAV_CMD_CONTROL_HIGH_LATENCY: - if (param1 == 0.0f || param1 == 1.0f) { - if (mavlinkGetProtocolVersion() == 1 && param1 > 0.5f) { - mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); - return true; - } - - mavActivePort->highLatencyEnabled = param1 > 0.5f; - if (mavActivePort->highLatencyEnabled) { - mavActivePort->lastHighLatencyMessageUs = 0; - } - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - } else { - mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); - } - return true; - case MAV_CMD_REQUEST_PROTOCOL_VERSION: - if (mavlinkGetProtocolVersion() == 1) { - mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); - } else { - mavlinkSendProtocolVersion(); - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - } - return true; - case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: - if (mavlinkGetProtocolVersion() == 1) { - mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); - } else { - mavlinkSendAutopilotVersion(); - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - } - return true; - case MAV_CMD_REQUEST_MESSAGE: - { - bool sent = false; - uint16_t messageId = (uint16_t)param1; - - switch (messageId) { - case MAVLINK_MSG_ID_HEARTBEAT: - mavlinkSendHeartbeat(); - sent = true; - break; - case MAVLINK_MSG_ID_AUTOPILOT_VERSION: - if (mavlinkGetProtocolVersion() != 1) { - mavlinkSendAutopilotVersion(); - sent = true; - } - break; - case MAVLINK_MSG_ID_PROTOCOL_VERSION: - if (mavlinkGetProtocolVersion() != 1) { - mavlinkSendProtocolVersion(); - sent = true; - } - break; - case MAVLINK_MSG_ID_SYS_STATUS: - mavlinkSendSystemStatus(); - sent = true; - break; - case MAVLINK_MSG_ID_ATTITUDE: - mavlinkSendAttitude(); - sent = true; - break; - case MAVLINK_MSG_ID_VFR_HUD: - mavlinkSendVfrHud(); - sent = true; - break; - case MAVLINK_MSG_ID_AVAILABLE_MODES: - { - const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); - if (isPlane) { - mavlinkSendAvailableModes(planeModes, planeModesCount, modeSelection.customMode, mavlinkPlaneModeIsConfigured); - } else { - mavlinkSendAvailableModes(copterModes, copterModesCount, modeSelection.customMode, mavlinkCopterModeIsConfigured); - } - sent = true; - } - break; - case MAVLINK_MSG_ID_CURRENT_MODE: - { - const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); - mavlink_msg_current_mode_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - MAV_STANDARD_MODE_NON_STANDARD, - modeSelection.customMode, - modeSelection.customMode); - mavlinkSendMessage(); - sent = true; - } - break; - case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: - mavlinkSendExtendedSysState(); - sent = true; - break; - case MAVLINK_MSG_ID_RC_CHANNELS: - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: - case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: - mavlinkSendRCChannelsAndRSSI(); - sent = true; - break; - case MAVLINK_MSG_ID_GPS_RAW_INT: - #ifdef USE_GPS - mavlinkSendGpsRawInt(micros()); - sent = true; - #endif - break; - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: - #ifdef USE_GPS - mavlinkSendGlobalPositionInt(micros()); - sent = true; - #endif - break; - case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: - #ifdef USE_GPS - mavlinkSendGpsGlobalOrigin(); - sent = true; - #endif - break; - case MAVLINK_MSG_ID_BATTERY_STATUS: - mavlinkSendBatteryStatus(); - sent = true; - break; - case MAVLINK_MSG_ID_SCALED_PRESSURE: - mavlinkSendScaledPressure(); - sent = true; - break; - case MAVLINK_MSG_ID_SYSTEM_TIME: - mavlinkSendSystemTime(); - sent = true; - break; - case MAVLINK_MSG_ID_STATUSTEXT: - sent = mavlinkSendStatusText(); - 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, msg.target_component, mavRecvMsg.sysid, mavRecvMsg.compid, msg.command, msg.frame, msg.param1, msg.param2, msg.param3, msg.param4, (float)msg.x / 1e7f, (float)msg.y / 1e7f, msg.z); -} - -static bool handleIncoming_COMMAND_LONG(void) -{ - mavlink_command_long_t msg; - mavlink_msg_command_long_decode(&mavRecvMsg, &msg); - - // COMMAND_LONG has no frame field; location commands are WGS84 global by definition. - return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavRecvMsg.sysid, mavRecvMsg.compid, msg.command, MAV_FRAME_GLOBAL, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7); -} - -static bool handleIncoming_MISSION_ITEM_INT(void) -{ - mavlink_mission_item_int_t msg; - mavlink_msg_mission_item_int_decode(&mavRecvMsg, &msg); - - if (msg.target_system != mavSystemId) { - return false; - } - - if (ARMING_FLAG(ARMED)) { - if (msg.command == MAV_CMD_NAV_WAYPOINT) { - return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, - MAV_FRAME_SUPPORTED_GLOBAL_INT | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT, - msg.x, msg.y, msg.z); - } - - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return mavlinkHandleMissionItemCommon(true, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, msg.x, msg.y, msg.z); -} - -static bool handleIncoming_MISSION_REQUEST_INT(void) -{ - mavlink_mission_request_int_t msg; - mavlink_msg_mission_request_int_decode(&mavRecvMsg, &msg); - - if (msg.target_system != mavSystemId) { - return false; - } - - int wpCount = getWaypointCount(); - - if (msg.seq < wpCount) { - navWaypoint_t wp; - getWaypoint(msg.seq + 1, &wp); - - mavlinkMissionItemData_t item; - if (fillMavlinkMissionItemFromWaypoint(&wp, true, &item)) { - mavlink_msg_mission_item_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, - msg.seq, - item.frame, - item.command, - 0, - 1, - item.param1, item.param2, item.param3, item.param4, - item.lat, - item.lon, - item.alt, - MAV_MISSION_TYPE_MISSION); - mavlinkSendMessage(); - } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - } - else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - - return true; -} - -static bool handleIncoming_REQUEST_DATA_STREAM(void) -{ - mavlink_request_data_stream_t msg; - mavlink_msg_request_data_stream_decode(&mavRecvMsg, &msg); - - if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { - return false; - } - - uint8_t rate = 0; - if (msg.start_stop != 0) { - rate = (uint8_t)msg.req_message_rate; - if (rate > TELEMETRY_MAVLINK_MAXRATE) { - rate = TELEMETRY_MAVLINK_MAXRATE; - } - } - - if (msg.req_stream_id == MAV_DATA_STREAM_ALL) { - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_STATUS, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_RC_CHANNELS, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_POSITION, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA1, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA2, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA3, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_SYS_STATE, rate); - return true; - } - - mavlinkSetStreamRate(msg.req_stream_id, rate); - return true; -} - -static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void) -{ - mavlink_set_position_target_global_int_t msg; - mavlink_msg_set_position_target_global_int_decode(&mavRecvMsg, &msg); - - if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { - return false; - } - - uint8_t frame = msg.coordinate_frame; - if (!mavlinkFrameIsSupported(frame, - MAV_FRAME_SUPPORTED_GLOBAL | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | - MAV_FRAME_SUPPORTED_GLOBAL_INT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { - return true; - } - - const uint16_t typeMask = msg.type_mask; - const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0; - const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0; - const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0; - - // Altitude-only SET_POSITION_TARGET_GLOBAL_INT mirrors MAV_CMD_DO_CHANGE_ALTITUDE semantics. - if (xIgnored && yIgnored && !zIgnored) { - if (isGCSValid()) { - mavlinkSetAltitudeTargetFromFrame(frame, msg.alt); - } - return true; - } - - if (xIgnored || yIgnored) { - return true; - } - - if (isGCSValid()) { - navWaypoint_t wp = {0}; - wp.action = NAV_WP_ACTION_WAYPOINT; - wp.lat = msg.lat_int; - wp.lon = msg.lon_int; - wp.alt = zIgnored ? 0 : (int32_t)(msg.alt * 100.0f); - wp.p1 = 0; - wp.p2 = 0; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - wp.flag = 0; - - setWaypoint(255, &wp); - } - - return true; -} - -static bool handleIncoming_SET_POSITION_TARGET_LOCAL_NED(void) -{ - mavlink_set_position_target_local_ned_t msg; - mavlink_msg_set_position_target_local_ned_decode(&mavRecvMsg, &msg); - - if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { - 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); - mavlinkRxHandleMessage(&msg); - return true; -} - -static bool handleIncoming_PARAM_REQUEST_LIST(void) { - mavlink_param_request_list_t msg; - mavlink_msg_param_request_list_decode(&mavRecvMsg, &msg); - - // Respond that we don't have any parameters to force Mission Planner to give up quickly - if (mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { - // mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); - mavlink_msg_param_value_pack(mavSystemId, mavComponentId, &mavSendMsg, 0, 0, 0, 0, 0); - mavlinkSendMessage(); - } - return true; -} - -static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { - switch(mavActiveConfig->radio_type) { - case MAVLINK_RADIO_NONE: - break; - case MAVLINK_RADIO_SIK: - // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html - rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; - rxLinkStatistics.uplinkSNR = msg->noise / 1.9; - rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; - break; - case MAVLINK_RADIO_ELRS: - rxLinkStatistics.uplinkRSSI = -msg->remrssi; - rxLinkStatistics.uplinkSNR = msg->noise; - rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); - break; - case MAVLINK_RADIO_GENERIC: - default: - rxLinkStatistics.uplinkRSSI = msg->rssi; - rxLinkStatistics.uplinkSNR = msg->noise; - rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; - break; - } -} - -static bool handleIncoming_RADIO_STATUS(void) { - mavlink_radio_status_t msg; - mavlink_msg_radio_status_decode(&mavRecvMsg, &msg); - if (msg.txbuf > 0) { - mavActivePort->txbuffValid = true; - mavActivePort->txbuffFree = msg.txbuf; - } else { - mavActivePort->txbuffValid = false; - mavActivePort->txbuffFree = 100; - } - - if (rxConfig()->receiverType == RX_TYPE_SERIAL && - rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { - mavlinkParseRxStats(&msg); - } - - return true; -} - -static bool handleIncoming_HEARTBEAT(void) { - mavlink_heartbeat_t msg; - mavlink_msg_heartbeat_decode(&mavRecvMsg, &msg); - - switch (msg.type) { -#ifdef USE_ADSB - case MAV_TYPE_ADSB: - return adsbHeartbeat(); -#endif - default: - break; - } - - return false; -} - -#ifdef USE_ADSB -static bool handleIncoming_ADSB_VEHICLE(void) { - mavlink_adsb_vehicle_t msg; - mavlink_msg_adsb_vehicle_decode(&mavRecvMsg, &msg); - - adsbVehicleValues_t* vehicle = getVehicleForFill(); - if(vehicle != NULL){ - vehicle->icao = msg.ICAO_address; - vehicle->gps.lat = msg.lat; - vehicle->gps.lon = msg.lon; - vehicle->alt = (int32_t)(msg.altitude / 10); - vehicle->horVelocity = msg.hor_velocity; - vehicle->heading = msg.heading; - vehicle->flags = msg.flags; - vehicle->altitudeType = msg.altitude_type; - memcpy(&(vehicle->callsign), msg.callsign, sizeof(vehicle->callsign)); - vehicle->emitterType = msg.emitter_type; - vehicle->tslc = msg.tslc; - - adsbNewVehicle(vehicle); - } - - return true; -} -#endif - -static bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid) -{ - return sysid == mavlinkGetCommonConfig()->sysid && compid == MAV_COMP_ID_AUTOPILOT1; -} - -static void mavlinkLearnRoute(uint8_t ingressPortIndex) -{ - if (mavRecvMsg.sysid == 0 || mavlinkIsFromLocalIdentity(mavRecvMsg.sysid, mavRecvMsg.compid)) { - return; - } - - for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { - mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; - if (route->sysid == mavRecvMsg.sysid && route->compid == mavRecvMsg.compid) { - route->ingressPortIndex = ingressPortIndex; - return; - } - } - - if (mavRouteCount >= MAVLINK_MAX_ROUTES) { - return; - } - - mavRouteTable[mavRouteCount].sysid = mavRecvMsg.sysid; - mavRouteTable[mavRouteCount].compid = mavRecvMsg.compid; - mavRouteTable[mavRouteCount].ingressPortIndex = ingressPortIndex; - mavRouteCount++; -} - -static void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent) -{ - *targetSystem = -1; - *targetComponent = -1; - - const mavlink_msg_entry_t *msgEntry = mavlink_get_msg_entry(msg->msgid); - if (!msgEntry) { - return; - } - - if (msgEntry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { - *targetSystem = _MAV_RETURN_uint8_t(msg, msgEntry->target_system_ofs); - } - - if (msgEntry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { - *targetComponent = _MAV_RETURN_uint8_t(msg, msgEntry->target_component_ofs); - } -} - -static uint8_t mavlinkComputeForwardMask(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent) -{ - uint8_t mask = 0; - - if (targetSystem <= 0) { - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - if (portIndex == ingressPortIndex) { - continue; - } - - const mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; - if (!state->telemetryEnabled || !state->port) { - continue; - } - - mask |= MAVLINK_PORT_MASK(portIndex); - } - return mask; - } - - for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { - const mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; - - if (route->sysid != targetSystem) { - continue; - } - if (targetComponent > 0 && route->compid != targetComponent) { - continue; - } - if (route->ingressPortIndex == ingressPortIndex || route->ingressPortIndex >= mavPortCount) { - continue; - } - - const mavlinkPortRuntime_t *state = &mavPortStates[route->ingressPortIndex]; - if (!state->telemetryEnabled || !state->port) { - continue; - } - - mask |= MAVLINK_PORT_MASK(route->ingressPortIndex); - } - - return mask; -} - -static void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent) -{ - if (mavRecvMsg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { - return; - } - - uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; - const int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavRecvMsg); - if (msgLength <= 0) { - return; - } - - const uint8_t forwardMask = mavlinkComputeForwardMask(ingressPortIndex, targetSystem, targetComponent); - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - if ((forwardMask & MAVLINK_PORT_MASK(portIndex)) == 0) { - continue; - } - - mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; - if (serialTxBytesFree(state->port) < (uint32_t)msgLength) { - state->txDroppedFrames++; - continue; - } - - serialBeginWrite(state->port); - serialWriteBuf(state->port, mavBuffer, msgLength); - serialEndWrite(state->port); - } -} - -static int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t targetComponent, uint8_t ingressPortIndex) -{ - if (targetSystem <= 0) { - return ingressPortIndex; - } - - if ((uint8_t)targetSystem != mavlinkGetCommonConfig()->sysid) { - return -1; - } - - if (targetComponent > 0 && (uint8_t)targetComponent != MAV_COMP_ID_AUTOPILOT1) { - return -1; - } - - if (ingressPortIndex < mavPortCount) { - return ingressPortIndex; - } - - return mavPortCount > 0 ? 0 : -1; -} - -static bool mavlinkShouldFanOutLocalBroadcast(const mavlink_message_t *msg) -{ - if (msg->msgid == MAVLINK_MSG_ID_REQUEST_DATA_STREAM) { - return true; - } - - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { - mavlink_command_long_t cmd; - mavlink_msg_command_long_decode(msg, &cmd); - return cmd.command == MAV_CMD_SET_MESSAGE_INTERVAL || cmd.command == MAV_CMD_CONTROL_HIGH_LATENCY; - } - - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_INT) { - mavlink_command_int_t cmd; - mavlink_msg_command_int_decode(msg, &cmd); - return cmd.command == MAV_CMD_SET_MESSAGE_INTERVAL || cmd.command == MAV_CMD_CONTROL_HIGH_LATENCY; - } - - return false; -} - -// Returns whether a message was processed -static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) -{ - mavlinkPortRuntime_t *state = &mavPortStates[ingressPortIndex]; - - while (serialRxBytesWaiting(state->port) > 0) { - // Limit handling to one message per cycle - const char c = serialRead(state->port); - const uint8_t result = mavlink_parse_char(ingressPortIndex, c, &state->mavRecvMsg, &state->mavRecvStatus); - if (result == MAVLINK_FRAMING_OK) { - mavRecvMsg = state->mavRecvMsg; - - if (mavlinkIsFromLocalIdentity(mavRecvMsg.sysid, mavRecvMsg.compid)) { - return false; - } - - mavlinkLearnRoute(ingressPortIndex); - - int16_t targetSystem; - int16_t targetComponent; - mavlinkExtractTargets(&mavRecvMsg, &targetSystem, &targetComponent); - mavlinkForwardMessage(ingressPortIndex, targetSystem, targetComponent); - - if (mavRecvMsg.msgid == MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE) { - mavlinkSetActivePortContext(ingressPortIndex); - mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); - handleIncoming_RC_CHANNELS_OVERRIDE(); - return false; - } - - const int8_t localPortIndex = mavlinkResolveLocalPortForTarget(targetSystem, targetComponent, ingressPortIndex); - if (localPortIndex < 0) { - return false; - } - - uint8_t localPortMask = MAVLINK_PORT_MASK((uint8_t)localPortIndex); - const bool isLocalOrSystemBroadcast = targetSystem == 0 || ((targetSystem > 0) && ((uint8_t)targetSystem == mavlinkGetCommonConfig()->sysid)); - if (targetComponent == 0 && isLocalOrSystemBroadcast && mavlinkShouldFanOutLocalBroadcast(&mavRecvMsg)) { - localPortMask = 0; - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - localPortMask |= MAVLINK_PORT_MASK(portIndex); - } - } - - bool handled = false; - for (uint8_t localIndex = 0; localIndex < mavPortCount; localIndex++) { - if (!(localPortMask & MAVLINK_PORT_MASK(localIndex))) { - continue; - } - - mavlinkSetActivePortContext(localIndex); - mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); - bool localHandled = false; - - switch (mavRecvMsg.msgid) { - case MAVLINK_MSG_ID_HEARTBEAT: - localHandled = handleIncoming_HEARTBEAT(); - break; - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - localHandled = handleIncoming_PARAM_REQUEST_LIST(); - break; - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: - localHandled = handleIncoming_MISSION_CLEAR_ALL(); - break; - case MAVLINK_MSG_ID_MISSION_COUNT: - localHandled = handleIncoming_MISSION_COUNT(); - break; - case MAVLINK_MSG_ID_MISSION_ITEM: - localHandled = handleIncoming_MISSION_ITEM(); - break; - case MAVLINK_MSG_ID_MISSION_ITEM_INT: - localHandled = handleIncoming_MISSION_ITEM_INT(); - break; - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: - localHandled = handleIncoming_MISSION_REQUEST_LIST(); - break; - case MAVLINK_MSG_ID_COMMAND_LONG: - localHandled = handleIncoming_COMMAND_LONG(); - break; - case MAVLINK_MSG_ID_COMMAND_INT: //7 parameters: parameters 1-4, 7 are floats, and parameters 5,6 are scaled integers - localHandled = handleIncoming_COMMAND_INT(); - break; - case MAVLINK_MSG_ID_MISSION_REQUEST: - localHandled = handleIncoming_MISSION_REQUEST(); - break; - case MAVLINK_MSG_ID_MISSION_REQUEST_INT: - localHandled = handleIncoming_MISSION_REQUEST_INT(); - break; - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - localHandled = handleIncoming_REQUEST_DATA_STREAM(); - break; - 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 - localHandled = false; - break; - case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: - localHandled = handleIncoming_SET_POSITION_TARGET_LOCAL_NED(); - break; - case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: - localHandled = handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(); - break; -#ifdef USE_ADSB - case MAVLINK_MSG_ID_ADSB_VEHICLE: - localHandled = handleIncoming_ADSB_VEHICLE(); - break; -#endif - case MAVLINK_MSG_ID_RADIO_STATUS: - handleIncoming_RADIO_STATUS(); - // Don't set that we handled a message, otherwise radio status packets will block telemetry messages. - localHandled = false; - break; - case MAVLINK_MSG_ID_TUNNEL: - localHandled = handleIncoming_TUNNEL(ingressPortIndex); - break; - default: - localHandled = false; - break; - } - - handled = handled || localHandled; - } - - return handled; - } - } - - return false; -} - -static bool isMAVLinkTelemetryHalfDuplex(void) { - return telemetryConfig()->halfDuplex || - (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK && tristateWithDefaultOffIsActive(rxConfig()->halfDuplex)); -} - -void handleMAVLinkTelemetry(timeUs_t currentTimeUs) -{ - for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { - mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; - if (!state->telemetryEnabled || !state->port) { - continue; - } - - mavlinkSetActivePortContext(portIndex); - - // Process incoming MAVLink on this port and forward when needed. - const bool receivedMessage = processMAVLinkIncomingTelemetry(portIndex); - - // Restore context back to this port before periodic send decisions. - mavlinkSetActivePortContext(portIndex); - bool shouldSendTelemetry = false; - - if (state->txbuffValid) { - // Use flow control if available. - shouldSendTelemetry = state->txbuffFree >= mavActiveConfig->min_txbuff; - } else { - // If not, use blind frame pacing and half-duplex backoff after RX activity. - const bool halfDuplexBackoff = isMAVLinkTelemetryHalfDuplex() && receivedMessage; - shouldSendTelemetry = ((currentTimeUs - state->lastMavlinkMessageUs) >= TELEMETRY_MAVLINK_DELAY) && !halfDuplexBackoff; - } - - if (!shouldSendTelemetry) { - continue; - } - - mavSendMask = MAVLINK_PORT_MASK(portIndex); - processMAVLinkTelemetry(currentTimeUs); - state->lastMavlinkMessageUs = currentTimeUs; - } - - mavSendMask = 0; + mavlinkRuntimeFreePorts(); } #endif diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 7debcc97758..04527fd7046 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -44,7 +44,8 @@ set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial 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/crc.c" "common/maths.c" "common/streambuf.c" "msp/msp_serial.c") + "fc/fc_mavlink.c" "mavlink/mavlink_ports.c" "mavlink/mavlink_routing.c" "mavlink/mavlink_runtime.c" + "mavlink/mavlink_streams.c" "telemetry/mavlink.c" "common/crc.c" "common/maths.c" "common/streambuf.c" "msp/msp_serial.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") diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index 7b579dcfa9f..749f10afcfd 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -295,6 +295,8 @@ static void initMavlinkTestState(void) lastAltitudeTargetCm = 0; testFlightMode = FLM_MANUAL; testSensorsMask = 0; + armingFlags = 0; + stateFlags = 0; memset(&gpsSol, 0, sizeof(gpsSol)); memset(&GPS_home, 0, sizeof(GPS_home)); memset(waypointStore, 0, sizeof(waypointStore)); From ed1628324beaa967945a8742fa758289c7348e66 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Fri, 3 Apr 2026 17:51:08 -0400 Subject: [PATCH 35/42] Rename MAVLink shared types header --- src/main/CMakeLists.txt | 1 + src/main/fc/fc_mavlink.h | 2 +- src/main/mavlink/mavlink_internal.h | 2 +- src/main/mavlink/mavlink_routing.h | 2 +- src/main/mavlink/mavlink_streams.h | 2 +- src/main/mavlink/mavlink_types.h | 189 ++++++++++++++++++++++++++++ src/main/telemetry/mavlink.h | 171 +------------------------ src/test/unit/mavlink_unittest.cc | 1 + 8 files changed, 197 insertions(+), 173 deletions(-) create mode 100644 src/main/mavlink/mavlink_types.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 26ca5b952c6..4fff0f3bf35 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -394,6 +394,7 @@ main_sources(COMMON_SRC io/osd/custom_elements.c mavlink/mavlink_internal.h + mavlink/mavlink_types.h mavlink/mavlink_ports.c mavlink/mavlink_ports.h mavlink/mavlink_routing.c diff --git a/src/main/fc/fc_mavlink.h b/src/main/fc/fc_mavlink.h index 29be4363157..c945ba1ba14 100644 --- a/src/main/fc/fc_mavlink.h +++ b/src/main/fc/fc_mavlink.h @@ -2,7 +2,7 @@ #include "common/time.h" -#include "telemetry/mavlink.h" +#include "mavlink/mavlink_types.h" typedef enum { MAVLINK_FC_DISPATCH_NOT_HANDLED = 0, diff --git a/src/main/mavlink/mavlink_internal.h b/src/main/mavlink/mavlink_internal.h index 4c365f364db..ecc9b54998f 100644 --- a/src/main/mavlink/mavlink_internal.h +++ b/src/main/mavlink/mavlink_internal.h @@ -67,7 +67,7 @@ #include "sensors/temperature.h" #include "sensors/esc_sensor.h" -#include "telemetry/mavlink.h" +#include "mavlink/mavlink_types.h" #include "telemetry/telemetry.h" #include "blackbox/blackbox_io.h" diff --git a/src/main/mavlink/mavlink_routing.h b/src/main/mavlink/mavlink_routing.h index 1aa472e0f8c..ae63f372416 100644 --- a/src/main/mavlink/mavlink_routing.h +++ b/src/main/mavlink/mavlink_routing.h @@ -3,7 +3,7 @@ #include #include -#include "telemetry/mavlink.h" +#include "mavlink/mavlink_types.h" bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid); void mavlinkLearnRoute(uint8_t ingressPortIndex); diff --git a/src/main/mavlink/mavlink_streams.h b/src/main/mavlink/mavlink_streams.h index 2084518a546..e3ffc467ef5 100644 --- a/src/main/mavlink/mavlink_streams.h +++ b/src/main/mavlink/mavlink_streams.h @@ -2,7 +2,7 @@ #include "common/time.h" -#include "telemetry/mavlink.h" +#include "mavlink/mavlink_types.h" extern const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT]; diff --git a/src/main/mavlink/mavlink_types.h b/src/main/mavlink/mavlink_types.h new file mode 100644 index 00000000000..09f21dc99ed --- /dev/null +++ b/src/main/mavlink/mavlink_types.h @@ -0,0 +1,189 @@ +/* + * 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 . + */ + +#pragma once + +#include "common/time.h" + +#include "io/serial.h" + +#include "target/common.h" + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#ifndef MAVLINK_COMM_NUM_BUFFERS +#define MAVLINK_COMM_NUM_BUFFERS MAX_MAVLINK_PORTS +#endif +#include "common/mavlink.h" +#pragma GCC diagnostic pop + +#define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX +#define TELEMETRY_MAVLINK_MAXRATE 50 +#define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) +#define TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US (5 * 1000 * 1000) +#define MAV_DATA_STREAM_EXTENDED_SYS_STATE (MAV_DATA_STREAM_EXTRA3 + 1) +#define MAV_DATA_STREAM_HEARTBEAT (MAV_DATA_STREAM_EXTENDED_SYS_STATE + 1) +#define MAVLINK_STREAM_COUNT (MAV_DATA_STREAM_HEARTBEAT + 1) +#define ARDUPILOT_VERSION_MAJOR 4 +#define ARDUPILOT_VERSION_MINOR 6 +#define ARDUPILOT_VERSION_PATCH 3 +#define MAVLINK_MAX_ROUTES 32 +#define MAVLINK_PORT_MASK(portIndex) (1U << (portIndex)) +#define MAXSTREAMS MAVLINK_STREAM_COUNT + +typedef enum { + MAVLINK_PERIODIC_MESSAGE_HEARTBEAT, + MAVLINK_PERIODIC_MESSAGE_SYS_STATUS, + MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE, + MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS, + MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT, + MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT, + MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN, + MAVLINK_PERIODIC_MESSAGE_ATTITUDE, + MAVLINK_PERIODIC_MESSAGE_VFR_HUD, + MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS, + MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE, + MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME, + MAVLINK_PERIODIC_MESSAGE_COUNT +} mavlinkPeriodicMessage_e; + +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. + * This converts angles from a range of 0..Pi to -Pi..Pi + */ +#define RADIANS_TO_MAVLINK_RANGE(angle) (((angle) > M_PIf) ? ((angle) - (2 * M_PIf)) : (angle)) + +/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ +typedef enum APM_PLANE_MODE +{ + PLANE_MODE_MANUAL=0, + PLANE_MODE_CIRCLE=1, + PLANE_MODE_STABILIZE=2, + PLANE_MODE_TRAINING=3, + PLANE_MODE_ACRO=4, + PLANE_MODE_FLY_BY_WIRE_A=5, + PLANE_MODE_FLY_BY_WIRE_B=6, + PLANE_MODE_CRUISE=7, + PLANE_MODE_AUTOTUNE=8, + PLANE_MODE_AUTO=10, + PLANE_MODE_RTL=11, + PLANE_MODE_LOITER=12, + PLANE_MODE_TAKEOFF=13, + PLANE_MODE_AVOID_ADSB=14, + PLANE_MODE_GUIDED=15, + PLANE_MODE_INITIALIZING=16, + PLANE_MODE_QSTABILIZE=17, + PLANE_MODE_QHOVER=18, + PLANE_MODE_QLOITER=19, + PLANE_MODE_QLAND=20, + PLANE_MODE_QRTL=21, + PLANE_MODE_QAUTOTUNE=22, + 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. */ +typedef enum APM_COPTER_MODE +{ + COPTER_MODE_STABILIZE=0, + COPTER_MODE_ACRO=1, + COPTER_MODE_ALT_HOLD=2, + COPTER_MODE_AUTO=3, + COPTER_MODE_GUIDED=4, + COPTER_MODE_LOITER=5, + COPTER_MODE_RTL=6, + COPTER_MODE_CIRCLE=7, + COPTER_MODE_LAND=9, + COPTER_MODE_DRIFT=11, + COPTER_MODE_SPORT=13, + COPTER_MODE_FLIP=14, + COPTER_MODE_AUTOTUNE=15, + COPTER_MODE_POSHOLD=16, + COPTER_MODE_BRAKE=17, + COPTER_MODE_THROW=18, + COPTER_MODE_AVOID_ADSB=19, + COPTER_MODE_GUIDED_NOGPS=20, + COPTER_MODE_SMART_RTL=21, + 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; + +typedef struct mavlinkRouteEntry_s { + uint8_t sysid; + uint8_t compid; + uint8_t ingressPortIndex; +} mavlinkRouteEntry_t; + +typedef struct mavlinkPortRuntime_s { + serialPort_t *port; + const serialPortConfig_t *portConfig; + portSharing_e portSharing; + bool telemetryEnabled; + bool txbuffValid; + uint8_t txbuffFree; + timeUs_t lastMavlinkMessageUs; + timeUs_t lastHighLatencyMessageUs; + bool highLatencyEnabled; + uint8_t mavRates[MAVLINK_STREAM_COUNT]; + uint8_t mavRatesConfigured[MAVLINK_STREAM_COUNT]; + timeUs_t mavStreamNextDue[MAVLINK_STREAM_COUNT]; + int32_t mavMessageOverrideIntervalsUs[MAVLINK_PERIODIC_MESSAGE_COUNT]; + timeUs_t mavMessageNextDue[MAVLINK_PERIODIC_MESSAGE_COUNT]; + uint8_t txSeq; + uint32_t txDroppedFrames; + mavlink_message_t mavRecvMsg; + mavlink_status_t mavRecvStatus; +} mavlinkPortRuntime_t; + +typedef struct mavlinkModeDescriptor_s { + uint8_t customMode; + const char *name; +} mavlinkModeDescriptor_t; + +extern const mavlinkModeDescriptor_t planeModes[]; +extern const uint8_t planeModesCount; +extern const mavlinkModeDescriptor_t copterModes[]; +extern const uint8_t copterModesCount; + +typedef struct mavlinkMissionItemData_s { + uint8_t frame; + uint16_t command; + float param1; + float param2; + float param3; + float param4; + int32_t lat; + int32_t lon; + float alt; +} mavlinkMissionItemData_t; diff --git a/src/main/telemetry/mavlink.h b/src/main/telemetry/mavlink.h index c732f6c3fff..29f9f2fb8cd 100755 --- a/src/main/telemetry/mavlink.h +++ b/src/main/telemetry/mavlink.h @@ -17,176 +17,9 @@ #pragma once -#include "common/time.h" - -#include "io/serial.h" - -#include "target/common.h" - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-function" -#ifndef MAVLINK_COMM_NUM_BUFFERS -#define MAVLINK_COMM_NUM_BUFFERS MAX_MAVLINK_PORTS -#endif -#include "common/mavlink.h" -#pragma GCC diagnostic pop - -#define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX -#define TELEMETRY_MAVLINK_MAXRATE 50 -#define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) -#define TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US (5 * 1000 * 1000) -#define MAV_DATA_STREAM_EXTENDED_SYS_STATE (MAV_DATA_STREAM_EXTRA3 + 1) -#define MAV_DATA_STREAM_HEARTBEAT (MAV_DATA_STREAM_EXTENDED_SYS_STATE + 1) -#define MAVLINK_STREAM_COUNT (MAV_DATA_STREAM_HEARTBEAT + 1) -#define ARDUPILOT_VERSION_MAJOR 4 -#define ARDUPILOT_VERSION_MINOR 6 -#define ARDUPILOT_VERSION_PATCH 3 -#define MAVLINK_MAX_ROUTES 32 -#define MAVLINK_PORT_MASK(portIndex) (1U << (portIndex)) -#define MAXSTREAMS MAVLINK_STREAM_COUNT - -typedef enum { - MAVLINK_PERIODIC_MESSAGE_HEARTBEAT, - MAVLINK_PERIODIC_MESSAGE_SYS_STATUS, - MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE, - MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS, - MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT, - MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT, - MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN, - MAVLINK_PERIODIC_MESSAGE_ATTITUDE, - MAVLINK_PERIODIC_MESSAGE_VFR_HUD, - MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS, - MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE, - MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME, - MAVLINK_PERIODIC_MESSAGE_COUNT -} mavlinkPeriodicMessage_e; - -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. - * This converts angles from a range of 0..Pi to -Pi..Pi - */ -#define RADIANS_TO_MAVLINK_RANGE(angle) (((angle) > M_PIf) ? ((angle) - (2 * M_PIf)) : (angle)) - -/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ -typedef enum APM_PLANE_MODE -{ - PLANE_MODE_MANUAL=0, - PLANE_MODE_CIRCLE=1, - PLANE_MODE_STABILIZE=2, - PLANE_MODE_TRAINING=3, - PLANE_MODE_ACRO=4, - PLANE_MODE_FLY_BY_WIRE_A=5, - PLANE_MODE_FLY_BY_WIRE_B=6, - PLANE_MODE_CRUISE=7, - PLANE_MODE_AUTOTUNE=8, - PLANE_MODE_AUTO=10, - PLANE_MODE_RTL=11, - PLANE_MODE_LOITER=12, - PLANE_MODE_TAKEOFF=13, - PLANE_MODE_AVOID_ADSB=14, - PLANE_MODE_GUIDED=15, - PLANE_MODE_INITIALIZING=16, - PLANE_MODE_QSTABILIZE=17, - PLANE_MODE_QHOVER=18, - PLANE_MODE_QLOITER=19, - PLANE_MODE_QLAND=20, - PLANE_MODE_QRTL=21, - PLANE_MODE_QAUTOTUNE=22, - 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. */ -typedef enum APM_COPTER_MODE -{ - COPTER_MODE_STABILIZE=0, - COPTER_MODE_ACRO=1, - COPTER_MODE_ALT_HOLD=2, - COPTER_MODE_AUTO=3, - COPTER_MODE_GUIDED=4, - COPTER_MODE_LOITER=5, - COPTER_MODE_RTL=6, - COPTER_MODE_CIRCLE=7, - COPTER_MODE_LAND=9, - COPTER_MODE_DRIFT=11, - COPTER_MODE_SPORT=13, - COPTER_MODE_FLIP=14, - COPTER_MODE_AUTOTUNE=15, - COPTER_MODE_POSHOLD=16, - COPTER_MODE_BRAKE=17, - COPTER_MODE_THROW=18, - COPTER_MODE_AVOID_ADSB=19, - COPTER_MODE_GUIDED_NOGPS=20, - COPTER_MODE_SMART_RTL=21, - 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; - -typedef struct mavlinkRouteEntry_s { - uint8_t sysid; - uint8_t compid; - uint8_t ingressPortIndex; -} mavlinkRouteEntry_t; +#include "mavlink/mavlink_types.h" -typedef struct mavlinkPortRuntime_s { - serialPort_t *port; - const serialPortConfig_t *portConfig; - portSharing_e portSharing; - bool telemetryEnabled; - bool txbuffValid; - uint8_t txbuffFree; - timeUs_t lastMavlinkMessageUs; - timeUs_t lastHighLatencyMessageUs; - bool highLatencyEnabled; - uint8_t mavRates[MAVLINK_STREAM_COUNT]; - uint8_t mavRatesConfigured[MAVLINK_STREAM_COUNT]; - timeUs_t mavStreamNextDue[MAVLINK_STREAM_COUNT]; - int32_t mavMessageOverrideIntervalsUs[MAVLINK_PERIODIC_MESSAGE_COUNT]; - timeUs_t mavMessageNextDue[MAVLINK_PERIODIC_MESSAGE_COUNT]; - uint8_t txSeq; - uint32_t txDroppedFrames; - mavlink_message_t mavRecvMsg; - mavlink_status_t mavRecvStatus; -} mavlinkPortRuntime_t; - -typedef struct mavlinkModeDescriptor_s { - uint8_t customMode; - const char *name; -} mavlinkModeDescriptor_t; - -extern const mavlinkModeDescriptor_t planeModes[]; -extern const uint8_t planeModesCount; -extern const mavlinkModeDescriptor_t copterModes[]; -extern const uint8_t copterModesCount; - -typedef struct mavlinkMissionItemData_s { - uint8_t frame; - uint16_t command; - float param1; - float param2; - float param3; - float param4; - int32_t lat; - int32_t lon; - float alt; -} mavlinkMissionItemData_t; +#include "common/time.h" void initMAVLinkTelemetry(void); void handleMAVLinkTelemetry(timeUs_t currentTimeUs); diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index 749f10afcfd..846ed7efd0f 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -76,6 +76,7 @@ extern "C" { #include "sensors/sensors.h" #include "sensors/temperature.h" + #include "mavlink/mavlink_types.h" #include "telemetry/mavlink.h" #include "telemetry/telemetry.h" From 3847f8d48d574a1fb179c099a3bd7e221e4d2143 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Fri, 3 Apr 2026 18:04:23 -0400 Subject: [PATCH 36/42] Move MAVLink sends into streams --- src/main/fc/fc_mavlink.c | 1269 +--------------------------- src/main/fc/fc_mavlink.h | 18 - src/main/mavlink/mavlink_streams.c | 1135 ++++++++++++++++++++++++- src/main/mavlink/mavlink_streams.h | 3 + src/main/mavlink/mavlink_types.h | 5 - 5 files changed, 1138 insertions(+), 1292 deletions(-) diff --git a/src/main/fc/fc_mavlink.c b/src/main/fc/fc_mavlink.c index bd7f31eed5e..503bc75ed43 100644 --- a/src/main/fc/fc_mavlink.c +++ b/src/main/fc/fc_mavlink.c @@ -7,120 +7,6 @@ #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) -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) - { - case FLM_ACRO: return COPTER_MODE_ACRO; - case FLM_ACRO_AIR: return COPTER_MODE_ACRO; - case FLM_ANGLE: return COPTER_MODE_STABILIZE; - case FLM_HORIZON: return COPTER_MODE_STABILIZE; - case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; - case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; - case FLM_POSITION_HOLD: - { - if (isGCSValid()) { - return COPTER_MODE_GUIDED; - } else { - return COPTER_MODE_POSHOLD; - } - } - case FLM_RTH: return COPTER_MODE_RTL; - case FLM_MISSION: return COPTER_MODE_AUTO; - case FLM_LAUNCH: return COPTER_MODE_THROW; - case FLM_FAILSAFE: - { - if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { - return COPTER_MODE_RTL; - } else if (failsafePhase() == FAILSAFE_LANDING) { - return COPTER_MODE_LAND; - } else { - return COPTER_MODE_RTL; - } - } - default: return COPTER_MODE_STABILIZE; - } -} - -static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) -{ - switch (flightMode) - { - case FLM_MANUAL: return PLANE_MODE_MANUAL; - case FLM_ACRO: return PLANE_MODE_ACRO; - case FLM_ACRO_AIR: return PLANE_MODE_ACRO; - case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; - case FLM_HORIZON: return PLANE_MODE_STABILIZE; - case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; - case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; - case FLM_POSITION_HOLD: - { - if (isGCSValid()) { - return PLANE_MODE_GUIDED; - } else { - return PLANE_MODE_LOITER; - } - } - case FLM_RTH: return PLANE_MODE_RTL; - case FLM_MISSION: return PLANE_MODE_AUTO; - case FLM_CRUISE: return PLANE_MODE_CRUISE; - case FLM_LAUNCH: return PLANE_MODE_TAKEOFF; - case FLM_FAILSAFE: //failsafePhase_e - { - if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { - return PLANE_MODE_RTL; - } - else if (failsafePhase() == FAILSAFE_LANDING) { - return PLANE_MODE_AUTOLAND; - } - else { - return PLANE_MODE_RTL; - } - } - 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 void mavlinkResetTunnelState(uint8_t ingressPortIndex) { resetMspPort(&mavTunnelMspPorts[ingressPortIndex], NULL); @@ -254,66 +140,6 @@ static bool handleIncoming_TUNNEL(uint8_t ingressPortIndex) return handled; } -static void mavlinkSendAutopilotVersion(void) -{ - if (mavlinkGetProtocolVersion() == 1) return; - - // Capabilities aligned with what we actually support. - uint64_t capabilities = 0; - capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; - capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; - capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT; - capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT; - capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; - capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT; - - 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 - flightSwVersion, // 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 (mavlinkGetProtocolVersion() == 1) return; - - uint8_t specHash[8] = {0}; - uint8_t libHash[8] = {0}; - const uint16_t protocolVersion = (uint16_t)mavlinkGetProtocolVersion() * 100; - - mavlink_msg_protocol_version_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - protocolVersion, - protocolVersion, - protocolVersion, - specHash, - libHash); - - mavlinkSendMessage(); -} - static bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask) { switch (frame) { @@ -445,983 +271,6 @@ static uint8_t navWaypointFrame(const navWaypoint_t *wp, bool useIntMessages) return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT; } -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" }, -}; -const uint8_t planeModesCount = (uint8_t)ARRAYLEN(planeModes); - -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" }, -}; -const uint8_t copterModesCount = (uint8_t)ARRAYLEN(copterModes); - -static bool mavlinkPlaneModeIsConfigured(uint8_t customMode) -{ - switch ((APM_PLANE_MODE)customMode) { - case PLANE_MODE_MANUAL: - return isModeActivationConditionPresent(BOXMANUAL); - case PLANE_MODE_ACRO: - return true; - case PLANE_MODE_STABILIZE: - return isModeActivationConditionPresent(BOXHORIZON) || - isModeActivationConditionPresent(BOXANGLEHOLD); - case PLANE_MODE_FLY_BY_WIRE_A: - return isModeActivationConditionPresent(BOXANGLE); - case PLANE_MODE_FLY_BY_WIRE_B: - return isModeActivationConditionPresent(BOXNAVALTHOLD); - case PLANE_MODE_CRUISE: - return isModeActivationConditionPresent(BOXNAVCRUISE) || - (isModeActivationConditionPresent(BOXNAVCOURSEHOLD) && - isModeActivationConditionPresent(BOXNAVALTHOLD)); - case PLANE_MODE_AUTO: - return isModeActivationConditionPresent(BOXNAVWP); - case PLANE_MODE_RTL: - return isModeActivationConditionPresent(BOXNAVRTH); - case PLANE_MODE_GUIDED: - return isModeActivationConditionPresent(BOXNAVPOSHOLD) && - isModeActivationConditionPresent(BOXGCSNAV); - case PLANE_MODE_LOITER: - return isModeActivationConditionPresent(BOXNAVPOSHOLD); - case PLANE_MODE_TAKEOFF: - return isModeActivationConditionPresent(BOXNAVLAUNCH); - default: - return false; - } -} - -static bool mavlinkCopterModeIsConfigured(uint8_t customMode) -{ - switch ((APM_COPTER_MODE)customMode) { - case COPTER_MODE_ACRO: - return true; - case COPTER_MODE_STABILIZE: - return isModeActivationConditionPresent(BOXANGLE) || - isModeActivationConditionPresent(BOXHORIZON) || - isModeActivationConditionPresent(BOXANGLEHOLD); - case COPTER_MODE_ALT_HOLD: - return isModeActivationConditionPresent(BOXNAVALTHOLD); - case COPTER_MODE_GUIDED: - return isModeActivationConditionPresent(BOXNAVPOSHOLD) && - isModeActivationConditionPresent(BOXGCSNAV); - case COPTER_MODE_POSHOLD: - return isModeActivationConditionPresent(BOXNAVPOSHOLD); - case COPTER_MODE_RTL: - return isModeActivationConditionPresent(BOXNAVRTH); - case COPTER_MODE_AUTO: - return isModeActivationConditionPresent(BOXNAVWP); - case COPTER_MODE_THROW: - return isModeActivationConditionPresent(BOXNAVLAUNCH); - case COPTER_MODE_BRAKE: - return isModeActivationConditionPresent(BOXBRAKING); - default: - return false; - } -} - -static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom, - bool (*isModeConfigured)(uint8_t customMode)) -{ - uint8_t availableCount = 0; - for (uint8_t i = 0; i < count; i++) { - if (isModeConfigured(modes[i].customMode)) { - availableCount++; - } - } - - if (availableCount == 0) { - return; - } - - uint8_t modeIndex = 0; - for (uint8_t i = 0; i < count; i++) { - if (!isModeConfigured(modes[i].customMode)) { - continue; - } - - modeIndex++; - const uint8_t stdMode = MAV_STANDARD_MODE_NON_STANDARD; - const uint32_t properties = 0; - - mavlink_msg_available_modes_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - availableCount, - modeIndex, - stdMode, - modes[i].customMode, - properties, - modes[i].name); - - mavlinkSendMessage(); - - if (modes[i].customMode == currentCustom) { - mavlink_msg_current_mode_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - stdMode, - currentCustom, - currentCustom); - mavlinkSendMessage(); - } - } -} - -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 - uint32_t onboard_control_sensors_present = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER); - // GYRO and RC are assumed as minimum requirements - uint32_t onboard_control_sensors_enabled = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER); - uint32_t onboard_control_sensors_health = 0; - - if (getHwGyroStatus() == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO; - // Missing presence will report as sensor unhealthy - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO; - } - - hardwareSensorStatus_e accStatus = getHwAccelerometerStatus(); - if (accStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } else if (accStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } else if (accStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } - - hardwareSensorStatus_e compassStatus = getHwCompassStatus(); - if (compassStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } else if (compassStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } else if (compassStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } - - hardwareSensorStatus_e baroStatus = getHwBarometerStatus(); - if (baroStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } else if (baroStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } else if (baroStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } - - hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus(); - if (pitotStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } else if (pitotStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } else if (pitotStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } - - hardwareSensorStatus_e gpsStatus = getHwGPSStatus(); - if (gpsStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; - } else if (gpsStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; - } else if (gpsStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; - } - - hardwareSensorStatus_e opFlowStatus = getHwOpticalFlowStatus(); - if (opFlowStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } else if (opFlowStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } else if (opFlowStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } - - hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus(); - if (rangefinderStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } else if (rangefinderStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } else if (rangefinderStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - - if (rxIsReceivingSignal() && rxAreFlightChannelsValid()) { - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } - -#ifdef USE_BLACKBOX - // BLACKBOX is assumed enabled and present for boards with capability - onboard_control_sensors_present |= MAV_SYS_STATUS_LOGGING; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; - // Unhealthy only for cases with not enough space to record - if (!isBlackboxDeviceFull()) { - onboard_control_sensors_health |= MAV_SYS_STATUS_LOGGING; - } -#endif - - mavlink_msg_sys_status_pack(mavSystemId, mavComponentId, &mavSendMsg, - // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. - //Value of 0: not present. Value of 1: present. Indices according MAV_SYS_STATUS_SENSOR - onboard_control_sensors_present, - // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled - onboard_control_sensors_enabled, - // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error. - onboard_control_sensors_health, - // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - constrain(averageSystemLoadPercent*10, 0, 1000), - // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) - feature(FEATURE_VBAT) ? getBatteryVoltage() * 10 : 0, - // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - isAmperageConfigured() ? getAmperage() : -1, - // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery - feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100, - // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - 0, - // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - 0, - // errors_count1 Autopilot-specific errors - 0, - // errors_count2 Autopilot-specific errors - 0, - // errors_count3 Autopilot-specific errors - 0, - // errors_count4 Autopilot-specific errors - 0, 0, 0, 0); - - mavlinkSendMessage(); -} - -void mavlinkSendRCChannelsAndRSSI(void) -{ -#define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) - if (mavlinkGetProtocolVersion() == 1) { - mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - millis(), - // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - 0, - // chan1_raw RC channel 1 value, in microseconds - GET_CHANNEL_VALUE(0), - // chan2_raw RC channel 2 value, in microseconds - GET_CHANNEL_VALUE(1), - // chan3_raw RC channel 3 value, in microseconds - GET_CHANNEL_VALUE(2), - // chan4_raw RC channel 4 value, in microseconds - GET_CHANNEL_VALUE(3), - // chan5_raw RC channel 5 value, in microseconds - GET_CHANNEL_VALUE(4), - // chan6_raw RC channel 6 value, in microseconds - GET_CHANNEL_VALUE(5), - // chan7_raw RC channel 7 value, in microseconds - GET_CHANNEL_VALUE(6), - // chan8_raw RC channel 8 value, in microseconds - GET_CHANNEL_VALUE(7), - // rssi Receive signal strength indicator, 0: 0%, 254: 100% - //https://github.com/mavlink/mavlink/issues/1027 - scaleRange(getRSSI(), 0, 1023, 0, 254)); - } - else { - mavlink_msg_rc_channels_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - millis(), - // Total number of RC channels being received. - rxRuntimeConfig.channelCount, - // chan1_raw RC channel 1 value, in microseconds - GET_CHANNEL_VALUE(0), - // chan2_raw RC channel 2 value, in microseconds - GET_CHANNEL_VALUE(1), - // chan3_raw RC channel 3 value, in microseconds - GET_CHANNEL_VALUE(2), - // chan4_raw RC channel 4 value, in microseconds - GET_CHANNEL_VALUE(3), - // chan5_raw RC channel 5 value, in microseconds - GET_CHANNEL_VALUE(4), - // chan6_raw RC channel 6 value, in microseconds - GET_CHANNEL_VALUE(5), - // chan7_raw RC channel 7 value, in microseconds - GET_CHANNEL_VALUE(6), - // chan8_raw RC channel 8 value, in microseconds - GET_CHANNEL_VALUE(7), - // chan9_raw RC channel 9 value, in microseconds - GET_CHANNEL_VALUE(8), - // chan10_raw RC channel 10 value, in microseconds - GET_CHANNEL_VALUE(9), - // chan11_raw RC channel 11 value, in microseconds - GET_CHANNEL_VALUE(10), - // chan12_raw RC channel 12 value, in microseconds - GET_CHANNEL_VALUE(11), - // chan13_raw RC channel 13 value, in microseconds - GET_CHANNEL_VALUE(12), - // chan14_raw RC channel 14 value, in microseconds - GET_CHANNEL_VALUE(13), - // chan15_raw RC channel 15 value, in microseconds - GET_CHANNEL_VALUE(14), - // chan16_raw RC channel 16 value, in microseconds - GET_CHANNEL_VALUE(15), - // chan17_raw RC channel 17 value, in microseconds - GET_CHANNEL_VALUE(16), - // chan18_raw RC channel 18 value, in microseconds - GET_CHANNEL_VALUE(17), - // rssi Receive signal strength indicator, 0: 0%, 254: 100% - //https://github.com/mavlink/mavlink/issues/1027 - scaleRange(getRSSI(), 0, 1023, 0, 254)); - } -#undef GET_CHANNEL_VALUE - - mavlinkSendMessage(); -} - -#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 mavlinkSendGpsRawInt(timeUs_t currentTimeUs) -{ - uint8_t gpsFixType = 0; - - if (!(sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - )) - return; - - if (gpsSol.fixType == GPS_NO_FIX) - gpsFixType = 1; - else if (gpsSol.fixType == GPS_FIX_2D) - gpsFixType = 2; - else if (gpsSol.fixType == GPS_FIX_3D) - gpsFixType = 3; - - mavlink_msg_gps_raw_int_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - currentTimeUs, - // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - gpsFixType, - // lat Latitude in 1E7 degrees - gpsSol.llh.lat, - // lon Longitude in 1E7 degrees - gpsSol.llh.lon, - // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 10, - // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - gpsSol.eph, - // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - gpsSol.epv, - // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - gpsSol.groundSpeed, - // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - gpsSol.groundCourse * 10, - // satellites_visible Number of satellites visible. If unknown, set to 255 - gpsSol.numSat, - // alt_ellipsoid Altitude (above WGS84, EGM96 ellipsoid). Positive for up - 0, - // h_acc Position uncertainty in mm, - gpsSol.eph * 10, - // v_acc Altitude uncertainty in mm, - gpsSol.epv * 10, - // vel_acc Speed uncertainty in mm (??) - 0, - // hdg_acc Heading uncertainty in degE5 - 0, - // yaw Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. - 0); - - mavlinkSendMessage(); -} - -void mavlinkSendGlobalPositionInt(timeUs_t currentTimeUs) -{ - if (!(sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - )) { - return; - } - - mavlink_msg_global_position_int_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - currentTimeUs / 1000, - // lat Latitude in 1E7 degrees - gpsSol.llh.lat, - // lon Longitude in 1E7 degrees - gpsSol.llh.lon, - // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 10, - // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) - getEstimatedActualPosition(Z) * 10, - // [cm/s] Ground X Speed (Latitude, positive north) - getEstimatedActualVelocity(X), - // [cm/s] Ground Y Speed (Longitude, positive east) - getEstimatedActualVelocity(Y), - // [cm/s] Ground Z Speed (Altitude, positive down) - -getEstimatedActualVelocity(Z), - // [cdeg] Vehicle heading (yaw angle) (0.0..359.99 degrees, 0=north) - DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw) - ); - - mavlinkSendMessage(); -} - -void mavlinkSendGpsGlobalOrigin(void) -{ - mavlink_msg_gps_global_origin_pack(mavSystemId, mavComponentId, &mavSendMsg, - // latitude Latitude (WGS84), expressed as * 1E7 - GPS_home.lat, - // longitude Longitude (WGS84), expressed as * 1E7 - GPS_home.lon, - // altitude Altitude(WGS84), expressed as * 1000 - GPS_home.alt * 10, // FIXME - // time_usec Timestamp (microseconds since system boot) - // Use millis() * 1000 as micros() will overflow after 1.19 hours. - ((uint64_t) millis()) * 1000); - - mavlinkSendMessage(); -} - -void mavlinkSendPosition(timeUs_t currentTimeUs) -{ - mavlinkSendGpsRawInt(currentTimeUs); - mavlinkSendGlobalPositionInt(currentTimeUs); - mavlinkSendGpsGlobalOrigin(); -} -#endif - -void mavlinkSendAttitude(void) -{ - mavlink_msg_attitude_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - millis(), - // roll Roll angle (rad) - RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.roll)), - // pitch Pitch angle (rad) - RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(-attitude.values.pitch)), - // yaw Yaw angle (rad) - RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)), - // rollspeed Roll angular speed (rad/s) - DEGREES_TO_RADIANS(gyro.gyroADCf[FD_ROLL]), - // pitchspeed Pitch angular speed (rad/s) - DEGREES_TO_RADIANS(gyro.gyroADCf[FD_PITCH]), - // yawspeed Yaw angular speed (rad/s) - DEGREES_TO_RADIANS(gyro.gyroADCf[FD_YAW])); - - mavlinkSendMessage(); -} - -void mavlinkSendVfrHud(void) -{ - float mavAltitude = 0; - float mavGroundSpeed = 0; - float mavAirSpeed = 0; - float mavClimbRate = 0; - -#if defined(USE_GPS) - // use ground speed if source available - if (sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - ) { - mavGroundSpeed = gpsSol.groundSpeed / 100.0f; - } -#endif - -#if defined(USE_PITOT) - if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - mavAirSpeed = getAirspeedEstimate() / 100.0f; - } -#endif - - // select best source for altitude - mavAltitude = getEstimatedActualPosition(Z) / 100.0f; - mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; - - int16_t thr = getThrottlePercent(osdUsingScaledThrottle()); - mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg, - // airspeed Current airspeed in m/s - mavAirSpeed, - // groundspeed Current ground speed in m/s - mavGroundSpeed, - // heading Current heading in degrees, in compass units (0..360, 0=north) - DECIDEGREES_TO_DEGREES(attitude.values.yaw), - // throttle Current throttle setting in integer percent, 0 to 100 - thr, - // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) - mavAltitude, - // climb Current climb rate in meters/second - mavClimbRate); - - mavlinkSendMessage(); -} - -static uint8_t mavlinkGetAutopilotEnum(void); - -void mavlinkSendHeartbeat(void) -{ - uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - - 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) { - mavSystemType = MAV_TYPE_FIXED_WING; - } - else { - mavSystemType = mavlinkGetVehicleType(); - } - - 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 && isGCSValid()) { - mavModes |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - else if (flm == FLM_MISSION || flm == FLM_RTH ) { - mavModes |= MAV_MODE_FLAG_AUTO_ENABLED; - } - else if (flm != FLM_MANUAL && flm!= FLM_ACRO && flm!=FLM_ACRO_AIR) { - mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - if (ARMING_FLAG(ARMED)) - mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; - - uint8_t mavSystemState = 0; - if (ARMING_FLAG(ARMED)) { - if (failsafeIsActive()) { - mavSystemState = MAV_STATE_CRITICAL; - } - else { - mavSystemState = MAV_STATE_ACTIVE; - } - } - else if (areSensorsCalibrating()) { - mavSystemState = MAV_STATE_CALIBRATING; - } - else { - mavSystemState = MAV_STATE_STANDBY; - } - - mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg, - // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - mavSystemType, - // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - mavlinkGetAutopilotEnum(), - // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - mavModes, - // custom_mode A bitfield for use for autopilot-specific flags. - mavCustomMode, - // system_status System status flag, see MAV_STATE ENUM - mavSystemState); - - mavlinkSendMessage(); -} - -void mavlinkSendBatteryStatus(void) -{ - uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; - uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; - memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages)); - memset(batteryVoltagesExt, 0, sizeof(batteryVoltagesExt)); - if (feature(FEATURE_VBAT)) { - uint8_t batteryCellCount = getBatteryCellCount(); - if (batteryCellCount > 0) { - for (int cell=0; cell < batteryCellCount && cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN + MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; cell++) { - if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { - batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; - } else { - batteryVoltagesExt[cell-MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN] = getBatteryAverageCellVoltage() * 10; - } - } - } - else { - batteryVoltages[0] = getBatteryVoltage() * 10; - } - } - else { - batteryVoltages[0] = 0; - } - - mavlink_msg_battery_status_pack(mavSystemId, mavComponentId, &mavSendMsg, - // id Battery ID - 0, - // battery_function Function of the battery - MAV_BATTERY_FUNCTION_UNKNOWN, - // type Type (chemistry) of the battery - MAV_BATTERY_TYPE_UNKNOWN, - // temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature - INT16_MAX, - // voltages Battery voltage of cells, in millivolts (1 = 1 millivolt). Cells above the valid cell count for this battery should have the UINT16_MAX value. - batteryVoltages, - // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - isAmperageConfigured() ? getAmperage() : -1, - // current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate - isAmperageConfigured() ? getMAhDrawn() : -1, - // energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate - isAmperageConfigured() ? getMWhDrawn()*36 : -1, - // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery); - feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1, - // time_remaining Remaining battery time, 0: autopilot does not provide remaining battery time estimate - 0, // TODO this could easily be implemented - // charge_state State for extent of discharge, provided by autopilot for warning or external reactions - 0, - // voltages_ext Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. - batteryVoltagesExt, - // mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. - 0, - // fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). - 0); - - mavlinkSendMessage(); -} - -void mavlinkSendScaledPressure(void) -{ - int16_t temperature; - sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); - mavlink_msg_scaled_pressure_pack(mavSystemId, mavComponentId, &mavSendMsg, - millis(), - 0, - 0, - temperature * 10, - 0); - - mavlinkSendMessage(); -} - -void mavlinkSendSystemTime(void) -{ - mavlink_msg_system_time_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - 0, - millis()); - - mavlinkSendMessage(); -} - -bool mavlinkSendStatusText(void) -{ -// FIXME - Status text is limited to boards with USE_OSD -#ifdef USE_OSD - char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "}; - textAttributes_t elemAttr = osdGetSystemMessage(buff, sizeof(buff), false); - if (buff[0] != SYM_BLANK) { - MAV_SEVERITY severity = MAV_SEVERITY_NOTICE; - if (TEXT_ATTRIBUTES_HAVE_BLINK(elemAttr)) { - severity = MAV_SEVERITY_CRITICAL; - } else if TEXT_ATTRIBUTES_HAVE_INVERTED(elemAttr) { - severity = MAV_SEVERITY_WARNING; - } - - mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg, - (uint8_t)severity, - buff, - 0, - 0); - - mavlinkSendMessage(); - return true; - } -#endif - return false; -} - -void mavlinkSendBatteryTemperatureStatusText(void) -{ - mavlinkSendBatteryStatus(); - mavlinkSendScaledPressure(); - mavlinkSendStatusText(); -} - -static uint8_t mavlinkGetAutopilotEnum(void) -{ - if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { - return MAV_AUTOPILOT_ARDUPILOTMEGA; - } - - return MAV_AUTOPILOT_GENERIC; -} - -static uint8_t mavlinkHeadingDeg2FromCd(int32_t headingCd) -{ - return (uint8_t)(wrap_36000(headingCd) / 200); -} - -static uint16_t mavlinkComputeHighLatencyFailures(void) -{ - uint16_t flags = 0; - -#if defined(USE_GPS) - if (!(sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - ) || gpsSol.fixType == GPS_NO_FIX) { - flags |= HL_FAILURE_FLAG_GPS; - } -#endif - -#ifdef USE_PITOT - if (getHwPitotmeterStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; - } -#endif - - if (getHwBarometerStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; - } - - if (getHwAccelerometerStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_3D_ACCEL; - } - - if (getHwGyroStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_3D_GYRO; - } - - if (getHwCompassStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_3D_MAG; - } - - if (getHwRangefinderStatus() != HW_SENSOR_OK) { - flags |= HL_FAILURE_FLAG_TERRAIN; - } - - const batteryState_e batteryState = getBatteryState(); - if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { - flags |= HL_FAILURE_FLAG_BATTERY; - } - - if (!rxIsReceivingSignal() || !rxAreFlightChannelsValid()) { - flags |= HL_FAILURE_FLAG_RC_RECEIVER; - } - - return flags; -} - -void mavlinkSendHighLatency2(timeUs_t currentTimeUs) -{ - const bool isPlane = (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); - - int32_t latitude = 0; - int32_t longitude = 0; - int16_t altitude = (int16_t)constrain((int32_t)(getEstimatedActualPosition(Z) / 100), INT16_MIN, INT16_MAX); - int16_t targetAltitude = (int16_t)constrain((int32_t)(posControl.desiredState.pos.z / 100), INT16_MIN, INT16_MAX); - uint16_t targetDistance = 0; - uint16_t wpNum = 0; - uint8_t heading = mavlinkHeadingDeg2FromCd(DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)); - uint8_t targetHeading = mavlinkHeadingDeg2FromCd(posControl.desiredState.yaw); - uint8_t groundspeed = 0; - uint8_t airspeed = 0; - uint8_t airspeedSp = 0; - uint8_t windspeed = 0; - uint8_t windHeading = 0; - uint8_t eph = UINT8_MAX; - uint8_t epv = UINT8_MAX; - int8_t temperatureAir = 0; - int8_t climbRate = (int8_t)constrain(lrintf(getEstimatedActualVelocity(Z) / 10.0f), INT8_MIN, INT8_MAX); - int8_t battery = feature(FEATURE_VBAT) ? (int8_t)calculateBatteryPercentage() : -1; - -#if defined(USE_GPS) - if (sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - ) { - latitude = gpsSol.llh.lat; - longitude = gpsSol.llh.lon; - altitude = (int16_t)constrain((int32_t)(gpsSol.llh.alt / 100), INT16_MIN, INT16_MAX); - - const int32_t desiredAltCm = lrintf(posControl.desiredState.pos.z); - const int32_t targetAltCm = GPS_home.alt + desiredAltCm; - targetAltitude = (int16_t)constrain(targetAltCm / 100, INT16_MIN, INT16_MAX); - - groundspeed = (uint8_t)constrain(lrintf(gpsSol.groundSpeed / 20.0f), 0, UINT8_MAX); - - if (gpsSol.flags.validEPE) { - eph = (uint8_t)constrain((gpsSol.eph + 5) / 10, 0, UINT8_MAX); - epv = (uint8_t)constrain((gpsSol.epv + 5) / 10, 0, UINT8_MAX); - } - - if (posControl.activeWaypointIndex >= 0) { - wpNum = (uint16_t)posControl.activeWaypointIndex; - targetDistance = (uint16_t)constrain(lrintf(posControl.wpDistance / 1000.0f), 0, UINT16_MAX); - } - } -#endif - -#if defined(USE_PITOT) - if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - airspeed = (uint8_t)constrain(lrintf(getAirspeedEstimate() / 20.0f), 0, UINT8_MAX); - airspeedSp = airspeed; - } -#endif - - if (airspeedSp == 0) { - const float desiredVelXY = calc_length_pythagorean_2D(posControl.desiredState.vel.x, posControl.desiredState.vel.y); - airspeedSp = (uint8_t)constrain(lrintf(desiredVelXY / 20.0f), 0, UINT8_MAX); - } - - if (airspeed == 0) { - airspeed = groundspeed; - } - -#ifdef USE_WIND_ESTIMATOR - if (isEstimatedWindSpeedValid()) { - uint16_t windAngleCd = 0; - const float windHoriz = getEstimatedHorizontalWindSpeed(&windAngleCd); - windspeed = (uint8_t)constrain(lrintf(windHoriz / 20.0f), 0, UINT8_MAX); - windHeading = mavlinkHeadingDeg2FromCd(windAngleCd); - } -#endif - - int16_t temperature; - sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); - temperatureAir = (int8_t)constrain(temperature, INT8_MIN, INT8_MAX); - - const uint16_t failureFlags = mavlinkComputeHighLatencyFailures(); - - mavlink_msg_high_latency2_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - (uint32_t)(currentTimeUs / 1000), - mavlinkGetVehicleType(), - mavlinkGetAutopilotEnum(), - modeSelection.customMode, - latitude, - longitude, - altitude, - targetAltitude, - heading, - targetHeading, - targetDistance, - (uint8_t)constrain(getThrottlePercent(osdUsingScaledThrottle()), 0, 100), - airspeed, - airspeedSp, - groundspeed, - windspeed, - windHeading, - eph, - epv, - temperatureAir, - climbRate, - battery, - wpNum, - failureFlags, - 0, - 0, - 0); - - mavlinkSendMessage(); -} - - static void mavlinkResetIncomingMissionTransaction(void); static bool handleIncoming_MISSION_CLEAR_ALL(void) @@ -2061,127 +910,13 @@ static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t targetComponent return true; case MAV_CMD_REQUEST_MESSAGE: { - bool sent = false; - uint16_t messageId = (uint16_t)param1; - - switch (messageId) { - case MAVLINK_MSG_ID_HEARTBEAT: - mavlinkSendHeartbeat(); - sent = true; - break; - case MAVLINK_MSG_ID_AUTOPILOT_VERSION: - if (mavlinkGetProtocolVersion() != 1) { - mavlinkSendAutopilotVersion(); - sent = true; - } - break; - case MAVLINK_MSG_ID_PROTOCOL_VERSION: - if (mavlinkGetProtocolVersion() != 1) { - mavlinkSendProtocolVersion(); - sent = true; - } - break; - case MAVLINK_MSG_ID_SYS_STATUS: - mavlinkSendSystemStatus(); - sent = true; - break; - case MAVLINK_MSG_ID_ATTITUDE: - mavlinkSendAttitude(); - sent = true; - break; - case MAVLINK_MSG_ID_VFR_HUD: - mavlinkSendVfrHud(); - sent = true; - break; - case MAVLINK_MSG_ID_AVAILABLE_MODES: - { - const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); - if (isPlane) { - mavlinkSendAvailableModes(planeModes, planeModesCount, modeSelection.customMode, mavlinkPlaneModeIsConfigured); - } else { - mavlinkSendAvailableModes(copterModes, copterModesCount, modeSelection.customMode, mavlinkCopterModeIsConfigured); - } - sent = true; - } - break; - case MAVLINK_MSG_ID_CURRENT_MODE: - { - const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); - mavlink_msg_current_mode_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - MAV_STANDARD_MODE_NON_STANDARD, - modeSelection.customMode, - modeSelection.customMode); - mavlinkSendMessage(); - sent = true; - } - break; - case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: - mavlinkSendExtendedSysState(); - sent = true; - break; - case MAVLINK_MSG_ID_RC_CHANNELS: - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: - case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: - mavlinkSendRCChannelsAndRSSI(); - sent = true; - break; - case MAVLINK_MSG_ID_GPS_RAW_INT: - #ifdef USE_GPS - mavlinkSendGpsRawInt(micros()); - sent = true; - #endif - break; - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: - #ifdef USE_GPS - mavlinkSendGlobalPositionInt(micros()); - sent = true; - #endif - break; - case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: - #ifdef USE_GPS - mavlinkSendGpsGlobalOrigin(); - sent = true; - #endif - break; - case MAVLINK_MSG_ID_BATTERY_STATUS: - mavlinkSendBatteryStatus(); - sent = true; - break; - case MAVLINK_MSG_ID_SCALED_PRESSURE: - mavlinkSendScaledPressure(); - sent = true; - break; - case MAVLINK_MSG_ID_SYSTEM_TIME: - mavlinkSendSystemTime(); - sent = true; - break; - case MAVLINK_MSG_ID_STATUSTEXT: - sent = mavlinkSendStatusText(); - break; - case MAVLINK_MSG_ID_HOME_POSITION: - #ifdef USE_GPS - if (STATE(GPS_FIX_HOME)) { - mavlinkSendHomePosition(); - sent = true; - } - #endif - break; - default: - break; - } - + const bool sent = mavlinkSendRequestedMessage((uint16_t)param1); 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(); + if (mavlinkSendRequestedMessage(MAVLINK_MSG_ID_HOME_POSITION)) { mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); } else { mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); diff --git a/src/main/fc/fc_mavlink.h b/src/main/fc/fc_mavlink.h index c945ba1ba14..dd4543318c9 100644 --- a/src/main/fc/fc_mavlink.h +++ b/src/main/fc/fc_mavlink.h @@ -1,7 +1,5 @@ #pragma once -#include "common/time.h" - #include "mavlink/mavlink_types.h" typedef enum { @@ -10,20 +8,4 @@ typedef enum { MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY, } mavlinkFcDispatchResult_e; -void mavlinkSendSystemStatus(void); -void mavlinkSendRCChannelsAndRSSI(void); -void mavlinkSendPosition(timeUs_t currentTimeUs); -void mavlinkSendGpsRawInt(timeUs_t currentTimeUs); -void mavlinkSendGlobalPositionInt(timeUs_t currentTimeUs); -void mavlinkSendGpsGlobalOrigin(void); -void mavlinkSendAttitude(void); -void mavlinkSendVfrHud(void); -void mavlinkSendHeartbeat(void); -void mavlinkSendBatteryTemperatureStatusText(void); -void mavlinkSendExtendedSysState(void); -void mavlinkSendBatteryStatus(void); -void mavlinkSendScaledPressure(void); -void mavlinkSendSystemTime(void); -bool mavlinkSendStatusText(void); -void mavlinkSendHighLatency2(timeUs_t currentTimeUs); mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIndex); diff --git a/src/main/mavlink/mavlink_streams.c b/src/main/mavlink/mavlink_streams.c index a774f2ef00e..312df303137 100644 --- a/src/main/mavlink/mavlink_streams.c +++ b/src/main/mavlink/mavlink_streams.c @@ -1,7 +1,5 @@ #include "mavlink/mavlink_internal.h" -#include "fc/fc_mavlink.h" - #include "mavlink/mavlink_runtime.h" #include "mavlink/mavlink_streams.h" @@ -240,6 +238,1139 @@ void configureMAVLinkStreamRates(uint8_t portIndex) } } +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) + { + case FLM_ACRO: return COPTER_MODE_ACRO; + case FLM_ACRO_AIR: return COPTER_MODE_ACRO; + case FLM_ANGLE: return COPTER_MODE_STABILIZE; + case FLM_HORIZON: return COPTER_MODE_STABILIZE; + case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; + case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return COPTER_MODE_GUIDED; + } else { + return COPTER_MODE_POSHOLD; + } + } + case FLM_RTH: return COPTER_MODE_RTL; + case FLM_MISSION: return COPTER_MODE_AUTO; + case FLM_LAUNCH: return COPTER_MODE_THROW; + case FLM_FAILSAFE: + { + if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { + return COPTER_MODE_RTL; + } else if (failsafePhase() == FAILSAFE_LANDING) { + return COPTER_MODE_LAND; + } else { + return COPTER_MODE_RTL; + } + } + default: return COPTER_MODE_STABILIZE; + } +} + +static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) +{ + switch (flightMode) + { + case FLM_MANUAL: return PLANE_MODE_MANUAL; + case FLM_ACRO: return PLANE_MODE_ACRO; + case FLM_ACRO_AIR: return PLANE_MODE_ACRO; + case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; + case FLM_HORIZON: return PLANE_MODE_STABILIZE; + case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; + case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return PLANE_MODE_GUIDED; + } else { + return PLANE_MODE_LOITER; + } + } + case FLM_RTH: return PLANE_MODE_RTL; + case FLM_MISSION: return PLANE_MODE_AUTO; + case FLM_CRUISE: return PLANE_MODE_CRUISE; + case FLM_LAUNCH: return PLANE_MODE_TAKEOFF; + case FLM_FAILSAFE: + { + if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { + return PLANE_MODE_RTL; + } else if (failsafePhase() == FAILSAFE_LANDING) { + return PLANE_MODE_AUTOLAND; + } else { + return PLANE_MODE_RTL; + } + } + 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; +} + +void mavlinkSendAutopilotVersion(void) +{ + if (mavlinkGetProtocolVersion() == 1) { + return; + } + + // Capabilities aligned with what we actually support. + uint64_t capabilities = 0; + capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; + capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; + capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT; + capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT; + capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; + capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT; + + 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, + flightSwVersion, + 0, + 0, + 0, + 0ULL, + 0ULL, + 0ULL, + 0ULL, + 0ULL, + (uint64_t)mavSystemId, + 0ULL + ); + mavlinkSendMessage(); +} + +void mavlinkSendProtocolVersion(void) +{ + if (mavlinkGetProtocolVersion() == 1) { + return; + } + + uint8_t specHash[8] = {0}; + uint8_t libHash[8] = {0}; + const uint16_t protocolVersion = (uint16_t)mavlinkGetProtocolVersion() * 100; + + mavlink_msg_protocol_version_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + protocolVersion, + protocolVersion, + protocolVersion, + specHash, + libHash); + + mavlinkSendMessage(); +} + +static const mavlinkModeDescriptor_t planeModes[] = { + { PLANE_MODE_MANUAL, "MANUAL" }, + { PLANE_MODE_ACRO, "ACRO" }, + { PLANE_MODE_STABILIZE, "STABILIZE" }, + { PLANE_MODE_FLY_BY_WIRE_A,"FBWA" }, + { PLANE_MODE_FLY_BY_WIRE_B,"FBWB" }, + { PLANE_MODE_CRUISE, "CRUISE" }, + { PLANE_MODE_AUTO, "AUTO" }, + { PLANE_MODE_RTL, "RTL" }, + { PLANE_MODE_LOITER, "LOITER" }, + { PLANE_MODE_TAKEOFF, "TAKEOFF" }, + { PLANE_MODE_GUIDED, "GUIDED" }, +}; + +static const mavlinkModeDescriptor_t copterModes[] = { + { COPTER_MODE_ACRO, "ACRO" }, + { COPTER_MODE_STABILIZE, "STABILIZE" }, + { COPTER_MODE_ALT_HOLD, "ALT_HOLD" }, + { COPTER_MODE_POSHOLD, "POSHOLD" }, + { COPTER_MODE_LOITER, "LOITER" }, + { COPTER_MODE_AUTO, "AUTO" }, + { COPTER_MODE_GUIDED, "GUIDED" }, + { COPTER_MODE_RTL, "RTL" }, + { COPTER_MODE_LAND, "LAND" }, + { COPTER_MODE_BRAKE, "BRAKE" }, + { COPTER_MODE_THROW, "THROW" }, + { COPTER_MODE_SMART_RTL, "SMART_RTL" }, + { COPTER_MODE_DRIFT, "DRIFT" }, +}; + +static bool mavlinkPlaneModeIsConfigured(uint8_t customMode) +{ + switch ((APM_PLANE_MODE)customMode) { + case PLANE_MODE_MANUAL: + return isModeActivationConditionPresent(BOXMANUAL); + case PLANE_MODE_ACRO: + return true; + case PLANE_MODE_STABILIZE: + return isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXANGLEHOLD); + case PLANE_MODE_FLY_BY_WIRE_A: + return isModeActivationConditionPresent(BOXANGLE); + case PLANE_MODE_FLY_BY_WIRE_B: + return isModeActivationConditionPresent(BOXNAVALTHOLD); + case PLANE_MODE_CRUISE: + return isModeActivationConditionPresent(BOXNAVCRUISE) || + (isModeActivationConditionPresent(BOXNAVCOURSEHOLD) && + isModeActivationConditionPresent(BOXNAVALTHOLD)); + case PLANE_MODE_AUTO: + return isModeActivationConditionPresent(BOXNAVWP); + case PLANE_MODE_RTL: + return isModeActivationConditionPresent(BOXNAVRTH); + case PLANE_MODE_GUIDED: + return isModeActivationConditionPresent(BOXNAVPOSHOLD) && + isModeActivationConditionPresent(BOXGCSNAV); + case PLANE_MODE_LOITER: + return isModeActivationConditionPresent(BOXNAVPOSHOLD); + case PLANE_MODE_TAKEOFF: + return isModeActivationConditionPresent(BOXNAVLAUNCH); + default: + return false; + } +} + +static bool mavlinkCopterModeIsConfigured(uint8_t customMode) +{ + switch ((APM_COPTER_MODE)customMode) { + case COPTER_MODE_ACRO: + return true; + case COPTER_MODE_STABILIZE: + return isModeActivationConditionPresent(BOXANGLE) || + isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXANGLEHOLD); + case COPTER_MODE_ALT_HOLD: + return isModeActivationConditionPresent(BOXNAVALTHOLD); + case COPTER_MODE_GUIDED: + return isModeActivationConditionPresent(BOXNAVPOSHOLD) && + isModeActivationConditionPresent(BOXGCSNAV); + case COPTER_MODE_POSHOLD: + return isModeActivationConditionPresent(BOXNAVPOSHOLD); + case COPTER_MODE_RTL: + return isModeActivationConditionPresent(BOXNAVRTH); + case COPTER_MODE_AUTO: + return isModeActivationConditionPresent(BOXNAVWP); + case COPTER_MODE_THROW: + return isModeActivationConditionPresent(BOXNAVLAUNCH); + case COPTER_MODE_BRAKE: + return isModeActivationConditionPresent(BOXBRAKING); + default: + return false; + } +} + +static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom, + bool (*isModeConfigured)(uint8_t customMode)) +{ + uint8_t availableCount = 0; + for (uint8_t i = 0; i < count; i++) { + if (isModeConfigured(modes[i].customMode)) { + availableCount++; + } + } + + if (availableCount == 0) { + return; + } + + uint8_t modeIndex = 0; + for (uint8_t i = 0; i < count; i++) { + if (!isModeConfigured(modes[i].customMode)) { + continue; + } + + modeIndex++; + const uint8_t stdMode = MAV_STANDARD_MODE_NON_STANDARD; + const uint32_t properties = 0; + + mavlink_msg_available_modes_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + availableCount, + modeIndex, + stdMode, + modes[i].customMode, + properties, + modes[i].name); + + mavlinkSendMessage(); + + if (modes[i].customMode == currentCustom) { + mavlink_msg_current_mode_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + stdMode, + currentCustom, + currentCustom); + mavlinkSendMessage(); + } + } +} + +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 + uint32_t onboard_control_sensors_present = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER); + // GYRO and RC are assumed as minimum requirements + uint32_t onboard_control_sensors_enabled = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER); + uint32_t onboard_control_sensors_health = 0; + + if (getHwGyroStatus() == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO; + // Missing presence will report as sensor unhealthy + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO; + } + + hardwareSensorStatus_e accStatus = getHwAccelerometerStatus(); + if (accStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } else if (accStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } else if (accStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } + + hardwareSensorStatus_e compassStatus = getHwCompassStatus(); + if (compassStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } else if (compassStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } else if (compassStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } + + hardwareSensorStatus_e baroStatus = getHwBarometerStatus(); + if (baroStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } else if (baroStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } else if (baroStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } + + hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus(); + if (pitotStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } else if (pitotStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } else if (pitotStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } + + hardwareSensorStatus_e gpsStatus = getHwGPSStatus(); + if (gpsStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; + } else if (gpsStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; + } else if (gpsStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; + } + + hardwareSensorStatus_e opFlowStatus = getHwOpticalFlowStatus(); + if (opFlowStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } else if (opFlowStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } else if (opFlowStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } + + hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus(); + if (rangefinderStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } else if (rangefinderStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } else if (rangefinderStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + + if (rxIsReceivingSignal() && rxAreFlightChannelsValid()) { + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + } + +#ifdef USE_BLACKBOX + // BLACKBOX is assumed enabled and present for boards with capability + onboard_control_sensors_present |= MAV_SYS_STATUS_LOGGING; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; + // Unhealthy only for cases with not enough space to record + if (!isBlackboxDeviceFull()) { + onboard_control_sensors_health |= MAV_SYS_STATUS_LOGGING; + } +#endif + + mavlink_msg_sys_status_pack(mavSystemId, mavComponentId, &mavSendMsg, + onboard_control_sensors_present, + onboard_control_sensors_enabled, + onboard_control_sensors_health, + constrain(averageSystemLoadPercent * 10, 0, 1000), + feature(FEATURE_VBAT) ? getBatteryVoltage() * 10 : 0, + isAmperageConfigured() ? getAmperage() : -1, + feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100, + 0, + 0, + 0, + 0, + 0, + 0, 0, 0, 0); + + mavlinkSendMessage(); +} + +void mavlinkSendRCChannelsAndRSSI(void) +{ +#define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) + if (mavlinkGetProtocolVersion() == 1) { + mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, + millis(), + 0, + GET_CHANNEL_VALUE(0), + GET_CHANNEL_VALUE(1), + GET_CHANNEL_VALUE(2), + GET_CHANNEL_VALUE(3), + GET_CHANNEL_VALUE(4), + GET_CHANNEL_VALUE(5), + GET_CHANNEL_VALUE(6), + GET_CHANNEL_VALUE(7), + scaleRange(getRSSI(), 0, 1023, 0, 254)); + } + else { + mavlink_msg_rc_channels_pack(mavSystemId, mavComponentId, &mavSendMsg, + millis(), + rxRuntimeConfig.channelCount, + GET_CHANNEL_VALUE(0), + GET_CHANNEL_VALUE(1), + GET_CHANNEL_VALUE(2), + GET_CHANNEL_VALUE(3), + GET_CHANNEL_VALUE(4), + GET_CHANNEL_VALUE(5), + GET_CHANNEL_VALUE(6), + GET_CHANNEL_VALUE(7), + GET_CHANNEL_VALUE(8), + GET_CHANNEL_VALUE(9), + GET_CHANNEL_VALUE(10), + GET_CHANNEL_VALUE(11), + GET_CHANNEL_VALUE(12), + GET_CHANNEL_VALUE(13), + GET_CHANNEL_VALUE(14), + GET_CHANNEL_VALUE(15), + GET_CHANNEL_VALUE(16), + GET_CHANNEL_VALUE(17), + scaleRange(getRSSI(), 0, 1023, 0, 254)); + } +#undef GET_CHANNEL_VALUE + + mavlinkSendMessage(); +} + +#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 mavlinkSendGpsRawInt(timeUs_t currentTimeUs) +{ + uint8_t gpsFixType = 0; + + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { + return; + } + + if (gpsSol.fixType == GPS_NO_FIX) { + gpsFixType = 1; + } else if (gpsSol.fixType == GPS_FIX_2D) { + gpsFixType = 2; + } else if (gpsSol.fixType == GPS_FIX_3D) { + gpsFixType = 3; + } + + mavlink_msg_gps_raw_int_pack(mavSystemId, mavComponentId, &mavSendMsg, + currentTimeUs, + gpsFixType, + gpsSol.llh.lat, + gpsSol.llh.lon, + gpsSol.llh.alt * 10, + gpsSol.eph, + gpsSol.epv, + gpsSol.groundSpeed, + gpsSol.groundCourse * 10, + gpsSol.numSat, + 0, + gpsSol.eph * 10, + gpsSol.epv * 10, + 0, + 0, + 0); + + mavlinkSendMessage(); +} + +void mavlinkSendGlobalPositionInt(timeUs_t currentTimeUs) +{ + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { + return; + } + + mavlink_msg_global_position_int_pack(mavSystemId, mavComponentId, &mavSendMsg, + currentTimeUs / 1000, + gpsSol.llh.lat, + gpsSol.llh.lon, + gpsSol.llh.alt * 10, + getEstimatedActualPosition(Z) * 10, + getEstimatedActualVelocity(X), + getEstimatedActualVelocity(Y), + -getEstimatedActualVelocity(Z), + DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw) + ); + + mavlinkSendMessage(); +} + +void mavlinkSendGpsGlobalOrigin(void) +{ + mavlink_msg_gps_global_origin_pack(mavSystemId, mavComponentId, &mavSendMsg, + GPS_home.lat, + GPS_home.lon, + GPS_home.alt * 10, + ((uint64_t) millis()) * 1000); + + mavlinkSendMessage(); +} + +void mavlinkSendPosition(timeUs_t currentTimeUs) +{ + mavlinkSendGpsRawInt(currentTimeUs); + mavlinkSendGlobalPositionInt(currentTimeUs); + mavlinkSendGpsGlobalOrigin(); +} +#endif + +void mavlinkSendAttitude(void) +{ + mavlink_msg_attitude_pack(mavSystemId, mavComponentId, &mavSendMsg, + millis(), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.roll)), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(-attitude.values.pitch)), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)), + DEGREES_TO_RADIANS(gyro.gyroADCf[FD_ROLL]), + DEGREES_TO_RADIANS(gyro.gyroADCf[FD_PITCH]), + DEGREES_TO_RADIANS(gyro.gyroADCf[FD_YAW])); + + mavlinkSendMessage(); +} + +void mavlinkSendVfrHud(void) +{ + float mavAltitude = 0; + float mavGroundSpeed = 0; + float mavAirSpeed = 0; + float mavClimbRate = 0; + +#if defined(USE_GPS) + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + mavGroundSpeed = gpsSol.groundSpeed / 100.0f; + } +#endif + +#if defined(USE_PITOT) + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + mavAirSpeed = getAirspeedEstimate() / 100.0f; + } +#endif + + mavAltitude = getEstimatedActualPosition(Z) / 100.0f; + mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; + + int16_t thr = getThrottlePercent(osdUsingScaledThrottle()); + mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavAirSpeed, + mavGroundSpeed, + DECIDEGREES_TO_DEGREES(attitude.values.yaw), + thr, + mavAltitude, + mavClimbRate); + + mavlinkSendMessage(); +} + +static uint8_t mavlinkGetAutopilotEnum(void); + +void mavlinkSendHeartbeat(void) +{ + uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + 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) { + mavSystemType = MAV_TYPE_FIXED_WING; + } + else { + mavSystemType = mavlinkGetVehicleType(); + } + + 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 && isGCSValid()) { + mavModes |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + else if (flm == FLM_MISSION || flm == FLM_RTH ) { + mavModes |= MAV_MODE_FLAG_AUTO_ENABLED; + } + else if (flm != FLM_MANUAL && flm != FLM_ACRO && flm != FLM_ACRO_AIR) { + mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + + if (ARMING_FLAG(ARMED)) { + mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + uint8_t mavSystemState = 0; + if (ARMING_FLAG(ARMED)) { + if (failsafeIsActive()) { + mavSystemState = MAV_STATE_CRITICAL; + } + else { + mavSystemState = MAV_STATE_ACTIVE; + } + } + else if (areSensorsCalibrating()) { + mavSystemState = MAV_STATE_CALIBRATING; + } + else { + mavSystemState = MAV_STATE_STANDBY; + } + + mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavSystemType, + mavlinkGetAutopilotEnum(), + mavModes, + mavCustomMode, + mavSystemState); + + mavlinkSendMessage(); +} + +void mavlinkSendBatteryStatus(void) +{ + uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; + uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; + memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages)); + memset(batteryVoltagesExt, 0, sizeof(batteryVoltagesExt)); + if (feature(FEATURE_VBAT)) { + uint8_t batteryCellCount = getBatteryCellCount(); + if (batteryCellCount > 0) { + for (int cell = 0; cell < batteryCellCount && cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN + MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; cell++) { + if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { + batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; + } else { + batteryVoltagesExt[cell - MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN] = getBatteryAverageCellVoltage() * 10; + } + } + } + else { + batteryVoltages[0] = getBatteryVoltage() * 10; + } + } + else { + batteryVoltages[0] = 0; + } + + mavlink_msg_battery_status_pack(mavSystemId, mavComponentId, &mavSendMsg, + 0, + MAV_BATTERY_FUNCTION_UNKNOWN, + MAV_BATTERY_TYPE_UNKNOWN, + INT16_MAX, + batteryVoltages, + isAmperageConfigured() ? getAmperage() : -1, + isAmperageConfigured() ? getMAhDrawn() : -1, + isAmperageConfigured() ? getMWhDrawn() * 36 : -1, + feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1, + 0, + 0, + batteryVoltagesExt, + 0, + 0); + + mavlinkSendMessage(); +} + +void mavlinkSendScaledPressure(void) +{ + int16_t temperature; + sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); + mavlink_msg_scaled_pressure_pack(mavSystemId, mavComponentId, &mavSendMsg, + millis(), + 0, + 0, + temperature * 10, + 0); + + mavlinkSendMessage(); +} + +void mavlinkSendSystemTime(void) +{ + mavlink_msg_system_time_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + 0, + millis()); + + mavlinkSendMessage(); +} + +bool mavlinkSendStatusText(void) +{ +#ifdef USE_OSD + char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "}; + textAttributes_t elemAttr = osdGetSystemMessage(buff, sizeof(buff), false); + if (buff[0] != SYM_BLANK) { + MAV_SEVERITY severity = MAV_SEVERITY_NOTICE; + if (TEXT_ATTRIBUTES_HAVE_BLINK(elemAttr)) { + severity = MAV_SEVERITY_CRITICAL; + } else if TEXT_ATTRIBUTES_HAVE_INVERTED(elemAttr) { + severity = MAV_SEVERITY_WARNING; + } + + mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg, + (uint8_t)severity, + buff, + 0, + 0); + + mavlinkSendMessage(); + return true; + } +#endif + return false; +} + +void mavlinkSendBatteryTemperatureStatusText(void) +{ + mavlinkSendBatteryStatus(); + mavlinkSendScaledPressure(); + mavlinkSendStatusText(); +} + +static uint8_t mavlinkGetAutopilotEnum(void) +{ + if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { + return MAV_AUTOPILOT_ARDUPILOTMEGA; + } + + return MAV_AUTOPILOT_GENERIC; +} + +static uint8_t mavlinkHeadingDeg2FromCd(int32_t headingCd) +{ + return (uint8_t)(wrap_36000(headingCd) / 200); +} + +static uint16_t mavlinkComputeHighLatencyFailures(void) +{ + uint16_t flags = 0; + +#if defined(USE_GPS) + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) || gpsSol.fixType == GPS_NO_FIX) { + flags |= HL_FAILURE_FLAG_GPS; + } +#endif + +#ifdef USE_PITOT + if (getHwPitotmeterStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; + } +#endif + + if (getHwBarometerStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; + } + + if (getHwAccelerometerStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_ACCEL; + } + + if (getHwGyroStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_GYRO; + } + + if (getHwCompassStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_MAG; + } + + if (getHwRangefinderStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_TERRAIN; + } + + const batteryState_e batteryState = getBatteryState(); + if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { + flags |= HL_FAILURE_FLAG_BATTERY; + } + + if (!rxIsReceivingSignal() || !rxAreFlightChannelsValid()) { + flags |= HL_FAILURE_FLAG_RC_RECEIVER; + } + + return flags; +} + +void mavlinkSendHighLatency2(timeUs_t currentTimeUs) +{ + const bool isPlane = (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE); + const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); + + int32_t latitude = 0; + int32_t longitude = 0; + int16_t altitude = (int16_t)constrain((int32_t)(getEstimatedActualPosition(Z) / 100), INT16_MIN, INT16_MAX); + int16_t targetAltitude = (int16_t)constrain((int32_t)(posControl.desiredState.pos.z / 100), INT16_MIN, INT16_MAX); + uint16_t targetDistance = 0; + uint16_t wpNum = 0; + uint8_t heading = mavlinkHeadingDeg2FromCd(DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)); + uint8_t targetHeading = mavlinkHeadingDeg2FromCd(posControl.desiredState.yaw); + uint8_t groundspeed = 0; + uint8_t airspeed = 0; + uint8_t airspeedSp = 0; + uint8_t windspeed = 0; + uint8_t windHeading = 0; + uint8_t eph = UINT8_MAX; + uint8_t epv = UINT8_MAX; + int8_t temperatureAir = 0; + int8_t climbRate = (int8_t)constrain(lrintf(getEstimatedActualVelocity(Z) / 10.0f), INT8_MIN, INT8_MAX); + int8_t battery = feature(FEATURE_VBAT) ? (int8_t)calculateBatteryPercentage() : -1; + +#if defined(USE_GPS) + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + latitude = gpsSol.llh.lat; + longitude = gpsSol.llh.lon; + altitude = (int16_t)constrain((int32_t)(gpsSol.llh.alt / 100), INT16_MIN, INT16_MAX); + + const int32_t desiredAltCm = lrintf(posControl.desiredState.pos.z); + const int32_t targetAltCm = GPS_home.alt + desiredAltCm; + targetAltitude = (int16_t)constrain(targetAltCm / 100, INT16_MIN, INT16_MAX); + + groundspeed = (uint8_t)constrain(lrintf(gpsSol.groundSpeed / 20.0f), 0, UINT8_MAX); + + if (gpsSol.flags.validEPE) { + eph = (uint8_t)constrain((gpsSol.eph + 5) / 10, 0, UINT8_MAX); + epv = (uint8_t)constrain((gpsSol.epv + 5) / 10, 0, UINT8_MAX); + } + + if (posControl.activeWaypointIndex >= 0) { + wpNum = (uint16_t)posControl.activeWaypointIndex; + targetDistance = (uint16_t)constrain(lrintf(posControl.wpDistance / 1000.0f), 0, UINT16_MAX); + } + } +#endif + +#if defined(USE_PITOT) + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + airspeed = (uint8_t)constrain(lrintf(getAirspeedEstimate() / 20.0f), 0, UINT8_MAX); + airspeedSp = airspeed; + } +#endif + + if (airspeedSp == 0) { + const float desiredVelXY = calc_length_pythagorean_2D(posControl.desiredState.vel.x, posControl.desiredState.vel.y); + airspeedSp = (uint8_t)constrain(lrintf(desiredVelXY / 20.0f), 0, UINT8_MAX); + } + + if (airspeed == 0) { + airspeed = groundspeed; + } + +#ifdef USE_WIND_ESTIMATOR + if (isEstimatedWindSpeedValid()) { + uint16_t windAngleCd = 0; + const float windHoriz = getEstimatedHorizontalWindSpeed(&windAngleCd); + windspeed = (uint8_t)constrain(lrintf(windHoriz / 20.0f), 0, UINT8_MAX); + windHeading = mavlinkHeadingDeg2FromCd(windAngleCd); + } +#endif + + int16_t temperature; + sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); + temperatureAir = (int8_t)constrain(temperature, INT8_MIN, INT8_MAX); + + const uint16_t failureFlags = mavlinkComputeHighLatencyFailures(); + + mavlink_msg_high_latency2_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + (uint32_t)(currentTimeUs / 1000), + mavlinkGetVehicleType(), + mavlinkGetAutopilotEnum(), + modeSelection.customMode, + latitude, + longitude, + altitude, + targetAltitude, + heading, + targetHeading, + targetDistance, + (uint8_t)constrain(getThrottlePercent(osdUsingScaledThrottle()), 0, 100), + airspeed, + airspeedSp, + groundspeed, + windspeed, + windHeading, + eph, + epv, + temperatureAir, + climbRate, + battery, + wpNum, + failureFlags, + 0, + 0, + 0); + + mavlinkSendMessage(); +} + +bool mavlinkSendRequestedMessage(uint16_t messageId) +{ + switch (messageId) { + case MAVLINK_MSG_ID_HEARTBEAT: + mavlinkSendHeartbeat(); + return true; + case MAVLINK_MSG_ID_AUTOPILOT_VERSION: + if (mavlinkGetProtocolVersion() != 1) { + mavlinkSendAutopilotVersion(); + return true; + } + return false; + case MAVLINK_MSG_ID_PROTOCOL_VERSION: + if (mavlinkGetProtocolVersion() != 1) { + mavlinkSendProtocolVersion(); + return true; + } + return false; + case MAVLINK_MSG_ID_SYS_STATUS: + mavlinkSendSystemStatus(); + return true; + case MAVLINK_MSG_ID_ATTITUDE: + mavlinkSendAttitude(); + return true; + case MAVLINK_MSG_ID_VFR_HUD: + mavlinkSendVfrHud(); + return true; + case MAVLINK_MSG_ID_AVAILABLE_MODES: + { + const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); + const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); + if (isPlane) { + mavlinkSendAvailableModes(planeModes, (uint8_t)ARRAYLEN(planeModes), modeSelection.customMode, mavlinkPlaneModeIsConfigured); + } else { + mavlinkSendAvailableModes(copterModes, (uint8_t)ARRAYLEN(copterModes), modeSelection.customMode, mavlinkCopterModeIsConfigured); + } + return true; + } + case MAVLINK_MSG_ID_CURRENT_MODE: + { + const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); + const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); + mavlink_msg_current_mode_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + MAV_STANDARD_MODE_NON_STANDARD, + modeSelection.customMode, + modeSelection.customMode); + mavlinkSendMessage(); + return true; + } + case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: + mavlinkSendExtendedSysState(); + return true; + case MAVLINK_MSG_ID_RC_CHANNELS: + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: + mavlinkSendRCChannelsAndRSSI(); + return true; +#ifdef USE_GPS + case MAVLINK_MSG_ID_GPS_RAW_INT: + mavlinkSendGpsRawInt(micros()); + return true; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + mavlinkSendGlobalPositionInt(micros()); + return true; + case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: + mavlinkSendGpsGlobalOrigin(); + return true; + case MAVLINK_MSG_ID_HOME_POSITION: + if (STATE(GPS_FIX_HOME)) { + mavlinkSendHomePosition(); + return true; + } + return false; +#endif + case MAVLINK_MSG_ID_BATTERY_STATUS: + mavlinkSendBatteryStatus(); + return true; + case MAVLINK_MSG_ID_SCALED_PRESSURE: + mavlinkSendScaledPressure(); + return true; + case MAVLINK_MSG_ID_SYSTEM_TIME: + mavlinkSendSystemTime(); + return true; + case MAVLINK_MSG_ID_STATUSTEXT: + return mavlinkSendStatusText(); + default: + return false; + } +} + void processMAVLinkTelemetry(timeUs_t currentTimeUs) { if (mavActivePort->highLatencyEnabled && mavlinkGetProtocolVersion() != 1) { diff --git a/src/main/mavlink/mavlink_streams.h b/src/main/mavlink/mavlink_streams.h index e3ffc467ef5..514a8e01c38 100644 --- a/src/main/mavlink/mavlink_streams.h +++ b/src/main/mavlink/mavlink_streams.h @@ -6,6 +6,9 @@ extern const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT]; +void mavlinkSendAutopilotVersion(void); +void mavlinkSendProtocolVersion(void); +bool mavlinkSendRequestedMessage(uint16_t messageId); uint8_t mavlinkClampStreamRate(uint8_t rate); int32_t mavlinkRateToIntervalUs(uint8_t rate); bool mavlinkPeriodicMessageFromMessageId(uint16_t messageId, mavlinkPeriodicMessage_e *periodicMessage); diff --git a/src/main/mavlink/mavlink_types.h b/src/main/mavlink/mavlink_types.h index 09f21dc99ed..c050d319392 100644 --- a/src/main/mavlink/mavlink_types.h +++ b/src/main/mavlink/mavlink_types.h @@ -171,11 +171,6 @@ typedef struct mavlinkModeDescriptor_s { const char *name; } mavlinkModeDescriptor_t; -extern const mavlinkModeDescriptor_t planeModes[]; -extern const uint8_t planeModesCount; -extern const mavlinkModeDescriptor_t copterModes[]; -extern const uint8_t copterModesCount; - typedef struct mavlinkMissionItemData_s { uint8_t frame; uint16_t command; From afa34e0b0d165276fe763ab47db36e75729acce1 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Fri, 3 Apr 2026 18:23:19 -0400 Subject: [PATCH 37/42] Split MAVLink modes and mission helpers --- src/main/CMakeLists.txt | 4 + src/main/fc/fc_mavlink.c | 665 +---------------------------- src/main/mavlink/mavlink_mission.c | 621 +++++++++++++++++++++++++++ src/main/mavlink/mavlink_mission.h | 42 ++ src/main/mavlink/mavlink_modes.c | 295 +++++++++++++ src/main/mavlink/mavlink_modes.h | 82 ++++ src/main/mavlink/mavlink_streams.c | 298 +------------ src/main/mavlink/mavlink_types.h | 89 ---- src/test/unit/CMakeLists.txt | 2 +- 9 files changed, 1061 insertions(+), 1037 deletions(-) create mode 100644 src/main/mavlink/mavlink_mission.c create mode 100644 src/main/mavlink/mavlink_mission.h create mode 100644 src/main/mavlink/mavlink_modes.c create mode 100644 src/main/mavlink/mavlink_modes.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 4fff0f3bf35..29e98de4da1 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -394,6 +394,10 @@ main_sources(COMMON_SRC io/osd/custom_elements.c mavlink/mavlink_internal.h + mavlink/mavlink_mission.c + mavlink/mavlink_mission.h + mavlink/mavlink_modes.c + mavlink/mavlink_modes.h mavlink/mavlink_types.h mavlink/mavlink_ports.c mavlink/mavlink_ports.h diff --git a/src/main/fc/fc_mavlink.c b/src/main/fc/fc_mavlink.c index 503bc75ed43..1d413bf7e08 100644 --- a/src/main/fc/fc_mavlink.c +++ b/src/main/fc/fc_mavlink.c @@ -2,6 +2,7 @@ #include "fc/fc_mavlink.h" +#include "mavlink/mavlink_mission.h" #include "mavlink/mavlink_runtime.h" #include "mavlink/mavlink_streams.h" @@ -140,590 +141,6 @@ static bool handleIncoming_TUNNEL(uint8_t ingressPortIndex) return handled; } -static bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask) -{ - switch (frame) { - case MAV_FRAME_GLOBAL: - return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL; - case MAV_FRAME_GLOBAL_INT: - return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_INT; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT; - case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: - return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT; - default: - return false; - } -} - -static bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame) -{ - return frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_INT; -} - -static MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters) -{ -#ifdef USE_BARO - geoAltitudeDatumFlag_e datum; - - switch (frame) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_INT: - datum = NAV_WP_MSL_DATUM; - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: - datum = NAV_WP_TAKEOFF_DATUM; - break; - default: - return MAV_RESULT_UNSUPPORTED; - } - - const int32_t targetAltitudeCm = (int32_t)lrintf(altitudeMeters * 100.0f); - return navigationSetAltitudeTargetWithDatum(datum, targetAltitudeCm) ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED; -#else - UNUSED(frame); - UNUSED(altitudeMeters); - return MAV_RESULT_UNSUPPORTED; -#endif -} - -static MAV_MISSION_RESULT mavlinkMissionResultFromCommandResult(MAV_RESULT result) -{ - switch (result) { - case MAV_RESULT_ACCEPTED: - return MAV_MISSION_ACCEPTED; - case MAV_RESULT_UNSUPPORTED: - return MAV_MISSION_UNSUPPORTED; - default: - return MAV_MISSION_ERROR; - } -} - -static bool mavlinkHandleArmedGuidedMissionItem(uint8_t current, uint8_t frame, mavFrameSupportMask_e allowedFrames, int32_t latitudeE7, int32_t longitudeE7, float altitudeMeters) -{ - if (!isGCSValid()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, - MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - if (!mavlinkFrameIsSupported(frame, allowedFrames)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, - mavlinkMissionResultFromCommandResult(result), MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, - MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; -} - -static uint8_t navWaypointFrame(const navWaypoint_t *wp, bool useIntMessages) -{ - switch (wp->action) { - case NAV_WP_ACTION_RTH: - case NAV_WP_ACTION_JUMP: - case NAV_WP_ACTION_SET_HEAD: - return MAV_FRAME_MISSION; - default: - break; - } - - const bool absoluteAltitude = (wp->p3 & NAV_WP_ALTMODE) == NAV_WP_ALTMODE; - - if (absoluteAltitude) { - return useIntMessages ? MAV_FRAME_GLOBAL_INT : MAV_FRAME_GLOBAL; - } - - return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT; -} - -static void mavlinkResetIncomingMissionTransaction(void); - -static bool handleIncoming_MISSION_CLEAR_ALL(void) -{ - mavlink_mission_clear_all_t msg; - mavlink_msg_mission_clear_all_decode(&mavlinkContext.recvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - resetWaypointList(); - mavlinkResetIncomingMissionTransaction(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return false; -} - -static void mavlinkResetIncomingMissionTransaction(void) -{ - incomingMissionWpCount = 0; - incomingMissionWpSequence = 0; - incomingMissionSourceSystem = 0; - incomingMissionSourceComponent = 0; - incomingMissionLastActivityMs = 0; -} - -static void mavlinkStartIncomingMissionTransaction(int missionCount) -{ - incomingMissionWpCount = missionCount; - incomingMissionWpSequence = 0; - incomingMissionSourceSystem = mavlinkContext.recvMsg.sysid; - incomingMissionSourceComponent = mavlinkContext.recvMsg.compid; - incomingMissionLastActivityMs = millis(); -} - -static void mavlinkTouchIncomingMissionTransaction(void) -{ - incomingMissionLastActivityMs = millis(); -} - -static bool mavlinkIsIncomingMissionTransactionActive(void) -{ - if (incomingMissionWpCount <= 0) { - return false; - } - - if ((timeMs_t)(millis() - incomingMissionLastActivityMs) > MAVLINK_MISSION_UPLOAD_TIMEOUT_MS) { - mavlinkResetIncomingMissionTransaction(); - return false; - } - - return true; -} - -static bool mavlinkIsIncomingMissionTransactionOwner(void) -{ - return mavlinkContext.recvMsg.sysid == incomingMissionSourceSystem && - mavlinkContext.recvMsg.compid == incomingMissionSourceComponent; -} - -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 (!mavlinkIsIncomingMissionTransactionActive() || !mavlinkIsIncomingMissionTransactionOwner()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - mavlinkTouchIncomingMissionTransaction(); - - const bool lastMissionItem = incomingMissionWpCount > 0 && ((int)seq + 1 >= incomingMissionWpCount); - - if (autocontinue == 0 && !lastMissionItem) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.action = NAV_WP_ACTION_WAYPOINT; - wp.lat = lat; - wp.lon = lon; - wp.alt = (int32_t)(altMeters * 100.0f); - wp.p1 = 0; - wp.p2 = 0; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - break; - - case MAV_CMD_NAV_LOITER_TIME: - if (!mavlinkFrameIsSupported(frame, - MAV_FRAME_SUPPORTED_GLOBAL | - MAV_FRAME_SUPPORTED_GLOBAL_INT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.action = NAV_WP_ACTION_HOLD_TIME; - wp.lat = lat; - wp.lon = lon; - wp.alt = (int32_t)(altMeters * 100.0f); - wp.p1 = (int16_t)lrintf(param1); - wp.p2 = 0; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - { - const bool coordinateFrame = mavlinkFrameIsSupported(frame, - MAV_FRAME_SUPPORTED_GLOBAL | - MAV_FRAME_SUPPORTED_GLOBAL_INT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT); - - if (!coordinateFrame && frame != MAV_FRAME_MISSION) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.action = NAV_WP_ACTION_RTH; - wp.lat = 0; - wp.lon = 0; - wp.alt = coordinateFrame ? (int32_t)(altMeters * 100.0f) : 0; - wp.p1 = 0; // Land if non-zero - wp.p2 = 0; - wp.p3 = 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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.p1 = 0; // Speed (cm/s) - wp.p2 = 0; // Elevation Adjustment (m): P2 defines the ground elevation (in metres) for the LAND WP. If the altitude mode is absolute, this is also absolute; for relative altitude, then it is the difference between the assumed home location and the LAND WP. - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; // Altitude Mode & Actions, P3 defines the altitude mode. 0 (default, legacy) = Relative to Home, 1 = Absolute (AMSL). - break; - - case MAV_CMD_DO_JUMP: - if (frame != MAV_FRAME_MISSION) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.lat = 0; - wp.lon = 0; - wp.alt = 0; - wp.action = NAV_WP_ACTION_JUMP; - wp.p1 = (int16_t)lrintf(param1 + 1.0f); - wp.p2 = (int16_t)lrintf(param2); - wp.p3 = 0; - break; - - case MAV_CMD_DO_SET_ROI: - if (param1 != MAV_ROI_LOCATION || - !mavlinkFrameIsSupported(frame, - MAV_FRAME_SUPPORTED_GLOBAL | - MAV_FRAME_SUPPORTED_GLOBAL_INT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.action = NAV_WP_ACTION_SET_POI; - wp.lat = lat; - wp.lon = lon; - wp.alt = (int32_t)(altMeters * 100.0f); - wp.p1 = 0; - wp.p2 = 0; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - break; - - case MAV_CMD_CONDITION_YAW: - if (frame != MAV_FRAME_MISSION) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - wp.lat = 0; - wp.lon = 0; - wp.alt = 0; - wp.action = NAV_WP_ACTION_SET_HEAD; - wp.p1 = (int16_t)lrintf(param1); - wp.p2 = 0; - wp.p3 = 0; - break; - - default: - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - if (seq == incomingMissionWpSequence) { - incomingMissionWpSequence++; - - 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - mavlinkResetIncomingMissionTransaction(); - } - else { - if (useIntMessages) { - mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); - } else { - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); - } - mavlinkSendMessage(); - } - } - else { - // If we get a duplicate of the last accepted item, re-request the next one instead of aborting. - if (seq + 1 == incomingMissionWpSequence) { - mavlinkTouchIncomingMissionTransaction(); - if (useIntMessages) { - mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); - } else { - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); - } - mavlinkSendMessage(); - } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - } - - return true; -} - -static bool handleIncoming_MISSION_COUNT(void) -{ - mavlink_mission_count_t msg; - mavlink_msg_mission_count_decode(&mavlinkContext.recvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - mavlinkResetIncomingMissionTransaction(); - if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - if (msg.count == 0) { - resetWaypointList(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - if (msg.count <= NAV_MAX_WAYPOINTS) { - mavlinkStartIncomingMissionTransaction(msg.count); - mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); - mavlinkSendMessage(); - return true; - } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - } - - return false; -} - -static bool handleIncoming_MISSION_ITEM(void) -{ - mavlink_mission_item_t msg; - mavlink_msg_mission_item_decode(&mavlinkContext.recvMsg, &msg); - - if (msg.target_system != mavSystemId) { - return false; - } - - if (ARMING_FLAG(ARMED)) { - if (msg.command == MAV_CMD_NAV_WAYPOINT) { - return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, - MAV_FRAME_SUPPORTED_GLOBAL | 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, (int32_t)(msg.x * 1e7f), (int32_t)(msg.y * 1e7f), msg.z); -} - -static bool handleIncoming_MISSION_REQUEST_LIST(void) -{ - mavlink_mission_request_list_t msg; - mavlink_msg_mission_request_list_decode(&mavlinkContext.recvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return false; -} - -static bool fillMavlinkMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item) -{ - mavlinkMissionItemData_t data = {0}; - - data.frame = navWaypointFrame(wp, useIntMessages); - - switch (wp->action) { - case NAV_WP_ACTION_WAYPOINT: - data.command = MAV_CMD_NAV_WAYPOINT; - data.lat = wp->lat; - data.lon = wp->lon; - data.alt = wp->alt / 100.0f; - break; - - case NAV_WP_ACTION_HOLD_TIME: - data.command = MAV_CMD_NAV_LOITER_TIME; - data.param1 = wp->p1; - data.lat = wp->lat; - data.lon = wp->lon; - data.alt = wp->alt / 100.0f; - break; - - case NAV_WP_ACTION_RTH: - data.command = MAV_CMD_NAV_RETURN_TO_LAUNCH; - break; - - case NAV_WP_ACTION_LAND: - data.command = MAV_CMD_NAV_LAND; - data.lat = wp->lat; - data.lon = wp->lon; - data.alt = wp->alt / 100.0f; - break; - - case NAV_WP_ACTION_JUMP: - data.command = MAV_CMD_DO_JUMP; - data.param1 = (wp->p1 > 0) ? (float)(wp->p1 - 1) : 0.0f; - data.param2 = wp->p2; - break; - - case NAV_WP_ACTION_SET_POI: - data.command = MAV_CMD_DO_SET_ROI; - data.param1 = MAV_ROI_LOCATION; - data.lat = wp->lat; - data.lon = wp->lon; - data.alt = wp->alt / 100.0f; - break; - - case NAV_WP_ACTION_SET_HEAD: - data.command = MAV_CMD_CONDITION_YAW; - data.param1 = wp->p1; - break; - - default: - return false; - } - - *item = data; - return true; -} - -static bool handleIncoming_MISSION_REQUEST(void) -{ - mavlink_mission_request_t msg; - mavlink_msg_mission_request_decode(&mavlinkContext.recvMsg, &msg); - - if (msg.target_system != mavSystemId) { - return false; - } - - int wpCount = getWaypointCount(); - - if (msg.seq < wpCount) { - navWaypoint_t wp; - getWaypoint(msg.seq + 1, &wp); - - mavlinkMissionItemData_t item; - if (fillMavlinkMissionItemFromWaypoint(&wp, false, &item)) { - mavlink_msg_mission_item_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - } - else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - - return true; -} - static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) { if (targetSystem != 0 && targetSystem != mavSystemId) { @@ -946,72 +363,6 @@ static bool handleIncoming_COMMAND_LONG(void) return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, msg.command, MAV_FRAME_GLOBAL, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7); } -static bool handleIncoming_MISSION_ITEM_INT(void) -{ - mavlink_mission_item_int_t msg; - mavlink_msg_mission_item_int_decode(&mavlinkContext.recvMsg, &msg); - - if (msg.target_system != mavSystemId) { - return false; - } - - if (ARMING_FLAG(ARMED)) { - if (msg.command == MAV_CMD_NAV_WAYPOINT) { - return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, - MAV_FRAME_SUPPORTED_GLOBAL_INT | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT, - msg.x, msg.y, msg.z); - } - - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return mavlinkHandleMissionItemCommon(true, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, msg.x, msg.y, msg.z); -} - -static bool handleIncoming_MISSION_REQUEST_INT(void) -{ - mavlink_mission_request_int_t msg; - mavlink_msg_mission_request_int_decode(&mavlinkContext.recvMsg, &msg); - - if (msg.target_system != mavSystemId) { - return false; - } - - int wpCount = getWaypointCount(); - - if (msg.seq < wpCount) { - navWaypoint_t wp; - getWaypoint(msg.seq + 1, &wp); - - mavlinkMissionItemData_t item; - if (fillMavlinkMissionItemFromWaypoint(&wp, true, &item)) { - mavlink_msg_mission_item_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - } - else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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; @@ -1244,23 +595,23 @@ mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIn case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: return handleIncoming_PARAM_REQUEST_LIST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: - return handleIncoming_MISSION_CLEAR_ALL() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingMissionClearAll() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_COUNT: - return handleIncoming_MISSION_COUNT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingMissionCount() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_ITEM: - return handleIncoming_MISSION_ITEM() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingMissionItem() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_ITEM_INT: - return handleIncoming_MISSION_ITEM_INT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingMissionItemInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: - return handleIncoming_MISSION_REQUEST_LIST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingMissionRequestList() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_COMMAND_LONG: return handleIncoming_COMMAND_LONG() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_COMMAND_INT: return handleIncoming_COMMAND_INT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST: - return handleIncoming_MISSION_REQUEST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingMissionRequest() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST_INT: - return handleIncoming_MISSION_REQUEST_INT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingMissionRequestInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: return handleIncoming_REQUEST_DATA_STREAM() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: diff --git a/src/main/mavlink/mavlink_mission.c b/src/main/mavlink/mavlink_mission.c new file mode 100644 index 00000000000..6ed125c3060 --- /dev/null +++ b/src/main/mavlink/mavlink_mission.c @@ -0,0 +1,621 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_mission.h" +#include "mavlink/mavlink_runtime.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +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; + } +} + +bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame) +{ + return frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_INT; +} + +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 +} + +uint8_t mavlinkWaypointFrame(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; + } + + if ((wp->p3 & NAV_WP_ALTMODE) == NAV_WP_ALTMODE) { + return useIntMessages ? MAV_FRAME_GLOBAL_INT : MAV_FRAME_GLOBAL; + } + + return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT; +} + +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 void mavlinkResetIncomingMissionTransaction(void) +{ + incomingMissionWpCount = 0; + incomingMissionWpSequence = 0; + incomingMissionSourceSystem = 0; + incomingMissionSourceComponent = 0; + incomingMissionLastActivityMs = 0; +} + +static void mavlinkStartIncomingMissionTransaction(int missionCount) +{ + incomingMissionWpCount = missionCount; + incomingMissionWpSequence = 0; + incomingMissionSourceSystem = mavlinkContext.recvMsg.sysid; + incomingMissionSourceComponent = mavlinkContext.recvMsg.compid; + incomingMissionLastActivityMs = millis(); +} + +static void mavlinkTouchIncomingMissionTransaction(void) +{ + incomingMissionLastActivityMs = millis(); +} + +static bool mavlinkIsIncomingMissionTransactionActive(void) +{ + if (incomingMissionWpCount <= 0) { + return false; + } + + if ((timeMs_t)(millis() - incomingMissionLastActivityMs) > MAVLINK_MISSION_UPLOAD_TIMEOUT_MS) { + mavlinkResetIncomingMissionTransaction(); + return false; + } + + return true; +} + +static bool mavlinkIsIncomingMissionTransactionOwner(void) +{ + return mavlinkContext.recvMsg.sysid == incomingMissionSourceSystem && + mavlinkContext.recvMsg.compid == incomingMissionSourceComponent; +} + +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, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, + MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + if (!mavlinkFrameIsSupported(frame, allowedFrames)) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, + mavlinkMissionResultFromCommandResult(result), MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, + MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; +} + +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 (!mavlinkIsIncomingMissionTransactionActive() || !mavlinkIsIncomingMissionTransactionOwner()) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + mavlinkTouchIncomingMissionTransaction(); + + const bool lastMissionItem = incomingMissionWpCount > 0 && ((int)seq + 1 >= incomingMissionWpCount); + + if (autocontinue == 0 && !lastMissionItem) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + UNUSED(param3); + + navWaypoint_t wp = {0}; + + switch (command) { + case MAV_CMD_NAV_WAYPOINT: + if (!mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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_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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + wp.action = NAV_WP_ACTION_RTH; + 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + if (seq == incomingMissionWpSequence) { + incomingMissionWpSequence++; + 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); + } else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION, 0); + } + mavlinkSendMessage(); + mavlinkResetIncomingMissionTransaction(); + } else { + if (useIntMessages) { + mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + } else { + mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + } + mavlinkSendMessage(); + } + } else { + if (seq + 1 == incomingMissionWpSequence) { + mavlinkTouchIncomingMissionTransaction(); + if (useIntMessages) { + mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + } else { + mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + } + mavlinkSendMessage(); + } else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + } + + return true; +} + +bool mavlinkHandleIncomingMissionClearAll(void) +{ + mavlink_mission_clear_all_t msg; + mavlink_msg_mission_clear_all_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system == mavSystemId) { + resetWaypointList(); + mavlinkResetIncomingMissionTransaction(); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + return false; +} + +bool mavlinkHandleIncomingMissionCount(void) +{ + mavlink_mission_count_t msg; + mavlink_msg_mission_count_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system == mavSystemId) { + mavlinkResetIncomingMissionTransaction(); + if (ARMING_FLAG(ARMED)) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + if (msg.count == 0) { + resetWaypointList(); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + if (msg.count <= NAV_MAX_WAYPOINTS) { + mavlinkStartIncomingMissionTransaction(msg.count); + mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + mavlinkSendMessage(); + return true; + } + + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + return false; +} + +bool mavlinkHandleIncomingMissionItem(void) +{ + mavlink_mission_item_t msg; + mavlink_msg_mission_item_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + if (ARMING_FLAG(ARMED)) { + if (msg.command == MAV_CMD_NAV_WAYPOINT) { + return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, + MAV_FRAME_SUPPORTED_GLOBAL | 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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); +} + +bool mavlinkHandleIncomingMissionRequestList(void) +{ + mavlink_mission_request_list_t msg; + mavlink_msg_mission_request_list_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system == mavSystemId) { + mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + return false; +} + +bool mavlinkFillMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item) +{ + mavlinkMissionItemData_t data = {0}; + + data.frame = mavlinkWaypointFrame(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; +} + +bool mavlinkHandleIncomingMissionRequest(void) +{ + mavlink_mission_request_t msg; + mavlink_msg_mission_request_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + const int wpCount = getWaypointCount(); + + if (msg.seq < wpCount) { + navWaypoint_t wp; + getWaypoint(msg.seq + 1, &wp); + + mavlinkMissionItemData_t item; + if (mavlinkFillMissionItemFromWaypoint(&wp, false, &item)) { + mavlink_msg_mission_item_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + } else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + + return true; +} + +bool mavlinkHandleIncomingMissionItemInt(void) +{ + mavlink_mission_item_int_t msg; + mavlink_msg_mission_item_int_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + if (ARMING_FLAG(ARMED)) { + if (msg.command == MAV_CMD_NAV_WAYPOINT) { + return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, + MAV_FRAME_SUPPORTED_GLOBAL_INT | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT, + msg.x, msg.y, msg.z); + } + + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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); +} + +bool mavlinkHandleIncomingMissionRequestInt(void) +{ + mavlink_mission_request_int_t msg; + mavlink_msg_mission_request_int_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + const int wpCount = getWaypointCount(); + + if (msg.seq < wpCount) { + navWaypoint_t wp; + getWaypoint(msg.seq + 1, &wp); + + mavlinkMissionItemData_t item; + if (mavlinkFillMissionItemFromWaypoint(&wp, true, &item)) { + mavlink_msg_mission_item_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + } else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + } + + return true; +} + +#endif diff --git a/src/main/mavlink/mavlink_mission.h b/src/main/mavlink/mavlink_mission.h new file mode 100644 index 00000000000..d8139dfbc96 --- /dev/null +++ b/src/main/mavlink/mavlink_mission.h @@ -0,0 +1,42 @@ +#pragma once + +#include +#include + +#include "navigation/navigation.h" + +#include "mavlink/mavlink_types.h" + +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; + +typedef struct mavlinkMissionItemData_s { + uint8_t frame; + uint16_t command; + float param1; + float param2; + float param3; + float param4; + int32_t lat; + int32_t lon; + float alt; +} mavlinkMissionItemData_t; + +bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask); +bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame); +MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters); +uint8_t mavlinkWaypointFrame(const navWaypoint_t *wp, bool useIntMessages); +bool mavlinkFillMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item); + +bool mavlinkHandleIncomingMissionClearAll(void); +bool mavlinkHandleIncomingMissionCount(void); +bool mavlinkHandleIncomingMissionItem(void); +bool mavlinkHandleIncomingMissionRequestList(void); +bool mavlinkHandleIncomingMissionRequest(void); +bool mavlinkHandleIncomingMissionItemInt(void); +bool mavlinkHandleIncomingMissionRequestInt(void); diff --git a/src/main/mavlink/mavlink_modes.c b/src/main/mavlink/mavlink_modes.c new file mode 100644 index 00000000000..33ff0bfe1b9 --- /dev/null +++ b/src/main/mavlink/mavlink_modes.c @@ -0,0 +1,295 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_modes.h" +#include "mavlink/mavlink_runtime.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +typedef struct mavlinkModeDescriptor_s { + uint8_t customMode; + const char *name; +} mavlinkModeDescriptor_t; + +static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode) +{ + switch (flightMode) + { + case FLM_ACRO: return COPTER_MODE_ACRO; + case FLM_ACRO_AIR: return COPTER_MODE_ACRO; + case FLM_ANGLE: return COPTER_MODE_STABILIZE; + case FLM_HORIZON: return COPTER_MODE_STABILIZE; + case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; + case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return COPTER_MODE_GUIDED; + } else { + return COPTER_MODE_POSHOLD; + } + } + case FLM_RTH: return COPTER_MODE_RTL; + case FLM_MISSION: return COPTER_MODE_AUTO; + case FLM_LAUNCH: return COPTER_MODE_THROW; + case FLM_FAILSAFE: + { + if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { + return COPTER_MODE_RTL; + } else if (failsafePhase() == FAILSAFE_LANDING) { + return COPTER_MODE_LAND; + } else { + return COPTER_MODE_RTL; + } + } + default: return COPTER_MODE_STABILIZE; + } +} + +static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) +{ + switch (flightMode) + { + case FLM_MANUAL: return PLANE_MODE_MANUAL; + case FLM_ACRO: return PLANE_MODE_ACRO; + case FLM_ACRO_AIR: return PLANE_MODE_ACRO; + case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; + case FLM_HORIZON: return PLANE_MODE_STABILIZE; + case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; + case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return PLANE_MODE_GUIDED; + } else { + return PLANE_MODE_LOITER; + } + } + case FLM_RTH: return PLANE_MODE_RTL; + case FLM_MISSION: return PLANE_MODE_AUTO; + case FLM_CRUISE: return PLANE_MODE_CRUISE; + case FLM_LAUNCH: return PLANE_MODE_TAKEOFF; + case FLM_FAILSAFE: + { + if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { + return PLANE_MODE_RTL; + } else if (failsafePhase() == FAILSAFE_LANDING) { + return PLANE_MODE_AUTOLAND; + } else { + return PLANE_MODE_RTL; + } + } + default: return PLANE_MODE_MANUAL; + } +} + +static const mavlinkModeDescriptor_t planeModes[] = { + { PLANE_MODE_MANUAL, "MANUAL" }, + { PLANE_MODE_ACRO, "ACRO" }, + { PLANE_MODE_STABILIZE, "STABILIZE" }, + { PLANE_MODE_FLY_BY_WIRE_A, "FBWA" }, + { PLANE_MODE_FLY_BY_WIRE_B, "FBWB" }, + { PLANE_MODE_CRUISE, "CRUISE" }, + { PLANE_MODE_AUTO, "AUTO" }, + { PLANE_MODE_RTL, "RTL" }, + { PLANE_MODE_LOITER, "LOITER" }, + { PLANE_MODE_TAKEOFF, "TAKEOFF" }, + { PLANE_MODE_GUIDED, "GUIDED" }, +}; + +static const mavlinkModeDescriptor_t copterModes[] = { + { COPTER_MODE_ACRO, "ACRO" }, + { COPTER_MODE_STABILIZE, "STABILIZE" }, + { COPTER_MODE_ALT_HOLD, "ALT_HOLD" }, + { COPTER_MODE_POSHOLD, "POSHOLD" }, + { COPTER_MODE_LOITER, "LOITER" }, + { COPTER_MODE_AUTO, "AUTO" }, + { COPTER_MODE_GUIDED, "GUIDED" }, + { COPTER_MODE_RTL, "RTL" }, + { COPTER_MODE_LAND, "LAND" }, + { COPTER_MODE_BRAKE, "BRAKE" }, + { COPTER_MODE_THROW, "THROW" }, + { COPTER_MODE_SMART_RTL, "SMART_RTL" }, + { COPTER_MODE_DRIFT, "DRIFT" }, +}; + +static bool mavlinkPlaneModeIsConfigured(uint8_t customMode) +{ + switch ((APM_PLANE_MODE)customMode) { + case PLANE_MODE_MANUAL: + return isModeActivationConditionPresent(BOXMANUAL); + case PLANE_MODE_ACRO: + return true; + case PLANE_MODE_STABILIZE: + return isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXANGLEHOLD); + case PLANE_MODE_FLY_BY_WIRE_A: + return isModeActivationConditionPresent(BOXANGLE); + case PLANE_MODE_FLY_BY_WIRE_B: + return isModeActivationConditionPresent(BOXNAVALTHOLD); + case PLANE_MODE_CRUISE: + return isModeActivationConditionPresent(BOXNAVCRUISE) || + (isModeActivationConditionPresent(BOXNAVCOURSEHOLD) && + isModeActivationConditionPresent(BOXNAVALTHOLD)); + case PLANE_MODE_AUTO: + return isModeActivationConditionPresent(BOXNAVWP); + case PLANE_MODE_RTL: + return isModeActivationConditionPresent(BOXNAVRTH); + case PLANE_MODE_GUIDED: + return isModeActivationConditionPresent(BOXNAVPOSHOLD) && + isModeActivationConditionPresent(BOXGCSNAV); + case PLANE_MODE_LOITER: + return isModeActivationConditionPresent(BOXNAVPOSHOLD); + case PLANE_MODE_TAKEOFF: + return isModeActivationConditionPresent(BOXNAVLAUNCH); + default: + return false; + } +} + +static bool mavlinkCopterModeIsConfigured(uint8_t customMode) +{ + switch ((APM_COPTER_MODE)customMode) { + case COPTER_MODE_ACRO: + return true; + case COPTER_MODE_STABILIZE: + return isModeActivationConditionPresent(BOXANGLE) || + isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXANGLEHOLD); + case COPTER_MODE_ALT_HOLD: + return isModeActivationConditionPresent(BOXNAVALTHOLD); + case COPTER_MODE_GUIDED: + return isModeActivationConditionPresent(BOXNAVPOSHOLD) && + isModeActivationConditionPresent(BOXGCSNAV); + case COPTER_MODE_POSHOLD: + return isModeActivationConditionPresent(BOXNAVPOSHOLD); + case COPTER_MODE_RTL: + return isModeActivationConditionPresent(BOXNAVRTH); + case COPTER_MODE_AUTO: + return isModeActivationConditionPresent(BOXNAVWP); + case COPTER_MODE_THROW: + return isModeActivationConditionPresent(BOXNAVLAUNCH); + case COPTER_MODE_BRAKE: + return isModeActivationConditionPresent(BOXBRAKING); + default: + return false; + } +} + +static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom, + bool (*isModeConfigured)(uint8_t customMode)) +{ + uint8_t availableCount = 0; + for (uint8_t i = 0; i < count; i++) { + if (isModeConfigured(modes[i].customMode)) { + availableCount++; + } + } + + if (availableCount == 0) { + return; + } + + uint8_t modeIndex = 0; + for (uint8_t i = 0; i < count; i++) { + if (!isModeConfigured(modes[i].customMode)) { + continue; + } + + modeIndex++; + + mavlink_msg_available_modes_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + availableCount, + modeIndex, + MAV_STANDARD_MODE_NON_STANDARD, + modes[i].customMode, + 0, + modes[i].name); + + mavlinkSendMessage(); + + if (modes[i].customMode == currentCustom) { + const mavlinkModeSelection_t modeSelection = { + .flightMode = FLM_MANUAL, + .customMode = currentCustom, + }; + mavlinkSendCurrentMode(&modeSelection); + } + } +} + +bool mavlinkIsFixedWingVehicle(void) +{ + return STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE; +} + +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; + } +} + +uint8_t mavlinkGetAutopilotEnum(void) +{ + if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { + return MAV_AUTOPILOT_ARDUPILOTMEGA; + } + + return MAV_AUTOPILOT_GENERIC; +} + +mavlinkModeSelection_t mavlinkSelectMode(void) +{ + mavlinkModeSelection_t modeSelection; + modeSelection.flightMode = getFlightModeForTelemetry(); + + if (mavlinkIsFixedWingVehicle()) { + modeSelection.customMode = (uint8_t)inavToArduPlaneMap(modeSelection.flightMode); + } else { + modeSelection.customMode = (uint8_t)inavToArduCopterMap(modeSelection.flightMode); + } + + return modeSelection; +} + +void mavlinkSendAvailableModesForCurrentMode(void) +{ + const mavlinkModeSelection_t modeSelection = mavlinkSelectMode(); + + if (mavlinkIsFixedWingVehicle()) { + mavlinkSendAvailableModes(planeModes, (uint8_t)ARRAYLEN(planeModes), modeSelection.customMode, mavlinkPlaneModeIsConfigured); + } else { + mavlinkSendAvailableModes(copterModes, (uint8_t)ARRAYLEN(copterModes), modeSelection.customMode, mavlinkCopterModeIsConfigured); + } +} + +void mavlinkSendCurrentMode(const mavlinkModeSelection_t *modeSelection) +{ + mavlink_msg_current_mode_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + MAV_STANDARD_MODE_NON_STANDARD, + modeSelection->customMode, + modeSelection->customMode); + mavlinkSendMessage(); +} + +#endif diff --git a/src/main/mavlink/mavlink_modes.h b/src/main/mavlink/mavlink_modes.h new file mode 100644 index 00000000000..a793f4a834f --- /dev/null +++ b/src/main/mavlink/mavlink_modes.h @@ -0,0 +1,82 @@ +#pragma once + +#include +#include + +#include "fc/runtime_config.h" + +/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ +typedef enum APM_PLANE_MODE +{ + PLANE_MODE_MANUAL=0, + PLANE_MODE_CIRCLE=1, + PLANE_MODE_STABILIZE=2, + PLANE_MODE_TRAINING=3, + PLANE_MODE_ACRO=4, + PLANE_MODE_FLY_BY_WIRE_A=5, + PLANE_MODE_FLY_BY_WIRE_B=6, + PLANE_MODE_CRUISE=7, + PLANE_MODE_AUTOTUNE=8, + PLANE_MODE_AUTO=10, + PLANE_MODE_RTL=11, + PLANE_MODE_LOITER=12, + PLANE_MODE_TAKEOFF=13, + PLANE_MODE_AVOID_ADSB=14, + PLANE_MODE_GUIDED=15, + PLANE_MODE_INITIALIZING=16, + PLANE_MODE_QSTABILIZE=17, + PLANE_MODE_QHOVER=18, + PLANE_MODE_QLOITER=19, + PLANE_MODE_QLAND=20, + PLANE_MODE_QRTL=21, + PLANE_MODE_QAUTOTUNE=22, + 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. */ +typedef enum APM_COPTER_MODE +{ + COPTER_MODE_STABILIZE=0, + COPTER_MODE_ACRO=1, + COPTER_MODE_ALT_HOLD=2, + COPTER_MODE_AUTO=3, + COPTER_MODE_GUIDED=4, + COPTER_MODE_LOITER=5, + COPTER_MODE_RTL=6, + COPTER_MODE_CIRCLE=7, + COPTER_MODE_LAND=9, + COPTER_MODE_DRIFT=11, + COPTER_MODE_SPORT=13, + COPTER_MODE_FLIP=14, + COPTER_MODE_AUTOTUNE=15, + COPTER_MODE_POSHOLD=16, + COPTER_MODE_BRAKE=17, + COPTER_MODE_THROW=18, + COPTER_MODE_AVOID_ADSB=19, + COPTER_MODE_GUIDED_NOGPS=20, + COPTER_MODE_SMART_RTL=21, + 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; + +typedef struct mavlinkModeSelection_s { + flightModeForTelemetry_e flightMode; + uint8_t customMode; +} mavlinkModeSelection_t; + +bool mavlinkIsFixedWingVehicle(void); +uint8_t mavlinkGetVehicleType(void); +uint8_t mavlinkGetAutopilotEnum(void); +mavlinkModeSelection_t mavlinkSelectMode(void); +void mavlinkSendAvailableModesForCurrentMode(void); +void mavlinkSendCurrentMode(const mavlinkModeSelection_t *modeSelection); diff --git a/src/main/mavlink/mavlink_streams.c b/src/main/mavlink/mavlink_streams.c index 312df303137..15a6630f25c 100644 --- a/src/main/mavlink/mavlink_streams.c +++ b/src/main/mavlink/mavlink_streams.c @@ -1,5 +1,6 @@ #include "mavlink/mavlink_internal.h" +#include "mavlink/mavlink_modes.h" #include "mavlink/mavlink_runtime.h" #include "mavlink/mavlink_streams.h" @@ -238,118 +239,6 @@ void configureMAVLinkStreamRates(uint8_t portIndex) } } -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) - { - case FLM_ACRO: return COPTER_MODE_ACRO; - case FLM_ACRO_AIR: return COPTER_MODE_ACRO; - case FLM_ANGLE: return COPTER_MODE_STABILIZE; - case FLM_HORIZON: return COPTER_MODE_STABILIZE; - case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; - case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; - case FLM_POSITION_HOLD: - { - if (isGCSValid()) { - return COPTER_MODE_GUIDED; - } else { - return COPTER_MODE_POSHOLD; - } - } - case FLM_RTH: return COPTER_MODE_RTL; - case FLM_MISSION: return COPTER_MODE_AUTO; - case FLM_LAUNCH: return COPTER_MODE_THROW; - case FLM_FAILSAFE: - { - if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { - return COPTER_MODE_RTL; - } else if (failsafePhase() == FAILSAFE_LANDING) { - return COPTER_MODE_LAND; - } else { - return COPTER_MODE_RTL; - } - } - default: return COPTER_MODE_STABILIZE; - } -} - -static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) -{ - switch (flightMode) - { - case FLM_MANUAL: return PLANE_MODE_MANUAL; - case FLM_ACRO: return PLANE_MODE_ACRO; - case FLM_ACRO_AIR: return PLANE_MODE_ACRO; - case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; - case FLM_HORIZON: return PLANE_MODE_STABILIZE; - case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; - case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; - case FLM_POSITION_HOLD: - { - if (isGCSValid()) { - return PLANE_MODE_GUIDED; - } else { - return PLANE_MODE_LOITER; - } - } - case FLM_RTH: return PLANE_MODE_RTL; - case FLM_MISSION: return PLANE_MODE_AUTO; - case FLM_CRUISE: return PLANE_MODE_CRUISE; - case FLM_LAUNCH: return PLANE_MODE_TAKEOFF; - case FLM_FAILSAFE: - { - if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { - return PLANE_MODE_RTL; - } else if (failsafePhase() == FAILSAFE_LANDING) { - return PLANE_MODE_AUTOLAND; - } else { - return PLANE_MODE_RTL; - } - } - 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; -} - void mavlinkSendAutopilotVersion(void) { if (mavlinkGetProtocolVersion() == 1) { @@ -414,149 +303,6 @@ void mavlinkSendProtocolVersion(void) mavlinkSendMessage(); } -static const mavlinkModeDescriptor_t planeModes[] = { - { PLANE_MODE_MANUAL, "MANUAL" }, - { PLANE_MODE_ACRO, "ACRO" }, - { PLANE_MODE_STABILIZE, "STABILIZE" }, - { PLANE_MODE_FLY_BY_WIRE_A,"FBWA" }, - { PLANE_MODE_FLY_BY_WIRE_B,"FBWB" }, - { PLANE_MODE_CRUISE, "CRUISE" }, - { PLANE_MODE_AUTO, "AUTO" }, - { PLANE_MODE_RTL, "RTL" }, - { PLANE_MODE_LOITER, "LOITER" }, - { PLANE_MODE_TAKEOFF, "TAKEOFF" }, - { PLANE_MODE_GUIDED, "GUIDED" }, -}; - -static const mavlinkModeDescriptor_t copterModes[] = { - { COPTER_MODE_ACRO, "ACRO" }, - { COPTER_MODE_STABILIZE, "STABILIZE" }, - { COPTER_MODE_ALT_HOLD, "ALT_HOLD" }, - { COPTER_MODE_POSHOLD, "POSHOLD" }, - { COPTER_MODE_LOITER, "LOITER" }, - { COPTER_MODE_AUTO, "AUTO" }, - { COPTER_MODE_GUIDED, "GUIDED" }, - { COPTER_MODE_RTL, "RTL" }, - { COPTER_MODE_LAND, "LAND" }, - { COPTER_MODE_BRAKE, "BRAKE" }, - { COPTER_MODE_THROW, "THROW" }, - { COPTER_MODE_SMART_RTL, "SMART_RTL" }, - { COPTER_MODE_DRIFT, "DRIFT" }, -}; - -static bool mavlinkPlaneModeIsConfigured(uint8_t customMode) -{ - switch ((APM_PLANE_MODE)customMode) { - case PLANE_MODE_MANUAL: - return isModeActivationConditionPresent(BOXMANUAL); - case PLANE_MODE_ACRO: - return true; - case PLANE_MODE_STABILIZE: - return isModeActivationConditionPresent(BOXHORIZON) || - isModeActivationConditionPresent(BOXANGLEHOLD); - case PLANE_MODE_FLY_BY_WIRE_A: - return isModeActivationConditionPresent(BOXANGLE); - case PLANE_MODE_FLY_BY_WIRE_B: - return isModeActivationConditionPresent(BOXNAVALTHOLD); - case PLANE_MODE_CRUISE: - return isModeActivationConditionPresent(BOXNAVCRUISE) || - (isModeActivationConditionPresent(BOXNAVCOURSEHOLD) && - isModeActivationConditionPresent(BOXNAVALTHOLD)); - case PLANE_MODE_AUTO: - return isModeActivationConditionPresent(BOXNAVWP); - case PLANE_MODE_RTL: - return isModeActivationConditionPresent(BOXNAVRTH); - case PLANE_MODE_GUIDED: - return isModeActivationConditionPresent(BOXNAVPOSHOLD) && - isModeActivationConditionPresent(BOXGCSNAV); - case PLANE_MODE_LOITER: - return isModeActivationConditionPresent(BOXNAVPOSHOLD); - case PLANE_MODE_TAKEOFF: - return isModeActivationConditionPresent(BOXNAVLAUNCH); - default: - return false; - } -} - -static bool mavlinkCopterModeIsConfigured(uint8_t customMode) -{ - switch ((APM_COPTER_MODE)customMode) { - case COPTER_MODE_ACRO: - return true; - case COPTER_MODE_STABILIZE: - return isModeActivationConditionPresent(BOXANGLE) || - isModeActivationConditionPresent(BOXHORIZON) || - isModeActivationConditionPresent(BOXANGLEHOLD); - case COPTER_MODE_ALT_HOLD: - return isModeActivationConditionPresent(BOXNAVALTHOLD); - case COPTER_MODE_GUIDED: - return isModeActivationConditionPresent(BOXNAVPOSHOLD) && - isModeActivationConditionPresent(BOXGCSNAV); - case COPTER_MODE_POSHOLD: - return isModeActivationConditionPresent(BOXNAVPOSHOLD); - case COPTER_MODE_RTL: - return isModeActivationConditionPresent(BOXNAVRTH); - case COPTER_MODE_AUTO: - return isModeActivationConditionPresent(BOXNAVWP); - case COPTER_MODE_THROW: - return isModeActivationConditionPresent(BOXNAVLAUNCH); - case COPTER_MODE_BRAKE: - return isModeActivationConditionPresent(BOXBRAKING); - default: - return false; - } -} - -static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom, - bool (*isModeConfigured)(uint8_t customMode)) -{ - uint8_t availableCount = 0; - for (uint8_t i = 0; i < count; i++) { - if (isModeConfigured(modes[i].customMode)) { - availableCount++; - } - } - - if (availableCount == 0) { - return; - } - - uint8_t modeIndex = 0; - for (uint8_t i = 0; i < count; i++) { - if (!isModeConfigured(modes[i].customMode)) { - continue; - } - - modeIndex++; - const uint8_t stdMode = MAV_STANDARD_MODE_NON_STANDARD; - const uint32_t properties = 0; - - mavlink_msg_available_modes_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - availableCount, - modeIndex, - stdMode, - modes[i].customMode, - properties, - modes[i].name); - - mavlinkSendMessage(); - - if (modes[i].customMode == currentCustom) { - mavlink_msg_current_mode_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - stdMode, - currentCustom, - currentCustom); - mavlinkSendMessage(); - } - } -} - void mavlinkSendExtendedSysState(void) { uint8_t vtolState = MAV_VTOL_STATE_UNDEFINED; @@ -929,14 +675,12 @@ void mavlinkSendVfrHud(void) mavlinkSendMessage(); } -static uint8_t mavlinkGetAutopilotEnum(void); - void mavlinkSendHeartbeat(void) { uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - const bool isPlane = (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); + const bool isPlane = mavlinkIsFixedWingVehicle(); + const mavlinkModeSelection_t modeSelection = mavlinkSelectMode(); flightModeForTelemetry_e flm = modeSelection.flightMode; uint8_t mavCustomMode = modeSelection.customMode; uint8_t mavSystemType; @@ -1094,15 +838,6 @@ void mavlinkSendBatteryTemperatureStatusText(void) mavlinkSendStatusText(); } -static uint8_t mavlinkGetAutopilotEnum(void) -{ - if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { - return MAV_AUTOPILOT_ARDUPILOTMEGA; - } - - return MAV_AUTOPILOT_GENERIC; -} - static uint8_t mavlinkHeadingDeg2FromCd(int32_t headingCd) { return (uint8_t)(wrap_36000(headingCd) / 200); @@ -1162,8 +897,7 @@ static uint16_t mavlinkComputeHighLatencyFailures(void) void mavlinkSendHighLatency2(timeUs_t currentTimeUs) { - const bool isPlane = (STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); + const mavlinkModeSelection_t modeSelection = mavlinkSelectMode(); int32_t latitude = 0; int32_t longitude = 0; @@ -1306,28 +1040,12 @@ bool mavlinkSendRequestedMessage(uint16_t messageId) mavlinkSendVfrHud(); return true; case MAVLINK_MSG_ID_AVAILABLE_MODES: - { - const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); - if (isPlane) { - mavlinkSendAvailableModes(planeModes, (uint8_t)ARRAYLEN(planeModes), modeSelection.customMode, mavlinkPlaneModeIsConfigured); - } else { - mavlinkSendAvailableModes(copterModes, (uint8_t)ARRAYLEN(copterModes), modeSelection.customMode, mavlinkCopterModeIsConfigured); - } - return true; - } + mavlinkSendAvailableModesForCurrentMode(); + return true; case MAVLINK_MSG_ID_CURRENT_MODE: { - const bool isPlane = mixerConfig()->platformType == PLATFORM_AIRPLANE || STATE(FIXED_WING_LEGACY); - const mavlinkModeSelection_t modeSelection = selectMavlinkMode(isPlane); - mavlink_msg_current_mode_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - MAV_STANDARD_MODE_NON_STANDARD, - modeSelection.customMode, - modeSelection.customMode); - mavlinkSendMessage(); + const mavlinkModeSelection_t modeSelection = mavlinkSelectMode(); + mavlinkSendCurrentMode(&modeSelection); return true; } case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: diff --git a/src/main/mavlink/mavlink_types.h b/src/main/mavlink/mavlink_types.h index c050d319392..342479815ba 100644 --- a/src/main/mavlink/mavlink_types.h +++ b/src/main/mavlink/mavlink_types.h @@ -61,84 +61,12 @@ typedef enum { MAVLINK_PERIODIC_MESSAGE_COUNT } mavlinkPeriodicMessage_e; -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. * This converts angles from a range of 0..Pi to -Pi..Pi */ #define RADIANS_TO_MAVLINK_RANGE(angle) (((angle) > M_PIf) ? ((angle) - (2 * M_PIf)) : (angle)) -/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ -typedef enum APM_PLANE_MODE -{ - PLANE_MODE_MANUAL=0, - PLANE_MODE_CIRCLE=1, - PLANE_MODE_STABILIZE=2, - PLANE_MODE_TRAINING=3, - PLANE_MODE_ACRO=4, - PLANE_MODE_FLY_BY_WIRE_A=5, - PLANE_MODE_FLY_BY_WIRE_B=6, - PLANE_MODE_CRUISE=7, - PLANE_MODE_AUTOTUNE=8, - PLANE_MODE_AUTO=10, - PLANE_MODE_RTL=11, - PLANE_MODE_LOITER=12, - PLANE_MODE_TAKEOFF=13, - PLANE_MODE_AVOID_ADSB=14, - PLANE_MODE_GUIDED=15, - PLANE_MODE_INITIALIZING=16, - PLANE_MODE_QSTABILIZE=17, - PLANE_MODE_QHOVER=18, - PLANE_MODE_QLOITER=19, - PLANE_MODE_QLAND=20, - PLANE_MODE_QRTL=21, - PLANE_MODE_QAUTOTUNE=22, - 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. */ -typedef enum APM_COPTER_MODE -{ - COPTER_MODE_STABILIZE=0, - COPTER_MODE_ACRO=1, - COPTER_MODE_ALT_HOLD=2, - COPTER_MODE_AUTO=3, - COPTER_MODE_GUIDED=4, - COPTER_MODE_LOITER=5, - COPTER_MODE_RTL=6, - COPTER_MODE_CIRCLE=7, - COPTER_MODE_LAND=9, - COPTER_MODE_DRIFT=11, - COPTER_MODE_SPORT=13, - COPTER_MODE_FLIP=14, - COPTER_MODE_AUTOTUNE=15, - COPTER_MODE_POSHOLD=16, - COPTER_MODE_BRAKE=17, - COPTER_MODE_THROW=18, - COPTER_MODE_AVOID_ADSB=19, - COPTER_MODE_GUIDED_NOGPS=20, - COPTER_MODE_SMART_RTL=21, - 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; - typedef struct mavlinkRouteEntry_s { uint8_t sysid; uint8_t compid; @@ -165,20 +93,3 @@ typedef struct mavlinkPortRuntime_s { mavlink_message_t mavRecvMsg; mavlink_status_t mavRecvStatus; } mavlinkPortRuntime_t; - -typedef struct mavlinkModeDescriptor_s { - uint8_t customMode; - const char *name; -} mavlinkModeDescriptor_t; - -typedef struct mavlinkMissionItemData_s { - uint8_t frame; - uint16_t command; - float param1; - float param2; - float param3; - float param4; - int32_t lat; - int32_t lon; - float alt; -} mavlinkMissionItemData_t; diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 04527fd7046..e8de17e9959 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -44,7 +44,7 @@ set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial 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 - "fc/fc_mavlink.c" "mavlink/mavlink_ports.c" "mavlink/mavlink_routing.c" "mavlink/mavlink_runtime.c" + "fc/fc_mavlink.c" "mavlink/mavlink_mission.c" "mavlink/mavlink_modes.c" "mavlink/mavlink_ports.c" "mavlink/mavlink_routing.c" "mavlink/mavlink_runtime.c" "mavlink/mavlink_streams.c" "telemetry/mavlink.c" "common/crc.c" "common/maths.c" "common/streambuf.c" "msp/msp_serial.c") set_property(SOURCE mavlink_unittest.cc PROPERTY definitions USE_TELEMETRY USE_TELEMETRY_MAVLINK) set_property(SOURCE mavlink_unittest.cc PROPERTY includes From 6e20c117cd03c6d4e81b633a3ee67bef4bac0dcc Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Fri, 3 Apr 2026 18:36:46 -0400 Subject: [PATCH 38/42] Fix MAVLink user docs --- docs/Mavlink.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/Mavlink.md b/docs/Mavlink.md index 5194881c29f..7f02c00151b 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -51,7 +51,7 @@ Ports 2..N use a secondary startup profile (heartbeat at 1 Hz, other streams dis | `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 | +| `EXTRA1` | `ATTITUDE` | 2 Hz | | `EXTRA2` | `VFR_HUD` | 2 Hz | | `HEARTBEAT` | `HEARTBEAT` | 1 Hz (independent of stream groups) | | `EXT_SYS_STATE` | `EXTENDED_SYS_STATE` | 1 Hz (defaults to `mavlink_port1_extra3_rate`) | @@ -113,12 +113,14 @@ 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`: changes the current altitude target. `param1` is the target altitude in meters and `param2` is interpreted as the MAVLink frame (`MAV_FRAME_GLOBAL`, `MAV_FRAME_GLOBAL_INT`, `MAV_FRAME_GLOBAL_RELATIVE_ALT`, `MAV_FRAME_GLOBAL_RELATIVE_ALT_INT`); unsupported frames are rejected. - `MAV_CMD_CONDITION_YAW`: changes the current heading target when the active navigation state has yaw control. Accepts absolute heading (`param4=0`) and relative turns (`param4!=0`); turn-rate is ignored. - `MAV_CMD_SET_MESSAGE_INTERVAL` / `MAV_CMD_GET_MESSAGE_INTERVAL`: adjust or query per-message periodic output for `HEARTBEAT`, `SYS_STATUS`, `EXTENDED_SYS_STATE`, RC channels, `GPS_RAW_INT`, `GLOBAL_POSITION_INT`, `GPS_GLOBAL_ORIGIN`, `ATTITUDE`, `VFR_HUD`, `BATTERY_STATUS`, `SCALED_PRESSURE`, and `SYSTEM_TIME`. `REQUEST_DATA_STREAM` still controls the legacy base stream groups; `SET_MESSAGE_INTERVAL` overrides individual messages on top. - `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_RAW_INT`, `GLOBAL_POSITION_INT`, `GPS_GLOBAL_ORIGIN`, `BATTERY_STATUS`, `SCALED_PRESSURE`, `SYSTEM_TIME`, and `HOME_POSITION` when available) or `UNSUPPORTED`. - `MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES`: returns stub `AUTOPILOT_VERSION` (v2 only; v1 returns `UNSUPPORTED`) advertising `SET_POSITION_TARGET_GLOBAL_INT` and `SET_POSITION_TARGET_LOCAL_NED`. - `MAV_CMD_REQUEST_PROTOCOL_VERSION`: returns stub `PROTOCOL_VERSION` (v2 only; v1 returns `UNSUPPORTED`). +- `MAV_CMD_CONTROL_HIGH_LATENCY`: enables or disables `HIGH_LATENCY2` scheduling on the ingress MAVLink port (`param1` = `0` or `1`). Enabling is rejected on MAVLink1 links. ## Mode mappings (INAV → MAVLink/ArduPilot) From a412f8cc93d4fb9ef35d55adc052552edfcfb59c Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Fri, 3 Apr 2026 18:48:17 -0400 Subject: [PATCH 39/42] Fix MAVLink half-duplex RX backoff Scope MAVLink half-duplex to serial RX --- docs/Mavlink.md | 5 +++-- src/main/mavlink/mavlink_ports.c | 2 ++ src/main/mavlink/mavlink_runtime.c | 27 +++++++++++++++++++-------- src/main/mavlink/mavlink_types.h | 1 + 4 files changed, 25 insertions(+), 10 deletions(-) diff --git a/docs/Mavlink.md b/docs/Mavlink.md index 7f02c00151b..75fb61fa47a 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -11,11 +11,12 @@ INAV supports up to 4 concurrent MAVLink telemetry ports (`MAX_MAVLINK_PORTS`), - **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. +- **Half‑duplex etiquette**: on a MAVLink serial RX port configured for `serialrx_halfduplex`, INAV waits one telemetry tick after any received frame before transmitting to reduce collisions. ### Usage guidance -- If you rely on RC via MAVLink, ensure the serial receiver type is set to `SERIALRX_MAVLINK` and consider enabling `telemetry_halfduplex` when RX shares the port. +- If you rely on RC via MAVLink, ensure the serial receiver type is set to `SERIALRX_MAVLINK`. +- If MAVLink RX and telemetry intentionally share one half-duplex wire, enable `serialrx_halfduplex` for that setup. - To reduce bandwidth, lower the stream rates for groups you do not need, or disable them entirely by setting the rate to 0. - If a GCS or companion needs telemetry on ports 2..4, explicitly request streams (`REQUEST_DATA_STREAM` or `MAV_CMD_SET_MESSAGE_INTERVAL`) because only heartbeat is enabled by default. - If you depend on directed forwarding between links, ensure each remote endpoint transmits at least one frame early so route learning is populated. diff --git a/src/main/mavlink/mavlink_ports.c b/src/main/mavlink/mavlink_ports.c index 9d499071a51..c3b9f81706e 100644 --- a/src/main/mavlink/mavlink_ports.c +++ b/src/main/mavlink/mavlink_ports.c @@ -19,6 +19,7 @@ void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) state->txbuffValid = false; state->txbuffFree = 100; state->lastMavlinkMessageUs = 0; + state->lastRxFrameUs = 0; state->lastHighLatencyMessageUs = 0; state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; state->txSeq = 0; @@ -56,6 +57,7 @@ void configureMAVLinkTelemetryPort(uint8_t portIndex) state->txbuffValid = false; state->txbuffFree = 100; state->lastMavlinkMessageUs = 0; + state->lastRxFrameUs = 0; state->lastHighLatencyMessageUs = 0; state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; state->txSeq = 0; diff --git a/src/main/mavlink/mavlink_runtime.c b/src/main/mavlink/mavlink_runtime.c index 0c780e0f7f3..e4c77a68d3d 100644 --- a/src/main/mavlink/mavlink_runtime.c +++ b/src/main/mavlink/mavlink_runtime.c @@ -171,7 +171,7 @@ void mavlinkSendMessage(void) } } -static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) +static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex, timeUs_t currentTimeUs) { mavlinkPortRuntime_t *state = &mavPortStates[ingressPortIndex]; @@ -180,6 +180,7 @@ static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) const char c = serialRead(state->port); const uint8_t result = mavlink_parse_char(ingressPortIndex, c, &state->mavRecvMsg, &state->mavRecvStatus); if (result == MAVLINK_FRAMING_OK) { + state->lastRxFrameUs = currentTimeUs; mavlinkContext.recvMsg = state->mavRecvMsg; if (mavlinkIsFromLocalIdentity(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid)) { @@ -234,10 +235,15 @@ static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex) return false; } -static bool isMAVLinkTelemetryHalfDuplex(void) +static bool isMAVLinkTelemetryHalfDuplex(uint8_t portIndex) { - return telemetryConfig()->halfDuplex || - (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK && tristateWithDefaultOffIsActive(rxConfig()->halfDuplex)); + const mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + return state->portConfig && + (state->portConfig->functionMask & FUNCTION_RX_SERIAL) && + rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK && + tristateWithDefaultOffIsActive(rxConfig()->halfDuplex); } void mavlinkRuntimeHandle(timeUs_t currentTimeUs) @@ -251,19 +257,24 @@ void mavlinkRuntimeHandle(timeUs_t currentTimeUs) mavlinkSetActivePortContext(portIndex); // Process incoming MAVLink on this port and forward when needed. - const bool receivedMessage = processMAVLinkIncomingTelemetry(portIndex); + processMAVLinkIncomingTelemetry(portIndex, currentTimeUs); // Restore context back to this port before periodic send decisions. mavlinkSetActivePortContext(portIndex); bool shouldSendTelemetry = false; + const bool halfDuplexBackoff = isMAVLinkTelemetryHalfDuplex(portIndex) && + ((currentTimeUs - state->lastRxFrameUs) < TELEMETRY_MAVLINK_DELAY); + + if (halfDuplexBackoff) { + continue; + } if (state->txbuffValid) { // Use flow control if available. shouldSendTelemetry = state->txbuffFree >= mavActiveConfig->min_txbuff; } else { - // If not, use blind frame pacing and half-duplex backoff after RX activity. - const bool halfDuplexBackoff = isMAVLinkTelemetryHalfDuplex() && receivedMessage; - shouldSendTelemetry = ((currentTimeUs - state->lastMavlinkMessageUs) >= TELEMETRY_MAVLINK_DELAY) && !halfDuplexBackoff; + // If not, use blind frame pacing. + shouldSendTelemetry = (currentTimeUs - state->lastMavlinkMessageUs) >= TELEMETRY_MAVLINK_DELAY; } if (!shouldSendTelemetry) { diff --git a/src/main/mavlink/mavlink_types.h b/src/main/mavlink/mavlink_types.h index 342479815ba..70a2e5d1157 100644 --- a/src/main/mavlink/mavlink_types.h +++ b/src/main/mavlink/mavlink_types.h @@ -81,6 +81,7 @@ typedef struct mavlinkPortRuntime_s { bool txbuffValid; uint8_t txbuffFree; timeUs_t lastMavlinkMessageUs; + timeUs_t lastRxFrameUs; timeUs_t lastHighLatencyMessageUs; bool highLatencyEnabled; uint8_t mavRates[MAVLINK_STREAM_COUNT]; From 756c03174548c3a019632cc0607458822dc5312b Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Fri, 3 Apr 2026 19:09:02 -0400 Subject: [PATCH 40/42] Add MAVLink mission reached message --- docs/Mavlink.md | 2 +- src/main/mavlink/mavlink_mission.c | 24 ++++++++++++++++ src/main/mavlink/mavlink_mission.h | 1 + src/main/mavlink/mavlink_runtime.c | 3 ++ src/main/navigation/navigation.c | 33 ++++++++++++++++++++++ src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_private.h | 2 ++ src/test/unit/mavlink_unittest.cc | 36 ++++++++++++++++++++++++ 8 files changed, 101 insertions(+), 1 deletion(-) diff --git a/docs/Mavlink.md b/docs/Mavlink.md index 75fb61fa47a..e115abb6cc5 100644 --- a/docs/Mavlink.md +++ b/docs/Mavlink.md @@ -89,7 +89,7 @@ Messages are organized into MAVLink datastream groups. Each group sends **one me - `SCALED_PRESSURE`: for IMU/baro temperature. - `SYSTEM_TIME`: with `time_boot_ms = millis()` and `time_unix_usec = 0`. - `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`. +- Event/on-demand messages: `MISSION_ITEM_REACHED` when a mission item is reached, plus `AUTOPILOT_VERSION`, `PROTOCOL_VERSION`, `MESSAGE_INTERVAL`, `HOME_POSITION`, `AVAILABLE_MODES`, and `CURRENT_MODE`. ## Supported Incoming Messages diff --git a/src/main/mavlink/mavlink_mission.c b/src/main/mavlink/mavlink_mission.c index 6ed125c3060..368dd39bfc9 100644 --- a/src/main/mavlink/mavlink_mission.c +++ b/src/main/mavlink/mavlink_mission.c @@ -5,6 +5,30 @@ #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) +void mavlinkSendPendingMissionItemReached(void) +{ + uint16_t seq; + if (!navigationConsumeWaypointReached(&seq)) { + return; + } + + uint8_t sendMask = 0; + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if (mavPortStates[portIndex].telemetryEnabled && mavPortStates[portIndex].port) { + sendMask |= MAVLINK_PORT_MASK(portIndex); + } + } + + if (sendMask == 0) { + return; + } + + mavSendMask = sendMask; + mavlink_msg_mission_item_reached_pack(mavlinkGetCommonConfig()->sysid, MAV_COMP_ID_AUTOPILOT1, &mavSendMsg, seq); + mavlinkSendMessage(); + mavSendMask = 0; +} + bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask) { switch (frame) { diff --git a/src/main/mavlink/mavlink_mission.h b/src/main/mavlink/mavlink_mission.h index d8139dfbc96..650bdd06338 100644 --- a/src/main/mavlink/mavlink_mission.h +++ b/src/main/mavlink/mavlink_mission.h @@ -32,6 +32,7 @@ bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame); MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters); uint8_t mavlinkWaypointFrame(const navWaypoint_t *wp, bool useIntMessages); bool mavlinkFillMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item); +void mavlinkSendPendingMissionItemReached(void); bool mavlinkHandleIncomingMissionClearAll(void); bool mavlinkHandleIncomingMissionCount(void); diff --git a/src/main/mavlink/mavlink_runtime.c b/src/main/mavlink/mavlink_runtime.c index e4c77a68d3d..6905f0a0209 100644 --- a/src/main/mavlink/mavlink_runtime.c +++ b/src/main/mavlink/mavlink_runtime.c @@ -2,6 +2,7 @@ #include "fc/fc_mavlink.h" +#include "mavlink/mavlink_mission.h" #include "mavlink/mavlink_ports.h" #include "mavlink/mavlink_routing.h" #include "mavlink/mavlink_runtime.h" @@ -248,6 +249,8 @@ static bool isMAVLinkTelemetryHalfDuplex(uint8_t portIndex) void mavlinkRuntimeHandle(timeUs_t currentTimeUs) { + mavlinkSendPendingMissionItemReached(); + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; if (!state->telemetryEnabled || !state->port) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 55d6ccb8af7..33bd9ba4802 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2075,6 +2075,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } +static void navMarkWaypointReached(int8_t waypointIndex); + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState) { UNUSED(previousState); @@ -2103,12 +2105,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga case NAV_WP_ACTION_HOLD_TIME: // Save the current time for the time the waypoint was reached posControl.wpReachedTime = millis(); + navMarkWaypointReached(posControl.activeWaypointIndex); return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME; } UNREACHABLE(); } +static void navMarkWaypointReached(int8_t waypointIndex) +{ + if (waypointIndex < posControl.startWpIndex) { + return; + } + + posControl.wpReachedSeq = (uint16_t)(waypointIndex - posControl.startWpIndex); + posControl.wpReachedNotificationPending = true; +} + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState) { UNUSED(previousState); @@ -2126,6 +2139,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi if (posControl.wpAltitudeReached) { posControl.wpReachedTime = millis(); + navMarkWaypointReached(posControl.activeWaypointIndex); } else { return NAV_FSM_EVENT_NONE; } @@ -2165,6 +2179,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigatio { UNUSED(previousState); + if (!posControl.wpReachedNotificationPending) { + navMarkWaypointReached(posControl.activeWaypointIndex); + } + if (isLastMissionWaypoint()) { // Last waypoint reached return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; } @@ -3953,6 +3971,7 @@ void resetWaypointList(void) posControl.waypointListValid = false; posControl.geoWaypointCount = 0; posControl.startWpIndex = 0; + posControl.wpReachedNotificationPending = false; #ifdef USE_MULTI_MISSION posControl.totalMultiMissionWpCount = 0; posControl.loadedMultiMissionIndex = 0; @@ -4015,6 +4034,7 @@ void loadSelectedMultiMission(uint8_t missionIndex) posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0; posControl.activeWaypointIndex = posControl.startWpIndex; + posControl.wpReachedNotificationPending = false; } bool updateWpMissionChange(void) @@ -5011,6 +5031,8 @@ void navigationInit(void) posControl.waypointCount = 0; posControl.activeWaypointIndex = 0; posControl.waypointListValid = false; + posControl.wpReachedSeq = 0; + posControl.wpReachedNotificationPending = false; posControl.wpPlannerActiveWPIndex = 0; posControl.flags.wpMissionPlannerActive = false; posControl.startWpIndex = 0; @@ -5282,6 +5304,17 @@ const navigationPIDControllers_t* getNavigationPIDControllers(void) { return &posControl.pids; } +bool navigationConsumeWaypointReached(uint16_t *seq) +{ + if (!posControl.wpReachedNotificationPending) { + return false; + } + + *seq = posControl.wpReachedSeq; + posControl.wpReachedNotificationPending = false; + return true; +} + bool isAdjustingPosition(void) { return posControl.flags.isAdjustingPosition; } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 810fdc26ef8..572b8ff2cde 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -781,6 +781,7 @@ bool navigationIsFlyingAutonomousMode(void); bool navigationIsExecutingAnEmergencyLanding(void); bool navigationIsControllingAltitude(void); bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm); +bool navigationConsumeWaypointReached(uint16_t *seq); /* Returns true if navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. */ diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index f6cf9329e98..b838e6e88ee 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -497,6 +497,8 @@ typedef struct { float wpDistance; // Distance to active WP timeMs_t wpReachedTime; // Time the waypoint was reached bool wpAltitudeReached; // WP altitude achieved + uint16_t wpReachedSeq; // Last reached mission item sequence relative to startWpIndex + bool wpReachedNotificationPending; #ifdef USE_FW_AUTOLAND /* Fixedwing autoland */ diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index 846ed7efd0f..ba62200e3c7 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -302,6 +302,8 @@ static void initMavlinkTestState(void) memset(&GPS_home, 0, sizeof(GPS_home)); memset(waypointStore, 0, sizeof(waypointStore)); memset(&rxLinkStatistics, 0, sizeof(rxLinkStatistics)); + posControl.wpReachedSeq = 0; + posControl.wpReachedNotificationPending = false; telemetryConfigMutable()->mavlink_common.sysid = 1; telemetryConfigMutable()->mavlink_common.autopilot_type = MAVLINK_AUTOPILOT_ARDUPILOT; @@ -909,6 +911,29 @@ TEST(MavlinkTelemetryTest, MissionRequestSendsWaypoint) EXPECT_NEAR(item.z, 12.34f, 1e-4f); } +TEST(MavlinkTelemetryTest, MissionItemReachedIsBroadcastOnceWhenPending) +{ + initMavlinkTestState(); + + posControl.wpReachedSeq = 3; + posControl.wpReachedNotificationPending = true; + + handleMAVLinkTelemetry(1000); + + mavlink_message_t reachedMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &reachedMsg)); + + mavlink_mission_item_reached_t reached; + mavlink_msg_mission_item_reached_decode(&reachedMsg, &reached); + + EXPECT_EQ(reached.seq, 3); + + resetSerialBuffers(); + handleMAVLinkTelemetry(1000); + + EXPECT_FALSE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &reachedMsg)); +} + TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesAbsoluteAltitude) { initMavlinkTestState(); @@ -2035,6 +2060,17 @@ bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int3 return altitudeTargetSetResult; } +bool navigationConsumeWaypointReached(uint16_t *seq) +{ + if (!posControl.wpReachedNotificationPending) { + return false; + } + + *seq = posControl.wpReachedSeq; + posControl.wpReachedNotificationPending = false; + return true; +} + navigationFSMStateFlags_t navGetCurrentStateFlags(void) { return (navigationFSMStateFlags_t)0; From 4a433a614485e6f0bba12acf8cdf773d17759352 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Sat, 4 Apr 2026 13:57:00 -0400 Subject: [PATCH 41/42] Lightened fc_mavlink + command/guided files + small refactor --- docs/development/msp/README.md | 14 +- docs/development/msp/inav_enums.json | 121 +++---- docs/development/msp/inav_enums_ref.md | 129 +++---- docs/development/msp/msp_messages.checksum | 2 +- src/main/CMakeLists.txt | 4 + src/main/fc/fc_mavlink.c | 385 +-------------------- src/main/mavlink/mavlink_command.c | 245 +++++++++++++ src/main/mavlink/mavlink_command.h | 7 + src/main/mavlink/mavlink_guided.c | 153 ++++++++ src/main/mavlink/mavlink_guided.h | 13 + src/main/mavlink/mavlink_mission.c | 240 ++++++------- src/main/mavlink/mavlink_mission.h | 4 - src/main/mavlink/mavlink_streams.c | 62 ++++ src/main/mavlink/mavlink_streams.h | 2 + 14 files changed, 745 insertions(+), 636 deletions(-) create mode 100644 src/main/mavlink/mavlink_command.c create mode 100644 src/main/mavlink/mavlink_command.h create mode 100644 src/main/mavlink/mavlink_guided.c create mode 100644 src/main/mavlink/mavlink_guided.h diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index b0f871fa65a..a573591766b 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -66,6 +66,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **variable_len**: Optional boolean, if true, message does not have a predefined fixed length and needs appropriate handling\ **variants**: Optional special case, message has different cases of reply/request. Key/description is not a strict expression or code; just a readable condition\ **not_implemented**: Optional special case, message is not implemented (never or deprecated)\ +**replaced_by**: Optional array of MSP message names that replace this command. Present when a command is deprecated and scheduled for removal. Empty array if no replacement is needed\ **notes**: String with details of message ## Data dict fields: @@ -419,8 +420,8 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i [8730 - MSP2_INAV_SET_GLOBAL_TARGET](#msp2_inav_set_global_target) [8731 - MSP2_INAV_NAV_TARGET](#msp2_inav_nav_target) [8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) -[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) -[12289 - MSP2_RX_BIND](#msp2_rx_bind) +[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) +[12289 - MSP2_RX_BIND](#msp2_rx_bind) ## `MSP_API_VERSION (1 / 0x1)` **Description:** Provides the MSP protocol version and the INAV API version. @@ -2213,7 +2214,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i | `armingFlags` | `uint16_t` | 2 | Bitmask | Bitmask: Flight controller arming flags (`armingFlags`). Note: Truncated to 16 bits | | `accCalibAxisFlags` | `uint8_t` | 1 | Bitmask | Bitmask: Accelerometer calibrated axes flags (`accGetCalibrationAxisFlags()`) | -**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. +**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. The `accCalibAxisFlags` field is not present in `MSP2_INAV_STATUS` but is available via `MSP_CALIBRATION_DATA`. ## `MSP_SENSOR_STATUS (151 / 0x97)` **Description:** Provides the hardware status for each individual sensor system. @@ -2280,6 +2281,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i | `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) | | `eph` | `uint16_t` | 2 | cm | Estimated Horizontal Position Accuracy (`gpsSol.eph`) | | `epv` | `uint16_t` | 2 | cm | Estimated Vertical Position Accuracy (`gpsSol.epv`) | +| `hwVersion` | `uint32_t` | 4 | Version code | GPS hardware version (`gpsState.hwVersion`). Values: 500=UBLOX5, 600=UBLOX6, 700=UBLOX7, 800=UBLOX8, 900=UBLOX9, 1000=UBLOX10, 0=UNKNOWN | **Notes:** Requires `USE_GPS`. @@ -4650,14 +4652,14 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding. ## `MSP2_RX_BIND (12289 / 0x3001)` -**Description:** Initiates binding for MSP receivers (mLRS). - +**Description:** Initiates binding for MSP receivers (mLRS). + **Request Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| | `port_id` | `uint8_t` | 1 | Port ID | | `reserved_for_custom_use` | `uint8_t[3]` | 3 | Reserved for custom use | - + **Reply Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 63121588b83..eeaa393b7ef 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,19 +2242,17 @@ "MAG_FAKE": "16", "MAG_MAX": "MAG_FAKE" }, - "mavFrameSupportMask_e": { - "_source": "inav/src/main/telemetry/mavlink.h", - "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", "MAVLINK_AUTOPILOT_ARDUPILOT": "1" }, + "mavlinkFcDispatchResult_e": { + "_source": "inav/src/main/fc/fc_mavlink.h", + "MAVLINK_FC_DISPATCH_NOT_HANDLED": "0", + "MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY": "1", + "MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY": "2" + }, "mavlinkRadio_e": { "_source": "inav/src/main/telemetry/telemetry.h", "MAVLINK_RADIO_GENERIC": "0", @@ -2336,8 +2338,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", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index a12fd374756..e0048e8c621 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -173,8 +173,8 @@ - [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) +- [mavlinkFcDispatchResult_e](#enum-mavlinkfcdispatchresult_e) - [mavlinkRadio_e](#enum-mavlinkradio_e) - [measurementSteps_e](#enum-measurementsteps_e) - [mixerProfileATRequest_e](#enum-mixerprofileatrequest_e) @@ -371,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 | | --- @@ -1408,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` @@ -2490,7 +2492,8 @@ | `GYRO_ICM42605` | 8 | | | `GYRO_BMI270` | 9 | | | `GYRO_LSM6DXX` | 10 | | -| `GYRO_FAKE` | 11 | | +| `GYRO_ICM45686` | 11 | | +| `GYRO_FAKE` | 12 | | --- ## `HardwareMotorTypes_e` @@ -2926,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` @@ -3376,27 +3380,25 @@ | `MAG_MAX` | MAG_FAKE | | --- -## `mavFrameSupportMask_e` +## `mavlinkAutopilotType_e` -> Source: ../../../src/main/telemetry/mavlink.h +> Source: ../../../src/main/telemetry/telemetry.h | 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) | | +| `MAVLINK_AUTOPILOT_GENERIC` | 0 | | +| `MAVLINK_AUTOPILOT_ARDUPILOT` | 1 | | --- -## `mavlinkAutopilotType_e` +## `mavlinkFcDispatchResult_e` -> Source: ../../../src/main/telemetry/telemetry.h +> Source: ../../../src/main/fc/fc_mavlink.h | Enumerator | Value | Condition | |---|---:|---| -| `MAVLINK_AUTOPILOT_GENERIC` | 0 | | -| `MAVLINK_AUTOPILOT_ARDUPILOT` | 1 | | +| `MAVLINK_FC_DISPATCH_NOT_HANDLED` | 0 | | +| `MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY` | 1 | | +| `MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY` | 2 | | --- ## `mavlinkRadio_e` @@ -3548,8 +3550,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` diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum index 14e143e04b9..60780228071 100644 --- a/docs/development/msp/msp_messages.checksum +++ b/docs/development/msp/msp_messages.checksum @@ -1 +1 @@ -7146802c1d6c89bce6aeda0d5e95c137 +fd564ac26b85f236c4cf3f41bfa0e7c1 diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index eab317a2366..b8303915eaf 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -395,6 +395,10 @@ main_sources(COMMON_SRC io/osd/custom_elements.c + mavlink/mavlink_command.c + mavlink/mavlink_command.h + mavlink/mavlink_guided.c + mavlink/mavlink_guided.h mavlink/mavlink_internal.h mavlink/mavlink_mission.c mavlink/mavlink_mission.h diff --git a/src/main/fc/fc_mavlink.c b/src/main/fc/fc_mavlink.c index 1d413bf7e08..f59b91cd690 100644 --- a/src/main/fc/fc_mavlink.c +++ b/src/main/fc/fc_mavlink.c @@ -2,6 +2,8 @@ #include "fc/fc_mavlink.h" +#include "mavlink/mavlink_command.h" +#include "mavlink/mavlink_guided.h" #include "mavlink/mavlink_mission.h" #include "mavlink/mavlink_runtime.h" #include "mavlink/mavlink_streams.h" @@ -141,345 +143,6 @@ static bool handleIncoming_TUNNEL(uint8_t ingressPortIndex) return handled; } -static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) -{ - if (targetSystem != 0 && targetSystem != mavSystemId) { - return false; - } - - if (targetComponent != 0 && targetComponent != mavComponentId) { - return false; - } - - 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 targetComponent, 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 (!mavlinkIsLocalTarget(targetSystem, targetComponent)) { - 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 = {0}; - wp.action = NAV_WP_ACTION_WAYPOINT; - 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; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - wp.flag = 0; - - setWaypoint(255, &wp); - - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - } else { - mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); - } - return true; - case MAV_CMD_DO_CHANGE_ALTITUDE: - { - const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame((uint8_t)lrintf(param2), param1); - mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); - return true; - } - case MAV_CMD_CONDITION_YAW: - { - if (!(navGetCurrentStateFlags() & NAV_CTL_YAW)) { - mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); - return true; - } - - int32_t targetHeadingCd = wrap_36000((int32_t)lrintf(param1 * 100.0f)); - - if (param4 != 0.0f) { - const int32_t currentHeadingCd = STATE(AIRPLANE) ? posControl.actualState.cog : posControl.actualState.yaw; - const int32_t headingChangeCd = (int32_t)lrintf(fabsf(param1) * 100.0f); - - if (param3 < 0.0f) { - targetHeadingCd = wrap_36000(currentHeadingCd - headingChangeCd); - } else { - targetHeadingCd = wrap_36000(currentHeadingCd + headingChangeCd); - } - } - - updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(targetHeadingCd)); - posControl.desiredState.yaw = targetHeadingCd; - posControl.cruise.course = targetHeadingCd; - posControl.cruise.previousCourse = targetHeadingCd; - - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - return true; - } - case MAV_CMD_SET_MESSAGE_INTERVAL: - { - mavlinkPeriodicMessage_e periodicMessage; - MAV_RESULT result = MAV_RESULT_UNSUPPORTED; - - if (mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { - if (param2 == 0.0f) { - mavlinkSetMessageOverrideIntervalUs(periodicMessage, 0); - result = MAV_RESULT_ACCEPTED; - } else if (param2 < 0.0f) { - mavlinkSetMessageOverrideIntervalUs(periodicMessage, -1); - result = MAV_RESULT_ACCEPTED; - } else { - uint32_t intervalUs = (uint32_t)param2; - if (intervalUs > 0) { - const uint32_t minIntervalUs = 1000000UL / TELEMETRY_MAVLINK_MAXRATE; - if (intervalUs < minIntervalUs) { - intervalUs = minIntervalUs; - } - - mavlinkSetMessageOverrideIntervalUs(periodicMessage, (int32_t)intervalUs); - result = MAV_RESULT_ACCEPTED; - } - } - } - - mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); - return true; - } - case MAV_CMD_GET_MESSAGE_INTERVAL: - { - mavlinkPeriodicMessage_e periodicMessage; - if (!mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { - mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); - return true; - } - - mavlink_msg_message_interval_pack( - mavSystemId, - mavComponentId, - &mavSendMsg, - (uint16_t)param1, - mavlinkMessageIntervalUs(periodicMessage)); - mavlinkSendMessage(); - - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - return true; - } - case MAV_CMD_CONTROL_HIGH_LATENCY: - if (param1 == 0.0f || param1 == 1.0f) { - if (mavlinkGetProtocolVersion() == 1 && param1 > 0.5f) { - mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); - return true; - } - - mavActivePort->highLatencyEnabled = param1 > 0.5f; - if (mavActivePort->highLatencyEnabled) { - mavActivePort->lastHighLatencyMessageUs = 0; - } - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - } else { - mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); - } - return true; - case MAV_CMD_REQUEST_PROTOCOL_VERSION: - if (mavlinkGetProtocolVersion() == 1) { - mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); - } else { - mavlinkSendProtocolVersion(); - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - } - return true; - case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: - if (mavlinkGetProtocolVersion() == 1) { - mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); - } else { - mavlinkSendAutopilotVersion(); - mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); - } - return true; - case MAV_CMD_REQUEST_MESSAGE: - { - const bool sent = mavlinkSendRequestedMessage((uint16_t)param1); - mavlinkSendCommandAck(command, sent ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); - return true; - } -#ifdef USE_GPS - case MAV_CMD_GET_HOME_POSITION: - if (mavlinkSendRequestedMessage(MAVLINK_MSG_ID_HOME_POSITION)) { - 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(&mavlinkContext.recvMsg, &msg); - - return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.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(&mavlinkContext.recvMsg, &msg); - - // COMMAND_LONG has no frame field; location commands are WGS84 global by definition. - return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, msg.command, MAV_FRAME_GLOBAL, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7); -} - -static bool handleIncoming_REQUEST_DATA_STREAM(void) -{ - mavlink_request_data_stream_t msg; - mavlink_msg_request_data_stream_decode(&mavlinkContext.recvMsg, &msg); - - if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { - return false; - } - - uint8_t rate = 0; - if (msg.start_stop != 0) { - rate = (uint8_t)msg.req_message_rate; - if (rate > TELEMETRY_MAVLINK_MAXRATE) { - rate = TELEMETRY_MAVLINK_MAXRATE; - } - } - - if (msg.req_stream_id == MAV_DATA_STREAM_ALL) { - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_STATUS, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_RC_CHANNELS, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_POSITION, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA1, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA2, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA3, rate); - mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_SYS_STATE, rate); - return true; - } - - mavlinkSetStreamRate(msg.req_stream_id, rate); - return true; -} - -static bool handleIncoming_SET_POSITION_TARGET_GLOBAL_INT(void) -{ - mavlink_set_position_target_global_int_t msg; - mavlink_msg_set_position_target_global_int_decode(&mavlinkContext.recvMsg, &msg); - - if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { - return false; - } - - uint8_t frame = msg.coordinate_frame; - if (!mavlinkFrameIsSupported(frame, - MAV_FRAME_SUPPORTED_GLOBAL | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | - MAV_FRAME_SUPPORTED_GLOBAL_INT | - MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { - return true; - } - - const uint16_t typeMask = msg.type_mask; - const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0; - const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0; - const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0; - - // Altitude-only SET_POSITION_TARGET_GLOBAL_INT mirrors MAV_CMD_DO_CHANGE_ALTITUDE semantics. - if (xIgnored && yIgnored && !zIgnored) { - if (isGCSValid()) { - mavlinkSetAltitudeTargetFromFrame(frame, msg.alt); - } - return true; - } - - if (xIgnored || yIgnored) { - return true; - } - - if (isGCSValid()) { - navWaypoint_t wp = {0}; - wp.action = NAV_WP_ACTION_WAYPOINT; - wp.lat = msg.lat_int; - wp.lon = msg.lon_int; - wp.alt = zIgnored ? 0 : (int32_t)(msg.alt * 100.0f); - wp.p1 = 0; - wp.p2 = 0; - wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; - wp.flag = 0; - - setWaypoint(255, &wp); - } - - return true; -} - -static bool handleIncoming_SET_POSITION_TARGET_LOCAL_NED(void) -{ - mavlink_set_position_target_local_ned_t msg; - mavlink_msg_set_position_target_local_ned_decode(&mavlinkContext.recvMsg, &msg); - - if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { - 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(&mavlinkContext.recvMsg, &msg); @@ -491,12 +154,15 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { mavlink_param_request_list_t msg; mavlink_msg_param_request_list_decode(&mavlinkContext.recvMsg, &msg); - // Respond that we don't have any parameters to force Mission Planner to give up quickly - if (mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { - // mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); - mavlink_msg_param_value_pack(mavSystemId, mavComponentId, &mavSendMsg, 0, 0, 0, 0, 0); - mavlinkSendMessage(); + if (msg.target_system != 0 && msg.target_system != mavSystemId) { + return false; + } + if (msg.target_component != 0 && msg.target_component != mavComponentId) { + return false; } + + mavlink_msg_param_value_pack(mavSystemId, mavComponentId, &mavSendMsg, 0, 0, 0, 0, 0); + mavlinkSendMessage(); return true; } @@ -505,7 +171,6 @@ static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { case MAVLINK_RADIO_NONE: break; case MAVLINK_RADIO_SIK: - // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; rxLinkStatistics.uplinkSNR = msg->noise / 1.9; rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; @@ -534,7 +199,7 @@ static bool handleIncoming_RADIO_STATUS(void) { mavActivePort->txbuffValid = false; mavActivePort->txbuffFree = 100; } - + if (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { mavlinkParseRxStats(&msg); @@ -543,22 +208,6 @@ static bool handleIncoming_RADIO_STATUS(void) { return true; } -static bool handleIncoming_HEARTBEAT(void) { - mavlink_heartbeat_t msg; - mavlink_msg_heartbeat_decode(&mavlinkContext.recvMsg, &msg); - - switch (msg.type) { -#ifdef USE_ADSB - case MAV_TYPE_ADSB: - return adsbHeartbeat(); -#endif - default: - break; - } - - return false; -} - #ifdef USE_ADSB static bool handleIncoming_ADSB_VEHICLE(void) { mavlink_adsb_vehicle_t msg; @@ -591,7 +240,7 @@ mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIn switch (mavlinkContext.recvMsg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: - return handleIncoming_HEARTBEAT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingHeartbeat() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: return handleIncoming_PARAM_REQUEST_LIST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: @@ -605,22 +254,22 @@ mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIn case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: return mavlinkHandleIncomingMissionRequestList() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_COMMAND_LONG: - return handleIncoming_COMMAND_LONG() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingCommandLong() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_COMMAND_INT: - return handleIncoming_COMMAND_INT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingCommandInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST: return mavlinkHandleIncomingMissionRequest() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST_INT: return mavlinkHandleIncomingMissionRequestInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - return handleIncoming_REQUEST_DATA_STREAM() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingRequestDataStream() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: handleIncoming_RC_CHANNELS_OVERRIDE(); return MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY; case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: - return handleIncoming_SET_POSITION_TARGET_LOCAL_NED() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingSetPositionTargetLocalNed() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: - return handleIncoming_SET_POSITION_TARGET_GLOBAL_INT() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + return mavlinkHandleIncomingSetPositionTargetGlobalInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; #ifdef USE_ADSB case MAVLINK_MSG_ID_ADSB_VEHICLE: return handleIncoming_ADSB_VEHICLE() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; diff --git a/src/main/mavlink/mavlink_command.c b/src/main/mavlink/mavlink_command.c new file mode 100644 index 00000000000..94127c1fe7d --- /dev/null +++ b/src/main/mavlink/mavlink_command.c @@ -0,0 +1,245 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_command.h" +#include "mavlink/mavlink_guided.h" +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) +{ + if (targetSystem != 0 && targetSystem != mavSystemId) { + return false; + } + + if (targetComponent != 0 && targetComponent != mavComponentId) { + return false; + } + + 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 targetComponent, + 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 (!mavlinkIsLocalTarget(targetSystem, targetComponent)) { + 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 = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + 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; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + wp.flag = 0; + + setWaypoint(255, &wp); + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_DO_CHANGE_ALTITUDE: + { + const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame((uint8_t)lrintf(param2), param1); + mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_CONDITION_YAW: + { + if (!(navGetCurrentStateFlags() & NAV_CTL_YAW)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + int32_t targetHeadingCd = wrap_36000((int32_t)lrintf(param1 * 100.0f)); + + if (param4 != 0.0f) { + const int32_t currentHeadingCd = STATE(AIRPLANE) ? posControl.actualState.cog : posControl.actualState.yaw; + const int32_t headingChangeCd = (int32_t)lrintf(fabsf(param1) * 100.0f); + + if (param3 < 0.0f) { + targetHeadingCd = wrap_36000(currentHeadingCd - headingChangeCd); + } else { + targetHeadingCd = wrap_36000(currentHeadingCd + headingChangeCd); + } + } + + updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(targetHeadingCd)); + posControl.desiredState.yaw = targetHeadingCd; + posControl.cruise.course = targetHeadingCd; + posControl.cruise.previousCourse = targetHeadingCd; + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_SET_MESSAGE_INTERVAL: + { + mavlinkPeriodicMessage_e periodicMessage; + MAV_RESULT result = MAV_RESULT_UNSUPPORTED; + + if (mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { + if (param2 == 0.0f) { + mavlinkSetMessageOverrideIntervalUs(periodicMessage, 0); + result = MAV_RESULT_ACCEPTED; + } else if (param2 < 0.0f) { + mavlinkSetMessageOverrideIntervalUs(periodicMessage, -1); + result = MAV_RESULT_ACCEPTED; + } else { + uint32_t intervalUs = (uint32_t)param2; + if (intervalUs > 0) { + const uint32_t minIntervalUs = 1000000UL / TELEMETRY_MAVLINK_MAXRATE; + if (intervalUs < minIntervalUs) { + intervalUs = minIntervalUs; + } + + mavlinkSetMessageOverrideIntervalUs(periodicMessage, (int32_t)intervalUs); + result = MAV_RESULT_ACCEPTED; + } + } + } + + mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_GET_MESSAGE_INTERVAL: + { + mavlinkPeriodicMessage_e periodicMessage; + if (!mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + mavlink_msg_message_interval_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + (uint16_t)param1, + mavlinkMessageIntervalUs(periodicMessage)); + mavlinkSendMessage(); + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_CONTROL_HIGH_LATENCY: + if (param1 == 0.0f || param1 == 1.0f) { + if (mavlinkGetProtocolVersion() == 1 && param1 > 0.5f) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + mavActivePort->highLatencyEnabled = param1 > 0.5f; + if (mavActivePort->highLatencyEnabled) { + mavActivePort->lastHighLatencyMessageUs = 0; + } + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_REQUEST_PROTOCOL_VERSION: + if (mavlinkGetProtocolVersion() == 1) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendProtocolVersion(); + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: + if (mavlinkGetProtocolVersion() == 1) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendAutopilotVersion(); + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_REQUEST_MESSAGE: + { + const bool sent = mavlinkSendRequestedMessage((uint16_t)param1); + mavlinkSendCommandAck(command, sent ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } +#ifdef USE_GPS + case MAV_CMD_GET_HOME_POSITION: + if (mavlinkSendRequestedMessage(MAVLINK_MSG_ID_HOME_POSITION)) { + 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; + } +} + +bool mavlinkHandleIncomingCommandInt(void) +{ + mavlink_command_int_t msg; + mavlink_msg_command_int_decode(&mavlinkContext.recvMsg, &msg); + + return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, msg.command, msg.frame, msg.param1, msg.param2, msg.param3, msg.param4, (float)msg.x / 1e7f, (float)msg.y / 1e7f, msg.z); +} + +bool mavlinkHandleIncomingCommandLong(void) +{ + mavlink_command_long_t msg; + mavlink_msg_command_long_decode(&mavlinkContext.recvMsg, &msg); + + // COMMAND_LONG has no frame field; location commands are WGS84 global by definition. + return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, msg.command, MAV_FRAME_GLOBAL, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7); +} + +#endif diff --git a/src/main/mavlink/mavlink_command.h b/src/main/mavlink/mavlink_command.h new file mode 100644 index 00000000000..fb468e696f7 --- /dev/null +++ b/src/main/mavlink/mavlink_command.h @@ -0,0 +1,7 @@ +#pragma once + +#include +#include + +bool mavlinkHandleIncomingCommandLong(void); +bool mavlinkHandleIncomingCommandInt(void); diff --git a/src/main/mavlink/mavlink_guided.c b/src/main/mavlink/mavlink_guided.c new file mode 100644 index 00000000000..9bdb719c716 --- /dev/null +++ b/src/main/mavlink/mavlink_guided.c @@ -0,0 +1,153 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_guided.h" +#include "mavlink/mavlink_runtime.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +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; + } +} + +bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame) +{ + return frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_INT; +} + +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 bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) +{ + if (targetSystem != 0 && targetSystem != mavSystemId) { + return false; + } + + if (targetComponent != 0 && targetComponent != mavComponentId) { + return false; + } + + return true; +} + +bool mavlinkHandleIncomingSetPositionTargetGlobalInt(void) +{ + mavlink_set_position_target_global_int_t msg; + mavlink_msg_set_position_target_global_int_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + return false; + } + + uint8_t frame = msg.coordinate_frame; + if (!mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + return true; + } + + const uint16_t typeMask = msg.type_mask; + const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0; + const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0; + const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0; + + // Altitude-only SET_POSITION_TARGET_GLOBAL_INT mirrors MAV_CMD_DO_CHANGE_ALTITUDE semantics. + if (xIgnored && yIgnored && !zIgnored) { + if (isGCSValid()) { + mavlinkSetAltitudeTargetFromFrame(frame, msg.alt); + } + return true; + } + + if (xIgnored || yIgnored) { + return true; + } + + if (isGCSValid()) { + navWaypoint_t wp = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = msg.lat_int; + wp.lon = msg.lon_int; + wp.alt = zIgnored ? 0 : (int32_t)(msg.alt * 100.0f); + wp.p1 = 0; + wp.p2 = 0; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + wp.flag = 0; + + setWaypoint(255, &wp); + } + + return true; +} + +bool mavlinkHandleIncomingSetPositionTargetLocalNed(void) +{ + mavlink_set_position_target_local_ned_t msg; + mavlink_msg_set_position_target_local_ned_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + 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; +} + +#endif diff --git a/src/main/mavlink/mavlink_guided.h b/src/main/mavlink/mavlink_guided.h new file mode 100644 index 00000000000..e160134d29b --- /dev/null +++ b/src/main/mavlink/mavlink_guided.h @@ -0,0 +1,13 @@ +#pragma once + +#include +#include + +#include "mavlink/mavlink_mission.h" + +bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask); +bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame); +MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters); + +bool mavlinkHandleIncomingSetPositionTargetGlobalInt(void); +bool mavlinkHandleIncomingSetPositionTargetLocalNed(void); diff --git a/src/main/mavlink/mavlink_mission.c b/src/main/mavlink/mavlink_mission.c index 368dd39bfc9..70fcc9a06ea 100644 --- a/src/main/mavlink/mavlink_mission.c +++ b/src/main/mavlink/mavlink_mission.c @@ -1,5 +1,6 @@ #include "mavlink/mavlink_internal.h" +#include "mavlink/mavlink_guided.h" #include "mavlink/mavlink_mission.h" #include "mavlink/mavlink_runtime.h" @@ -29,54 +30,6 @@ void mavlinkSendPendingMissionItemReached(void) mavSendMask = 0; } -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; - } -} - -bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame) -{ - return frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_INT; -} - -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 -} - uint8_t mavlinkWaypointFrame(const navWaypoint_t *wp, bool useIntMessages) { switch (wp->action) { @@ -95,16 +48,20 @@ uint8_t mavlinkWaypointFrame(const navWaypoint_t *wp, bool useIntMessages) return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT; } -static MAV_MISSION_RESULT mavlinkMissionResultFromCommandResult(MAV_RESULT result) + +static void mavlinkSendMissionAck(MAV_MISSION_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; - } + mavlink_msg_mission_ack_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + result, + MAV_MISSION_TYPE_MISSION, + 0 + ); + mavlinkSendMessage(); } static void mavlinkResetIncomingMissionTransaction(void) @@ -150,21 +107,21 @@ static bool mavlinkIsIncomingMissionTransactionOwner(void) mavlinkContext.recvMsg.compid == incomingMissionSourceComponent; } -static bool mavlinkHandleArmedGuidedMissionItem(uint8_t current, uint8_t frame, mavFrameSupportMask_e allowedFrames, int32_t latitudeE7, int32_t longitudeE7, float altitudeMeters) +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, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, - MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ERROR); return true; } if (!mavlinkFrameIsSupported(frame, allowedFrames)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, - MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); return true; } @@ -178,34 +135,39 @@ static bool mavlinkHandleArmedGuidedMissionItem(uint8_t current, uint8_t frame, setWaypoint(255, &wp); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, - MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ACCEPTED); return true; } if (current == 3) { const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame(frame, altitudeMeters); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, - mavlinkMissionResultFromCommandResult(result), MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + MAV_MISSION_RESULT response = MAV_MISSION_ERROR; + if (result == MAV_RESULT_ACCEPTED) {response = MAV_MISSION_ACCEPTED;} + else if (result == MAV_RESULT_UNSUPPORTED) {response = MAV_MISSION_UNSUPPORTED;} + mavlinkSendMissionAck(response); return true; } - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, - MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ERROR); return true; } -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) +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 (!mavlinkIsIncomingMissionTransactionActive() || !mavlinkIsIncomingMissionTransactionOwner()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_INVALID_SEQUENCE); return true; } @@ -214,8 +176,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u const bool lastMissionItem = incomingMissionWpCount > 0 && ((int)seq + 1 >= incomingMissionWpCount); if (autocontinue == 0 && !lastMissionItem) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); return true; } @@ -230,8 +191,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); return true; } wp.action = NAV_WP_ACTION_WAYPOINT; @@ -247,8 +207,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); return true; } wp.action = NAV_WP_ACTION_HOLD_TIME; @@ -268,8 +227,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT); if (!coordinateFrame && frame != MAV_FRAME_MISSION) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); return true; } wp.action = NAV_WP_ACTION_RTH; @@ -284,8 +242,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); return true; } wp.action = NAV_WP_ACTION_LAND; @@ -297,13 +254,11 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u case MAV_CMD_DO_JUMP: if (frame != MAV_FRAME_MISSION) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); return true; } if (param1 < 0.0f) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); return true; } wp.action = NAV_WP_ACTION_JUMP; @@ -318,8 +273,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u 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, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); return true; } wp.action = NAV_WP_ACTION_SET_POI; @@ -331,13 +285,11 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u case MAV_CMD_CONDITION_YAW: if (frame != MAV_FRAME_MISSION) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); return true; } if (param4 != 0.0f) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); return true; } wp.action = NAV_WP_ACTION_SET_HEAD; @@ -345,8 +297,7 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u break; default: - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); return true; } @@ -357,17 +308,30 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u if (incomingMissionWpSequence >= incomingMissionWpCount) { if (isWaypointListValid()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMissionAck(MAV_MISSION_ACCEPTED); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMissionAck(MAV_MISSION_INVALID); } - mavlinkSendMessage(); mavlinkResetIncomingMissionTransaction(); } else { if (useIntMessages) { - mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_request_int_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); } else { - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_request_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); } mavlinkSendMessage(); } @@ -375,14 +339,27 @@ static bool mavlinkHandleMissionItemCommon(bool useIntMessages, uint8_t frame, u if (seq + 1 == incomingMissionWpSequence) { mavlinkTouchIncomingMissionTransaction(); if (useIntMessages) { - mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_request_int_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); } else { - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_request_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); } mavlinkSendMessage(); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_INVALID_SEQUENCE); } } @@ -397,8 +374,7 @@ bool mavlinkHandleIncomingMissionClearAll(void) if (msg.target_system == mavSystemId) { resetWaypointList(); mavlinkResetIncomingMissionTransaction(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ACCEPTED); return true; } @@ -413,25 +389,29 @@ bool mavlinkHandleIncomingMissionCount(void) if (msg.target_system == mavSystemId) { mavlinkResetIncomingMissionTransaction(); if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ERROR); return true; } if (msg.count == 0) { resetWaypointList(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ACCEPTED); return true; } if (msg.count <= NAV_MAX_WAYPOINTS) { mavlinkStartIncomingMissionTransaction(msg.count); - mavlink_msg_mission_request_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_request_int_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_NO_SPACE); return true; } @@ -454,8 +434,7 @@ bool mavlinkHandleIncomingMissionItem(void) (int32_t)lrintf(msg.x * 1e7f), (int32_t)lrintf(msg.y * 1e7f), msg.z); } - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ERROR); return true; } @@ -566,12 +545,10 @@ bool mavlinkHandleIncomingMissionRequest(void) MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ERROR); } } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_INVALID_SEQUENCE); } return true; @@ -593,8 +570,7 @@ bool mavlinkHandleIncomingMissionItemInt(void) msg.x, msg.y, msg.z); } - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ERROR); return true; } @@ -631,12 +607,10 @@ bool mavlinkHandleIncomingMissionRequestInt(void) MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_ERROR); } } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); + mavlinkSendMissionAck(MAV_MISSION_INVALID_SEQUENCE); } return true; diff --git a/src/main/mavlink/mavlink_mission.h b/src/main/mavlink/mavlink_mission.h index 650bdd06338..41010e85f07 100644 --- a/src/main/mavlink/mavlink_mission.h +++ b/src/main/mavlink/mavlink_mission.h @@ -27,13 +27,9 @@ typedef struct mavlinkMissionItemData_s { float alt; } mavlinkMissionItemData_t; -bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask); -bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame); -MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters); uint8_t mavlinkWaypointFrame(const navWaypoint_t *wp, bool useIntMessages); bool mavlinkFillMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item); void mavlinkSendPendingMissionItemReached(void); - bool mavlinkHandleIncomingMissionClearAll(void); bool mavlinkHandleIncomingMissionCount(void); bool mavlinkHandleIncomingMissionItem(void); diff --git a/src/main/mavlink/mavlink_streams.c b/src/main/mavlink/mavlink_streams.c index 15a6630f25c..6dfb0ef47b8 100644 --- a/src/main/mavlink/mavlink_streams.c +++ b/src/main/mavlink/mavlink_streams.c @@ -1089,6 +1089,68 @@ bool mavlinkSendRequestedMessage(uint16_t messageId) } } +bool mavlinkHandleIncomingHeartbeat(void) +{ + mavlink_heartbeat_t msg; + mavlink_msg_heartbeat_decode(&mavlinkContext.recvMsg, &msg); + + switch (msg.type) { +#ifdef USE_ADSB + case MAV_TYPE_ADSB: + return adsbHeartbeat(); +#endif + default: + break; + } + + return false; +} + +static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) +{ + if (targetSystem != 0 && targetSystem != mavSystemId) { + return false; + } + + if (targetComponent != 0 && targetComponent != mavComponentId) { + return false; + } + + return true; +} + +bool mavlinkHandleIncomingRequestDataStream(void) +{ + mavlink_request_data_stream_t msg; + mavlink_msg_request_data_stream_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + return false; + } + + uint8_t rate = 0; + if (msg.start_stop != 0) { + rate = (uint8_t)msg.req_message_rate; + if (rate > TELEMETRY_MAVLINK_MAXRATE) { + rate = TELEMETRY_MAVLINK_MAXRATE; + } + } + + if (msg.req_stream_id == MAV_DATA_STREAM_ALL) { + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_STATUS, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_RC_CHANNELS, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_POSITION, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA1, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA2, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA3, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_SYS_STATE, rate); + return true; + } + + mavlinkSetStreamRate(msg.req_stream_id, rate); + return true; +} + void processMAVLinkTelemetry(timeUs_t currentTimeUs) { if (mavActivePort->highLatencyEnabled && mavlinkGetProtocolVersion() != 1) { diff --git a/src/main/mavlink/mavlink_streams.h b/src/main/mavlink/mavlink_streams.h index 514a8e01c38..c385ef35e9a 100644 --- a/src/main/mavlink/mavlink_streams.h +++ b/src/main/mavlink/mavlink_streams.h @@ -21,3 +21,5 @@ void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate); int mavlinkMessageTrigger(mavlinkPeriodicMessage_e periodicMessage, timeUs_t currentTimeUs); void configureMAVLinkStreamRates(uint8_t portIndex); void processMAVLinkTelemetry(timeUs_t currentTimeUs); +bool mavlinkHandleIncomingHeartbeat(void); +bool mavlinkHandleIncomingRequestDataStream(void); From 403bcb5696ffb985ef25b62dda92612c0b94419c Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Sat, 4 Apr 2026 13:59:52 -0400 Subject: [PATCH 42/42] fixed docgen to include mavlink --- docs/development/msp/get_all_inav_enums_h.py | 1 + docs/development/msp/inav_enums.json | 24 +++++++++++++ docs/development/msp/inav_enums_ref.md | 36 ++++++++++++++++++++ 3 files changed, 61 insertions(+) diff --git a/docs/development/msp/get_all_inav_enums_h.py b/docs/development/msp/get_all_inav_enums_h.py index 971d077d5f4..79c5c57276a 100644 --- a/docs/development/msp/get_all_inav_enums_h.py +++ b/docs/development/msp/get_all_inav_enums_h.py @@ -16,6 +16,7 @@ 'flight', 'fc', 'drivers', + 'mavlink', ] def strip_comments(text: str) -> str: diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index eeaa393b7ef..c23774ce6e2 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -2242,6 +2242,14 @@ "MAG_FAKE": "16", "MAG_MAX": "MAG_FAKE" }, + "mavFrameSupportMask_e": { + "_source": "inav/src/main/mavlink/mavlink_mission.h", + "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", @@ -2253,6 +2261,22 @@ "MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY": "1", "MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY": "2" }, + "mavlinkPeriodicMessage_e": { + "_source": "inav/src/main/mavlink/mavlink_types.h", + "MAVLINK_PERIODIC_MESSAGE_HEARTBEAT": "0", + "MAVLINK_PERIODIC_MESSAGE_SYS_STATUS": "1", + "MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE": "2", + "MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS": "3", + "MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT": "4", + "MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT": "5", + "MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN": "6", + "MAVLINK_PERIODIC_MESSAGE_ATTITUDE": "7", + "MAVLINK_PERIODIC_MESSAGE_VFR_HUD": "8", + "MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS": "9", + "MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE": "10", + "MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME": "11", + "MAVLINK_PERIODIC_MESSAGE_COUNT": "12" + }, "mavlinkRadio_e": { "_source": "inav/src/main/telemetry/telemetry.h", "MAVLINK_RADIO_GENERIC": "0", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index e0048e8c621..1e7ba6db1de 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -173,8 +173,10 @@ - [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) - [mavlinkFcDispatchResult_e](#enum-mavlinkfcdispatchresult_e) +- [mavlinkPeriodicMessage_e](#enum-mavlinkperiodicmessage_e) - [mavlinkRadio_e](#enum-mavlinkradio_e) - [measurementSteps_e](#enum-measurementsteps_e) - [mixerProfileATRequest_e](#enum-mixerprofileatrequest_e) @@ -3379,6 +3381,19 @@ | `MAG_FAKE` | 16 | | | `MAG_MAX` | MAG_FAKE | | +--- +## `mavFrameSupportMask_e` + +> Source: ../../../src/main/mavlink/mavlink_mission.h + +| 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` @@ -3400,6 +3415,27 @@ | `MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY` | 1 | | | `MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY` | 2 | | +--- +## `mavlinkPeriodicMessage_e` + +> Source: ../../../src/main/mavlink/mavlink_types.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAVLINK_PERIODIC_MESSAGE_HEARTBEAT` | 0 | | +| `MAVLINK_PERIODIC_MESSAGE_SYS_STATUS` | 1 | | +| `MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE` | 2 | | +| `MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS` | 3 | | +| `MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT` | 4 | | +| `MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT` | 5 | | +| `MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN` | 6 | | +| `MAVLINK_PERIODIC_MESSAGE_ATTITUDE` | 7 | | +| `MAVLINK_PERIODIC_MESSAGE_VFR_HUD` | 8 | | +| `MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS` | 9 | | +| `MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE` | 10 | | +| `MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME` | 11 | | +| `MAVLINK_PERIODIC_MESSAGE_COUNT` | 12 | | + --- ## `mavlinkRadio_e`