precland: support receiving LANDING_TARGET message

[okalachev]

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 the landing_target_pose uORB topic.

Unfortunately, LANDING_TARGET lacks some helpful fields, that landing_target_pose topic has:

• Landing target relative velocities
• Is the target static? Considered, that not, for now
• Landing place size in meters (contains only the fields, that are described as sizes in radians)
• Covariances

I think, adding this fields to LANDING_TARGET should be considered.

Also, two issues in precland.cpp were fixed:

• typo, where y_rel should be y_abs
• removed using x_rel and y_rel for two reasons:
  • this is fields are lacked in LANDING_TARGET message
  • they can be easily recalculated from x_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).

[LorenzMeier]

Thanks!

[TSC21]

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.
Also, I should be able to test this with the PR I have open for MAVROS.

[TSC21]

About the fields you suggest:
I agree with the covariances and velocities. I don't see why the fact that the target is fixed or not should be on the message (that can be set on a parameter). About the size: you can compute the size of the target in meters by knowing the FOV of your camera or by computing the displacement. Remember that the size on the LANDING_TARGET msg is the current size of the target om the camera lens, not the true size of the target. The target size, IMO can also be set by parameters, but I guess we can put it also on the msg.

[TSC21]

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 landing_target_estimator and extend it to support attitude estimation too.

[okalachev]

@TSC21
I've also implemented a version of LANDING_TARGET support in MAVROS, but simpler, you can just simply publish a PoseStamped or a raw LandingTarget. I'm not sure MAVROS should deal with converting angles to distances and vice versa, it seems too complicated.

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.

I don't see why the fact that the target is fixed or not should be on the message (that can be set on a parameter).

Maybe you right here, but in landing_target_pose topic it's a message field. We may want to land to static and non-static targets at the same flight.

About the size: you can compute the size of the target in meters by knowing the FOV of your camera or by computing the displacement. Remember that the size on the LANDING_TARGET msg is the current size of the target om the camera lens, not the true size of the target.

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).

The main thing I really wanted was to actually pass this through the landing_target_estimator and extend it to support attitude estimation too.

If we have ready and precise target pose in meters, do we actually want to pass it through the estimator?

[TSC21]

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.

[TSC21]

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.

[okalachev]

The support on MAVROS should deal with all the fields of the msg, and not just part of it.

If we you PoseStamped for landing target topic, I would take another information (landing target size etc) from MAVROS parameters. Also, there always should be a topic with raw message.

And the same should apply with the Mavlink parsing side on the PX4 side.

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 position_valid field indicate this?), what should we do? To pass this to the irlock_report topic, so the estimator can recalculate and filter the data? Or we should create another topic for this kind of landing target data?

[ndepal]

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.

[okalachev]

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.

[ndepal]

_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.

[okalachev]

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

[ndepal]

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.

[okalachev]

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).

[ndepal]

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.

[okalachev]

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?

[ndepal]

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.

[okalachev]

@ndepal

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.

[ndepal]

This is a bug. But the fix should be in the other direction, see my other comment.

[mhkabir]

@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.

[okalachev]

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)).

[ndepal]

@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.

[mhkabir]

@okalachev Is this addressed now?

[ndepal]

Should use relative measurements, see my other comment.

[ndepal]

I don't see why the fact that the target is fixed or not should be on the message (that can be set on a parameter).

@TSC21 see these comments: #8160 (comment), #8676 (comment)

[korigod]

@TSC21

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.

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).

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.

I believe good partial support on PX4 side is much better than poor implemented support of all the fields or no support at all.

[korigod]

@ndepal

About landing_target_pose.msg — I believe necessary presence of both absolute and relative positions is not quite justified here. I see only one case when it can be acceptable — when for some reason in one calculations we need absolute positions and in other we need relative and it's acceptable if they are not very recent (because if they are very recent we can transform frames ourself). In other cases the need to put in message both absolute and relative positions is confusing the developer and complicating the code.

In case of precland relative positions are in fact used as strange way to check if the position of drone in landing_target_pose.msg changing as drone approaches — local position of drone itself stored in message implicitly as the difference between the absolute and relative positions of the target. That's obscure and inaccurate: e.g. drone can lose target when relative position is small enough to land — in this case precland will behave like target is there.

Usage of simple timeout here looks more reasonable and less complicated. If you believe more complex logic is necessary like waiting for fresh landing_target_pose message before switching to descent it's still possible to implement more straight-forward way.

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.

