[MrScothh]

What happens

In HITL the simulator plugin sends one MSP_SIMULATOR frame per simulator frame. The serial task runs at 100 Hz and
mspSerialProcessOnePort() answers one command per run, so a port takes at most 100 commands a second, shared with
anything else that talks to it (a ground station behind a serial mux, debug polling). Above that the frames queue up
in the port's buffer and each one reaches INAV later than the one before: the attitude, sensors and GPS INAV flies on
grow older until the buffer is full.

Measured on a TBS Lucid H7 Wing over USB with X-Plane 11 and a 1.2 m fixed-wing model, with 20 other MSP requests a
second on the same port:

• 64 simulator frames a second and 40 other requests (debug polling on, 104 in all): INAV's attitude settled 660 ms
  behind the simulator's, and the aircraft crashed after take-off.
• About 120 simulator frames a second, before this change: INAV took 97 frames a second, GPS samples came up to 640 ms
  apart, and at take-off INAV's attitude stayed at its ground value for about 5 s while the aircraft rotated. The
  aircraft was lost on the runway and the plugin dropped the link.
• The same with this change: 115 to 120 frames a second, GPS at its 10 Hz with gaps up to 110 ms, INAV's attitude
  matching the simulator's at every sample of a 10 Hz log; five flights completed.

The fix

While SIMULATOR_MODE_HITL is set, mspSerialProcessOnePort() goes on to the next received command instead of
stopping after the first, up to four per run: 400 commands a second, a frame per simulator frame up to about 300 fps
with room for the other requests. It goes on only while the last reply went out at once: the transmit buffer is free
and the run has lasted less than 1 ms. A command with a post-process function (a reboot, for example) still ends the
run as before. Outside HITL nothing changes.

What else this touches:

• It applies to every MSP port while HITL is active, and to bytes that arrive during the run. The flag is cleared by
  the simulator's stop frame or a reboot, so after an unclean simulator exit it stays on until the next boot.
• On a UART still sending the previous reply the run ends after one command, as before, so no reply is dropped for
  lack of room.
• A USB host that stops reading makes a reply wait out the driver's 50 ms timeout; that ends the run after one
  command, the same worst case as one command per run.
• Two GPS frames cannot reach the position estimator in the same run at the rates where runs take more than one
  command: the plugin flags GPS data at 10 Hz, and four consecutive frames can only hold two of them below about 30
  simulator frames a second, where one command per run already keeps up.

Testing

• HITL flights as above, on a build of maintenance-10.x plus my open PRs, with and without this change. With this
  version (cap, transmit check and 1 ms budget): 115 to 120 frames a second, GPS gaps up to 114 ms, INAV's attitude
  0 ms behind the simulator's, circuit flown and landed.
• Unit tests: four new ones in mavlink_unittest (one command per run outside HITL; in HITL four per run and the
  rest on the next run; one per run while the transmit buffer is busy; one per run after a slow reply). All pass
  (624/624).
• Size against maintenance-10.x 13ebed9: MATEKF722SE +48 B flash (98.57 %), MATEKF405 -32 B, KAKUTEF7 +80 B,
  KAKUTEF7HDV +64 B, TBS_LUCID_H7_WING +64 B; RAM and ITCM unchanged on all of them.

Not tested: other boards and USB stacks, a UART instead of USB, simulators other than X-Plane.

[qodo-code-review]

ⓘ Qodo reviews are paused because the subscription is no longer active. Ask your workspace admin to reactivate the subscription to resume reviews. Manage billing

[qodo-free-for-open-source-projects]

PR Summary by Qodo

Drain up to four MSP commands per serial run during HITL

🐞 Bug fix 🧪 Tests 🕐 20-40 Minutes

AI Description

• Process up to four MSP commands per run during HITL to reduce simulator-frame backlog.
• Preserve one-command runs outside HITL and stop immediately for post-processing commands.
• Test both the normal limit and HITL backlog draining across consecutive runs.

Diagram

graph TD
  A["Serial task"] --> B["Read port bytes"] --> C{"Command complete?"} --> D["Process command"] --> E{"Post-process?"} --> F{"HITL active?"} --> G{"Four processed?"} --> B
  C -- "no" --> B
  E -- "yes" --> H["Finish run"]
  F -- "no" --> H
  G -- "yes" --> H
  B -- "RX empty" --> H

