From 5af722fc2a557e90ab7fed2d096834de23c5481f Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 25 Jun 2026 13:49:16 +1000 Subject: [PATCH] feat(mavlink): MAV_CMD_DO_SET_GLOBAL_ORIGIN added --- msg/versioned/VehicleCommand.msg | 1 + src/modules/commander/Commander.cpp | 1 + src/modules/ekf2/EKF2.cpp | 3 ++- .../local_position_estimator/BlockLocalPositionEstimator.cpp | 3 ++- src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 5 files changed, 9 insertions(+), 3 deletions(-) diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index fb3121eba390..c07c8fb905e1 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -128,6 +128,7 @@ uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehic uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Unused|Unused|Unused|Unused|Latitude (WGS-84)|Longitude (WGS-84)|[m] Altitude (AMSL from GNSS, positive above ground)| uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Unused|Unused|Unused|Unused|Unused|Unused| +uint16 VEHICLE_CMD_DO_SET_GLOBAL_ORIGIN = 611 # Sets GNSS coordinates of the vehicle local origin (0,0,0) position. Send as COMMAND_INT with MAV_FRAME_GLOBAL_INT. |Unused|Unused|Unused|Unused|Latitude (WGS-84)|Longitude (WGS-84)|[m] Altitude (AMSL)| uint16 VEHICLE_CMD_GUIDED_CHANGE_HEADING = 43002 # Change heading/course. param1: heading type (0=course-over-ground, 1=heading). param2: target [deg]. param3: max rate [deg/s]. |Heading type (HEADING_TYPE enum)|[deg] Target bearing [0..360]|[deg/s] Max rate of change|Unused|Unused|Unused|Unused| uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization. diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f6084aa13786..fad9692382e8 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1707,6 +1707,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: case vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE: case vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN: + case vehicle_command_s::VEHICLE_CMD_DO_SET_GLOBAL_ORIGIN: case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW: case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9cb699cdf5f9..d8b678ce04f6 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -520,7 +520,8 @@ void EKF2::Run() command_ack.target_system = vehicle_command.source_system; command_ack.target_component = vehicle_command.source_component; - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) { + 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) { double latitude = vehicle_command.param5; double longitude = vehicle_command.param6; float altitude = vehicle_command.param7; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 04ea159fb806..df5570b86830 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -175,7 +175,8 @@ void BlockLocalPositionEstimator::Run() vehicle_command_s vehicle_command; if (_vehicle_command_sub.update(&vehicle_command)) { - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) { + 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) { const double latitude = vehicle_command.param5; const double longitude = vehicle_command.param6; const float altitude = vehicle_command.param7; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b5f7b554671f..45d46f0cb358 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -550,6 +550,9 @@ MavlinkReceiver::command_has_location(uint16_t command) case MAV_CMD_DO_SET_ROI: // 201 case MAV_CMD_PAYLOAD_PREPARE_DEPLOY: // 30001 case MAV_CMD_EXTERNAL_POSITION_ESTIMATE: // 43003 +#ifdef MAVLINK_ENABLED_DEVELOPMENT + case MAV_CMD_DO_SET_GLOBAL_ORIGIN: // 611 +#endif return true; // Not supported by PX4 as COMMAND_INT (mission items or unimplemented) @@ -575,7 +578,6 @@ MavlinkReceiver::command_has_location(uint16_t command) // case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION: // 5003 // case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION: // 5004 // case MAV_CMD_NAV_RALLY_POINT: // 5100 - // case MAV_CMD_DO_SET_GLOBAL_ORIGIN: // 611 // case MAV_CMD_WAYPOINT_USER_1: // 31000 // case MAV_CMD_WAYPOINT_USER_2: // 31001 // case MAV_CMD_WAYPOINT_USER_3: // 31002