That's not always true, there are many options:

• I can get landing target coordinates from some external system like mocap, in this case my native frame would be LOCAL_NED;
• I can use camera on gimbal to recognize landing target, in that case it would be logical to send relative position likely in BODY_OFFSET_NED (see my note below);
• Camera can be mounted without gimbal — in this case I naturally get landing target position in MAV frame (including pitch/roll), which is not supported by MAVLink.

It would be great to support all the frames, but the sad truth is, only LOCAL_NED is supported in overwhelming majority of MAVLink messages in PX4. Including, which is important, SET_POSITION_TARGET, so all the developers who use some kind of visual navigation or mocap now already convert coordinates to LOCAL_NED for themselves. I'm not saying that it's the only way and that we shouldn't add support for another frames, it's just more consistent with existing code to use absolute positions for targets (and landing target definitely is a position target in the end).

There is also some confusion with body frames. We have four main local frames in MAVLink: LOCAL_NED, LOCAL_OFFSET_NED, BODY_NED, BODY_OFFSET_NED. We use LOCAL_OFFSET_NED when we want to specify NED coordinates relative to current drone position. So it looks like we should use BODY_OFFSET_NED, not BODY_NED you suggested as it doesn't make much sense for positions at all (it's used only for speeds in PX4, not positions). But it looks if you read descriptions on MAVLink that these frames are not really NED — yaw is relative to MAV front, so your relative marker coordinates really correspond to LOCAL_OFFSET_NED. I opened separate issue on this as it's beyond the scope of current pull request (mavlink/mavlink#861), we can further discuss frames there.

I used the L1 norm because it's cheaper to compute and just as good in this situation.

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.

[ndepal]

It seems logical then to treat this information like any external position data e.g. VPE

See this PR #9029.

About landing_target_pose.msg

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).

Is it desirable that horizontal tolerance at approaching depends on approach direction?

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.

[korigod]

I don't suspect that you will be able to tell the difference in start of descent depending on the approach angle.

Ok, I agree.

[okalachev]

Tested precision landing through LANDING_TARGET on a real drone. Used an ArUco-maker for targeting, its position was sending to the FCU.
The copter landed several times successfully. Here the video of the flight: https://www.youtube.com/watch?v=HGX1RccfN1g.

So I think the results are not bad, and this is mergeable (with added strict landing_target_pose message checks) even without the support of landing target angles.

Issues:

• There is no yaw field in the LANDING_TARGET message. So the copter can't align its yaw to the marker (in some situations this is necessary, e. g. landing into a charing station).
• Sometimes copter is yawing when doing horizontal approach phase, is it really necessary when doing landing?
• There is some weird bug: sometimes when the copter transitions to SETPOINT_TYPE_LAND waypoint type, it stops correcting its velocity setpoints (sets them to zeros), so the landing isn't really precise. I can attach a log of such a situation.

[mhkabir]

There is some weird bug: sometimes when the copter transitions to SETPOINT_TYPE_LAND waypoint type, it stops correcting its velocity setpoints (sets them to zeros), so the landing isn't really precise. I can attach a log of such a situation.

Ok, I sometimes observe this too. Not sure where it comes from. Please attach a log and we'll have a look. @ndepal FYI.

[okalachev]

@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 position_setpoint_triplet.current.type switches to 4 (SETPOINT_TYPE_LAND), the vehicle_local_position_setpoint.vx suddenly changes to 0 and stays there until the end of the landing (the vy component does the same).

We observed this several times.

[ndepal]

There is some weird bug: sometimes when the copter transitions to SETPOINT_TYPE_LAND waypoint type, it stops correcting its velocity setpoints (sets them to zeros), so the landing isn't really precise. I can attach a log of such a situation.

Interesting. I checked some of my logs, none of them showed this problem.

The velocity setpoints vehicle_local_position_setpoint.vx/vy are zero starting at ~305s since boot. This coincides with SETPOINT_TYPE_LAND, which means it happens as soon as precland descends. The position setpoints vehicle_local_position_setpoint.x/y still change.

The only thing that changes between horizontal approach and descent above target is the setpoint type.

The vehicle_local_position_setpoint is published here.