Loading

High-Level Assessment

The following are alternative approaches to this PR:

1. Prioritize simulator commands only

• ➕ Limits extra processing of unrelated MSP traffic on shared ports.
• ➖ Requires command-aware scheduling or reordering and complicates serial receive handling.

2. Drain to a time budget

• ➕ Bounds elapsed processing time rather than only command count.
• ➖ Adds timing checks and makes the number of processed frames less predictable.

Recommendation: Keep the fixed four-command cap: it addresses the measured backlog with a simple, predictable limit and preserves normal behavior outside HITL. The alternatives may help if shared-port load or reply backpressure proves problematic, but their complexity is not justified by this diff.

Files changed (2) +46 / -1

Bug fix (1) +9 / -1

msp_serial.cBounded multi-command MSP processing during HITL +9/-1

Bounded multi-command MSP processing during HITL

• Checks the HITL runtime flag to process up to four received commands per port run instead of one. Processing still stops after a command requiring post-processing, and non-HITL behavior remains single-command.

src/main/msp/msp_serial.c

Tests (1) +37 / -0

mavlink_unittest.ccCover normal and HITL MSP command limits +37/-0

Cover normal and HITL MSP command limits

• Adds queued-request tests showing that a normal run processes one command, while HITL processes four and leaves the remaining two for the next run.

src/test/unit/mavlink_unittest.cc

[github-actions]

RAM / Flash usage vs. base commit 13ebed9 — commit 6eaaec9

Target	Flash Δ	RAM Δ
MATEKF405	-32 B (-0.00%)	CCM: ±0 B (±0.00%) RAM: ±0 B (±0.00%)
MATEKF722	+48 B (+0.01%)	ITCM_RAM: ±0 B (±0.00%) RAM: ±0 B (±0.00%) TCM: ±0 B (±0.00%)
MATEKF765	+64 B (+0.01%)	DTCM_RAM: ±0 B (±0.00%) SRAM1: ±0 B (±0.00%)
MATEKH743	+64 B (+0.01%)	D2_RAM: ±0 B (±0.00%) DTCM_RAM: ±0 B (±0.00%) ITCM_RAM: ±0 B (±0.00%) RAM: ±0 B (±0.00%)

See RAM/flash optimization guide for techniques to reduce usage.

[github-actions]

Test firmware build ready — commit 6eaaec9

Download firmware for PR #12177

251 targets built. Find your board's .hex file by name on that page (e.g. MATEKF405SE.hex). Files are individually downloadable — no GitHub login required.

Development build for testing only. Use Full Chip Erase when flashing.

[qodo-free-for-open-source-projects]

Code Review by Qodo

🐞 Bugs (1) 📘 Rule violations (0) 📎 Requirement gaps (0) 🎨 UX issues (0) 🔗 Cross-repo conflicts (0) 📜 Skill insights (0)

1. UART users lose replies during a backlog ✓ Resolved

Description

mspSerialProcessOnePort() now executes successive commands without ensuring that their replies fit
in the UART transmit buffer, while mspSerialProcessReceivedCommand() ignores a failed send. When
several replies fill the 256-byte buffer in one run, later commands still execute but their
replies—including simulator OSD data—are silently discarded.

Code

src/main/msp/msp_serial.c[R575-576]

