COMMAND_DENIED issue when attempting do_orbit() command for fixedwing

[patrickcgray]

I've been trying to run the do_orbit command for a fixed wing drone that I'm developing. This is running on a raspberry pi and a pixhawk. I am not tied to this command I just need a command that will get my plane to go to a specific lat,lon,alt. Is this the correct one? My code here is basically the same as the goto.py example but using the do_orbit command.

#!/usr/bin/env python3

import asyncio
from mavsdk import System
import mavsdk

async def run():
    drone = System()
    await drone.connect(system_address="serial:///dev/serial0:57600")

    print("Waiting for drone to connect...")
    async for state in drone.core.connection_state():
        if state.is_connected:
            print(f"Drone discovered!")
            break

    print("Waiting for drone to have a global position estimate...")
    async for health in drone.telemetry.health():
        if health.is_global_position_ok:
            print("Global position estimate ok")
            break

    print("Waiting for drone to have a global position estimate...")
    async for health in drone.telemetry.health():
        if health.is_global_position_ok:
            print("Global position estimate ok")
            break

    print("-- Arming")
    await drone.action.arm()

    print("-- Taking off")
    await drone.action.takeoff()

    await asyncio.sleep(1)
    await drone.action.do_orbit(20, 3, mavsdk.action.OrbitYawBehavior(2), 34.715, -76.6730, 20.0)

    # i've also tried this goto but it doesn't do anything
    #await drone.action.goto_location(34.715048, -76.673046, 10, 0)

if __name__ == "__main__":
    loop = asyncio.get_event_loop()
    loop.run_until_complete(run())

And this is the output and error:

Waiting for mavsdk_server to be ready...
Connected to mavsdk_server!
Waiting for drone to connect...
Drone discovered!
Waiting for drone to have a global position estimate...
Global position estimate ok
-- Arming
-- Taking off
Traceback (most recent call last):
  File "goto.py", line 43, in <module>
    loop.run_until_complete(run())
  File "/usr/lib/python3.7/asyncio/base_events.py", line 584, in run_until_complete
    return future.result()
  File "goto.py", line 38, in run
    await drone.action.do_orbit(20, 3, mavsdk.action.OrbitYawBehavior(2), 34.715, -76.6730, 20.0)
  File "/home/pi/.local/lib/python3.7/site-packages/mavsdk/action.py", line 591, in do_orbit
    raise ActionError(result, "do_orbit()", radius_m, velocity_ms, yaw_behavior, latitude_deg, longitude_deg, absolute_altitude_m)
mavsdk.action.ActionError: COMMAND_DENIED: 'Command Denied'; origin: do_orbit(); params: (20, 3, <OrbitYawBehavior.UNCONTROLLED: 2>, 34.715, -76.673, 20.0)
Exception ignored in: <function System.__del__ at 0xb50ca108>
Traceback (most recent call last):
  File "/home/pi/.local/lib/python3.7/site-packages/mavsdk/system.py", line 56, in __del__
  File "/home/pi/.local/lib/python3.7/site-packages/mavsdk/system.py", line 92, in _stop_mavsdk_server
ImportError: sys.meta_path is None, Python is likely shutting down

[JonasVautherin]

COMMAND_DENIED

This means that the autopilot received the command and denied it for some reason (not ready, not supported, ...). Are you using PX4, and does your version of PX4 support that command?

[julianoes]

I'm pretty sure ORBIT is not supported by PX4 fixedwing. Usually Loiter is used instead. You can get into Loiter mode using mission.pause_mission() to loiter at the current location and you should be able to set the loiter radius using the param NAV_LOITER_RAD.

I can see how that's not optimal though and a nicer interface exposing the loiter location would be nice for this case.

[JonasVautherin]

@julianoes: Would it make sense to do that from the MAVSDK implementation when do_orbit() is called for a fixedwing (i.e. loiter around the center of the orbit with the corresponding radius)? I guess that the correct way would be to add support to PX4, but I don't know if this workaround is worth it or not in the meantime...

[patrickcgray]

