that’s what I was starting to think, but I can find videos of them being used on lawnmowers outdoors in the sun.
I was just searching github for any LD06 drivers and i found this in the SDK,
const int CONFIDENCE_LOW = 15; /* Low confidence threshold */
const int CONFIDENCE_SINGLE = 220; /* Discrete points require higher confidence */
FYI: I use a LD19P, it has a 60k lux from light vs the LD06/LD19 only 30k lux from light.
with LD19P on 4.6 Beta2 works in sunlight with 4.6 Beta5 it does not.
Dennis
I’ve created a PR which I think reverts the driver to use an average distance instead of the shortest distance. I was wondering if you’d be willing to test this 4.7.0-dev firmware for the MicoAir743?
This is not a 4.6 binary but rather a 4.7 binary so it’s probably best not to test fly it (although it’s very likely safe to fly).
FYI @geofrancis, @Adam_Borowski any feedback on the PR (positive or negative) are more than welcome!
Seems averaging is done for the whole sector. I tested that on my desk, and the issue I see almost immediately is detecting narrow objects. I put my finger about 15cm from lidar, and because of averaging the whole sector was averaged to about 90cm with wall beeing about 2.5m from lidar.
I would say averaging should be just for single batch of measurements and rejecting low confidence values.
I would say this is kind of safety issue as narrower obstacles might not be detected as beeing too close.
I have done some more testing, in direct sunlight even with the confidence filter at 250 at still gets interference in direct sunlight, its not as much but It’s still there. Without the filter it’s a solid sector of noise.
What I’m thinking is that due to the random nature of the noise rather than averaging over the sector im going to compare data over 2 passes, that will drop the effective update speed from 10hz to 5hz but it should get rid of the random noise. basically, if the second pass isn’t within ±10cm then its rejected.
if that’s too slow then I will look at using doing the same but with odd and even angles.
Quick question - how do You get more than 8 sectors in MP proximity diagram?
because I’m not using it as a native device, I convert it into to a mavlink proximity device with its own heartbeat, then I can select it in mission planner and get the 72 segment high resolution display directly from the rangefinder when I open the proximity display.
if (ld06.readScan()) {
// Read lidar packets and return true when a new full 360° scan is available
uint16_t n = ld06.getNbPointsInScan(); // Give the number of points in the scan, can be usefull with filtering to tell if there are abstacles around the lidar
for (uint16_t i = 0; i < n; i++) {
//Serial.println(String() + ld06.getPoints(i)->angle + "," + ld06.getPoints(i)->distance + ";"); // example to show how to extract data. ->x, ->y and ->intensity are also available.
lidarAngle = ld06.getPoints(i)->angle;
messageAngle = map(lidarAngle, 0, 360, 0, 72);
//Serial.println("LIDAR");
distances[messageAngle] = (ld06.getPoints(i)->distance / 10);
RGBangle = map(lidarAngle, 0, 360, 0, 8);
LEDS[RGBangle] = (ld06.getPoints(i)->distance / 10);
}
}
void MAVLINK_PROX() {
if (ld06.isNewScan()) {
int sysid = 1;
int compid = 196;
uint64_t time_usec = 0;
uint8_t sensor_type = 0;
uint8_t increment = 5;
uint16_t min_distance = 50;
uint16_t max_distance = 10000;
float increment_f = 0;
float angle_offset = 0;
uint8_t frame = 12;
uint8_t system_type = MAV_TYPE_GENERIC;
uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;
uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
uint32_t custom_mode = 30; ///< Custom mode, can be defined by user/adopter
uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
int type = MAV_TYPE_GROUND_ROVER;
// Pack the message
mavlink_msg_obstacle_distance_pack(sysid, compid, &msg, time_usec, sensor_type, distances, increment, min_distance, max_distance, increment_f, angle_offset, frame);
len = mavlink_msg_to_send_buffer(buf, &msg);
Serial2.write(buf, len);
}
}
void MAVLINK_HB() {
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval) {
previousMillis = currentMillis;
uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;
uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
uint32_t custom_mode = 0; ///< Custom mode, can be defined by user/adopter
uint8_t system_state = 0; ///< System ready for flight
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
int type = MAV_TYPE_GIMBAL;
// Pack the message
mavlink_msg_heartbeat_pack(1, 196, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
len = mavlink_msg_to_send_buffer(buf, &msg);
Serial2.write(buf, len);
}
}
My current idea is a comparison filter, it records the first scan then takes a second scan and compares the data, if it’s not within tolerance then it’s ignored, currently untested.
void MAP_MAVLINK() {
if (send = 0) {
if (ld06.readScan()) {
uint16_t n = ld06.getNbPointsInScan();
for (uint16_t i = 0; i < n; i++) {
lidarAngle = ld06.getPoints(i)->angle;
distances0[lidarAngle] = (ld06.getPoints(i)->distance / 10);
}
send = 1;
}
}
if (send = 1) {
if (ld06.readScan()) {
uint16_t n = ld06.getNbPointsInScan();
for (uint16_t i = 0; i < n; i++) {
lidarAngle = ld06.getPoints(i)->angle;
distances1[lidarAngle] = (ld06.getPoints(i)->distance / 10);
if (distances1[i] > (distances0[i] - 20) && distances1[i] < (distances0[i] + 20)) {
messageAngle = map(lidarAngle, 0, 360, 0, 72);
if (distances1[lidarAngle] < messageAngle && distances1[lidarAngle] > 5) {
distances[messageAngle] = (ld06.getPoints(i)->distance / 10);
}
}
}
send = 0;
}
}
}
Hi @geofrancis,
@peterbarker has suggested that we should introduce a mode filter to remove the noise. I’ll probably try and update the PR linked above to use that instead of the averaging. The downside of a mode filter is that, like other filters, it introduces a bit of lag which will appear as a slight rotation. The effect will likely be very small though.
@Adam_Borowski, @Dennis_Bowman, @geofrancis,
I’ve created a new PR which uses a mode filter instead of doing the averaging.
I wonder if you could help me test these changes?
In case it helps I’ve put a Copter-4.7.0-dev binary for the MicoAir743 here but I’m also happy to create binaries for any other autopilots as well
Thanks so much for the feedback so far!
MatekH743-bdshot would be nice. I could do some testing today.
I did a quick test, and it seems measurements are stable,but I noticed some strange behavior I will have to test a little more to confirm it.
In general - there is about 3m to the wall, I put my hand like 0.5m from lidar, and it detects it fine, but… when I am moving my hand away for about 20cm suddenly it is no longer detected and distance jumps to 3m.
10cm hand from 1m would be 5.7deg, so angular resolution of LD06/LD19 should allow to detect it
Will test it in few hours in more controlled conditions than just holding it in a hand.
my understanding is that a mode filter is just using the most common value, so that would get rid of the noise but it could cause it to miss small objects at long range since they would only be getting a few returns.
Need to take a look into the code. I would expect to use mode filter for each 2 deg data pack, not the whole 45deg sector. 5.7 deg would be at least 2 full 2-deg data packets with distance to hand, not to the wall.
Thanks very much for testing.
The purpose of the mode filter is to ignore short term spikes that are either high or low. So this could explain why it would ignore something narrow that is far away.
The way the mode filter works is it has 3 buckets that are in distance order (e.g. 1st bucket has the shortest distance, 2nd bucket has the middle distance, 3rd has the longest distance).
Each time a new reading comes in it is placed in one of the three buckets and ends up pushing one element out. E.g If it falls into the 1st or 3rd bucket it ends up pushing those elements out. If it falls into the middle bucket then that previous middle bucket value goes into the 1st or 3rd bucket (depending upon it’s distance).
The distance used for that angle is always the value from the middle bucket.
EDIT: By the way, the mode filter is applied on the raw values coming out of the sensor
const float distance_m = _dist_filt_mm.apply(UINT16_VALUE(_response[i + 1], _response[i])) * 0.001;
shouldn’t that be called after rejecting samples against min max distance and low confidence?
I think distance_m needs to be calculated from sample, compared to check rejection condistions, and then processed by filter.
Also… not sure if that is required, but I do not see any filter reset to clear data from previous data batch run.
If I understand that correctly - this would require about 1/3 of sector wide obstacle to be correctly detected, as this rejects not only noise, but also most of samples.
I would try to get all non-rejected samples into an array, sort an array and get n usable measurements.
add some r as “rejection ratio”, then get min values of samples r…(n-r-1) basically ignoring extremas at 0..r-1 and n-r…n-1 indexes.
EDIT: damn… obviously no need to find minimum, as array is already sorted, just need to take [r] element. This perfectly shows how too much coding at work drains brain 
Hi @Adam_Borowski,
Thanks again for the testing and feedback.
Re whether we should do the mode filtering before or after the distance checks. I also wondered which way would be better but the reason I decided on filtering first was that it means the filter is less likely to ever use very out-of-date values.
So for example, in an extreme case where the rangefinder returns out-of-range distances for a wide 90 degree area (perhaps the sun is shining from that direction), if the mode filter was done afterwards then it would never get these out-of-range distances, it would instead be filled with the last few distances from before the bad section. Then on the first good value (after the bad section) the filter could return a distance from before the bad section.
| Filter after dist checks | ||||
|---|---|---|---|---|
| Input | FilterPos1 | FilterPos2 | FilterPos3 | Output |
| 10 | 10 | 10 | 10 | 10 |
| 11 | 10 | 10 | 11 | 10 |
| 12 | 10 | 11 | 12 | 11 |
| 999 | 10 | 11 | 12 | none |
| 999 | 10 | 11 | 12 | none |
| 999 | 10 | 11 | 12 | none |
| 999 | 10 | 11 | 12 | none |
| 999 | 10 | 11 | 12 | none |
| 40 | 10 | 11 | 40 | 11 ← this dist is old |
| 39 | 11 | 39 | 40 | 39 |
| Filter before dist checks | ||||
|---|---|---|---|---|
| Input | FilterPos1 | FilterPos2 | FilterPos3 | Output |
| 10 | 10 | 10 | 10 | 10 |
| 11 | 10 | 10 | 11 | 10 |
| 12 | 10 | 11 | 12 | 11 |
| 999 | 10 | 11 | 999 | 11 |
| 999 | 10 | 999 | 999 | 999 (none) |
| 999 | 999 | 999 | 999 | 999 (none) |
| 999 | 999 | 999 | 999 | 999 (none) |
| 999 | 999 | 999 | 999 | 999 (none) |
| 40 | 40 | 999 | 999 | 999 (none) |
| 39 | 39 | 40 | 999 | 40 |
Not sure what is wrong with that. It is not that this will persist or something.
Also filtering before checks raises potential to actual correct distance detection to be pushed out of filter be invalid values in case those 999 will be 3 or more last data points. That takes into filtering values that should be ignored due to PRXn_IGN_ANGx/ PRXn_IGN_WIDx params.
I will try to implement what I described before to take Nth minimum value from data data points
Hi @Adam_Borowski,
Please have a peek at the “Filter after dist checks” table above and you’ll see the “11 ← this dist is old” written. That’s the last good value from before the out-of-range area. That “11” is very far from the distances at that angle. In the table below you’ll see that a more correct “40” is returned.
Perhaps we should just focus on whether the PR works acceptably well or not, I think it does..