+                if (mspPostProcessFn || !ARMING_FLAG(SIMULATOR_MODE_HITL) || ++commandsThisRun >= MSP_HITL_COMMANDS_PER_RUN) {
+                    break; // process one command at a time so as not to block.

Evidence

The new loop permits multiple immediate sends. The existing send guard returns zero if a nonempty
transmit buffer lacks room, but command processing ignores that result; UART buffers have 255 usable
bytes, and simulator replies include an OSD payload.

src/main/msp/msp_serial.c[294-306]
src/main/msp/msp_serial.c[478-507]
src/main/drivers/serial_uart.h[22-43]
src/main/drivers/serial_uart.c[195-209]
src/main/fc/fc_msp.c[4415-4457]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
Batching commands on a UART can exhaust its transmit buffer and silently discard later replies.
## Fix Focus Areas
- src/main/msp/msp_serial.c[574-577]
- src/main/msp/msp_serial.c[278-310]
- src/main/msp/msp_serial.c[478-507]
## Recommended Fix
Make the batching path retain or defer a reply when transmit capacity is insufficient, rather than executing further requests whose responses cannot be sent. Add a test with multiple large replies and a constrained UART transmit buffer.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

---

2. A stalled USB host delays flight control ✓ Resolved

Description

mspSerialProcessOnePort() can now send four replies in one serial-task invocation, and each USB
flush can spin for over 50 ms when the host transmit buffer is full. Because the scheduler runs that
task to completion before selecting another, a stalled host can hold up flight-control tasks for
roughly 200 ms instead of one reply's timeout; timed-out flushes can also discard reply bytes.

Code

src/main/msp/msp_serial.c[R575-576]

+                if (mspPostProcessFn || !ARMING_FLAG(SIMULATOR_MODE_HITL) || ++commandsThisRun >= MSP_HITL_COMMANDS_PER_RUN) {
+                    break; // process one command at a time so as not to block.

Evidence

USB reports its transmit buffer as empty, so the MSP send guard does not prevent another reply. Each
end-write flush loops until the host accepts data or the 50 ms timeout expires, and the scheduler
does not invoke another task until the serial task returns.

src/main/drivers/serial_usb_vcp.c[50-50]
src/main/drivers/serial_usb_vcp.c[76-79]
src/main/drivers/serial_usb_vcp.c[122-159]
src/main/msp/msp_serial.c[294-306]
src/main/fc/fc_tasks.c[110-120]
src/main/scheduler/scheduler.c[321-334]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
Four consecutive USB replies can each wait for a full host buffer, blocking the cooperative scheduler well beyond the serial task's period.
## Fix Focus Areas
- src/main/msp/msp_serial.c[574-577]
- src/main/drivers/serial_usb_vcp.c[105-159]
## Recommended Fix
Apply a run-level transmit-time budget or defer further command processing when USB output cannot be flushed promptly. Ensure an unsuccessful flush is not reported as a successfully sent reply.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

---

3. Backlogged GPS fixes get too little weight 🐞 Bug ≡ Correctness

Description

Batching MSP_SIMULATOR commands lets successive GPS fixes call onNewGPSData() within one run,
where it derives each fix's time step from the few microseconds since the preceding call rather than
the simulator's sample interval. When a backlog contains multiple GPS-bearing frames, the last fix
supplies the estimator's updateDt, so its position and velocity corrections are scaled by that
near-zero interval.

Code

src/main/msp/msp_serial.c[R575-576]

+                if (mspPostProcessFn || !ARMING_FLAG(SIMULATOR_MODE_HITL) || ++commandsThisRun >= MSP_HITL_COMMANDS_PER_RUN) {
+                    break; // process one command at a time so as not to block.

Evidence

Each simulator GPS fix immediately reaches onNewGPSData(), which timestamps it using micros()
and overwrites updateDt from the elapsed processing time. The estimator subsequently multiplies
GPS corrections by that updateDt.

src/main/fc/fc_msp.c[4586-4616]
src/main/navigation/navigation_pos_estimator.c[175-187]
src/main/navigation/navigation_pos_estimator.c[239-245]
src/main/navigation/navigation_pos_estimator.c[279-287]
src/main/navigation/navigation_pos_estimator.c[693-715]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

## Issue description
Processing queued simulator GPS fixes back-to-back replaces their real sampling interval with the short command-processing interval used by navigation corrections.
## Fix Focus Areas
- src/main/msp/msp_serial.c[574-577]
- src/main/fc/fc_msp.c[4586-4616]
- src/main/navigation/navigation_pos_estimator.c[175-187]
- src/main/navigation/navigation_pos_estimator.c[239-245]
## Recommended Fix
Preserve an appropriate sample interval for simulator GPS updates during backlog draining, or defer GPS-bearing frames so consecutive fixes are not timed solely by their processing interval. Test two queued GPS fixes in one run and verify the estimator correction time step.

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

---

Tip of the day

💡 Did you know, you can start a comment with 'qodo' or '@qodo' to chat about any finding

More tips ↗ | Customize Qodo ↗ | Qodo docs ↗