Different behavior from SimpleFlight vs PX4 (Control via API's)

[jasonhbartlett]

Is there some thing I need to do different in PX4 to make the drone behave the same as it does with SimpleFlight when controlling the drone via C++ Api's?

I have a simple Hello Drone program and when it runs I get a much different behavior when using PX4 rather than SimpleFlight. (Tested in both Unreal and Unity) With SimpleFlight it seems to behave as I expect given the program. But with PX4:

1. It rotates the drone slightly at takeoff
2. It seems to ignore the instructions as soon as they start. Almost blending them together as if it isn't waiting for the each instruction to complete before jumping into the next instruction. Here is my basic program: (Also, if there is better documentation for these API's to move the drone I would love to see it. )

   
   #include <iostream>
   #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"

int main() { using namespace std; msr::airlib::MultirotorRpcLibClient client;

cout << "Press Enter to enable API control\n";
cin.get();
client.enableApiControl(true);

cout << "Press Enter to arm the drone\n";
cin.get();
client.armDisarm(true);

cout << "Press Enter to takeoff\n";
cin.get();
client.takeoffAsync(5)->waitOnLastTask();

cout << "Press Enter to move up 20 meters at speed 10, then fly a 50 meter box pattern and return.\n" << endl; cin.get();
auto position = client.getMultirotorState().getPosition();
auto homepos = client.getMultirotorState().getPosition();

cout << "Moving up 20 meters.\n";
client.moveToPositionAsync(position.x(), position.y(), position.z() - 20.0f, 2.0f)->waitOnLastTask();

position = client.getMultirotorState().getPosition();

cout << "Moving north 50 meters.\n";
client.moveToPositionAsync(position.x() + 50.0f, position.y(), position.z(), 2.0f)->waitOnLastTask();
position = client.getMultirotorState().getPosition();

cout << "Moving east 50 meters.\n";
client.moveToPositionAsync(position.x(), position.y() + 50.0f, position.z(), 2.0f)->waitOnLastTask();
position = client.getMultirotorState().getPosition();

cout << "Moving south 50 meters.\n";
client.moveToPositionAsync(position.x() - 50.0f, position.y(), position.z(), 2.0f)->waitOnLastTask();
position = client.getMultirotorState().getPosition();

cout << "Moving west 50 meters.\n";
client.moveToPositionAsync(position.x(), position.y() - 50.0f, position.z(), 2.0f)->waitOnLastTask();
position = client.getMultirotorState().getPosition();

cout << "Moving home.\n";
client.moveToPositionAsync(homepos.x(), homepos.y(), homepos.z(), 2.0f)->waitOnLastTask();
position = client.getMultirotorState().getPosition();
client.hoverAsync()->waitOnLastTask();

cout << "Press Enter to land" << endl; cin.get();
client.landAsync()->waitOnLastTask();

std::cout << "Press Enter to disarm" << std::endl; std::cin.get();
client.armDisarm(false);

return 0;

}

Also, one more question.  Do these API's actually translate to Mavlink commands?  Or is there a different way to interface with the simulation drone using Mavlink commands directly?  (Other than QGroundControl) 

[lovettchris]

Issue 1 is issue #1340

Issue 2 might be that PX4 is more picky about client.enableApiControl(true); The reason is if you are doing autonomous flight with a real drone and something goes wrong, you need to be able to let the user take over flight using remote control by flipping a "mode switch" so drone is in manual control and no longer listens to autonomous commands. This way if a moveToPositionAsync is going rogue, the user can save the drone from crashing. For this to work, when a mode switch is detected PX4 turns off API Control and stops listening to them. This happens also in the scripts any time you do a NON offboard command like takeoff, land, and hover. So for PX4 scripts you will probably need to move the client.enableApiControl(true); call to after the takeoff succeeded. Notice also that PX4 will also bump the drone out of API control (also known as offboard control) if your script sleeps for more than about 100 milliseconds, this is also a safety feature, so if you are in the debugger for example and you stop on one of these lines then it stops AirSim from sending offboard commands and that will cause PX4 to disable offboard control. This is protection against an offboard agent that hangs.

And yes, for PX4 drones these commands are sent to the PX4 using mavlink protocol. SimpleFlight does not use mavlink. So yes, you could control PX4 directly over mavlink but it may require setting up some MavLinkConnection proxies, see Intercepting MavLink messages.

[jasonhbartlett]

Hi @lovettchris I appreciate your responses. This is good to know and I'll experiment more with the structure of these move API's. (Would love to see a few more examples included in the repo)

[jasonhbartlett]

Hi @lovettchris I seem to still be having trouble with this. I always loose control to the Failsafe after this line completes:

client.moveToPositionAsync(position.x(), position.y(), position.z() - 20.0f, velocity,
 timeout, drivetrain, yaw_mode)->waitOnLastTask();

Here is the whole program:

#include "common/common_utils/StrictMode.hpp"
STRICT_MODE_OFF
#ifndef RPCLIB_MSGPACK
#define RPCLIB_MSGPACK clmdep_msgpack
#endif // !RPCLIB_MSGPACK
#include "rpc/rpc_error.h"
STRICT_MODE_ON

#include <iostream>
#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"

