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

AP_Rangefinder: Fix VL53L1X ignoring return status

This commit is contained in:
Michael du Breuil 2019-04-09 02:10:10 -07:00 committed by Andrew Tridgell
parent f1d5269f9e
commit 6dd05db3b7
2 changed files with 42 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp
View File

@ -421,6 +421,8 @@ bool AP_RangeFinder_VL53L1X::get_reading(uint16_t &reading_mm)
} }
} }
const uint8_t range_status = read_register(RESULT__RANGE_STATUS);
reading_mm = read_register16(RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0); reading_mm = read_register16(RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0);
// "apply correction gain" // "apply correction gain"
// gain factor of 2011 is tuning parm default (VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT) // gain factor of 2011 is tuning parm default (VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT)
@ -435,7 +437,18 @@ bool AP_RangeFinder_VL53L1X::get_reading(uint16_t &reading_mm)
} }
write_register(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range write_register(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range
return true;
switch ((DeviceError)range_status) {
case RANGECOMPLETE:
return true;
default:
#ifdef VL53L1X_DEBUG
hal.console->printf("VL53L1X: %d ms status %d\n", AP_HAL::millis(), (int)range_status);
#endif // VL53L1X_DEBUG
return false;
}
} }
uint8_t AP_RangeFinder_VL53L1X::read_register(uint16_t reg) uint8_t AP_RangeFinder_VL53L1X::read_register(uint16_t reg)
@ -492,6 +505,7 @@ void AP_RangeFinder_VL53L1X::timer(void)
{ {
uint16_t range_mm; uint16_t range_mm;
if ((get_reading(range_mm)) && (range_mm <= 4000)) { if ((get_reading(range_mm)) && (range_mm <= 4000)) {
WITH_SEMAPHORE(_sem);
sum_mm += range_mm; sum_mm += range_mm;
counter++; counter++;
} }

27
libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h
View File

@ -21,6 +21,33 @@ protected:
} }
private: private:
enum DeviceError : uint8_t
{
NOUPDATE = 0,
VCSELCONTINUITYTESTFAILURE = 1,
VCSELWATCHDOGTESTFAILURE = 2,
NOVHVVALUEFOUND = 3,
MSRCNOTARGET = 4,
RANGEPHASECHECK = 5,
SIGMATHRESHOLDCHECK = 6,
PHASECONSISTENCY = 7,
MINCLIP = 8,
RANGECOMPLETE = 9,
ALGOUNDERFLOW = 10,
ALGOOVERFLOW = 11,
RANGEIGNORETHRESHOLD = 12,
USERROICLIP = 13,
REFSPADCHARNOTENOUGHDPADS = 14,
REFSPADCHARMORETHANTARGET = 15,
REFSPADCHARLESSTHANTARGET = 16,
MULTCLIPFAIL = 17,
GPHSTREAMCOUNT0READY = 18,
RANGECOMPLETE_NO_WRAP_CHECK = 19,
EVENTCONSISTENCY = 20,
MINSIGNALEVENTCHECK = 21,
RANGECOMPLETE_MERGED_PULSE = 22,
};
// register addresses from API vl53l1x_register_map.h // register addresses from API vl53l1x_register_map.h
enum regAddr : uint16_t enum regAddr : uint16_t
{ {