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

Move precision landing from Navigator into Flight Tasks #18973

Open
wants to merge 30 commits into
base: main
Choose a base branch
from

Conversation

potaito
Copy link
Contributor

@potaito potaito commented Jan 7, 2022

@ndepal FYI

Describe problem solved by this pull request
Partially solves #14320

Precision landing is congesting navigator when it's actually better suited to be a flight task. The navigator based precision landing implementation is quite tedious as it's constantly converting between local and global coordinates. This is a limitation of Navigator. By moving it to the flight task library, precision landing can directly interface with the local position and velocity setpoints. It's also easier to create new search patterns in the future for those who need it. Last but not least the implementation as a FlightTask should make it possible to use precision landing without a global position (GPS), which is a requirement in the previous implementation. I have not tested precision landing without GPS, but in theory it's possible to use it with only a local frame.

Describe your solution
Precision landing was moved from navigator to the flight task library. It's running in parallel to navigator's RTL mode. Initially the precland flight task will listen to Navigator setpoints while Navigator thinks it's returning home. Once the drone either detects the landing target, or is above home and starts descending, the flight task will take over and land on the target instead on home. It it does not see the target, it will pause during Navigator's landing sequence, wait and see if it can still detect the target within a timeout.

The advantage of "hijacking" the navigator's RTL is, that all Navigator features and parameters are functional (return altitude, geofences etc.) until the drone enters the landing phase.

I tried to maintain the previous functionality of precland in Navigator as good as possible. One feature is now a bit different though: If the target is lost while it's being approached, the drone will now continue with the original Navigator RTL until it sees the target again. Originally the drone would freeze mid-air and just wait for the target to re-appear. Despite this fact, the refactored implementation does not violate the flow described in the PX4 docs: http://docs.px4.io/master/en/advanced_features/precland.html#precision-landing

Precland Refactor

In summary, the drone will either land on the target, or it will land on home. Those are the only options.

Test data / coverage
Testing is a bit tricky, as MAVSDK currently does not have the IR_beacon message implemented and it's therefore not possible to create SITL tests. What I did is send the land_target message from within PX4 and run manual tests.


The following log shows how RTL is executed and the drone lands on a target at (x,y) = (3,1.5)

image

source: https://logs.px4.io/plot_app?log=30855969-ae58-48a9-b06e-b33599a202eb


Here we see the configuration where precision landing is required, but the target not visible. The drone will wait and "search" for the target where the time marker is located in the graphs. After the configurable timeout of 10 seconds is reached, the drone proceeds with the usual landing as instructed by Navigator:

image

source: https://logs.px4.io/plot_app?log=f8da0f50-69e3-40f2-a687-d0520e74a6db

TODO

  • Fix flimsy land detection in simulation
  • Prevent flight task from being activated over and over again between land detected and disarm. This happens with all flight tasks currently.
  • Implement switching to precland as part of the mission, where nav_state == AUTO_MISSION
  • Fix opportunistic landing in RTL: Currently precland witll switch to fallback right away when the beacon is not visible, instead of flying back to HOME first before abandoning search.

@potaito
Copy link
Contributor Author

potaito commented Jan 7, 2022

@MaEtUgR I noticed that the flight tasks are constantly being activated() between the drone touching the ground, i.e. land detected, and the actual disarm. It's not causing any issues other than the console is being spammed by the precision landing state machine. Are you aware of this and this a problem, or intended behaviour?

Also, land-detection is currently flimsy. I'm landing with the z-position setpoint set to NAN, therefore using only the z-velocity setpoint. I think this should be enough, but sometimes the simulated drone in jmavsim enters a never ending cycle of

INFO  [commander] Landing detected	
INFO  [commander] Takeoff detected	
INFO  [commander] Landing detected	
INFO  [commander] Takeoff detected	
INFO  [commander] Landing detected	
INFO  [commander] Takeoff detected	
INFO  [commander] Landing detected	

Any thoughts on that? 😅

@potaito potaito requested a review from MaEtUgR January 7, 2022 16:52
@dusan19
Copy link
Contributor

