Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix navigator bug: "Takeoff to x meters above home." after mission ended #14020

Merged
merged 5 commits into from
Jan 24, 2020

Conversation

bkueng
Copy link
Member

@bkueng bkueng commented Jan 24, 2020

Fixes #14011

The problem described there actually goes deeper: the message is also sent when switching to mission mode while disarmed and with a valid mission. Even worse: if the first mission item is a command, it will be published at the wrong time.

The root cause is that the (mission) state machine is executed too early/at the wrong time. This is fixed with this PR by going into navigator idle mode while disarmed (i.e. not entering mission until armed).
All existing arming checks in navigator are unaffected by this, except that there is a slight risk that if takeoff is detected immediately upon arming, the first takeoff waypoint is converted into a position waypoint (however the position controller has its own takeoff logic anyway).

Another consequence is that mc_pos_control starts to complain about invalid setpoint triplets while disarmed. The PR resolves this by also accepting invalid triplets and interpret them as loiter.
The risk with this is that it could hide problems, where navigator sends invalid triplets when it should not.

Best reviewed by commits.

This has been quite extensively SITL + flight tested, MC + VTOL. Thanks to @julianoes and @sfuhrer for the support.

Log: https://logs.px4.io/plot_app?log=66a6062f-cbdb-47cd-83f3-fc71724b58c5

It's possible to get invalid or no setpoints from navigator while disarmed.
…ter)

This will allow navigator to not output any setpoints while disarmed.
Otherwise the position controller outputs warnings in the form of:
Auto activation failed with error: Activation Failed

The risk here is that it could hide problems, where navigator sends invalid
triplets when it should not.
This avoids premature state machine execution, most notably for missions.

In particular this fixes these issues:
- when landed, and the vehicle has a valid mission: switching to mission
  would execute the first item.
- after executing a mission with a land waypoint, the mission is reset
  and the first item is executed.

In case the first waypoint is a takeoff, it would print a user message:
"Takeoff to x meters above home.". If it was a vehicle command, it would
execute that.
Note that because the vehicle is disarmed, the vehicle would not actually
do anything (except for commands that are accepted while disarmed).
This can be simplified because we now only activate Mission while armed.
@dusan19
Copy link
Contributor

dusan19 commented Jan 28, 2020

FYI:
With this pr, in auto modes the triplets are published on every navigator iteration when disarmed.
This condition passes in navigator_main.cpp and sets the triplets updated flag:

		/* if nothing is running, set position setpoint triplet invalid once */
		if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
			_pos_sp_triplet_published_invalid_once = true;
			PX4_INFO("nothing running reset triplets once");
			reset_triplets();
		}

Before this pr, the triplets were reset and published only once when the drone is disarmed and in nav state AUTO LOITER for example. Now they are published all the time (disarm the drone and set it in auto loiter to reproduce)
It doesnt really effect anything, just wanted to point out the different behavior.

@julianoes
Copy link
Contributor

@dusan19 I recently realized that too, thanks.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

navigator bug: "Takeoff to x meters above home." after mission ended
4 participants