FlightTask Subscription Initialization

[Stifael]

Describe the bug
FlightTaskAuto depends on theposition_setpoint_triplet-msg. During the activation of FlightTaskAuto, it can happen that FlightTaskAuto is activated with an old position_setpoint_triplet msg from a previous publification.

To Reproduce
Steps to reproduce the behavior in simulation:

1. Add a print for the type of triplet-msg during activation here: PX4_INFO("triplet type: %d", _sub_triplet_setpoint.get().current.type);

2. Start simulation

3. Wait until the vehicle has a GPS lock and can be put into Hold

4. Arm

5. Give thrust-input to takeoff which also puts the vehicle into Manual Position-mode

6. Do a GoTo maneuver: observe the prints, where the triplets are of type 5, which is equal to IDLE and hence zero thrust (https://github.com/PX4/Firmware/blob/master/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp#L114-L119)

Expected behavior
During the initialization, old triplets should be ignored.

Additional context
A solution to this problem could be to reset a specific uorb-message once a FlightTask has been exited to prevent an initialization with an old message.

Depending on the delay between the activation and a new triplet, the vehicle might enter free fall due to zero thrust.

[dagar]

When changing nav states there's a race condition here with navigator. Without a larger change what do you want the FlightTask to do before the position_setpoint_triplet is available?

[Stifael]

without any larger refactor, I see two options:

replace lazy publisher

Currently, triplets are implemented as a lazy publisher, where the triplets are only published if a new triplet is available and the navigator is running (not sure about the latter). A solution could be to replace the lazy publisher with a stream where the messages are invalid if the navigator is not running.

subscriber ability to flush out the current message

Another option could be the ability to reset uorb-data to its default whenever requested. This way the Flighttask or any other subscriber can reset the data if no longer needed.

[dagar]

We can't figure it out based on metadata? There's the timestamp of the triplet (publication), but also the individual timestamps of the current, prev, and next. The timestamp of when the control mode first enabled auto would be the thing to compare it again from the FlightTaskAuto activation perspective.

[Stifael]

The timestamp of when the control mode first enabled auto would be the thing to compare it again from the FlightTaskAuto activation perspective

Why compare the timestamp between auto and FlightTaskAuto?

On our fork the change that I did was similar, but I compared the timestamp between the triplets and FlightTaskAuto activation.

However, there can still be the case where a new triplet is published before the Flighttask has been activated. If that is the case, the vehicle will not execute the new position_setpoint_triplet, but rather will hover at the current location until the user just restarts the mission again. This case, however, has not been encountered yet. It would be an issue during low-battery RTL though.

On the other hand, what could work is to compare the timestamp between the triplets and when auto was activated. This would guarantee that the triplets were generated after the commander switched to auto.

I still consider the meta-data comparison an option if there is no other solution, especially it is not just an auto/triplet issue.

What about my suggestion to flush out / reset the current uorb message?

[Stifael]

it turns out that it is not as simple as originally hoped.
The vehicle_status msg is getting published continuously. As a consequence it can happen that the line

if (_vehicle_status.timestamp >= _triplet_setpoint.timestamp)

is true within FlightTasks because a newer vehicle_status has already been received. The result would be that the triplet is always ignored.

[dagar]

In vehicle_status we could separately have a timestamp for the last nav_state change.

[Stifael]

yep, that would work.

Or the navigator just sends out a position_setpoint_triplet.valid = false if vehicle_status.nav_state switches from a Auto-mode into a non-auto mode. Not sure how easy that would be.

[dagar]

Related idea to this, but beyond the scope of the immediate fix.

• mc_pos_control split out FlightTasks into new dedicated flight_tasks module
• move navigator and new flight_tasks module to shared WQ thread (wq:guidance?)
• navigator additionally runs immediately on vehicle_status updates (potential nav_state change)
• flight_tasks callback scheduled to run immediately on position_setpoint_triplet update

The end result would be when changing into Auto the flight task would first run (activation) right after the actual position_setpoint_triplet publication.

@MaEtUgR FYI

[julianoes]

@MaEtUgR and I just ran into another race condition with this. This time the culprit is that we never "unpublish" the issue, so we don't publish if valid is not true here:

https://github.com/PX4/Firmware/blob/01c7a475ff36a4c1df9b17ccbdae62b5967632cf/src/modules/navigator/navigator_main.cpp#L731-L734

[dagar]

Can you expand on the details? I'd like to think about a complete solution we can work towards that will get us beyond race condition whack-a-mole.

[Stifael]

I think the root of the race condition whack-a-mole is the way the modules trust the commander. The way it is implemented right now, the commander would have to know everything to actually make a valid state-transition.

[Stifael]

we never "unpublish" the issue, so we don't publish if valid is not true here:

That's what I meant with having an outdated triplet.

[julianoes]

Can you expand on the details?

#13533
#13534

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.