dusan19 commented Jan 7, 2022

didnt read the code yet, just have a quick question.
Compared to the RTL "hijacking" logic, what about auto missions that end with precision landing mission item, is that still supported? In that case this flight task has to run in parallel with navigators mission not RTL.

Testing is a bit tricky, as MAVSDK currently does not have the IR_beacon message implemented and it's therefore not possible to create SITL tests. What I did is send the land_target message from within PX4 and run manual tests.

Btw I normally test precision landing in simulation using gazebo iris_irlock world, that one has an irlock model and reports landing_target_pose when it sees it.

@potaito
Copy link
Contributor Author

potaito commented Jan 7, 2022

Hey Dusan 😊

didnt read the code yet, just have a quick question. Compared to the RTL "hijacking" logic, what about auto missions that end with precision landing mission item, is that still supported? In that case this flight task has to run in parallel with navigators mission not RTL.

Good point. I think it should work if the mission has an RTL element at the end. So from a Navigator / Mission perspective it will execute RTL, but ultimately it will not land at home and on the target instead.

Btw I normally test precision landing in simulation using gazebo iris_irlock world, that one has an irlock model and reports landing_target_pose when it sees it.

Ah yes, thanks!

@@ -0,0 +1,10 @@
# ORBIT_YAW_BEHAVIOUR
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ORBIT_YAW_BEHAVIOUR

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

cough thanks. Looks almost like somebody copy-pasted the uORB message from elsewhere

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @potaito , I's love to see this PR make it in. Do you want to commit above suggestion and rebase on upstream?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure can do 👍
I was expecting more comments. This can't be the only thing that's not optimal 😂

As far as I know, PrecisionLanding is not used a lot by the community, and the folks who use it probably have their own implementation of it. So maybe there is someone who would be unhappy with this PR, but since the PrecisionLanding is then a flight task, customizing and changing it would be super simple.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done, there were only minor conflicts when rebasing.

@potaito potaito force-pushed the potaito/refactor-precland-into-flighttask branch from 626ac9b to 2a610e7 Compare February 7, 2022 10:27
@potaito
Copy link
Contributor Author

potaito commented Feb 7, 2022

Tested with make px4_sitl gazebo_iris_irlock as suggested by @dusan19.
Landing on the target works when the RTL's home location is close to the tag, and precision landing also works if the RTL return path intersects the beacon, in which case it lands early and on the beacon 👍

So, in my opinion this is good to go in.

@potaito
Copy link
Contributor Author

potaito commented Feb 23, 2022

I pushed two new commits that put the LandingTargetEstimator on the work queue and start/stop the estimator when needed, i.e. when the flight task is activated / deactivated.

I'll rebase soon and provide some SITL tests to have that covered as well.

@julianoes julianoes force-pushed the potaito/refactor-precland-into-flighttask branch from cdc639b to 4593bf1 Compare March 24, 2022 04:10
@julianoes
Copy link
Contributor

julianoes commented Mar 24, 2022

@potaito I rebased and force-pushed this. I hope you don't mind. Would be nice to get it in.

And you're saying MAVSDK should have support for https://mavlink.io/en/messages/common.html#LANDING_TARGET and then we could test this?

@potaito
Copy link
Contributor Author

potaito commented Mar 24, 2022

@julianoes

I rebased and force-pushed this. I hope you don't mind. Would be nice to get it in.

Thanks! No, not at all :)

And you're saying MAVSDK should have support for https://mavlink.io/en/messages/common.html#LANDING_TARGET and then we could test this?

Yes! It's been on my TODO list, but I never created a new plugin in MAVSDK before. Basically I would like to have the same interface in MAVSDK as for the follow-me message FOLLOW_TARGET, which proved very useful.

I have access to a drone in our office that could be used to test precision landing with IR beacons. That would also be an option.

@julianoes julianoes force-pushed the potaito/refactor-precland-into-flighttask branch from 4593bf1 to 37c2d24 Compare March 24, 2022 18:06
@julianoes
Copy link
Contributor

