MSP2 message exposing 3D attitude, position, and velocity

[yakorch]

User description

This PR implements a recent issue.
I believe the community working on autonomous navigation would benefit from the proposed message.

The implementation breaks attitude format compatibility with MSP_ATTITUDE message in favor of more accuracy.

Once the PR is accepted, I could add the documentation on MSP2_INAV_FULL_LOCAL_POSE in the corresponding Wiki page.

The changes have been tested using SITL. Compilation:

mkdir -p build/sitl
cmake -S . -B build/sitl -G Ninja -DSITL=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo
cmake --build build/sitl -j

SITL was started with build/sitl/inav_9.0.0_SITL.

A Python script was run to ensure the message is being read and decoded

import socket, struct, time

HOST, PORT, CMD_ID, FLAGS, TIMEOUT = "127.0.0.1", 5761, 0x2220, 0x00, 2.0

def crc8(data: bytes) -> int:
    c = 0
    for b in data:
        c ^= b
        for _ in range(8):
            c = ((c << 1) ^ 0xD5) & 0xFF if (c & 0x80) else (c << 1) & 0xFF
    return c

def mspv2_req(cmd: int, payload: bytes = b"", flags: int = 0) -> bytes:
    head = b"$X<" + struct.pack("<BHH", flags, cmd, len(payload))
    return head + payload + bytes([crc8(head[3:] + payload)])

def recv_mspv2(sock: socket.socket, timeout=TIMEOUT) -> bytes:
    sock.settimeout(timeout)
    buf, t0 = b"", time.time()
    while time.time() - t0 < timeout:
        chunk = sock.recv(4096)
        if not chunk: break
        buf += chunk
        i = min([x for x in (buf.find(b"$X>"), buf.find(b"$X!")) if x != -1], default=-1)
        if i == -1 or len(buf) < i + 8: continue
        _, cmd, size = struct.unpack_from("<BHH", buf, i + 3)
        need = i + 3 + 1 + 2 + 2 + size + 1
        if len(buf) < need: continue
        frame = buf[i:need]
        if crc8(frame[3:-1]) == frame[-1]:
            return frame
        buf = buf[need:]
    return b""

with socket.create_connection((HOST, PORT), timeout=TIMEOUT) as s:
    s.sendall(mspv2_req(CMD_ID))
    f = recv_mspv2(s)
    print("raw:", f)
    print("hex:", f.hex())
    if not f:
        print("Timeout or CRC mismatch.")
    else:
        kind = "OK" if f[2:3] == b">" else "ERR"
        flags, cmd, size = f[3], *struct.unpack_from("<HH", f, 4)
        pl = f[8:8+size]
        print(f"type={kind} flags=0x{flags:02X} cmd=0x{cmd:04X} size={size}B")
        if size >= 24:
            roll_cd, pitch_cd, yaw_cd = struct.unpack_from("<hhh", pl, 0)
            posX_cm, posY_cm, posZ_cm = struct.unpack_from("<iii", pl, 6)
            velX_cms, velY_cms, velZ_cms = struct.unpack_from("<hhh", pl, 18)
            print(f"Att: roll={roll_cd/100:.2f}° pitch={pitch_cd/100:.2f}° yaw={yaw_cd/100:.2f}°")
            print(f"Pos: X={posX_cm}cm Y={posY_cm}cm Z={posZ_cm}cm")
            print(f"Vel: X={velX_cms}cm/s Y={velY_cms}cm/s Z={velZ_cms}cm/s")
        else:
            print("Payload < 24 bytes.")

Output:

raw: b'$X>\x00 "\x18\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xa4'
hex: 24583e0020221800000000000000000000000000000000000000000000000000a4
type=OK flags=0x00 cmd=0x2220 size=24B
Att: roll=0.00° pitch=0.00° yaw=0.00°
Pos: X=0cm Y=0cm Z=0cm
Vel: X=0cm/s Y=0cm/s Z=0cm/s

I believe further testing can be omitted since the essence of the changes lies in just reading from the variables into a buffer, similar to what other MSP messages do.

---

PR Type

Enhancement

---

Description

This description is generated by an AI tool. It may have inaccuracies

• Add new MSP2 message for full 3D pose data

• Expose attitude, position, and velocity in single message

• Refactor attitude payload writing into reusable function

