fix(mavlink): correct BODY_NED position setpoint parsing and bitmasks

[PuRe-ToX]

Previously, 'SET_POSITION_TARGET_LOCAL_NED' messages using 'MAV_FRAME_BODY_NED' with position-only or partial-velocity bitmasks were incorrectly rejected. This fix properly rotates body-position offsets to local Earth coordinates and replaces the bitwise OR with AND for the velocity/acceleration ignore masks.

Solved Problem

Fixes #27617

The current implementation of 'MavlinkReceiver::handle_message_set_position_target_local_ned' incorrectly rejects valid 'MAV_FRAME_BODY_NED' messages from companion computers.

1. It forcefully sets the position to 'NAN', failing position-only setpoints.
2. It uses a bitwise 'OR' ('|') for velocity and acceleration ignore masks, causing the entire message to be rejected if even a single axis is masked out.

Solution

• Bitmask Logic: Replaced the bitwise '|' with '&&' when evaluating the 'ignore_velocity' and 'ignore_acceleration' booleans to perfectly match the 'LOCAL_NED' parsing behavior.
• Position Transformation: Added logic to handle body-frame position setpoints. The receiver now creates a 'uORB::Subscription' to 'vehicle_local_position', rotates the requested body-frame offsets by the vehicle's current yaw, and applies them to the current local position to generate a valid 'LOCAL_NED' target.

Changelog Entry

Bugfix: MavlinkReceiver now correctly parses position-only and partial-velocity SET_POSITION_TARGET_LOCAL_NED messages in the BODY_NED frame.

Alternatives

N/A. Without this fix, offboard companion computers cannot reliably use the BODY_NED frame for targeted positional movements or partial velocity vectors.

Test coverage

Tested locally in PX4 SITL (Gazebo gz_x500). Wrote a custom PyMAVLink script to send position-only MAV_FRAME_BODY_NED messages with a type_mask ignoring velocity and acceleration.

Before: Firmware rejected the command with WARN [mavlink] SET_POSITION_TARGET_LOCAL_NED invalid.

After: Firmware successfully parsed the bitmask, transformed the position coordinates, and accepted the command silently without throwing warnings.

