I have a drone with a flow sense, barometer, magnetometer and INS which I want to use for outdoor no-GPS flight in offboard mode using ROS2 interface.
In version 1.16 i used VehicleGlobalPosition to set the initial GPS position, but in 1.17 (from a month ago) it has stopped working. It is just before VehicleGlobalPosition is replaced with AuxGlobalPosition to set the topic aux_global_position. We have seen it work in 1.17 but have not been able to reproduce the result.
These are the values set in the PX4 code:
param set UAVCAN_ENABLE 3
param set-default EKF2_OF_CTRL 1
param set-default EKF2_GPS_CTRL 0
param set-default EKF2_RNG_CTRL 1
param set-default SENS_FLOW_ROT 2
param set-default SENS_FLOW_SCALE 1
param set-default UAVCAN_SUB_FLOW 1
param set-default UAVCAN_SUB_RNG 1
param set-default UAVCAN_SUB_IMU 1
param set-default EKF2_RNG_A_HMAX 10
param set-default EKF2_RNG_QLTY_T 0.2
param set-default UAVCAN_RNG_MIN 0.08
param set-default UAVCAN_RNG_MAX 30
param set-default SENS_FLOW_MINHGT 0.08
param set-default SENS_FLOW_MAXHGT 30
param set-default SENS_FLOW_MAXR 7.4
param set-default SYS_HAS_GPS 0
param set-default GPS_1_CONFIG 0
param set-default EKF2_AGP_CTRL 1
param set-default EKF2_HGT_REF 2
There is a good communication via ROS2 and I have a loop where the initial GPS position is sent to PX4. But it seems that in version 1.17 it is ignored.
Does anyone have a good idea have to proceed and get the problem solved? Should I downgrade to 1.16 or try to update to the latest master version?
Or am I missing some settings?
I got a bit further.
I think the problem is that the VehicleGlobalPosition/aux_global_position is a continues flow of GPS data. This is not what i needed, only the initial GPS position. The rest is handled by EKS/flow/vio etc.
But I still need to set the origin. I tried to open QGroundControl and could set an estimater origin, and voila, my companion received a GPS update. QGC is using mavlink and I think it is using the SET_GPS_GLOBAL_ORIGIN message, Using Vision or Motion Capture Systems for Position Estimation | PX4 Guide (main)
I think it is possible to set via a ROS2 command, VehicleCommand, VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN. But this macro is set to 100000u in code, not 48u as expected. 48u is not even present.
So, Am I off topic?
Hi Anders, I dug through the code but I haven’t personally tested this workflow. I think I see what’s going on.
1. Parameter rename
The old EKF2_AGP_CTRL parameter was replaced with multi-instance variants: EKF2_AGP0_CTRL, EKF2_AGP1_CTRL, etc. Your startup script setting EKF2_AGP_CTRL 1 is likely being silently ignored in v1.17.
Try changing it to EKF2_AGP0_CTRL 1.
2. Origin-only vs continuous position
AuxGlobalPosition is designed for continuous position fusion — an external positioning system feeding the EKF ongoing updates. If you only need to set the initial origin, the right approach is to publish a VehicleCommand on /fmu/in/vehicle_command with:
command = 100000 # VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN
param5 = latitude # decimal degrees
param6 = longitude # decimal degrees
param7 = altitude # meters AMSL
This is the same internal path that QGC’s “Set Estimator Origin” button uses — QGC sends the MAVLink message SET_GPS_GLOBAL_ORIGIN (msg ID 48), and PX4’s MAVLink receiver translates it into an internal vehicle_command with command 100000. Via ROS2 you bypass the MAVLink layer, so you use the internal command ID directly.
3. On SET_GPS_GLOBAL_ORIGIN vs command 48
You’re right that the MAVLink command ID 48 doesn’t appear in PX4 code — that’s because SET_GPS_GLOBAL_ORIGIN is a dedicated MAVLink message (not a COMMAND_LONG), and PX4 handles it in its MAVLink receiver and translates it to the internal command 100000. Worth noting: MAVLink has a newer command MAV_CMD_DO_SET_GLOBAL_ORIGIN (command ID 611) in the development spec that supersedes the old message, but PX4 doesn’t support it yet (PR #24697 is open but stalled).
In short: for setting just the origin, try VehicleCommand with command = 100000. If you do want continuous AGP fusion, update the parameter to EKF2_AGP0_CTRL
I solved the issue using vehicle_command. This is the code. Important note=>use param 5,6 and 7. It is in the doc but the offboard example have a function, publish_vehicle_command, but this only handles param 1,2 and 3.
void MissionCommand::set_gps_global_origin(double lat, double lon)
{
px4_msgs::msg::VehicleCommand msg{};
msg.param5 = lat;
msg.param6 = lon;
msg.param7 = 0;
msg.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN;
msg.target_system = 1;
msg.target_component = 0;
msg.source_system = 1;
msg.source_component = 1;
msg.confirmation = false;
msg.from_external = true;
msg.timestamp = node_->get_clock()->now().nanoseconds() / 1000;
vehicle_command_publisher_->publish(msg);
}
Thanks for the info!
IT will be quite valuable since we will be using VIO later.
Yeah, happy for someone to take over #24697 . I will get to it one day.