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

AP_RangeFinder: TFMiniPlus: fix out-of-range returned a no-data 

Other drivers consider that they received data even if the value is
reported as "out of range" by sensor. On the I2C driver for TFMiniPlus
we considered this case, too. However when the signal strength is very
low (and thus the distance would likely be out of range), we would end up
ignoring the new sample. With enough samples without any value this
would lead the status to turn to "NoData".
This commit is contained in:
Lucas De Marchi 2020-09-28 17:45:45 -07:00 committed by Andrew Tridgell
parent 36eb605de5
commit 0e63a833e9
2 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp
View File

@ -149,20 +149,26 @@ void AP_RangeFinder_Benewake_TFMiniPlus::update()
} }
} }
bool AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(le16_t distance_raw, le16_t strength_raw, void AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(le16_t distance_raw, le16_t strength_raw,
uint16_t &output_distance_cm) uint16_t &output_distance_cm)
{ {
uint16_t strength = le16toh(strength_raw); uint16_t strength = le16toh(strength_raw);
const uint16_t MAX_DIST_CM = 1200;
const uint16_t MIN_DIST_CM = 10;
output_distance_cm = le16toh(distance_raw); output_distance_cm = le16toh(distance_raw);
if (strength < 100 || strength == 0xFFFF) { if (strength < 100 || strength == 0xFFFF || output_distance_cm > MAX_DIST_CM) {
return false; /*
* From manual: "when the signal strength is lower than 100 or equal to
* 65535, the detection is unreliable, TFmini Plus will set distance
* value to 0." - force it to the max distance so status is set to OutOfRangeHigh
* rather than NoData.
*/
output_distance_cm = MAX_DIST_CM;
} }
output_distance_cm = constrain_int16(output_distance_cm, 10, 1200); output_distance_cm = constrain_int16(output_distance_cm, MIN_DIST_CM, MAX_DIST_CM);
return true;
} }
bool AP_RangeFinder_Benewake_TFMiniPlus::check_checksum(uint8_t *arr, int pkt_len) bool AP_RangeFinder_Benewake_TFMiniPlus::check_checksum(uint8_t *arr, int pkt_len)
@ -203,7 +209,9 @@ void AP_RangeFinder_Benewake_TFMiniPlus::timer()
if (u.val.header1 != 0x59 || u.val.header2 != 0x59 || !check_checksum(u.arr, sizeof(u))) if (u.val.header1 != 0x59 || u.val.header2 != 0x59 || !check_checksum(u.arr, sizeof(u)))
return; return;
if (process_raw_measure(u.val.distance, u.val.strength, distance)) { process_raw_measure(u.val.distance, u.val.strength, distance);
{
WITH_SEMAPHORE(_sem); WITH_SEMAPHORE(_sem);
accum.sum += distance; accum.sum += distance;
accum.count++; accum.count++;

2
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.h
View File

@ -48,7 +48,7 @@ private:
bool init(); bool init();
void timer(); void timer();
bool process_raw_measure(le16_t distance_raw, le16_t strength_raw, void process_raw_measure(le16_t distance_raw, le16_t strength_raw,
uint16_t &output_distance_cm); uint16_t &output_distance_cm);
bool check_checksum(uint8_t *arr, int pkt_len); bool check_checksum(uint8_t *arr, int pkt_len);