Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: removed momentary spikes in PulsedLight I2C Lidar

This commit is contained in:
Andrew Tridgell 2016-11-20 11:31:36 +11:00
parent 252f90ba36
commit 6c09758447
2 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -93,8 +93,13 @@ bool AP_RangeFinder_PulsedLightLRF::timer(void)
be16_t val;
// read the high and low byte distance registers
if (_dev->read_registers(LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT, (uint8_t*)&val, sizeof(val))) {
state.distance_cm = be16toh(val);
update_status();
uint16_t distance_cm = be16toh(val);
// remove momentary spikes
if (abs(distance_cm - last_distance_cm) < 100) {
state.distance_cm = distance_cm;
update_status();
}
last_distance_cm = distance_cm;
} else {
set_status(RangeFinder::RangeFinder_NoData);
}

3
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
View File

@ -42,6 +42,7 @@ private:
uint8_t hw_version;
uint8_t check_reg_counter;
bool v2_hardware;
uint16_t last_distance_cm;
enum { PHASE_MEASURE, PHASE_COLLECT } phase;
};