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;
}
}
}