ardupilot
ardupilot copied to clipboard
ArduPilot
PrecLand: support LANDING_TARGET ext field
This PR add support for LANDING_TARGET mavlink2 position extension field. For companion computer using fiducial markers, it makes FC integration easier. I will do more tests once weather condition is ok.
Note to myself, check on mavlink1 or old mavlink what happens if the new field doens't exist
Hi @khancyr Thank you for reviewing. It would be appreciated if you could help testing on current mavlink1 msg.
Hi @khancyr I add some debug message and tested it with mavlink1 msg. Original code flow is working fine.
@chobitsfan we probably need to be careful about Coordinate frame with this PR. What frames should we support (Probably NED)? There is a mavlink field that we can check to ensure we don't process a wrong frame.
We really need this PR to go in, I have been asked several times for this in the past. Thanks!
we probably need to be careful about Coordinate frame with this PR. What frames should we support (Probably NED)? There is a mavlink field that we can check to ensure we don't process a wrong frame.
Hi @rishabsingh3003 Thank you very much for suggestions. I agree with you that we need to make sure it (we only support MAV_FRAME_BODY_FRD for now). I have updated it
@chobitsfan looks good to me. One question though: Are we sure we want to implement MAV_FRAME_BODY_FRD? I think there may be some benefit to keeping things consistent with PX4 since they have been using this for much longer than us. I think they use: MAV_FRAME_LOCAL_NED
To be clear, I am not asking you to change the implemented frame... but rather just wanted to discuss it.
Hi @rishabsingh3003 Thank you for suggestions.
I implement MAV_FRAME_BODY_FRD for 2 reasons:
- In most vision based precision landing setup, the camera is attached in the drone. The camera reports relative position from the drone to the landing target.
- It only change/add a few lines of code and very unlikely to break existed function.
Maybe we can try to implement MAV_FRAME_LOCAL_NED in another PR? Thank you.
P.S: I cannot find any example setup using PX4 and LANDING_TARGET. In my view, it seems a little strange to use MAV_FRAME_LOCAL_NED. If landing target is stationary, the relative position between landing target and EKF origin does not change. In this case, the x, y, z component of LANDING_TARGET message is always the same during landing. I cannot see how it helps "precision" landing.
Hi. Yes, actually, it is important functionality. I'm working on it as well. But IMHO it needs to be fixed in the backend and precision landing algorithms. In my case in backend added support "get_pos_rel_ned_target" and "get_pos_abs_ned_target", changed handle_msg in companion.cpp and added support in. But to be honest I got some strange glitch: the vehicle sometimes tried to accelerate in front and left direction then going above target then again (with all target positions and velocities ok) So I'm unable to PR it before completing all tests. If someone are interested to check the code you can go GitHub /UAVLAS/uavlas-ap fork of AP and check uls-main branch.
Hi @chobitsfan, could you squash those two commits into the existing commits?
My guess is that if you and @rishabsingh3003 think this is OK we will merge it.
Hi @rmackay9 Thank you. I have squashed and rebased on current master detailed testing https://discuss.ardupilot.org/t/precision-landing-with-multiple-apriltag/89911?u=chobitsfan
Hi. I recently posted a pull request ( #21556 ) regarding this feature. I used part of the comments and code from this request. Look, it will be possible to unite our efforts and make a more universal version of the code. I would be grateful for comments and suggestions. Thank you.
@rishabsingh3003 can you look at this PR and the one from @kapacheuski and decide if we merge this first then work on the extra from from the PR from @kapacheuski or if we should combine first?
I think this is good to go in. I am looking at the other PR but it will need a lot of changes, rework and testing before it can be merged. Meanwhile, we should at least have this PR in. @rmackay9 if you are happy with it, can you push the trigger?
@rishabsingh3003 , Yes this PR is much easy to merge. By the way, the main question is - is it changed anything? IMHO In the current situation, we process frame messages in some way (just convert vector representation from angles to cartesian). Maybe (I think) @chobitsfan made it to support landing target processing in a frame not related to the body (body orientation) but related to FRD in the horizontal plane, to suppress calculation related to orientation (roll/pith) of the drone (for example he have some IMU on camera and can process vector rotation directly on companion). On #21556 I made the same as here, but for now, I'm not sure what means: "MAV_FRAME_BODY_FRD" is it fixed to the body or it just looks like "MAV_FRAME_VEHICLE_FRD" with Front and Right vectors on the plane which parallel to earth surface (no roll and pith talked in account). @chobitsfan what do you think about it?
@kapacheuski this PR makes it easy for the people who use fiducial markers to do Prec Land. Back when this library was initially made, fiducial markers weren't that common. Therefore, most of the time users only sent in approximate angles to the landing target, and we used downward-facing RangeFinder to figure out a real-world 3-D vector. However, now that most people are using fiducial markers, with this PR they can just send the coordinates in the right frame. That's the advantage.
My understanding of MAV_FRAME_BODY_FRD is that it's a BODY frame vector relative to the heading of the vehicle. I.e the roll and pitch of the vehicle aren't accounted for (which we take into account in the Prec Land library once a final NED vector is built). This is the frame a typical camera detecting ArucoMarker/AprilTag would use.
For LOCAL frame (i.e frame relative to earth, where roll and pitch are already accounted for), we'll probably use your PR. I will do a detailed review soon.