Mavros: PX4 seemingly ignores MAV_FRAME and uses MSL altitude instead of relative altitude when in orbit mode

I’m on Ubuntu 20.04 with ROS1 Noetic. I have a simulated PX4 quadcopter, which can be addressed using mavlink, which can in turn be addressed through mavros. I’m trying to get the drone to orbit a certain setpoint from a certain radius. I can get this to work using orbit mode, but the altitude at which this orbit will take place is the altitude above the mean sea level. I cannot get it to use relative altitude instead. Depending on my location, I may have to set altitude to something like 500 meters. What I want is for the altitude to be a distance between a drone and either its home location or the ground. That way, I don’t have to take the altitude of whatever location I happen to be at into account. If I set the altitude to 4 meters now, the drone tries to plow straight into the ground.

In my code, the drone first enters takeoff mode and flies to 4 meters above ground. At this point, it does not use mean sea level. I then try to enter orbit mode, but have to set the altitude of orbit mode to around 500 meters. The drone will then fly to the circle and start its circular path, as it is supposed to do.

It is possible that the drone’s home position is set to mean sea level, but this seems unlikely. Earlier in my code, I did set a home position by sending a mavros_msgs.CommandHome message to the /mavros/cmd/set_home service. The bool current_gps is set to true, everything else is set to 0. As I understand it, this means that the home position is set to the drone’s current GPS position, rather than the longitude, latitude, and altitude that are specified in the message (and that as mentioned are set to 0). I have tried setting the altitude to 500 just for good measure, but the behaviour did not change.

My suspicion is that the MAV_FRAME is getting ignored. This is a variable by which you can tell the system what coordinate system you want to work in. MAV_FRAME is specified in this link. I can engage orbit mode by either sending a CommandInt or a CommandLong message to the corresponding service. CommandInt is recommended since you can specify a MAV_FRAME that the drone should work with. The service that corresponds to this is /mavros/cmd/command_int, which itself sends a Command Int mavlink message. The command for orbit mode is numbered as 34, which is defined here. As long as I make sure to send a number that corresponds to a MAV_FRAME with relative altitude, e.g. number 3, 6, or 11 (as defined in the MAV_FRAME documentation), but this does not change anything. Even sending some randomly selected frame numbers does nothing. The x and y I’m sending right now are longitude and latitude times 10^7. This is working as expected. Radius seems also correct. Pretty much everything other than altitude seems to be doing what I want. I’d also like to be able to use local coordinates instead of GPS coordinates, but for that I also need to be able to change the MAV_FRAME/coordinate frame. This is why frame is a function parameter as seen below.

My orbit function is as follows:

ros::ServiceClient command_int_client = nh.serviceClient<mavros_msgs::CommandInt>("mavros/cmd/command_int"); //used for orbit commands

void orbit(uint8_t frame, double radius, int32_t x, int32_t y){
  mavros_msgs::CommandInt orbit_cmd; //CommandLong could be used along with /mavros/cmd/command service, but CommandInt can select a frame to work in
  orbit_cmd.request.broadcast = false;
  orbit_cmd.request.command=34; //orbit mode

  orbit_cmd.request.frame=frame;
  orbit_cmd.request.param1 = radius; // Radius of the circle 
  orbit_cmd.request.param2 = 0.5; //Tangential Velocity. 
  orbit_cmd.request.param3 = 0; //Orbit yaw behaviour 
  orbit_cmd.request.param4 = 4*M_PI; //Number of rotations in radians 
  orbit_cmd.request.x = x; //Center point latitude 
  orbit_cmd.request.y = y; //Center point longitude 
  orbit_cmd.request.z = 500.0;  //TODO altitude is currently sea level rather than home, because frame is completely ignored

  command_int_client.call(orbit_cmd);

  if(orbit_cmd.response.success){
    if(VERBOSE)ROS_INFO("Orbit Successful");
  }else{
    if(VERBOSE)ROS_INFO("Orbit Failed");
  }
}

Commands used to simulate a drone:

~/PX4-Autopilot$ make px4_sitl_default jmavsim
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
./QGroundControl.AppImage
1 Like