int main()
{
    using namespace msr::airlib;

    msr::airlib::MultirotorRpcLibClient client;

    try {
        client.confirmConnection();

        std::cout << "Press Enter to arm the drone\n"; // << endl; std::cin.get();
        client.armDisarm(true);

        std::cout << "Press Enter to takeoff\n";
        std::cin.get();
        client.takeoffAsync(5)->waitOnLastTask();

        // switch to explicit hover mode so that this is the fall back when 
        // move* commands are finished.
        std::this_thread::sleep_for(std::chrono::duration<double>(5));
        client.hoverAsync()->waitOnLastTask();

        std::cout << "Press enter to begin movement.\n" << std::endl; std::cin.get();

        auto velocity = 2.0f;
        auto timeout = 300.0f;
        DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom;
        YawMode yaw_mode = YawMode(true, 0);

        std::cout << "Press enter to move up 20 meters.\n"; std::cin.get();
        client.enableApiControl(true);
        auto position = client.getMultirotorState().getPosition();

        client.moveToPositionAsync(position.x(), position.y(), position.z() - 20.0f, velocity, timeout, drivetrain, yaw_mode)->waitOnLastTask();
        // Once this line completes, no matter what I do I loose control to the failsafe!
        // Even if I request control again with the next line:
        client.enableApiControl(true);

        std::cout << "Press enter to Move north 20 meters\n"; std::cin.get();
        client.enableApiControl(true);
        position = client.getMultirotorState().getPosition();
        client.moveToPositionAsync(position.x() + 20.0f, position.y(), position.z(), velocity, timeout, drivetrain, yaw_mode)->waitOnLastTask();

        client.hoverAsync()->waitOnLastTask();

        std::cout << "Press Enter to land" << std::endl;  std::cin.get();
        client.landAsync()->waitOnLastTask();

        std::cout << "Press Enter to disarm" << std::endl; std::cin.get();
        client.armDisarm(false);

    }
    catch (rpc::rpc_error&  e) {
        std::string msg = e.get_error().as<std::string>();
        std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl;
    }

    return 0;
}

As you can see I have tried requesting control again immediately with another call to client.enableApiControl(true).

IF I omit that, THEN sometimes I get an exception:

rpclib: function 'moveToPosition" (called with 10 arg(s)) threw an exception.
 The exception contained this information: You must call requestControl first...

Here is a what my PX4 params look like: param show

I believe the only change I've made to the defaults are:

// enable the new position estimator module:
param set SYS_MC_EST_GROUP 2

// increase default limits on cruise speed so you can move around a large map more quickly.
param set MPC_XY_CRUISE 10
param set MPC_XY_VEL_MAX 10
param set MPC_Z_VEL_MAX_DN 2

// increase timeout for auto-disarm on landing
param set COM_DISARM_LAND 60

// make it possible to fly without radio control attached (do NOT do this one on a real drone)
param set NAV_RCL_ACT 0

// enable new syslogger to get more information from PX4 logs
param set SYS_LOGGER 1

// You might also want to set this one so that the drone automatically hovers after each
// offboard control command (the default setting is to land):
param set COM_OBL_ACT 1

Lastly, in testing the default Hello Drone without any changes, when starting the process of flying in a box pattern, the drone doesn't stay at that z position, but instead drifts upwards while it flies in the box pattern. With SimpleFlight it flies as it should.

I'm thinking there are some more PX4 params that need to be adjusted. My PX4 is running in Bash on Windows in SITL. Could the delay in communicating to that (the shell running it's own network stack on the same windows box) be causing a problem?

I would like to get my setup for SITL exactly the same as you have and test the same program you have and verify that I can get the exact same results. If you wouldn't mind sharing your param file and another hello drone type program.

[jasonhbartlett]

One additional item of interest. When I print the position:

        std::cout << "Hovering at ("
            << position.x() << ", "
            << position.y() << ", "
            << position.z() << ")\n";

I am always getting (0, 0, 0). So does this mean that auto position = client.getMultirotorState().getPosition(); isn't working? What is the correct way to get the position for the drone when using PX4 SITL?

[jasonhbartlett]

I have replaced

auto position = client.getMultirotorState().getPosition(); 

with

auto position = client.simGetGroundTruthKinematics().pose.position;

which at least shows me something other than zeros. But I suspect it shouldn't matter which I use, based on the fact that the drone changed position as expected in response to the command in either case.

I would like to know why I show zero's when using the first method.

In any case I'm still not able to follow one client.moveToPositionAsync()->waitOnLastTask(); with another. It just runs right through them until the

std::cout << "Press Enter to land" << std::endl; 
        std::cin.get();
        client.landAsync()->waitOnLastTask();

How exactly is that waitOnLastTask() supposed to work? According to the documentation: "If you want to wait for this task to complete then you can call waitOnLastTask like this:" and then "If you start another command then it automatically cancels the previous task and starts new command".

This is confusing me. How do I write a program to fly in a basic box pattern? Do I need to go ahead and check if position matches my desired position?

[jasonhbartlett]

FYI - Reason for this behavior is identified in Issue #1643 as a bug in AirSim where @lovettchris describes that "sometimes the moveToPositionAsync call terminates prematurely, possibly thinking it has already reached the intended destination, when it clearly has not" and "we have a repro for the bug and we should make this a high priority item". That issue was opened by @love-mj for a different problem however, while this issue relates directly to the moveToPositionAsync call bug. Therefore this issue still remains as I don't know of any other issues tracking that bug. If there is another issue tracking that, please let me know so we can close this one.

[jasonhbartlett]

Just curious if there was any update on this? @lovettchris

[marcociara379]

I have the same issue of @jasonhbartlett for what concerns the PX4 SITL position from getMultirotorState, that is always (0,0,0)

[jonyMarino]

Closed due to age. Please feel free to open a new issue (referring to this one) if you still have this problem. Thanks!

[lovettchris]

I just want to also point out the latest docs for PX4 setup usage include these important parameters and these ensure the PX4 doesn't drop out of offboard flight because of a failsafe trigger:

            "Parameters": {
                "NAV_RCL_ACT": 0,
                "NAV_DLL_ACT": 0,
                "COM_OBL_ACT": 1,
                "LPE_LAT": 47.641468,
                "LPE_LON": -122.140165
            }

[jonyMarino]

Thanks Chris!