Context
This is highly relevant for edge-AI companion computers (such as the Jetson Nano) that attempt to send relative coordinate movements to the flight controller via MAVLink.

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 248 byte (0.01 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +248  +0.0%    +248    .text
     +35%    +244   +35%    +244    MavlinkReceiver::handle_message_set_position_target_local_ned()
    [NEW]     +60  [NEW]     +60    CSWTCH.1917
    [NEW]     +42  [NEW]     +42    CSWTCH.2728
    [NEW]     +15  [NEW]     +15    CSWTCH.3446
    [NEW]     +15  [NEW]     +15    CSWTCH.3447
    +0.8%      +4  +0.8%      +4    MavlinkReceiver::handle_message_set_attitude_target()
    [DEL]     -15  [DEL]     -15    CSWTCH.3440
    [DEL]     -15  [DEL]     -15    CSWTCH.3441
    [DEL]     -42  [DEL]     -42    CSWTCH.2722
    [DEL]     -60  [DEL]     -60    CSWTCH.1911
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%      +8  [ = ]       0    .debug_frame
  +0.0%    +477  [ = ]       0    .debug_info
  +0.0%    +324  [ = ]       0    .debug_line
    [DEL]      -4  [ = ]       0    [Unmapped]
    +0.0%    +328  [ = ]       0    [section .debug_line]
  +0.0%     +74  [ = ]       0    .debug_loclists
  +0.0%     +29  [ = ]       0    .debug_rnglists
   -50.0%      -1  [ = ]       0    [Unmapped]
    +0.0%     +30  [ = ]       0    [section .debug_rnglists]
  +0.0%     +33  [ = ]       0    .debug_str
  +0.0%     +32  [ = ]       0    .symtab
    [DEL]     -32  [ = ]       0    CSWTCH.1911
    [NEW]     +32  [ = ]       0    CSWTCH.1917
    [DEL]     -32  [ = ]       0    CSWTCH.2722
    [NEW]     +32  [ = ]       0    CSWTCH.2728
    [DEL]     -48  [ = ]       0    CSWTCH.3440
    [DEL]     -48  [ = ]       0    CSWTCH.3441
    [NEW]     +48  [ = ]       0    CSWTCH.3446
    [NEW]     +48  [ = ]       0    CSWTCH.3447
     +67%     +32  [ = ]       0    MavlinkReceiver::handle_message_set_position_target_local_ned()
     +67%     +32  [ = ]       0    __arm_switchcontext_veneer
   -40.0%     -32  [ = ]       0    __nxsem_trywait_veneer
     +50%     +16  [ = ]       0    __stm32_ep0out_setup_veneer
   -33.3%     -16  [ = ]       0    __stm32_i2c_disable_reload_veneer
  -2.3%    -248  [ = ]       0    [Unmapped]
  +0.0% +1.01Ki  +0.0%    +248    TOTAL

px4_fmu-v6x [Total VM Diff: 248 byte (0.01 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +248  +0.0%    +248    .text
     +35%    +244   +35%    +244    MavlinkReceiver::handle_message_set_position_target_local_ned()
    [NEW]     +60  [NEW]     +60    CSWTCH.1917
    [NEW]     +42  [NEW]     +42    CSWTCH.2728
    [NEW]     +15  [NEW]     +15    CSWTCH.3446
    [NEW]     +15  [NEW]     +15    CSWTCH.3447
    +0.8%      +4  +0.8%      +4    MavlinkReceiver::handle_message_set_attitude_target()
    [DEL]     -15  [DEL]     -15    CSWTCH.3440
    [DEL]     -15  [DEL]     -15    CSWTCH.3441
    [DEL]     -42  [DEL]     -42    CSWTCH.2722
    [DEL]     -60  [DEL]     -60    CSWTCH.1911
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%      +8  [ = ]       0    .debug_frame
  +0.0%    +477  [ = ]       0    .debug_info
  +0.0%    +332  [ = ]       0    .debug_line
    +133%      +4  [ = ]       0    [Unmapped]
    +0.0%    +328  [ = ]       0    [section .debug_line]
  +0.0%     +74  [ = ]       0    .debug_loclists
  +0.0%     +29  [ = ]       0    .debug_rnglists
   -33.3%      -1  [ = ]       0    [Unmapped]
    +0.0%     +30  [ = ]       0    [section .debug_rnglists]
  +0.0%     +33  [ = ]       0    .debug_str
  +0.0%     +32  [ = ]       0    .symtab
    [DEL]     -32  [ = ]       0    CSWTCH.1911
    [NEW]     +32  [ = ]       0    CSWTCH.1917
    [DEL]     -32  [ = ]       0    CSWTCH.2722
    [NEW]     +32  [ = ]       0    CSWTCH.2728
    [DEL]     -48  [ = ]       0    CSWTCH.3440
    [DEL]     -48  [ = ]       0    CSWTCH.3441
    [NEW]     +48  [ = ]       0    CSWTCH.3446
    [NEW]     +48  [ = ]       0    CSWTCH.3447
     +67%     +32  [ = ]       0    MavlinkReceiver::handle_message_set_position_target_local_ned()
  -3.5%    -248  [ = ]       0    [Unmapped]
  +0.0% +1.02Ki  +0.0%    +248    TOTAL

Updated: 2026-06-17T19:57:36

[mrpollo]

Thanks for digging into #27617. Stepping back from the diff: this isn't actually a regression, and I don't think a code change is the right first move here.

BODY_NED position setpoints have never worked in PX4. Before the v1.12 offboard overhaul the coordinate frame was only ever applied to velocity, and the 2021 rework (#16739) kept exactly that: BODY_NED rotates velocity/acceleration and intentionally NANs position, because per the MAVLink spec BODY_NED is a rotated frame and "position in body frame" is ambiguous unless you mean an offset from the current position, which is a different frame (BODY_OFFSET_NED) that PX4 also never supported. Both frames are deprecated in MAVLink in favor of BODY_FRD.

The real bug was in our docs, which listed the supported input combinations and the supported coordinate frames as two independent lists. That's what misled the reporter in #27617, and it's fixed in #27806.

What this PR adds, "current position + yaw-rotated offset, re-anchored on every message", is a new feature with invented semantics, not a bugfix. Since offboard requires streaming setpoints above 2 Hz, a constant "2 m forward" target never converges and effectively becomes an unbounded velocity command. It would also silently flip existing BODY_NED velocity users into position hold if their type_mask doesn't set the position ignore bits. Whether PX4 should support body-relative position stepping at all, and on which frame (BODY_FRD being the non-deprecated candidate), is a design decision the maintainers need to make first.

The one piece of #27617 that's a genuine code quirk is the grouped velocity ignore bits in the BODY_NED branch (vx-only gets rejected). If you want to carve that out into a small focused PR we can discuss the right per-axis semantics there, keeping in mind that "ignore vy" isn't representable after rotating body to earth frame, and that ignored axes should stay NAN rather than become commanded zeros.

Closing this one: the position half needs a maintainers design decision before any implementation, and the mask fix deserves its own focused PR where basically none of this diff would carry over. Would genuinely welcome that PR if you're up for it, and the body-relative position question is worth raising, I'll bring it up with the maintainers.

That said, this was a well put together first contribution: clean diff, clear description, tested before submitting. Good job, and I hope to see you back for the mask fix.