mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_RangeFinder: Rework function structure in TeraRanger driver
This commit is contained in:
parent
b6f1647c31
commit
5577b8b33c
@ -137,18 +137,17 @@ bool AP_RangeFinder_TeraRangerI2C::process_raw_measure(uint16_t raw_distance, ui
|
||||
if (raw_distance == 0xFFFF) {
|
||||
// Too far away
|
||||
return false;
|
||||
}
|
||||
if (raw_distance == 0x0000) {
|
||||
} else if (raw_distance == 0x0000) {
|
||||
// Too close
|
||||
return false;
|
||||
}
|
||||
if (raw_distance == 0x0001) {
|
||||
} else if (raw_distance == 0x0001) {
|
||||
// Unable to measure
|
||||
return false;
|
||||
}
|
||||
output_distance_cm = raw_distance/10;
|
||||
} else {
|
||||
output_distance_cm = raw_distance/10; // Conversion to centimeters
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
timer called at 20Hz
|
||||
|
Loading…
Reference in New Issue
Block a user