Support for Hex/Proficnc UAVCAN based Flow Sensor #11017

bugobliterator · 2019-04-05T14:17:50Z

No description provided.

rmackay9 · 2019-04-09T01:29:20Z

I added the DevCallTopic but I don't think we actually got to it this week.

WickedShell

A very non thorough review...

libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp

WickedShell · 2019-04-11T12:08:42Z

libraries/AP_RangeFinder/RangeFinder.cpp

+    if (num_instances < RANGEFINDER_MAX_INSTANCES) {
+        if (drivers[num_instances] == nullptr && params[num_instances].type == RangeFinder_TYPE_UAVCAN) {
+            //keep looking until found
+            drivers[num_instances] = AP_RangeFinder_UAVCAN::detect(state[num_instances], params[num_instances]);


If you failed to allocate it the first time, how is this likely to succeed?

There is a boot delay associated with the sensor module, it might come up after init was actually called.

WickedShell · 2019-04-11T12:10:45Z

libraries/AP_RangeFinder/RangeFinder.cpp

+            //keep looking until found
+            drivers[num_instances] = AP_RangeFinder_UAVCAN::detect(state[num_instances], params[num_instances]);
+            if (drivers[num_instances] != nullptr) {
+                num_instances++;


This doesn't honor the contract of num_instances being the highest detected instance correctly, and can corrupt other sensors.

Sorry, didn't quite get that. num_instances is the number of instances isn't it, with highest possible value of it being RANGEFINDER_MAX_INSTANCES. Both of that seems to be taken care of here.

While that sounds reasonable, it actually isn't the API we have in a lot of libraries. num_instances is the number of the highest detected backend. If you don't setup rangefinder 1, but do setup 2 and detect a driver on 2, then num_instances will be 2. https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_RangeFinder/RangeFinder.cpp#L312

The problem here is if someone leaves an empty slot, then things will go wrong.

libraries/AP_UAVCAN/dsdl/com/hex/equipment/flow/20200.Measurement.uavcan

rmackay9 · 2019-04-16T01:00:47Z

libraries/AP_OpticalFlow/OpticalFlow.cpp

@@ -154,7 +154,10 @@ void OpticalFlow::update(void)
    if (!enabled()) {
        return;
    }
-
+    if (backend == nullptr && (OpticalFlowType)_type.get() == OpticalFlowType::UAVCAN) {


We discussed this on the dev call and @tridge suggested that if the _TYPE = UAVCAN then we should always create the driver but it can be unhealthy which should then be caught in the pre-arm checks.

We thought that checking this at 200hz (which is how often it is run on Copter I think) is a bit excessive. I'm also not a fan of how this breaks the regular patter of initialising drivers.

rmackay9

I think we should initialise the driver in a similar way to other drivers.

libraries/AP_OpticalFlow/AP_OpticalFlow_UAVCAN.cpp

rmackay9 · 2019-04-19T02:36:59Z

As a side note, it would be good to have the PR description filled out with info including how much testing has been done.

rmackay9 · 2019-04-19T05:11:47Z

I've test flown this and it worked quite well (video)

I haven't tested the range of the lidar but I can see it works.

libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp

WickedShell · 2019-04-19T07:46:37Z

libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp

+        update_status();
+        _new_data = false;
+    } else {
+        set_status(_status);


You can't do this if there is no new data. This increments the range_valid_count even though there is no new evidence.

You also need to be manually tracking/invalidating the sensor to NoData if a reading hasn't come in recently (say 200 milliseconds) or you will be passing stale/frozen rangefinder data into the EKF and vehicle.

Fixed in this patch: c275a02

This is still stroking the range_valid_count to rapidly. You did actually want the new_data flag you had.

Basically this should loosely look like:

if new_data copy data update_status() else if millis() - last_reading > 200 set_status(NoData)

With nothing being done here if one of those 2 conditions aren't true.

Fixed with this: 1e5fc1e

Needed to add status set for other states being sent by sensor.

libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp

libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h

libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp

libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h

libraries/AP_RangeFinder/RangeFinder.h

WickedShell · 2019-04-19T21:21:28Z

libraries/AP_UAVCAN/dsdl/com/hex/equipment/flow/20200.Measurement.uavcan

+float32 integration_interval    # radians
+float32[2] rate_gyro_integral   # radians
+float32[2] flow_integral        # seconds
+uint8 quality


How is this interpreted?

There's a mistake there I will fix it. Needs to be seconds, radians, radians. I will also add more details.

Fixed in 37490b4

What's better 0 or 255?

Fixed, thanks

libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp

rmackay9

I think we should remove the commit to double the length of the EKF2's flow buffer. I've done a PR (EKF2/3: increase maximum optical flow sensor rate #11126) which I think is a slightly better change.
I think it would be good to squash a lot of these commits down. while that's being done, perhaps group the commits by library (i.e. separate rangefinder commits from flow commits) to make the review a bit easier.
AP_RangeFinder_UAVCAN and AP_OpticalFlow_HereFlow drivers have very few comments. I think it would be good to add more especially considering that as we add more UAVCAN drivers, developers are going to be looking for other code examples to start from and these could be used. One tiny thing is that normally our comments have a space after the "//". I.e. "// I was here" instead of "//I was here".
AP_OpticalFlow_HereFlow.cpp has a line of commented out debug code in there. I understand that it's often useful to have there but normally we don't leave it in.
AP_OpticalFlow_HereFlow uses a lot of static variables. I guess that's necessary?

rmackay9 · 2019-04-27T01:05:45Z

I'm keen to get this into master so I've added the DevCallTopic so that it will go through a review during next week's dev call. I think it would be nice to get it cleaned up a bit before then (ie. the items in my review). Thanks!

bugobliterator · 2019-04-30T08:03:25Z

@tridge I have squashed the commits.

rmackay9 · 2019-04-30T23:22:31Z

@bugobliterator, we should remove the EKF2 change..

bugobliterator · 2019-05-01T09:03:53Z

@bugobliterator, we should remove the EKF2 change..

Done.

rmackay9 · 2019-05-08T01:55:33Z

@bugobliterator, I was wondering if you've re-tested this on a real vehicle? I'm just referring to the two issues that @WickedShell found that indicate it may not have been tested.

bugobliterator · 2019-05-08T10:19:44Z

@bugobliterator, I was wondering if you've re-tested this on a real vehicle? I'm just referring to the two issues that @WickedShell found that indicate it may not have been tested.

@rmackay9 it is now tested working. I forgot to test the RangeFinder, so my bad on that. RangeFinder is now working with latest changes as well.

rbachtell · 2019-05-09T18:15:08Z

I can verify the functionality of the code, I have two vehicles currently flying with OF and RF working as designed.

rmackay9 · 2019-05-10T01:50:39Z

@bugobliterator,

I'm seeing Bad LiDAR Health a lot in the MP during testing. It's intermittent but the message appear very frequently (for me at least).

I think it's not a good idea to be setting the _status both within the handle_measurement() function and then again within the update() function. I think it makes it more difficult to understand which functions are responsible and leads to bugs. Also I think there is a real issue - for example, I think handle_measurement could set the _status to Good because the sensor says it got a good reading, but this reading go be further away than the RNGFND1_MAX_CM value. In these cases the _status should really be set to RangeFinder_OutOfRangeHigh.

By the way, I don't think you've gone through and added any additional comments. It's not a blocking requirement but I think this driver has perhaps less comments than any other driver we have so it's a bit of the norm.

rbachtell · 2019-05-10T02:51:45Z

I did have bad lidar health on one of my vehicles but in a conversation with Philip he looked at my log and said that at this point the lidar was designed for low altitude well lit indoor use and that outdoors could be problematic. I took this to be be more of a hardware issue than firmware so disabled the rangefinder and moved on. It does make sense to have an out of range high warning though instead of bad health. Might be worth getting Philip involved, if he isn’t already, just to be sure that we’re not fighting a hardware limitation. I’ll watch the progress and when changes come up I’ll continue testing. Randy Bachtell Sent from my iPad

…

On May 9, 2019, at 6:51 PM, Randy Mackay ***@***.***> wrote: @bugobliterator, I'm seeing Bad LiDAR Health a lot in the MP during testing. It's intermittent but the message appear very frequently (for me at least). I think it's not a good idea to be setting the _status both within the handle_measurement() function and then again within the update() function. I think it makes it more difficult to understand which functions are responsible and leads to bugs. Also I think there is a real issue - for example, I think handle_measurement could set the _status to Good because the sensor says it got a good reading, but this reading go be further away than the RNGFND1_MAX_CM value. In these cases the _status should really be set to RangeFinder_OutOfRangeHigh. By the way, I don't think you've gone through and added any additional comments. It's not a blocking requirement but I think this driver has perhaps less comments than any other driver we have so it's a bit of the norm. — You are receiving this because you commented. Reply to this email directly, view it on GitHub, or mute the thread.

…tory

bugobliterator · 2019-05-10T10:18:51Z

@rmackay9

@bugobliterator,

I'm seeing Bad LiDAR Health a lot in the MP during testing. It's intermittent but the message appear very frequently (for me at least).

I have updated the code to handle undefined as no data received and let top level method to have timeout based estimate on NoData. You might be getting lot of undefined data, that's why it behaved like that.

I think it's not a good idea to be setting the _status both within the handle_measurement() function and then again within the update() function. I think it makes it more difficult to understand which functions are responsible and leads to bugs. Also I think there is a real issue - for example, I think handle_measurement could set the _status to Good because the sensor says it got a good reading, but this reading go be further away than the RNGFND1_MAX_CM value. In these cases the _status should really be set to RangeFinder_OutOfRangeHigh.

The RangeHigh and RangeLow limit are handled based on response from HW as well as Ardupilot's internal parameter setting. update_state handles the good range measurements to be bounded by MAX and MIN as well. In case user sets too high or too low values than HW can handle, the vehicle will be protected by bounds set by HW itself as well.

By the way, I don't think you've gone through and added any additional comments. It's not a blocking requirement but I think this driver has perhaps less comments than any other driver we have so it's a bit of the norm.
I have updated the source codes with some comments now.

rmackay9 · 2019-05-10T23:24:48Z

I think what may be happening is that two different threads (the main thread that runs the update and then the I/O thread that runs the handle_measurement are both updating the status. I don't think we can do this because it will result in a race condition and there's no semaphores protecting us.

Maybe I'm wrong, perhaps another developer (like @OXINARF) will know better and say I'm wrong but for barring that, I think this needs to be handled like other range finder drivers (like the Benewake) where we average across readings while also handling feedback from the sensor saying that some readings are out of range.

rmackay9 · 2019-05-14T12:37:11Z

I've tested this again and if I use @bugobliterator's branch (i.e. the branch from this PR) then the range finder seems to work OK. If I rebase it off master though then I see "NoData" status from the range finder. I've included log files below:

rbachtell · 2019-05-14T16:07:48Z

I have compiled the code from latest master with the latest PR files copied over. I am not a programmer and know just enough C++ to be dangerous. That being said the OF and RF are working, the OF appears to be solid with good data. The RF shows accurate data and I have set max range to 140 to be safely within limits, at this point I get consistent recurring bad lidar health messages with altitudes above max. I'm unsure if this is code or hardware triggering the warning. I have included a log that is primarily around the max altitude set point and one that has some time well above.
00000156.BIN
00000157.BIN

rmackay9 · 2019-05-15T07:01:48Z

The flow sensor itself is working well but the range finder doesn't seem to perform very well. I suspect the rangefinder is going to be a source of user pain and support calls but it doesn't appear that much more than be done on the driver side to improve things so I'm going to merge this.

bugobliterator requested review from rmackay9 and tridge April 5, 2019 14:18

bugobliterator force-pushed the pr-uavcan-flow branch 3 times, most recently from 6e40979 to 25b0bef Compare April 8, 2019 09:46

rmackay9 added the DevCallTopic label Apr 8, 2019

WickedShell requested changes Apr 11, 2019

View reviewed changes

bugobliterator requested a review from WickedShell April 15, 2019 02:31

rmackay9 reviewed Apr 16, 2019

View reviewed changes

rmackay9 requested changes Apr 16, 2019

View reviewed changes

WickedShell reviewed Apr 16, 2019

View reviewed changes

libraries/AP_OpticalFlow/AP_OpticalFlow_UAVCAN.cpp Outdated Show resolved Hide resolved

rmackay9 removed the DevCallTopic label Apr 19, 2019

bugobliterator force-pushed the pr-uavcan-flow branch 3 times, most recently from b7e2aff to 61b3a6b Compare April 19, 2019 08:03

WickedShell requested changes Apr 19, 2019

View reviewed changes

bugobliterator requested review from WickedShell and rmackay9 April 19, 2019 08:55

bugobliterator force-pushed the pr-uavcan-flow branch from d9a8ed4 to ba2a95d Compare April 19, 2019 09:35

WickedShell reviewed Apr 19, 2019

View reviewed changes

libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp Show resolved Hide resolved

WickedShell reviewed Apr 19, 2019

View reviewed changes

libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp Show resolved Hide resolved

bugobliterator force-pushed the pr-uavcan-flow branch 2 times, most recently from 661c394 to 1e5fc1e Compare April 20, 2019 03:25

rmackay9 added the DevCallTopic label Apr 27, 2019

rmackay9 requested changes Apr 27, 2019

View reviewed changes

bugobliterator force-pushed the pr-uavcan-flow branch from 1e5fc1e to 9547273 Compare April 30, 2019 07:57

bugobliterator force-pushed the pr-uavcan-flow branch 3 times, most recently from c0cc313 to d8c6b5e Compare April 30, 2019 20:43

bugobliterator force-pushed the pr-uavcan-flow branch from d8c6b5e to af82342 Compare May 1, 2019 09:03

bugobliterator force-pushed the pr-uavcan-flow branch 2 times, most recently from d0ecf00 to c3d082e Compare May 10, 2019 10:09

bugobliterator and others added 5 commits May 10, 2019 15:48

OpticalFlow: add support for HereFlow message over can

c7bb9cd

waf: support building headers from uavcan_vendor_specific_types direc…

ee4adb7

…tory

waf: add support for extra messages from AP_UAVCAN

2892556

AP_UAVCAN: add support for hereflow and range sensor message over can

0d13e17

AP_UAVCAN:add hex flow sensor message

5581633

bugobliterator force-pushed the pr-uavcan-flow branch from c3d082e to cd57159 Compare May 10, 2019 10:18

bugobliterator requested a review from rmackay9 May 10, 2019 10:19

RangeFinder: add support for rangefinder sensor over can

51a236e

bugobliterator force-pushed the pr-uavcan-flow branch from cd57159 to 51a236e Compare May 10, 2019 10:20

rmackay9 merged commit 479c238 into ArduPilot:master May 15, 2019

Support for Hex/Proficnc UAVCAN based Flow Sensor #11017

Support for Hex/Proficnc UAVCAN based Flow Sensor #11017

Conversation

bugobliterator commented Apr 5, 2019

rmackay9 commented Apr 9, 2019

WickedShell left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rmackay9 Apr 16, 2019 • edited

Choose a reason for hiding this comment

rmackay9 left a comment

Choose a reason for hiding this comment

rmackay9 commented Apr 19, 2019

rmackay9 commented Apr 19, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rmackay9 left a comment

Choose a reason for hiding this comment

rmackay9 commented Apr 27, 2019

bugobliterator commented Apr 30, 2019

rmackay9 commented Apr 30, 2019

bugobliterator commented May 1, 2019

rmackay9 commented May 8, 2019

bugobliterator commented May 8, 2019

rbachtell commented May 9, 2019

rmackay9 commented May 10, 2019

rbachtell commented May 10, 2019 via email

bugobliterator commented May 10, 2019 • edited

rmackay9 commented May 10, 2019 • edited

rmackay9 commented May 14, 2019

rbachtell commented May 14, 2019

rmackay9 commented May 15, 2019

rmackay9 Apr 16, 2019 •

edited

bugobliterator commented May 10, 2019 •

edited

rmackay9 commented May 10, 2019 •

edited