-
Notifications
You must be signed in to change notification settings - Fork 13.4k
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
precland: support receiving LANDING_TARGET message #9050
Conversation
Thanks! |
Perfect! Seems someone antecipated my work eheh. @okalachev I am open to discuss the further implementation of this, regarding the details you referenced above and that I referenced on the first PR for supporting the precision landing. |
About the fields you suggest: |
Also for further implementation, please consider taking a look at my suggested roadmap in #8160. The main thing I really wanted was to actually pass this through the |
0b19bc3
to
30e21c0
Compare
@TSC21 Most often, if we detect some visual marker on an external computer, we already know x, y, z positions of the marker and the size. So we want to just publish a marker pose.
Maybe you right here, but in
This should be consistent: we either want to publish the target angles and size in radians (raw data), so the PX4 calculate actual pose, or we want to publish ready data – target pose in meters and target size in meters (it's what I have implemented).
If we have ready and precise target pose in meters, do we actually want to pass it through the estimator? |
The support on MAVROS should deal with all the fields of the msg, and not just part of it. And the same should apply with the Mavlink parsing side on the PX4 side. So, currently, this PR is not complete. |
You can choose if you want to filter it or not. The precision can always be increase if you actually can use another layer of filtering with an IMU for example. So the actual precision you get before the data gets to PX4 is not that great, even if we can take it as acceptable. |
If we you
Yes, this implementation is not complete. This is the implementation of the basic feature: to perform auto-landing to a precisely known position, but I think it's kinda useful. If we have only angles (does |
src/modules/navigator/precland.cpp
Outdated
@@ -522,11 +522,13 @@ bool PrecLand::check_state_conditions(PrecLandState state) | |||
} else { | |||
// if not already in this state, need to be above target to enter it | |||
return _target_pose_valid && _target_pose.abs_pos_valid | |||
&& fabsf(_target_pose.x_rel) < _param_hacc_rad.get() && fabsf(_target_pose.y_rel) < _param_hacc_rad.get(); | |||
&& fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_hacc_rad.get() | |||
&& fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_hacc_rad.get(); |
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.
The use of _target_pose.*_rel
was deliberate here. By using relative positions for the check you can be sure that you are actually over the target. If you use absolute data you can have the situation where e.g. you measure the target once, with some offset from the vehicle. The vehicle flies to that location but for some reason does not detect the target again, and thus precland still has the same old target pose message. The absolute x/y now matches the vehicle's and the check passes, while the relative one would tell you that something is not right.
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.
I don't understand. I consider *_rel
and *_abs
fields differ only by the coordinate frame, don't they? So they are the same data (the "real" detected position of the target).
The "up-to-dateness" of the message is checked by the flag _target_pose_valid
.
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.
_target_pose_valid
is marked false
after PLD_BTOUT
seconds.
It's true that *_rel
and *_abs
are just in different coordinate frames. What's important is when this transformation is done. It is landing_target_estimator
that fills the field if and only if a new measurement is received and fused. If, for some time period (< PLD_BTOUT
), landing_target_estimator
does not fuse a new measurement, the content of landing_target_pose.msg
stays the same. If you look at the absolute values in this message you may come to the conclusion that you are directly above the target. If you look at the relative values you will realize that you are not, due to the fact that they do not say ~0, but they would be if you were on top of the beacon and getting measurements.
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.
LandingTargetEstimator.cpp
doesn't publish new landing_target_pose
message, if there is no new data (line 122):
if (!_new_irlockReport) {
// nothing to do
return;
}
Also, the only condition to publish *_rel
fields is if we have a valid local position (line 217):
if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) {
_target_pose.x_abs = x + _vehicleLocalPosition.x;
_target_pose.y_abs = y + _vehicleLocalPosition.y;
_target_pose.z_abs = dist + _vehicleLocalPosition.z;
_target_pose.abs_pos_valid = true;
} else {
_target_pose.abs_pos_valid = false;
}
The calculation method, however, is the same as in the code above.
So, in my opinion, the correct way to determine if we are above the target right now is not using *_rel
instead of *_abs
(which are actually the same data), but to make sure, that we have a "fresh" landing_target_pose
message:
bool updated = false;
orb_check(_targetPoseSub, &updated);
_target_pose_updated = updated;
// ...
return _target_pose_valid && _target_pose_updated
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.
I don't see the benefit of using (x_rel + _vehicleLocalPosition.x) - _vehicleLocalPosition.x
with an additional applicability check instead of just x_rel
.
What you care about is the relative position of the beacon. This is also the quantity that is being estimated by landing_target_estimator
. The abs pos is a derived value that is not relevant in this computation.
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.
But the companion computer MUST use the local position or at least attitude for the following reason: it needs to compensate vehicle angles to get the correct landing target position.
Also, even when using MAV_FRAME_BODY_NED
we also have to calculate the absolute position in mavlink_receiver
, because currently precland.cpp
requires the both relative AND absolute positions to work (that is what I was trying to fix).
So what do you think, where such a transformation can be performed? For me, mavlink_receiver
doesn't seem the best place for it (even precland.cpp
is better).
We can add the following to the precland.cpp
: when it gets the new landing_target_pose
message, it automatically calculates lacking fields (relative or absolute positions).
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.
But the companion computer MUST use the local position or at least attitude for the following reason
Valid point.
For me, mavlink_receiver doesn't seem the best place for it
Agreed. But I don't like it in precland
either. That's a control module that should not have to worry about where the data came from and should need to do as little transformations as possible.
Perhaps it would be better if landing_target_estimator
is the consumer of the mavlink message and constructs a landing_target_pose
message with all the required data. Consider also the case where the mavlink message only contains angles but no valid position.
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.
So, you mean we have to add a new message, like landing_target.msg
, and publish it from mavlink_receiver
to landing_target_estimator
so it just calculates relative or absolute positions?
Doesn't it seem too complicated?
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.
If we only want to support MAV_FRAME_BODY_NED
I see how it might look like overkill.
If we want to support the other frames as well, and LANDING_TARGET
messages that only contain angles, it is not. Now you're doing conversions and estimating things that belong in neither mavlink_receiver
nor precland
.
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.
That's a control module that should not have to worry about where the data came from and should need to do as little transformations as possible.
I examined another modules, in particular, mc_pos_control
, and though it's a control module, it gets velocity setpoint + velocity frame and does the transformations itself.
What do you think about adding position_frame
(and velocity_frame
) fields to landing_target_pose
, that determines position's coordinate frame, so precland
could calculate all needed data?
It's true that *_rel and *_abs are just in different coordinate frames. What's important is when this transformation is done.
We can check the time of the transformation by the timestamp
field of the landing_target_pose
.
@@ -493,7 +493,7 @@ bool PrecLand::check_state_conditions(PrecLandState state) | |||
// if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore | |||
if (_state == PrecLandState::HorizontalApproach) { | |||
if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_hacc_rad.get() | |||
&& fabsf(_target_pose.y_rel - vehicle_local_position->y) < _param_hacc_rad.get()) { | |||
&& fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_hacc_rad.get()) { |
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.
This is a bug. But the fix should be in the other direction, see my other comment.
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.
@ndepal could you submit the correct fix as a separate PR? We are doing preclands and have been observing weird horizontal approach failures. Probably related to this.
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.
OK, I can do it.
@ndepal, you mean we just have to use _target_pose.x_rel
instead of _target_pose.x_abs - vehicle_local_position->x
?
Also, I wanted to notice that this is incorrect calculation if we're inside the desired radius. The correct would be sqrt(pow(_target_pose.x_rel, 2) + pow(_target_pose.y_rel, 2))
.
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.
@mhkabir Sure thing. But if @okalachev does it, that's great.
you mean we just have to use _target_pose.x_rel instead of _target_pose.x_abs - vehicle_local_position->x?
Correct.
The correct would be sqrt(pow(_target_pose.x_rel, 2) + pow(_target_pose.y_rel, 2)).
That would be the L2 norm. I used the L1 norm because it's cheaper to compute and just as good in this situation.
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.
@okalachev Is this addressed now?
} | ||
|
||
case PrecLandState::FinalApproach: | ||
return _target_pose_valid && _target_pose.rel_pos_valid && _target_pose.z_rel < _param_final_approach_alt.get(); | ||
return _target_pose_valid && _target_pose.abs_pos_valid | ||
&& (_target_pose.z_abs - vehicle_local_position->z) < _param_final_approach_alt.get(); |
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.
Should use relative measurements, see my other comment.
@TSC21 see these comments: #8160 (comment), #8676 (comment) |
When landing on static target (which is by far more common than landing on moving one) we know for sure that location of the marker in LOCAL_NED frame remains the same (except rare cases e.g. GPS reinitialization). So in fact information about target relative location is the information about drone's location in LOCAL_NED up to some constant shift (target coordinates in LOCAL_NED). It seems logical then to treat this information like any external position data e.g. VPE — process it in EKF2/LPE instead of some additional KF for landing target estimation, because for static target it's truly information about drone's location itself, not just the point to which to fly. Maybe we should use this information in drone position calculation instead of just some filtering/predictions using outside KF? Of course I'm talking about setup with quite precise visual target position estimation, not about IR-LOCK. When we have this kind of data — data which is usually fed as VPE and/or used to generate SET_POSITION_TARGET messages — current architecture with additional KF doesn't make much sense to me (if I got it right).
I believe good partial support on PX4 side is much better than poor implemented support of all the fields or no support at all. |
About In case of Usage of simple timeout here looks more reasonable and less complicated. If you believe more complex logic is necessary like waiting for fresh
That's not always true, there are many options:
It would be great to support all the frames, but the sad truth is, only There is also some confusion with body frames. We have four main local frames in MAVLink:
Is it desirable that horizontal tolerance at approaching depends on approach direction? I don't think so, √2 is quite a large variation. Do we really need this hundred of nanoseconds here? MCUs are not so weak nowadays, I would not be afraid of one square root. |
ec417fb
to
5bebc35
Compare
See this PR #9029.
You make some fair points. If you want to change it feel free to make a PR or amend this one, but the changes as this PR stands now are not sufficient (I'd like to see stricter validity checks of the data used to check the acceptance radius).
This is not some strict mathematical condition that needs to be met for the algorithm to work. It's some threshold to start descending, where you're continuing to center over the target. I don't suspect that you will be able to tell the difference in start of descent depending on the approach angle. We could certainly afford the more complex computation without any immediate downsides on other parts of the system. I decided to forgo computational complexity for the sake of mathematical rigor. |
Ok, I agree. |
Tested precision landing through So I think the results are not bad, and this is mergeable (with added strict Issues:
|
Ok, I sometimes observe this too. Not sure where it comes from. Please attach a log and we'll have a look. @ndepal FYI. |
@mhkabir Here is the log: log_10_2018-3-16-16-48-34.ulg.zip https://logs.px4.io/plot_app?log=bf595bae-b37c-4cc0-9303-1a5351dd7ea5 Here you can see, that exactly when We observed this several times. |
Interesting. I checked some of my logs, none of them showed this problem. The velocity setpoints The only thing that changes between horizontal approach and descent above target is the setpoint type. The
Or here but |
@ndepal, when I inspected the code, the only reasonable cause of this I found, is So I also can't find where the problem is. |
@ndepal if (hrt_absolute_time() - _target_pose.timestamp > (uint64_t)(_param_timeout.get()*SEC2USEC)) {
_target_pose_valid = false;
} never actually «works» (if correct timestamps are put in That’s why. The main Am I not right?
So the correct solution is, at my opinion, is to just switch to the |
2c2469f
to
ce68f11
Compare
Sorry, I don't understand what you're trying to say.
Yes they do:
That sounds reasonable. |
@ndepal I say, that in context of switching the state machine state, the proper way is to check if the message was updated on this iteration. That was the answer to
So, what is has to be done to get this merged? |
Got it. Reviewed and tested in SITL. Looks good to me. Could you please rebase on master to fix the conflicts? |
c6c60ab
to
02d52a8
Compare
@ndepal, I rebased and squashed the commits. Also, I have found and fixed two new issues, introduced with the commit 0ef5d89:
|
@ndepal @okalachev Can you confirm the correct fix at https://github.com/PX4/Firmware/pull/9050/files#diff-f45ed77ef246e60c91f6300a68629229L469 ? |
@mhkabir, yes, it's correct. |
Great, thanks! @okalachev |
Fantastic! How do you deal with varying vision sensor/encoding/processing latencies? |
The message contains Actually, if you send the landing target position in the local coordinates system, these latencies are not a big deal, assuming that EKF2 calculates the drone's position more or less well. |
How does it correlate the sensor frame with the inertial frame, to calculate attitude correction? If you don't match the frames, then the correction will be all off. |
The external computer must transform the landing target position to the local coordinate frame (relative to the local origin), as the body frame is not supported now.
BUT, if the external computer reports the landing target position in the local coordinate frame, and if it's static (doesn't move), it's ok. The EKF2 tracks the copter position itself, and the copter just flies to the local position of the landing target.
Note, this PR is only about the supporting of the |
I added support for receiving
LANDING_TARGET
MAVLINK message. This is useful for setups, when we don't use IRLock for precise landing, but an external computer, that detects the landing target (a visual marker, for example). The receiver simply retransmits the message to thelanding_target_pose
uORB topic.Unfortunately,
LANDING_TARGET
lacks some helpful fields, thatlanding_target_pose
topic has:I think, adding this fields to
LANDING_TARGET
should be considered.Also, two issues in precland.cpp were fixed:
y_rel
should bey_abs
x_rel
andy_rel
for two reasons:LANDING_TARGET
messagex_abs
,y_abs
(and this is done in other parts of the code).Tested the landing is SITL and on a real drone (see the video below).