@potaito ok, would you use the position fields in the message as well?

And why do we need to send the position using mavsdk if there is a beacon already in Gazebo using the _irlock model? I guess I'm a bit confused how it all works together.

@julianoes
Copy link
Contributor

Currently only precision landing is using the LandingTargetEstimator and it is a waste to have it running in the background all the time. With this change the estimator is only started once the precision landing flight task is activated, and stopped when the flight task is deactivated.

I'm not sure that makes sense. Shouldn't the estimator always run and estimate the landing target and then once you switch to landing it already knows where the landing target is, if it is available?

@julianoes
Copy link
Contributor

@potaito ok, I reverted your two last commits, because I think you want to estimate the landing target at all times, and then use it immediately when you're trying to land.

I then tested it with make px4_sitl gazebo_iris_irlock and realized I had to actually enable precision landing with the param, so I made that the default.

And, I added in the land mode, so for me now both RTL and Land mode work.

@julianoes
Copy link
Contributor

@MaEtUgR would be great to get your review of the new flight task part.

@potaito
Copy link
Contributor Author

potaito commented Mar 25, 2022

ok, I reverted your two last commits, because I think you want to estimate the landing target at all times, and then use it immediately when you're trying to land.

That was on purpose 😅
But down the line I want to have the same estimator for follow-me and precision landing, and use the same uORB interface for both. Whether we chase or land on a target should not make a huge difference. So I guess it's fine if you want to have the estimator running all the time. For now I just didn't see a benefit, because it initializes within one second when measurements start coming in.

I then tested it with make px4_sitl gazebo_iris_irlock and realized I had to actually enable precision landing with the param, so I made that the default.

Thanks!

And, I added in the land mode, so for me now both RTL and Land mode work.

Thanks also for that. It's probably expected that precision landing also works in LAND flight mode, especially if it works in RTL 👍

@julianoes
Copy link
Contributor

julianoes commented Mar 25, 2022

That was on purpose 😅

I know I know, feel free to remove my commits again if you disagree. Sorry that I was a bit harsh. I just sort of worked my way through and it didn't actually work with these changes. I think in general we try to avoid allocating and de-allocating once flying, so I'm not sure if we should start and stop work queues while in-air.

@potaito
Copy link
Contributor Author

potaito commented Mar 25, 2022

That was on purpose sweat_smile

I know I know, feel free to remove my commits again if you disagree. Sorry that I was a bit harsh. I just sort of worked my way through and it didn't actually work with these changes. I think in general we try to avoid allocating and de-allocating once flying, so I'm not sure if we should start and stop work queues while in-air.

Interesting. I ended up doing this because @bkueng made the suggestion when I was working on moving follow-me from navigator to flight tasks. I.e. start the follow-me estimator when the follow-me flight mode is entered. I don't have an opinion as to what's better on a low level OS perspective

@julianoes
Copy link
Contributor

@potaito yer I might be wrong. But even for follow me, wouldn't you want to have the follower ready and tracking when you switch to it? You wouldn't want to wait 1s for it to converge. You basically would want to know if it is tracking before you switch, no?

@bkueng
Copy link
Member

bkueng commented Mar 28, 2022

I think in general we try to avoid allocating and de-allocating once flying, so I'm not sure if we should start and stop work queues while in-air.

It won't (re)allocate as it runs on the same wq. The run condition is something to think about though.

@potaito
Copy link
Contributor Author

potaito commented Mar 28, 2022

@potaito yer I might be wrong. But even for follow me, wouldn't you want to have the follower ready and tracking when you switch to it? You wouldn't want to wait 1s for it to converge. You basically would want to know if it is tracking before you switch, no?

@julianoes
From an estimation perspective yes, absolutely. You'd want your estimator to be running for as long as possible before you start using it. An argument against would be that the estimator is wasting CPU cycles while it's not used. Then again it will be wasting the same cycles when precision landing is enabled, since no other task is kicked out of the work queue. So in effect we would not be saving anything 🤔

