-
Notifications
You must be signed in to change notification settings - Fork 17k
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
AP_OpticalFlow - Add rover support #21510
Conversation
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.
LGTM
This was my attempt at the same thing #20038 @stephendade I guess your implementation actually works? :-). Mine has a lot of code changes but it was only partially successful during actual testing (on a Copter). |
very cool. does it have to track the ceiling? Would ground tracking work if it was in focus? |
Yes, tested and does work nicely. This PR is simpler than your implemention - I only assume the sensor is upwards or downwards facing.
Yes, ground tracking should work for Rover. Most sensors have a minimum focus distance of 10 or 20cm, so the chassis will need to be some distance off the ground.
This does work with copter (though untested). The only sensor I've tested with is the UpFlow sensor, but it should work with all other supported sensors. Note that ArduPilot only supports having 1 optical flow sensor connected. |
Happy to assist anyone who wants to test this too! EDIT: |
Hi @stephendade My config Cube Orange HereFlow + 1 upwards LightWare lidar SF000/B on quadcopter. |
@stephendade I was trying to come up with something very similar a while back. As far as I can recall, when the EKF fuses the rangefinder data, it pretty much assumes that the orientation is downwards. It's hardcoded (I think) and it required a lot of changes to the EKF, so I gave up mid-way. I see those changes in @rmackay9 's PR. |
@simon88 and @rishabsingh3003: There is no support for upward-facing rangefinders. I suspect, since I've only been testing with rovers, any issues with height-above-ground (rangefinders) being fused with optical flow have been hidden. Obviously, rovers are always at 0m height-above-ground. Unfortunately, I haven't got any copters to test with :( What I'll likely do is limit the upwards-facing sensors to rover vehicles only. |
As @rishabsingh3003 mentioned, my PR #20038 modifies the EKF to use the appropriate range finder to match the orientation of the optical flow sensor. So I think we could get this PR in if it only covered adding downward facing optical flow support to Rover. Once we try to add in the upward facing support then we need the EKF changes. |
@simon88 and @rishabsingh3003: There is no support for upward-facing rangefinders. I suspect, since I've only been testing with rovers, any issues with height-above-ground (rangefinders) being fused with optical flow have been hidden. Obviously, rovers are always at 0m height-above-ground. Unfortunately, I haven't got any copters to test with :(
Agree. |
50de9ee
to
6d6a863
Compare
I've updated this to remove the upwards facing support and have added a It did require a number of changes in the AHRS/EKF to pass the height variable through correctly. Initial tests were good. |
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 looks better. However, I would imagine that since most optical flow sensors typically perform best when they are slightly high above the ground (at least 5-10 meters), the use case for this would be very limited. I think for Rover, it'll be much more useful to have an upward-facing optical flow (which I think now you have removed from this PR).
I would also imagine that this probably does not work for fast rovers, since the flow change will be too quick.
where I think this will be very useful is tracking the ground outside on terrain where wheel slip is an issue so wheel encoders wont work. your correct that it would be speed limited but i suspect even 1m/s would be enough for most people when moving in a confined space. combined with proximity it should allow more precise navigation between obstacles without needing RTK. looking at the px4flow specs show that it could do reasonable speeds just 1m above the ground, so i suspect it should work down to 10 or 20cm with some adjustments to focus.
|
6d6a863
to
856bfb5
Compare
I've got a UPFlow sensor mounted ~10cm above the ground. It's not great quality - I'll need to look at mounting it a bit higher for decent measurements. This is mostly for indoor and/or slow vehicle, particularly non-GPS vehicles that need an EKF velocity source (and use computer vision for the position source). |
856bfb5
to
06818ab
Compare
752d7d1
to
e7715de
Compare
have you tried adding a bright light at an angle similar to an optical mouse, it should give it more contrast and cast shadows on surface imperfections making them easier to track. |
909c7e6
to
c0bd0f4
Compare
@rishabsingh3003 and @rmackay9 this should be good to go. There appear to be a few unrelated tests failing though. It's been tested with a rover and works fairly well! |
c0bd0f4
to
ca9a8b8
Compare
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.
need some checks by @priseborough
@@ -289,8 +289,12 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus | |||
ftype vd = stateStruct.velocity.z; | |||
ftype pd = stateStruct.position.z; | |||
|
|||
// constrain height above ground to be above range measured on ground | |||
// constrain height above ground to be above range measured on ground, or user specified height (if given, for rover) | |||
#if APM_BUILD_TYPE(APM_BUILD_Rover) |
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.
@priseborough why is this working? @stephendade reports he can get optical flow fusion with no rangefinder. Does this mean the EKF is not properly checking the health of the rangefinder before fusing flow data?
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 optical flow fusion shouldn't be checking the range finder, it should be checking that the terrain height offsets state estimate is still valid. I had a quick look at the code and it does not appear that this is being done.
maybe we should not be dividing by DCM c.z ? |
As discussed on the dev call I tested disabling the rangefinder in Copter while using optical flow and it appears we have a bug (as @tridge suspected). I've raised an issue here. |
232f19b
to
428a4e1
Compare
428a4e1
to
66cc3b3
Compare
testing copter flow in CIRCLE mode would be good, checking we get low innovations on flow and good pos hold in wind |
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 wth EKF changes but need to confirm that the height override cannot be set by other platforms.
I've now tested this in copter SITL:
Have confirmed that height override param can't be set in other platforms.
Circle mode still works well in windy (10m/s) conditions. Flow Innovations were the same vs master. |
Great stuff, merged, thanks! |
This PR adds optical flow support for Rover. No rangefinder is needed, as a new parameter (FLOW_HGT_OVR) is used to specify an "override" constant height above ground for the sensor.
Successfully tested with an UPixels UPFlow sensor, Mro X2.1-777 and https://www.servocity.com/bogie-runt-rover/