Thanks for the insight @julianoes and @JonasVautherin. Yes I am using PX4. Using the mission.pause_mission() command is fine for our use case and I would assume the approach would be to use goto_location() and then mission.pause_mission() correct? If so, that doesn't seem to be working because the goto_location() command just doesn't seem to elicit any response from my aircraft whereas all the others generate some response even if it is an error. Do you have any recommendations for what to use to send a fixedwing to a specificed lat/lon if this isn't the correct command? Thanks again!

[JonasVautherin]

How do you do that in QGC? Or do you know the mavlink messages that need to be sent to do that?

[patrickcgray]

In QGC you're just able to right click on the map and click "goto location" that will send a goto command to the drone. I feel like I must be missing something, shouldn't sending a command for a vehicle to go to a specific lat/lon/alt be a really common use case within mavsdk?

I've changed our firmware to be a copter and when I try to run the goto command (e.g. drone.action.goto_location(34.715048, -76.673046, 10, 0)) it doesn't receive any confirmation within QGC. Is this the intended response or should I get some kind of confirmation? Is there a way with mavsdk to get confirmation that it has started flying off towards a point?

[JonasVautherin]

goto_location sends the MAV_CMD_DO_REPOSITION command (see here):

void ActionImpl::goto_location_async(
    const double latitude_deg,
    const double longitude_deg,
    const float altitude_amsl_m,
    const float yaw_deg,
    const Action::ResultCallback& callback)
{
    MavlinkCommandSender::CommandInt command{};

    command.command = MAV_CMD_DO_REPOSITION;
    command.target_component_id = _parent->get_autopilot_id();
    command.params.param4 = to_rad_from_deg(yaw_deg);
    command.params.x = int32_t(std::round(latitude_deg * 1e7));
    command.params.y = int32_t(std::round(longitude_deg * 1e7));
    command.params.z = altitude_amsl_m;

    _parent->send_command_async(
        command, [this, callback](MavlinkCommandSender::Result result, float) {
            command_result_callback(result, callback);
        });
}

If the ActionResult is Success, it means that the command was accepted by the autopilot.

I would assume the approach would be to use goto_location() and then mission.pause_mission() correct?

Did you try the other way round? First pause_mission() to go to loiter mode, and then use goto_location?

[patrickcgray]

Where should I find the ActionResult? If I try to print the return from await drone.action.goto_location() it is None even after waiting until the goto should be finished.

To debug I've now set up and run this in the simulator (gazebo running headless via Docker) and after running the goto_location() command I still don't get any sort of confirmation from either python or the psh> prompt. Should I receive some sort of confirmation or how can I access ActionResult? I ran the mission example and it completed without any issue so I think things are working I'm just not getting this information.

Thanks for all your help here!

[JonasVautherin]

In python, anything other than a Success result will raise an exception. If await drone.action.goto_location() does not raise an exception, it means that it was a Success 👍.

[patrickcgray]

Okay that's good news! But then what is the correct procedure for polling whether or not the goto_location command is finished? AKA that the drone is now at the intended location? I've coded up this loop which works correctly but is there a preferred approach other that something like this?

await drone.action.goto_location(goto_lat, goto_lon, 50, 0)

print('drone is headed toward goto location point')
async for pos in drone.telemetry.position():
    print(pos.latitude_deg)
    print(pos.longitude_deg)
    dist = geopy.distance.distance((goto_lat, goto_lon), (pos.latitude_deg, pos.longitude_deg)).km
    print('dist in km to goto point is: ', dist)
    if dist < 0.25:
        print("Goto position has been reached")
        break
    await asyncio.sleep(3)

[JonasVautherin]

My understanding is that "goto" changes the position setpoint. So you tell the drone that it should "stay there" instead of "staying here", and it will consider that its current position is an error an correct for it (as in, it will correct for the position drift instead of "going there" as an action). Which means that it does not tell you that it has reached the new setpoint (because it does not always tell you when it corrects for some drift, right?).

For a multicopter, you can check the position manually. For a fixedwing, I don't know... does it fly straight to the goto location and then loiter around it?