Both approaches are fine for me. As I wrote before, I'm certain I'll be merging follow-me and precision-landing at some point in the future such that the same estimator will be used for both, and that PX4 can land on the target it was following. For this feature to work, the estimator will need to keep running when switching from follow-me to precision landing and vice versa. I'm not sure how to implement this in an elegant fashion using the work queue. Instead of having the flight tasks start / stop the estimator job on the work queue, we could instead have the flight task manager perform this action and keep the estimator running when switching between tasks that both need it. If this approach works, then I have no bigger arguments at all in favour or against running the estimator on the work queue.

@julianoes one argument I can think of: When using the work queue, you don't need to worry about the user "enabling" the estimator when he/she wants to use follow-me or precision landing, as it will be started on demand. Currently the estimator is started inside of the airframe configuration:

which I find cumbersome for new users. Alternatively we put landing_target_estimator start into the general rcS script and enable / disable the estimator via parameter. But that again requires a manual step by the user. Having it started on the work queue by the flight task itself eliminates the need for any user configuration.

So in the end it's "Ease of use" versus "Estimator already converged". Given this, and @bkueng's comment about the (lack of) allocation, I vote for ease of use. There are messages on Slack asking how to use follow-me or precision landing every 1-2 weeks. So anything that helps with UX should be the main focus.

@dusan19
Copy link
Contributor

dusan19 commented Mar 28, 2022

} else if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL ||
		    _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)

This is the condition to switch to precland flight task. It seems to me that it wont switch to precland when precland is requested as a part of the mission (for mission item of type MAV_CMD_NAV_LAND with a precland parameter set (param 2)), In that case _vehicle_status_sub.get().nav_state is of type AUTO_MISSION which isnt handled here. Am I missing something?

I have another question. I dont understand the RTL logic now, im sure you tested and it works, but would like some clarification for my own sake. The condition above switches the flight task to precland when nav_state is RTL, but does not necessarily check for the landing phase of RTL. So if the vehicle is not above the beacon and RTL is triggered, then how does running the precland task work? Wouldnt it just start searching for the beacon instead of climbing and going to the home position first?

@potaito
Copy link
Contributor Author

potaito commented Mar 28, 2022

@dusan19

This is the condition to switch to precland flight task. It seems to me that it wont switch to precland when precland is requested as a part of the mission (for mission item of type MAV_CMD_NAV_LAND with a precland parameter set (param 2)), In that case _vehicle_status_sub.get().nav_state is of type AUTO_MISSION which isnt handled here. Am I missing something?

Nope, you are right. It's not handled. I was not aware of this command flow. Thanks for your feedback! 👍
I have to figure out how exactly this works and make the corresponding changes.

I have another question. I dont understand the RTL logic now, im sure you tested and it works, but would like some clarification for my own sake. The condition above switches the flight task to precland when nav_state is RTL, but does not necessarily check for the landing phase of RTL.

Yeah it's a bit confusing, I have to admit that. Here's how it works:
The flight task is activated and follows navigator's position setpoint triplet: https://github.com/PX4/PX4-Autopilot/pull/18973/files#diff-89d61979c7d58eb43d987eaaf69c511d11a7eee9bd04b990f85ea3fd8e029ed0R122

The flight task stops listening to navigator / the position setpoint triplet once navigators begins to land: https://github.com/PX4/PX4-Autopilot/pull/18973/files#diff-89d61979c7d58eb43d987eaaf69c511d11a7eee9bd04b990f85ea3fd8e029ed0R133-R136

I wanted to keep this refactor as close as possible to the existing implementation, which is why the logic is now a bit of a mishmash.

So if the vehicle is not above the beacon and RTL is triggered, then how does running the precland task work? Wouldnt it just start searching for the beacon instead of climbing and going to the home position first?

See the start block of the flight task: https://github.com/PX4/PX4-Autopilot/pull/18973/files#diff-89d61979c7d58eb43d987eaaf69c511d11a7eee9bd04b990f85ea3fd8e029ed0R124-R137