_vel_sp can be set to 0 here if current_setpoint_valid is false. This is set to true here, if _curr_pos_sp is finite. _curr_pos_sp is set a few lines below, i.e. current_setpoint_valid is set based on the previously computed position setpoint (not sure if intentional?). Either way it should always be finite. But you could try adding a PX4_WARN here so you can see it in the log (you might want to rate-limit it though).

_vel_sp(0/1) can also be set to 0 here if _pos_sp(0/1) is not finite, but that would show a warning in the log.

Or here but _control_mode.flag_control_velocity_enabled is enabled in vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND.

[okalachev]

@ndepal, when I inspected the code, the only reasonable cause of this I found, is _control_mode.flag_control_velocity_enabled is false. But this also can't happen, because of vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND state.

So I also can't find where the problem is.
But I have seen that that copter is not correcting its position while normal landing (not precision) not so rarely.

[okalachev]

@ndepal
Speaking of switching the state machines states, this code:

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 landing_target_pose).

That’s why. The main navigator loop works, polling the vehicle_local_position topic. This topic gets new messages from EKF2 with a high rate (even there is no valid local position). So the main navigator loop can’t just «hang». So if a new landing_target_pose comes, the state machine will switch (if necessary) immediately.

Am I not right?

descend_over_target and horizontal_approach states don’t check _target_pose_valid for setting setpoints at all.

So the correct solution is, at my opinion, is to just switch to the descend and horizontal_approach states on fresh messages only. This way we won’t switch the state machine if we’re in correct position by the EKF2, but don’t see the landing target right now.

[ndepal]

That’s why. The main navigator loop works, polling the vehicle_local_position topic. This topic gets new messages from EKF2 with a high rate (even there is no valid local position). So the main navigator loop can’t just «hang». So if a new landing_target_pose comes, the state machine will switch (if necessary) immediately.

Sorry, I don't understand what you're trying to say.

descend_over_target and horizontal_approach states don’t check _target_pose_valid for setting setpoints at all.

Yes they do:
https://github.com/CopterExpress/Firmware/blob/master/src/modules/navigator/precland.cpp#L484
https://github.com/CopterExpress/Firmware/blob/master/src/modules/navigator/precland.cpp#L494
https://github.com/CopterExpress/Firmware/blob/master/src/modules/navigator/precland.cpp#L505
https://github.com/CopterExpress/Firmware/blob/master/src/modules/navigator/precland.cpp#L510

So the correct solution is, at my opinion, is to just switch to the descend and horizontal_approach states on fresh messages only.

That sounds reasonable.

[okalachev]

@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

I'd like to see stricter validity checks of the data used to check the acceptance radius

So, what is has to be done to get this merged?

[ndepal]

Got it.

Reviewed and tested in SITL. Looks good to me.

Could you please rebase on master to fix the conflicts?

[okalachev]

@ndepal, I rebased and squashed the commits.

Also, I have found and fixed two new issues, introduced with the commit 0ef5d89:

• if (!_targetPoseSub) – this check for topic subscription became incorrect as the initial _targetPoseSub value became -1 instead of 0.
• if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e-6f) > _param_timeout.get()) { – incorrect converting from microseconds to seconds (should be 1e6f).

[mhkabir]

@ndepal @okalachev Can you confirm the correct fix at https://github.com/PX4/Firmware/pull/9050/files#diff-f45ed77ef246e60c91f6300a68629229L469 ?

[okalachev]

@mhkabir, yes, it's correct.

[mhkabir]

Great, thanks! @okalachev

[fnoop]

Fantastic! How do you deal with varying vision sensor/encoding/processing latencies?

[okalachev]

The message contains time_usec field, which contains a correct shot time (if you've done everything right). But, this information stays almost unused on the PX4 side for now.

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.

[fnoop]

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.
Latencies are a big deal, as vision processed sensor data varies hugely. Something that is processing at 90fps can be as low as 20-50ms depending on various factors, something that is processing at 10-20fps can be as high as 100-150ms.
For this reason, testing on a single system is insufficient, it must be tested on a variety of cameras/computers.

[okalachev]

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.

Latencies are a big deal, as vision processed sensor data varies hugely. Something that is processing at 90fps can be as low as 20-50ms depending on various factors, something that is processing at 10-20fps can be as high as 100-150ms.

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.

For this reason, testing on a single system is insufficient, it must be tested on a variety of cameras/computers.

Note, this PR is only about the supporting of the LANDING_TARGET packet, not about the whole precision landing module.