MAVSDK 2.3.0: Ardupilot system not disconnecting after soft-reboot

[jnomikos]

I am running into a peculiar issue where MAVSDK isn't detecting a system disconnecting when rebooting the vehicle (by rebooting via QGC vehicle settings, rather than a hard reboot).

This is on Arducopter 4.5.4. The reason it is a problem is that I am subscribing to telemetry position, and upon soft rebooting, the position stops updating, probably because the system is stale after reboot.

This is specifically on MAVSDK 2.3.0.

I created this function to wait for disconnect (based on example code)

void cam::wait_for_mavsdk_disconnect()
{
    auto system = mavsdk_system.value();
    auto prom = std::promise<void>();
    auto fut = prom.get_future();
    mavsdk::System::IsConnectedHandle handle =
        mavsdk::Handle<bool>(system->subscribe_is_connected([&system, &prom, &handle](bool connected) {
            if (!connected) {
                std::cout << "Disconnection detected" << std::endl;
                system->unsubscribe_is_connected(handle);
                prom.set_value();
            }
        }));

    fut.wait();
}

This is how I subscribe to telemetry

bool cam::mavsdk_telemetry_init() {
    telemetry = std::make_shared<mavsdk::Telemetry>(mavsdk_system.value());

    std::cout << "Setting rate updates..." << '\n';

    auto prom = std::make_shared<std::promise<mavsdk::Telemetry::Result>>();
    auto future_result = prom->get_future();
    // Set position update rate to 10 Hz.
    telemetry->set_rate_position_async(10.0, [prom](mavsdk::Telemetry::Result result) {
        prom->set_value(result); //fulfill promise
    });

    //Block until promise is fulfilled (in callback function)
    const mavsdk::Telemetry::Result result = future_result.get();
    if (result != mavsdk::Telemetry::Result::Success) {
        // handle rate-setting failure (in this case print error)
        std::cout << "Setting telemetry rate failed (" << result << ")." << '\n';
        std::this_thread::sleep_for(std::chrono::seconds(2));
        mavsdk_telemetry_init();
    }

    telemetry->subscribe_position([this](mavsdk::Telemetry::Position position) {
        //std::cout << "Position: " << std::fixed << std::setprecision(6) << position.latitude_deg << ", " << std::fixed << std::setprecision(6) << position.longitude_deg << ", " <<
                  std::fixed << std::setprecision(6) << position.absolute_altitude_m << std::endl;
    });

Is it expected behavior for a system to not be disconnected on MAVSDK if soft rebooting?

[jnomikos]

As a temporary solution to this problem, I'm probably going to subscribe to that reboot command and unsubscribe and resubscribe to telemetry position when it happens

[jnomikos]

tl;dr

Rebooting via MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN does not actually make the system disconnect, which is fine, except telemetry subscriptions become stale. I do not know of a way to check if the system has rebooted, because the system stays connected.

[jnomikos]

My idea of a solution of listening for MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN did not work, likely due to the system rebooting before the message gets forwarded.

Next idea of a solution is to actively track the time since we last received a telemetry position message. If above a threshold value, we stop/start telemetry

Edit: For my particular use case, this worked

Somewhere in my class I have this. (I plan on refactoring this, but it's just proof of concept). I'm going to probably put this on a different thread

    while(true) {
        std::chrono::steady_clock::time_point currentTime = std::chrono::steady_clock::now();
        if(std::chrono::duration_cast<std::chrono::seconds>(currentTime - last_position_time).count() > 5) {
            std::cout << "No position updates for 5 seconds." << std::endl;
            // Destroy current telemetry object
            telemetry.reset();

            // Restart telemetry
            mavsdk_telemetry_init();
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

In my class, I have this private variable

std::chrono::steady_clock::time_point last_position_time;

In subscribe position:

telemetry->subscribe_position([this](mavsdk::Telemetry::Position position) {
        std::cout << "Position: " << std::fixed << std::setprecision(6) << position.latitude_deg << ", " << std::fixed << std::setprecision(6) << position.longitude_deg << ", " <<
                  std::fixed << std::setprecision(6) << position.absolute_altitude_m << std::endl;

last_position_time = std::chrono::steady_clock::now();

       // do stuff here with the data we have

    });

[julianoes]

I assume it doesn't actually "see" the disconnect because it appears so quickly, right?

I would propose exactly what you do, check for position updates to come, and if they stall, request them again.

[jnomikos]

I assume it doesn't actually "see" the disconnect because it appears so quickly, right?

It appears this is the reason.

In general, for my use case, I always want telemetry data either way, so probably for the best that I am actively tracking if I am still receiving it.