• Support autonomous navigation use cases

---

Diagram Walkthrough

flowchart LR
  A["MSP_ATTITUDE handler"] --> B["mspWriteAttitudePayload()"]
  B --> C["MSP2_INAV_FULL_LOCAL_POSE"]
  C --> D["Attitude + Position + Velocity"]
  E["Protocol Header"] --> C

Loading

File Walkthrough

Relevant files

Enhancement

fc_msp.c Add full pose MSP message handler                                                src/main/fc/fc_msp.c Extract attitude writing logic into mspWriteAttitudePayload() function Refactor MSP_ATTITUDE case to use new helper function Add MSP2_INAV_FULL_LOCAL_POSE case handler Write attitude, position, and velocity data to message buffer +17/-3    msp_protocol_v2_inav.h Define new MSP2 command constant                                                  src/main/msp/msp_protocol_v2_inav.h Define new MSP2_INAV_FULL_LOCAL_POSE command constant Set command ID to 0x2220 Fix formatting inconsistencies in existing definitions +6/-4

---

[qodo-merge-pro]

PR Reviewer Guide 🔍

Here are some key observations to aid the review process:

🎫 Ticket compliance analysis 🔶 11016 - Partially compliant Compliant requirements: Ensure attitude format matches MSP_ATTITUDE. Provide a single MSP message to avoid duplicate calls and keep pose components synchronized. Prefer solution that works without GPS (e.g., local/dead-reckoned pose). [using abs local state] Non-compliant requirements: Expose a full pose estimate via MSP, including 3D attitude, local/global position, and 3D velocity. [Global position not exposed; only local/abs pose provided] Requires further human verification: Confirm that posControl.actualState.abs corresponds to the desired local pose estimate across all navigation modes. Validate units/scaling and endianness against MSP v2 conventions and consumers. Verify SITL and on-hardware consistency, performance, and bandwidth impact at typical MSP rates.

⏱️ Estimated effort to review: 2 🔵🔵⚪⚪⚪

⚡ Recommended focus areas for review Units/Scaling The new payload writes attitude as U16 + yaw in degrees, position as U32 (casted from lrintf of float meters?), and velocity as U16. MSP traditionally defines specific units/scales (e.g., centi-degrees, cm, cm/s). Confirm that the chosen integer sizes and implicit units match existing MSP conventions and are documented, otherwise clients may misinterpret values or overflow. const navEstimatedPosVel_t *absoluteActualState = &posControl.actualState.abs; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { sbufWriteU32(dst, (int32_t)lrintf(absoluteActualState->pos.v[axis])); sbufWriteU16(dst, (int16_t)lrintf(absoluteActualState->vel.v[axis])); } Signedness/Range Position and velocity can be negative. Using sbufWriteU32 and sbufWriteU16 may corrupt negative values. Consider sbufWriteS32 for position and sbufWriteS16 for velocity to preserve sign and expected ranges. const navEstimatedPosVel_t *absoluteActualState = &posControl.actualState.abs; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { sbufWriteU32(dst, (int32_t)lrintf(absoluteActualState->pos.v[axis])); sbufWriteU16(dst, (int16_t)lrintf(absoluteActualState->vel.v[axis])); } Attitude Consistency MSP_ATTITUDE traditionally uses centi-degrees for roll/pitch/yaw. The helper writes roll and pitch as raw U16 and yaw as DECIDEGREES_TO_DEGREES, which may change scaling. Verify that this exactly matches MSP_ATTITUDE expectations and does not introduce regression for the MSP_ATTITUDE message now using the helper. void mspWriteAttitudePayload(sbuf_t *dst) { sbufWriteU16(dst, attitude.values.roll); sbufWriteU16(dst, attitude.values.pitch); sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); }

[qodo-merge-pro]

PR Code Suggestions ✨

Explore these optional code suggestions:

