feat(mavlink): Support MAV_CMD_DO_SET_GLOBAL_ORIGIN

[hamishwillee]

MAV_CMD_DO_SET_GLOBAL_ORIGIN was added in mavlink/mavlink#2247 to replace the old message version SET_GPS_GLOBAL_ORIGIN.
This has the same behaviour - it sets the origin, emits an ACK if the command succeeded, emitting GPS_GLOBAL_ORIGIN.
Note that this ACKs if the origin is not changed and still emits the message.

I have tested this by sending the command with different and same values and noting that the message is emitted in both cases.

Note:

• Unsupported params are not rejected - which we can fix with fix(mavlink): reject unsupported params in commands and missions #27541
• Invalid NaN altitude is ignored. We should perhaps fix that @julianoes ?
• Invalid lat-lon and NaN lat/lon are passed to EFK2 and then failed, not denied. @julianoes We can wait on the MAVLink update to be reviewed to test invalid lat-lon, but that won't fix that passing NaN to Lat/Lon is invalid in this message. Should we fix that particular case here?

Claude deetail on all this is below in details. The short version though is that this adds a VehicleCommand.msg enum for the command, then handles it everywhere that the old message is handled.

Also see:

• Add support for MAV_CMD_DO_SET_GLOBAL_ORIGIN (replaces SET_GPS_GLOBAL… mavlink/qgroundcontrol#14566

Details

PX4 already handles the legacy SET_GPS_GLOBAL_ORIGIN MAVLink message (ID 48) via
handle_message_set_gps_global_origin(), which converts it to the internal
VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN (100000) and publishes it for EKF2 to process.

MAV_CMD_DO_SET_GLOBAL_ORIGIN (command ID 611, development dialect) is the newer
COMMAND_INT-based replacement that supersedes the legacy message. It is currently
commented out in command_has_location() as "not supported". Adding support requires:

• Correct lat/lon decoding from COMMAND_INT's int32 degreeE7 format
• A new vehicle_command constant for the command ID so EKF2 can check for it without
  depending on MAVLink headers
• EKF2 (and the legacy estimator) handling the new command ID, so acks are sent back
  with the correct command ID (611, not 100000)

MAV_CMD_DO_SET_GLOBAL_ORIGIN is only defined in the MAVLink development dialect
(MAVLINK_ENABLED_DEVELOPMENT). Boards that don't use the development dialect must not
fail to compile.

Files to Modify

1. msg/versioned/VehicleCommand.msg

Add a constant for 611 near the other standard MAVLink commands (before line 126 where
PX4-internal commands begin):

uint16 VEHICLE_CMD_DO_SET_GLOBAL_ORIGIN = 611 # Sets GNSS coordinates of the vehicle local origin. Supersedes
SET_GPS_GLOBAL_ORIGIN message. |Unused|Unused|Unused|Unused|Latitude (WGS-84)|Longitude (WGS-84)|[m] Altitude
(AMSL)|

2. src/modules/mavlink/mavlink_receiver.cpp

In command_has_location() (line 537-553): Move MAV_CMD_DO_SET_GLOBAL_ORIGIN from
the commented-out "not supported" block into the supported return true section, guarded
so it only compiles when the development dialect is active:

case MAV_CMD_EXTERNAL_POSITION_ESTIMATE: // 43003
#ifdef MAVLINK_ENABLED_DEVELOPMENT
case MAV_CMD_DO_SET_GLOBAL_ORIGIN: // 611
#endif
return true;

No changes are needed to handle_message_command_both() — the command naturally falls
through to the default _cmd_pub.publish(vehicle_command) branch (line 856) with
send_ack = false, so EKF2 handles the ack.

3. src/modules/ekf2/EKF2.cpp

In the command handler (line 523): Add the new vehicle command constant alongside
the legacy one:

if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN
|| vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_GLOBAL_ORIGIN) {

Because command_ack.command = vehicle_command.command (line 519), the ack will echo
back 611 to the GCS when the new command is used, and 100000 for the legacy path.

4. src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

In the command handler (line 178): Same pattern as EKF2:

if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN
|| vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_GLOBAL_ORIGIN) {

Data Flow After Change

GCS sends COMMAND_INT (id=611, frame=GLOBAL, x=lat_1e7, y=lon_1e7, z=alt_m)
→ handle_message_command_int()
→ command_has_location(611) == true [new]
→ param5 = x / 1e7 (double degrees)
→ param6 = y / 1e7 (double degrees)
→ param7 = z (float meters)
→ vehicle_command.command = 611, published to uORB
→ EKF2 checks: command == 100000 || command == 611 [new: matches]
→ _ekf.setEkfGlobalOrigin(lat, lon, alt)
→ command_ack.command = 611, published
→ MAVLink streams COMMAND_ACK(611, ACCEPTED) back to GCS ✓

Legacy SET_GPS_GLOBAL_ORIGIN message path is unchanged (still uses 100000).

Frame note (MAVLink PR #2530)

Per mavlink/mavlink#2530, the correct
frame for MAV_CMD_DO_SET_GLOBAL_ORIGIN when sent as COMMAND_INT is MAV_FRAME_GLOBAL_INT
Because command_ack.command = vehicle_command.command (line 519), the ack will echo
back 611 to the GCS when the new command is used, and 100000 for the legacy path.

4. src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

In the command handler (line 178): Same pattern as EKF2:

if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN
|| vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_GLOBAL_ORIGIN) {

Data Flow After Change

GCS sends COMMAND_INT (id=611, frame=GLOBAL, x=lat_1e7, y=lon_1e7, z=alt_m)
→ handle_message_command_int()
→ command_has_location(611) == true [new]
→ param5 = x / 1e7 (double degrees)
→ param6 = y / 1e7 (double degrees)
→ param7 = z (float meters)
→ vehicle_command.command = 611, published to uORB
→ EKF2 checks: command == 100000 || command == 611 [new: matches]
→ _ekf.setEkfGlobalOrigin(lat, lon, alt)
→ command_ack.command = 611, published
→ MAVLink streams COMMAND_ACK(611, ACCEPTED) back to GCS ✓

Legacy SET_GPS_GLOBAL_ORIGIN message path is unchanged (still uses 100000).

Frame note (MAVLink PR #2530)

Per mavlink/mavlink#2530, the correct
frame for MAV_CMD_DO_SET_GLOBAL_ORIGIN when sent as COMMAND_INT is MAV_FRAME_GLOBAL_INT
(5), not MAV_FRAME_GLOBAL (0). Both are handled identically in PX4 — handle_message_command_int()
divides x/y by 1e7 for any non-LOCAL frame (the else branch at line 637) — so no
special frame handling is needed. The test code below uses the correct frame.

Verification
description and fix in a follow-up.

2. NaN altitude accepted

This is by EKF design — there's even a // altitude is optional comment on line 95 of ekf_helper.cpp.
setAltOrigin() returns false for NaN (via checkAltitudeValidity()), but setEkfGlobalOrigin() ignores that
return value and returns true, so EKF2 sends ACCEPTED with the altitude origin unchanged. The MAVLink spec for
MAV_CMD_DO_SET_GLOBAL_ORIGIN says altitude is required, so this is a spec divergence. However, the same
behaviour applies to the legacy path. Don't fix here — it needs an EKF-level decision about whether altitude
remains optional.

3. Non-NaN param1-4

PR #27541 is the right home for this — it builds a command lookup table for exactly this kind of param
validation. Once #27541 merges, add MAV_CMD_DO_SET_GLOBAL_ORIGIN to its table. Don't duplicate that mechanism
here.

Summary: Keep this PR focused on what it does — adding COMMAND_INT reception support. Document the three known
limitations in the PR description and link to #27541 for param1-4.

[github-actions]

🔎 FLASH Analysis

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%    +852  [ = ]       0    .debug_info
-0.0%     -11  [ = ]       0    .debug_line
 -80.0%      -4  [ = ]       0    [Unmapped]
  -0.0%      -7  [ = ]       0    [section .debug_line]
-0.0%     -15  [ = ]       0    .debug_loclists
+0.0%     +91  [ = ]       0    .debug_str
-0.0%     -16  [ = ]       0    .symtab
  -3.8%     -16  [ = ]       0    Commander::handle_command()
 -40.0%     -32  [ = ]       0    __param_get_default_value_veneer
   +67%     +32  [ = ]       0    __param_get_veneer
+0.0%    +956  [ = ]       0    TOTAL

px4_fmu-v6x [Total VM Diff: -8 byte (-0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%    +818  [ = ]       0    .debug_info
-0.0%      -9  [ = ]       0    .debug_line
  [DEL]      -2  [ = ]       0    [Unmapped]
  -0.0%      -7  [ = ]       0    [section .debug_line]
+0.0%      +7  [ = ]       0    .debug_loclists
+0.0%      +2  [ = ]       0    .debug_rnglists
+0.0%     +91  [ = ]       0    .debug_str
-0.0%     -16  [ = ]       0    .symtab
  -3.8%     -16  [ = ]       0    Commander::handle_command()
+0.2%      +8  [ = ]       0    [Unmapped]
-0.0%      -8  -0.0%      -8    .text
  -0.2%      -8  -0.2%      -8    Commander::handle_command()
+0.0%    +948  -0.0%      -8    TOTAL

Updated: 2025-07-30T08:06:56

[hamishwillee]

@julianoes Many tests fail, but I think that is unrelated.

I don't do any checking of the passed extra parameters. Given
See also mavlink/mavlink#2282, should I add a check at least for the affected param that if it is non-NaN the message should be rejected?

[Sayshara]

Sending MAV_CMD_DO_SET_GLOBAL_ORIGIN response with:

WARN [commander] command 611 unsupported
INFO [tone_alarm] notify negative

GPS_GLOBAL_ORIGIN is not emitted.

Edit: Ok, i see there is no handling of VEHICLE_CMD_DO_SET_GLOBAL_ORIGIN on PX4 side.

[github-actions]

This PR was identified as stale and it will be closed in 30 days unless any activity is detected.

[beniaminopozzan]

@julianoes was there something preventing this to be merged?

[julianoes]

It's conflicting now, and I don't see the actual code that makes this work? Somehow it disappeared from the diff?

[hamishwillee]

That is odd. I'm going to have to do this again, and retest. Let's leave it open, but I will push back into draft.

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/settings-for-topic-aux-global-position/48665/3

[hamishwillee]

FYI. Note, separate handling for new command and the old message-handled as a command - but in the same place. Same update had to happen in BlockLocalPositionEstimator.cpp‎ too.

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 24 byte (0 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +24  +0.0%     +24    .text
    +0.4%     +16  +0.4%     +16    Commander::handle_command()
    +0.3%      +8  +0.3%      +8    EKF2::Run()
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%    +969  [ = ]       0    .debug_info
  +0.0%     +24  [ = ]       0    .debug_line
    [NEW]      +3  [ = ]       0    [Unmapped]
    +0.0%     +21  [ = ]       0    [section .debug_line]
  +0.0%     +26  [ = ]       0    .debug_loclists
  +0.0%     +11  [ = ]       0    .debug_rnglists
     +50%      +1  [ = ]       0    [Unmapped]
    +0.0%     +10  [ = ]       0    [section .debug_rnglists]
  +0.0%     +91  [ = ]       0    .debug_str
  -0.3%     -24  [ = ]       0    [Unmapped]
  +0.0% +1.15Ki  +0.0%     +24    TOTAL

px4_fmu-v6x [Total VM Diff: 24 byte (0 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +24  +0.0%     +24    .text
    +0.4%     +16  +0.4%     +16    Commander::handle_command()
    +0.3%      +8  +0.3%      +8    EKF2::Run()
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%    +951  [ = ]       0    .debug_info
  +0.0%     +18  [ = ]       0    .debug_line
   -75.0%      -3  [ = ]       0    [Unmapped]
    +0.0%     +21  [ = ]       0    [section .debug_line]
  +0.0%     +31  [ = ]       0    .debug_loclists
  +0.0%     +10  [ = ]       0    .debug_rnglists
  +0.0%     +91  [ = ]       0    .debug_str
  -0.4%     -24  [ = ]       0    [Unmapped]
  +0.0% +1.13Ki  +0.0%     +24    TOTAL

Updated: 2026-07-01T21:41:57

[julianoes]

Looks right! And is backwards compatible.