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

AP_RangeFinder: fixed legacy parsing of 65436 for lightware i2c 

some lidars will probe as legacy protocol and return 65436 as range
This commit is contained in:
Andrew Tridgell 2020-07-24 13:13:27 +10:00
parent aba60c030e
commit a6e5e92ab7
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
View File

@ -350,8 +350,13 @@ bool AP_RangeFinder_LightWareI2C::legacy_get_reading(uint16_t &reading_cm)
// read the high and low byte distance registers
if (_dev->transfer(&read_reg, 1, (uint8_t *)&val, sizeof(val))) {
// combine results into distance
reading_cm = be16toh(val);
int16_t signed_val = int16_t(be16toh(val));
if (signed_val < 0) {
// some lidar firmwares will return 65436 for out of range
reading_cm = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM);
} else {
reading_cm = uint16_t(signed_val);
}
return true;
}
return false;