Mavlink compatibility patch#11180
Open
xznhj8129 wants to merge 25 commits intoiNavFlight:maintenance-10.xfrom
Open
Mavlink compatibility patch#11180xznhj8129 wants to merge 25 commits intoiNavFlight:maintenance-10.xfrom
xznhj8129 wants to merge 25 commits intoiNavFlight:maintenance-10.xfrom
Conversation
Branch Targeting SuggestionYou've targeted the
If This is an automated suggestion to help route contributions to the appropriate branch. |
Contributor
Author
Update since last month
Test infrastructure and coverage added:
Docs/enums changes:
|
# Conflicts: # docs/development/msp/inav_enums.json # src/main/telemetry/mavlink.c
Contributor
Review Summary by QodoModernize MAVLink protocol implementation with improved GCS compatibility and mission handling
WalkthroughsDescriptionThis description is generated by an AI tool. It may have inaccuracies • Extensive modernization and reworking of MAVLink protocol implementation to improve GCS (Ground Control Station) compatibility • Expanded MAVLink support including EXTENDED_SYS_STATE stream, protocol/autopilot version replies, available/current modes, and proper vehicle type detection • Reworked mission handling to accept and send both MISSION_ITEM and MISSION_ITEM_INT with unified parsing, frame validation, and waypoint-to-MAV command mapping • Improved stream-rate control with per-stream setters, GET/SET_MESSAGE_INTERVAL, and REQUEST_DATA_STREAM support • Added HOME_POSITION handling and SET_POSITION_TARGET_GLOBAL_INT support for GUIDED/GCS-navigation waypoint updates • Updated ArduPilot mode enumerations to current values with heartbeat mode flags based on actual flight mode • Fixed GLOBAL_POSITION_INT timestamp to use boot milliseconds instead of raw microseconds per MAVLink specification • Enhanced command handling for DO_REPOSITION, GET/SET_MESSAGE_INTERVAL, REQUEST_MESSAGE, REQUEST_PROTOCOL_VERSION, and REQUEST_AUTOPILOT_CAPABILITIES • Unified COMMAND_LONG and COMMAND_INT handling into a single function for improved compatibility • Tested with Speedybee F405WINGMINI in X-Plane HITL with MLRS mavlink receiver using QGC and UASTOOL Diagramflowchart LR
A["MAVLink Protocol"] -->|"Enhanced Support"| B["EXTENDED_SYS_STATE<br/>Version Replies<br/>Mode Detection"]
A -->|"Improved Missions"| C["MISSION_ITEM<br/>MISSION_ITEM_INT<br/>Unified Parsing"]
A -->|"Stream Control"| D["Per-Stream Setters<br/>GET/SET_MESSAGE_INTERVAL<br/>REQUEST_DATA_STREAM"]
A -->|"New Commands"| E["HOME_POSITION<br/>SET_POSITION_TARGET<br/>DO_REPOSITION"]
B --> F["GCS Compatibility"]
C --> F
D --> F
E --> F
File Changes |
Contributor
Code Review by Qodo
1. DO_REPOSITION missing range checks
|
Contributor
Author
|
Don't merge yet, i need to fully test mavlink altitude setting |
![qodo-code-review[bot]](https://avatars.githubusercontent.com/in/484649?s=60&v=4){kind=small}
Comment on lines
+1815
to
1851
| static bool handleIncoming_COMMAND(uint8_t targetSystem, uint8_t ackTargetSystem, uint8_t ackTargetComponent, uint16_t command, uint8_t frame, float param1, float param2, float param3, float param4, float latitudeDeg, float longitudeDeg, float altitudeMeters) | ||
| { | ||
| if (targetSystem != mavSystemId) { | ||
| return false; | ||
| } | ||
| UNUSED(param3); | ||
|
|
||
| switch (command) { | ||
| case MAV_CMD_DO_REPOSITION: | ||
| if (!mavlinkFrameIsSupported(frame, | ||
| MAV_FRAME_SUPPORTED_GLOBAL | | ||
| MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | | ||
| MAV_FRAME_SUPPORTED_GLOBAL_INT | | ||
| MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { | ||
| mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); | ||
| return true; | ||
| } | ||
|
|
||
| if (isnan(latitudeDeg) || isnan(longitudeDeg)) { | ||
| mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); | ||
| return true; | ||
| } | ||
|
|
||
| if (isGCSValid()) { | ||
| navWaypoint_t wp; | ||
| wp.action = NAV_WP_ACTION_WAYPOINT; | ||
| wp.lat = (int32_t)msg.x; | ||
| wp.lon = (int32_t)msg.y; | ||
| wp.alt = msg.z * 100.0f; | ||
| if (!isnan(msg.param4) && msg.param4 >= 0.0f && msg.param4 < 360.0f) { | ||
| wp.p1 = (int16_t)msg.param4; | ||
| wp.lat = (int32_t)(latitudeDeg * 1e7f); | ||
| wp.lon = (int32_t)(longitudeDeg * 1e7f); | ||
| wp.alt = (int32_t)(altitudeMeters * 100.0f); | ||
| if (!isnan(param4) && param4 >= 0.0f && param4 < 360.0f) { | ||
| wp.p1 = (int16_t)param4; | ||
| } else { | ||
| wp.p1 = 0; | ||
| } | ||
| wp.p2 = 0; // TODO: Alt modes | ||
| wp.p3 = 0; | ||
| wp.p2 = 0; | ||
| wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; | ||
| wp.flag = 0; |
Contributor
There was a problem hiding this comment.
1. do_reposition missing range checks 📘 Rule violation ⛨ Security
MAV_CMD_DO_REPOSITION only checks NaN for lat/lon, but does not validate latitude/longitude ranges or altitude finiteness/range before converting to scaled integers. This can lead to invalid waypoints or overflow/undefined navigation behavior from malformed external MAVLink inputs.
Agent Prompt
## Issue description
`MAV_CMD_DO_REPOSITION` accepts external MAVLink coordinates without validating latitude/longitude bounds or altitude finiteness/range before converting to scaled integers.
## Issue Context
This handler consumes untrusted MAVLink message payload values and writes them into a navigation waypoint (`setWaypoint(255, &wp)`), so inputs must be checked for `NaN`/infinite values and for valid coordinate ranges to prevent invalid state.
## Fix Focus Areas
- src/main/telemetry/mavlink.c[1815-1851]
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
src/main/telemetry/mavlink.c
Outdated
Comment on lines
1861
to
1868
| case MAV_CMD_DO_CHANGE_ALTITUDE: | ||
| { | ||
| const float altitudeMeters = param1; | ||
| geoAltitudeDatumFlag_e datum; | ||
|
|
||
| switch (frame) { | ||
| case MAV_FRAME_GLOBAL: | ||
| case MAV_FRAME_GLOBAL_INT: |
Contributor
There was a problem hiding this comment.
2. do_change_altitude accepts nan 📘 Rule violation ⛯ Reliability
MAV_CMD_DO_CHANGE_ALTITUDE converts param1 to cm using lrintf without validating that the input is finite and within a safe range. A malformed MAVLink command can inject NaN/Inf or extreme values, producing undefined or unsafe altitude targets.
Agent Prompt
## Issue description
`MAV_CMD_DO_CHANGE_ALTITUDE` uses `param1` directly without `NaN`/Inf/range validation, then converts via `lrintf`, which can yield undefined/unsafe results.
## Issue Context
This code processes external command payloads and sets an altitude target, so it must reject malformed inputs deterministically.
## Fix Focus Areas
- src/main/telemetry/mavlink.c[1861-1883]
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
Comment on lines
+1361
to
+1512
| 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) { | ||
| 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; |
Contributor
There was a problem hiding this comment.
3. Mission params not validated 📘 Rule violation ⛨ Security
Mission item handling converts external MAVLink values (altitude, jump/loiter/yaw params) into integral waypoint fields without checking NaN/finite status or bounds before lrintf/casts. This can yield overflow/undefined conversions and corrupted mission state from malformed mission uploads.
Agent Prompt
## Issue description
Mission upload handlers accept external MAVLink mission item numeric fields and convert them (casts/`lrintf`) without validating finiteness or bounds, risking overflow/undefined conversions and invalid stored mission state.
## Issue Context
This is fed by untrusted GCS mission uploads (`MISSION_ITEM`/`MISSION_ITEM_INT`) and writes to internal waypoint storage.
## Fix Focus Areas
- src/main/telemetry/mavlink.c[1361-1512]
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
Comment on lines
+94
to
+97
| const uint32_t baudRates[] = { | ||
| 0, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 250000, | ||
| 460800, 921600, 1000000, 1500000, 2000000, 2470000 | ||
| }; |
Contributor
There was a problem hiding this comment.
6. Unit test baudrates symbol 🐞 Bug ⛯ Reliability
The new mavlink unit test defines baudRates with C++ linkage, but mavlink.c references an external C symbol baudRates. This can cause a link failure (undefined reference to baudRates) when linking C object code against the mangled C++ definition.
Agent Prompt
### Issue description
`mavlink_unittest.cc` defines `baudRates` as a C++ global, but C code (`telemetry/mavlink.c`) expects an unmangled `baudRates` symbol.
### Issue Context
The unit test target links C objects (from `telemetry/mavlink.c`) with C++ objects (from the test). Without `extern "C"` on the definition, the linker may not match the symbol.
### Fix Focus Areas
- src/test/unit/mavlink_unittest.cc[94-97]
### Expected change
- Change the definition to have C linkage, e.g.:
- `extern "C" const uint32_t baudRates[] = { ... };`
- or wrap the definition in an `extern "C" { ... }` block.
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
…reverted strict rc_override sysid check (caused fs), SET_POSITION_TARGET_LOCAL_NED for qgc change alt
Contributor
Author
|
One must imagine Sisyphus happy
Testing: |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
WIP Extensive reworking of the MAVLink code to modernize, remove placeholder/boilerplate stuff and improve compatibility with GCS features like missions. No new Flight controller or MSP features added yet (to implement stuff like takeoff or setting altitude), that will be for a later stage.
Summary
Details
mavlinkHandleMissionItemCommonhandles frame validation and waypoint creation;fillMavlinkMissionItemFromWaypointbuilds outgoing items with frame selection via altitude mode; supports JUMP/SET_POI/SET_HEAD/RTH/LAND/LOITER/WAYPOINT. We only handle global and relative altitude frames, no NED or AMSL or terrain.Testing
Things in this PR that could need work