PathController: Undesired behaviour for (very) small Time of Arrival factors

[krisgry]

Hi,
When flying a plan consisting of Goto maneuvers with a ToA factor of 1 second, I get some undesired behaviour: instead of using the height of the end of the previous maneuver as the starting height, PathController uses the current height of the UAV. For example, when flying Gotos with height of 250 and 200 m, it flies towards the first Goto. When switching to the next Goto, the start_z in PathControlState is e.g. 248.42 m (the height the UAV had when it reached the first Goto), instead of 250m. When running the exact same plan with ToA=3, I get the expected behaviour.

I believe that this is caused by the following if:

dune/src/DUNE/Control/PathController.cpp

Lines 318 to 321 in 273ef7d

	else if ((!m_tracking && now - m_ts.end_time > 1) ||
	(!m_ts.nearby && !m_ts.loitering) ||
	(dpath->flags & IMC::DesiredPath::FL_DIRECT) != 0)
	{

From printouts, I see that the if is entered since m_ts.nearby is false, which I find a bit strange since the DesiredPath message is sent (meaning that we have been nearby, so that the maneuver complete has been signaled). I've tried to trace this to its source, but I'm currently in a big rabbit hole, seeking advice :)

Inputs are appreciated. Do you agree that this is a bug? Ideas to what I should check?

I will continue to dig once my motivation has recovered.

[mariacosta]

Hi krisgry,
I finally had the time to look at this properly. So I did the following tests:

• Run x8-02 in Simulation profile (using LOS and Height controllers instead of ArduTracker). Result: Issue not reproduced.

• Run x8-02 in AP-SIL profile (using ArduTracker) with "Time Of Arrival Factor" = 1.0 and "Seconds before Waypoint" = 2.0 (from Ardupilot Task). Result: Issue reproduced.

• Run x8-02 in AP-SIL profile (using ArduTracker) with "Time Of Arrival Factor" = 2.0 and "Seconds before Waypoint" = 2.0 (from Ardupilot Task). Result: Issue not reproduced.

This showed me that this issue only happens if "Time Of Arrival Factor" < "Seconds before Waypoint". It turns out that when this condition is true, sometimes the Ardupilot Task will set the flag FL_NEAR before the Path Controller sets m_ts.nearby (and therefore FL_NEAR). This will make the the GoTo maneuver signal completion and the Plan Engine proceed with the following maneuver on the plan (thus dispatching the following DesiredPath). And this is how the PathController will enter the condition you pointed out.

Regarding actions, I don't consider this as a bug on either of the tasks (neither PathController nor the Ardupilot), for the following reasons:

• Both tasks don't depend on each other and can work properly if the other one is not being used. You can run the ArduTracker and have path controller disabled and the other way around.

• As long as the condition "Time Of Arrival Factor" < "Seconds before Waypoint" isn't true, this doesn't happen. I can add in the description of the parameter "Seconds before Waypoint" that it should be smaller then "Time Of Arrival Factor" (or even a warning).

• If the user doesn't respect the condition that guarantees that this issue won't happen, it's not critical in any way I can think of.

Let me know what you think about this, so I know if I can close this issue or not.