Category	Suggestion	Impact
General	Use signed integer serialization functions Replace sbufWriteU32 and sbufWriteU16 with sbufWriteS32 and sbufWriteS16 respectively, to correctly serialize signed integer values for position and velocity. src/main/fc/fc_msp.c [653-656] for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - sbufWriteU32(dst, (int32_t)lrintf(absoluteActualState->pos.v[axis])); - sbufWriteU16(dst, (int16_t)lrintf(absoluteActualState->vel.v[axis])); + sbufWriteS32(dst, (int32_t)lrintf(absoluteActualState->pos.v[axis])); + sbufWriteS16(dst, (int16_t)lrintf(absoluteActualState->vel.v[axis])); } Apply / Chat Suggestion importance[1-10]: 8 __ Why: The suggestion correctly identifies that signed data (int32_t, int16_t) is being written with functions for unsigned integers (sbufWriteU32, sbufWriteU16). This is semantically incorrect and relies on implicit type conversion, which can be confusing and potentially lead to data corruption bugs. Using the corresponding signed functions (sbufWriteS32, sbufWriteS16) improves code correctness, clarity, and type safety.	Medium
High-level	The new message payload is inconsistent The new MSP2_INAV_FULL_LOCAL_POSE message should use a consistent data format for all attitude axes (e.g., deci-degrees for roll, pitch, and yaw), instead of reusing the inconsistent format from the legacy MSP_ATTITUDE message. Examples: src/main/fc/fc_msp.c [352-357] void mspWriteAttitudePayload(sbuf_t *dst) { sbufWriteU16(dst, attitude.values.roll); sbufWriteU16(dst, attitude.values.pitch); sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); } src/main/fc/fc_msp.c [650-657] case MSP2_INAV_FULL_LOCAL_POSE: mspWriteAttitudePayload(dst); const navEstimatedPosVel_t *absoluteActualState = &posControl.actualState.abs; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { sbufWriteU32(dst, (int32_t)lrintf(absoluteActualState->pos.v[axis])); sbufWriteU16(dst, (int16_t)lrintf(absoluteActualState->vel.v[axis])); } break; Solution Walkthrough: Before: void mspWriteAttitudePayload(sbuf_t *dst) { // roll and pitch are in 0.1 degrees (deci-degrees) sbufWriteU16(dst, attitude.values.roll); sbufWriteU16(dst, attitude.values.pitch); // yaw is converted to degrees sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); } // ... in mspFcProcessOutCommand case MSP2_INAV_FULL_LOCAL_POSE: mspWriteAttitudePayload(dst); // Reuses inconsistent format // ... write position and velocity break; After: // ... in mspFcProcessOutCommand case MSP_ATTITUDE: // Keep legacy behavior for backward compatibility sbufWriteU16(dst, attitude.values.roll); sbufWriteU16(dst, attitude.values.pitch); sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); break; case MSP2_INAV_FULL_LOCAL_POSE: // Use consistent units (deci-degrees) for the new message sbufWriteU16(dst, attitude.values.roll); sbufWriteU16(dst, attitude.values.pitch); sbufWriteU16(dst, attitude.values.yaw); // No conversion for yaw // ... write position and velocity break; Suggestion importance[1-10]: 7 __ Why: The suggestion correctly identifies a design flaw where a new MSPv2 message inherits a known inconsistency from a legacy message, which is important to fix for API clarity and future usability.	Medium
Update

[sensei-hacker]

The AI bot sometimes says dumb things. Sometimes, it has helpful suggestions.
@yakorch can you comment on the three things it mentioned? Any validity to any of those things?

Also, has anyone tested this with an actual aircraft, or can someone do so, please?

[yakorch]

@sensei-hacker, thank you for taking a look at this PR.

Sure, I'll comment on AI bot suggestions.

• Units/Scaling/Signedness/Range: U16 and U32 were chosen for velocity and position, respectively, similarly to how MSP_ALTITUDE message is implemented. INAV units for velocity and position are cm/s and cm, respectively.
  The out-of-range warning is too cautious: 16 velocity bits allow for storing (absolute) velocity up to 327 m/s. 32 bits for position span 40 000 km. Should be no problem here. No unit manipulation is performed. Negative values either for position or velocity are handled properly because functions sbufWriteU16/32 basically copy the bytes.

• Attitude Consistency: However, the bot accurately pointed out that it is debatable whether one should adhere to the legacy yaw by sacrificing precision. My second commit reverted compatibility with MSP_ATTITUDE message, which converts yaw from decidegrees to degrees. MSP2_INAV_FULL_LOCAL_POSE writes yaw in decidegrees, in the same units as pitch and roll.

I only tested these changes in SITL.