Skip to content

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

Open
PuRe-ToX wants to merge 4 commits into
PX4:mainfrom
PuRe-ToX:fix-mavlink-body-ned
Open

fix(mavlink): correct BODY_NED position setpoint parsing and bitmasks#27686
PuRe-ToX wants to merge 4 commits into
PX4:mainfrom
PuRe-ToX:fix-mavlink-body-ned

Conversation

@PuRe-ToX

@PuRe-ToX PuRe-ToX commented Jun 17, 2026

Copy link
Copy Markdown

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.

@PuRe-ToX
fix(mavlink): correct BODY_NED position setpoint parsing and bitmasks
d786e2f 
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 velocity/acceleration ignore masks.
@github-actions github-actions Bot added kind:bug Something is broken or behaving incorrectly. scope:offboard Offboard mode, external setpoints, companion-computer control, or offboard failsafe behavior. scope:mavlink MAVLink module, streams, commands, or protocol handling. labels Jun 17, 2026
@github-actions

github-actions Bot commented Jun 17, 2026 •
edited
Loading

Copy link
Copy Markdown
Contributor

🔎 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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

No reviews

Assignees

No one assigned

Labels

kind:bug Something is broken or behaving incorrectly. scope:mavlink MAVLink module, streams, commands, or protocol handling. scope:offboard Offboard mode, external setpoints, companion-computer control, or offboard failsafe behavior.

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

[Bug] SET_POSITION_TARGET_LOCAL_NED rejects BODY_NED position-only setpoints

1 participant

@PuRe-ToX