Telemetry::Health::is_global_position_ok does not gurarantee that global position estimate is ok

[junwoo091400]

Background

In the PX4's MAVSDK tests, the function wait_until_ready relies on the health_all_ok function of MAVSDK's telemetry returning true, to determine if the system is ready (sensor calibration, global position, local position estimate, etc).

void AutopilotTester::wait_until_ready()
{
    std::cout << time_str() << "Waiting for system to be ready" << std::endl;
    CHECK(poll_condition_with_timeout(
    [this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(30)));

    // FIXME: workaround to prevent race between PX4 switching to Hold mode
    // and us trying to arm and take off. If PX4 is not in Hold mode yet,
    // our arming presumably triggers a failsafe in manual mode.
    std::this_thread::sleep_for(std::chrono::seconds(1));
}

https://github.com/PX4/PX4-Autopilot/blob/de1fa11e968ce96a939020ae9194fd2ffe850411/test/mavsdk_tests/autopilot_tester.cpp#L81-L91

And this in turn relies on the Health message defined in MAVSDK-Proto, under the Telemetry:

    bool is_global_position_ok = 6 [(mavsdk.options.default_value)="false"]; // True if the global position estimate is good enough to fly in 'position control' mode

https://github.com/mavlink/MAVSDK-Proto/blob/b9a1c1a51fc3344ee914d0d625f1eafc4189660f/protos/telemetry/telemetry.proto#L604

Problem

As indicated in the proto file, the is_global_position_ok flag should indicate if global position estimate is good enough to fly in position mode. However, this is not the case as currently this flag is set to true simply if we have a 'healthy' GPS sensor:

    const bool global_position_ok =
        sys_status_present_enabled_health(sys_status, MAV_SYS_STATUS_SENSOR_GPS);

https://github.com/mavlink/MAVSDK/blob/main/src/mavsdk/plugins/telemetry/telemetry_impl.cpp#L1112-L1113

This is a problem since in the commander of PX4, there is a hysteresis that requires at least 3 seconds of valid GPS data to consider 'global position estimate' as valid. As shown here:

Therefore, this can lead to CI tests failing simply because the tester script thinks the vehicle is ready for position mode flight as the health_all_ok returned true, but in fact the vehicle can't arm since the 3 second hysteresis time hasn't passed and although we have a valid GPS data, our global position estimate is not valid yet.

Solutions

I think we need to redefine and change the telemetry_impl in MAVSDK to actually get the global position validity flag from the Commander, instead of pulling in the healthy flag from the SYS_STATUS message, as that simply gives the status of a 'GPS sensor'.

Any thoughts on this?

[JonasVautherin]

Nice post! Two questions:

1. Shouldn't PX4 send MAV_SYS_STATUS_SENSOR_GPS when it is actually ready to fly with it? Or is it a misinterpretation by MAVSDK and MAV_SYS_STATUS_SENSOR_GPS only means that the autopilot has a GNSS sensor (and nothing about the fix)?
2. If PX4 is using MAV_SYS_STATUS_SENSOR_GPS correctly, what would be the MAVLink way to know that it's ready to fly? Should MAVSDK look at the GpsInfo?

MAVSDK to actually get the global position validity flag from the Commander

Is there a MAVLink way to do that?

[julianoes]

to actually get the global position validity flag from the Commander

Yes we should, you're right! Which flag would that be? Is there one in SYS_STATUS?

[junwoo091400]

@bkueng hinted that there's a global_position_estimate event already in the Events interface (not implemented in PX4 yet), we could potentially leverage this to guarantee that position estimate is ok

[junwoo091400]

In https://github.com/PX4/PX4-Autopilot/pull/20489, a solution that solves half of the problem (doesn't check global position validity, but rather we can arm) was introduced. This global position estimate valid flag issue is still relevant. But at least now the SITL tests won't fail just because of this issue.

[julianoes]

I think we should start to use is_armable in MAVSDK everywhere. And we should switch to the events interface at some point. Contributions welcome :).

[swimmingseeds]

This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-community-q-a-march-01-2023/30941/1