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
base: main
Are you sure you want to change the base?
Conversation
@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
Any thoughts on that? 😅 |
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp
Outdated
Show resolved
Hide resolved
didnt read the code yet, just have a quick question.
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. |
Hey Dusan 😊
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.
Ah yes, thanks! |
msg/precision_landing_status.msg
Outdated
@@ -0,0 +1,10 @@ | |||
# ORBIT_YAW_BEHAVIOUR |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
ORBIT_YAW_BEHAVIOUR
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
626ac9b
to
2a610e7
Compare
Tested with So, in my opinion this is good to go in. |
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. |
cdc639b
to
4593bf1
Compare
@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? |
Thanks! No, not at all :)
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 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. |
4593bf1
to
37c2d24
Compare
@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 |
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? |
@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 And, I added in the land mode, so for me now both RTL and Land mode work. |
@MaEtUgR would be great to get your review of the new flight task part. |
That was on purpose 😅
Thanks!
Thanks also for that. It's probably expected that precision landing also works in LAND flight mode, especially if it works in RTL 👍 |
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 |
@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? |
It won't (re)allocate as it runs on the same wq. The run condition is something to think about though. |
@julianoes 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:
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. |
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? |
Nope, you are right. It's not handled. I was not aware of this command flow. Thanks for your feedback! 👍
Yeah it's a bit confusing, I have to admit that. Here's how it works: 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.
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 Here's where Navigator's RTL would start precision-landing once the state PX4-Autopilot/src/modules/navigator/rtl.cpp Lines 312 to 317 in 0595efb
|
The way this should work is that the estimator is automatically started when the param to enable it is set to
And that param then needs to be marked as 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. |
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.
This reverts commit 37c2d24.
This reverts commit 17ec100.
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
This reverts commit 7df1eca.
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.
abcf122
to
370af7c
Compare
TODO: Actually it would make more sense to transition to LAND flight mode instead.
// 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; |
There was a problem hiding this comment.
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)
There was a problem hiding this comment.
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.
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 |
@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
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)
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:
source: https://logs.px4.io/plot_app?log=f8da0f50-69e3-40f2-a687-d0520e74a6db
TODO
nav_state == AUTO_MISSION