In other words: If the beacon is visible, it will approach the beacon horizontally. If it's not visible and opportunistic precland is enabled, it will switch to fallback and ignore precland. Third option, and this is the most common one I think, if the beacon is not visible not precland is required, it will listen to Navigator's position setpoint triplet, which is performing a common RTL, and then it will take over in the land phase. This should be similar to how it was implemented before when precland was part of Navigator. Precland.cpp was not its own navigator mode, but would be used through Navigator::RTL. If anything my PR makes the split between Navigator's RTL and standalone precland more obvious than it was before. But the split was always there, it was just hidden away in navigator/rtl.cpp.

Here's where Navigator's RTL would start precision-landing once the state RTL_STATE_LAND is entered:

if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) {
_navigator->get_precland()->on_active();
} else if (_navigator->get_precland()->is_activated()) {
_navigator->get_precland()->on_inactivation();
}

@julianoes
Copy link
Contributor

@julianoes one argument I can think of: When using the work queue, you don't need to worry about the user "enabling" the estimator when he/she wants to use follow-me or precision landing, as it will be started on demand. Currently the estimator is started inside of the airframe configuration:

The way this should work is that the estimator is automatically started when the param to enable it is set to Opportunistic or Required, so in the startup script it should be:

if param greater RTL_PLD_MD 0
then
    landing_target_estimator start
fi

And that param then needs to be marked as @reboot_required.

The one argument I have to always run the estimator is that you want to know the CPU required and not be surprised mid-flight when it suddenly goes up. So, I'd say, you might as well always have it running.

@potaito
Copy link
Contributor Author

potaito commented Mar 29, 2022

@julianoes one argument I can think of: When using the work queue, you don't need to worry about the user "enabling" the estimator when he/she wants to use follow-me or precision landing, as it will be started on demand. Currently the estimator is started inside of the airframe configuration:

The way this should work is that the estimator is automatically started when the param to enable it is set to Opportunistic or Required, so in the startup script it should be:

if param greater RTL_PLD_MD 0
then
    landing_target_estimator start
fi

And that param then needs to be marked as @reboot_required.

The one argument I have to always run the estimator is that you want to know the CPU required and not be surprised mid-flight when it suddenly goes up. So, I'd say, you might as well always have it running.

Fair enough, you are right! I can add these changes and a check when switching to precision landing.

potaito and others added 20 commits March 29, 2023 13:18
Currently only precision landing is using the
LandingTargetEstimator and it is a waste to have it
running in the background all the time. With this change
the estimator is only started once the precision landing
flight task is activated, and stopped when the flight
task is deactivated.
With this model we should actually use the precision landing, otherwise
this is just confusing.
This also enables precision landing in landing mode, not just during
RTL.

This required an additional fix during the horizontal approach. It seems
like the z setpoint is not set when landing, so we can't blindly use
that setpoint but rather need to keep the altitude.
So far flight mode manager was checking navigator states,
but not if commander switches to precland on its own.

For example "commander mode auto:precland"
or the flight mode menu in QGC
The current name is confusing as it suggest an action,
which is switching to a new state. In reality these
functions first perform a check and then possibly
reject the state switch.
@potaito potaito force-pushed the potaito/refactor-precland-into-flighttask branch from abcf122 to 370af7c Compare March 29, 2023 11:57
// TODO: Should use current altitude when state was entered as the altitude setpoint
if (PX4_ISFINITE(_landing_target_pose.z_abs)) {
// For safety reasons take whichever reference altitude is higher up (lower in NED)
_position_setpoint(2) = math::min(_sub_home_position.get().z, _landing_target_pose.z_abs) - search_rel_altitude;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does this account for the case where home/takeoff point is above the landing tag? (you might end up too high and not find the tag)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You are right. This case aimed to solve a problem that does not really exist.

@DronecodeBot
Copy link

This pull request 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-june-14-2023/32616/1

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Admin: Enhancement (improvement) 💡 Flight Controls 🦅 Anything about flight control algorithm (Navigation, Attitude control, etc)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

10 participants