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

AP_RangeFinder: Add error codes rejection for TeraRanger sensors

This commit is contained in:
pierre-louis.k 2018-08-03 16:41:15 +02:00 committed by Andrew Tridgell
parent 21782835c4
commit cfd13d4e83
2 changed files with 42 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp
View File

@ -82,7 +82,7 @@ bool AP_RangeFinder_TeraRangerI2C::init(void)
whoami != TR_WHOAMI_VALUE) { whoami != TR_WHOAMI_VALUE) {
return false; return false;
} }
if (!measure()) { if (!measure()) {
dev->get_semaphore()->give(); dev->get_semaphore()->give();
return false; return false;
@ -92,7 +92,7 @@ bool AP_RangeFinder_TeraRangerI2C::init(void)
hal.scheduler->delay(70); hal.scheduler->delay(70);
uint16_t _distance_cm; uint16_t _distance_cm;
if (!collect(_distance_cm)) { if (!collect_raw(_distance_cm)) {
dev->get_semaphore()->give(); dev->get_semaphore()->give();
return false; return false;
} }
@ -115,23 +115,43 @@ bool AP_RangeFinder_TeraRangerI2C::measure()
} }
// collect - return last value measured by sensor // collect - return last value measured by sensor
bool AP_RangeFinder_TeraRangerI2C::collect(uint16_t &_distance_cm) bool AP_RangeFinder_TeraRangerI2C::collect_raw(uint16_t &raw_distance)
{ {
uint8_t d[3]; uint8_t d[3];
uint16_t distance = 0;
// take range reading and read back results // Take range reading
if (!dev->transfer(nullptr, 0, d, sizeof(d))) { if (!dev->transfer(nullptr, 0, d, sizeof(d))) {
return false; return false;
} }
// Check for CRC
if (d[2] != crc_crc8(d, 2)) { if (d[2] != crc_crc8(d, 2)) {
// bad CRC
return false; return false;
} else {
raw_distance = ((uint16_t(d[0]) << 8) | d[1]);
return true;
} }
}
_distance_cm = ((uint16_t(d[0]) << 8) | d[1]) / 10;
return true; // Checks for error code and if correct converts to cm
bool AP_RangeFinder_TeraRangerI2C::check_measure(uint16_t raw_distance, uint16_t &output_distance_cm)
{
// Check for error codes
if (raw_distance == 0xFFFF) {
// too far away
return false;
}
if (raw_distance == 0x0000) {
// too close
return false;
}
if (raw_distance == 0x0001) {
// Unable to measure
return false;
}
output_distance_cm = raw_distance/10;
return true;
} }
/* /*
@ -139,19 +159,21 @@ bool AP_RangeFinder_TeraRangerI2C::collect(uint16_t &_distance_cm)
*/ */
void AP_RangeFinder_TeraRangerI2C::timer(void) void AP_RangeFinder_TeraRangerI2C::timer(void)
{ {
// take a reading // Take a reading
uint16_t _distance_cm; uint16_t _raw_distance = 0;
if (collect(_distance_cm) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { uint16_t _distance_cm = 0;
accum.sum += _distance_cm;
accum.count++; if (collect_raw(_raw_distance) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if(check_measure(_raw_distance, _distance_cm)){
accum.sum += _distance_cm;
accum.count++;
}
_sem->give(); _sem->give();
} }
// and immediately ask for a new reading // and immediately ask for a new reading
measure(); measure();
} }
/* /*
update the state of the sensor update the state of the sensor
*/ */
@ -166,6 +188,6 @@ void AP_RangeFinder_TeraRangerI2C::update(void)
} else { } else {
set_status(RangeFinder::RangeFinder_NoData); set_status(RangeFinder::RangeFinder_NoData);
} }
_sem->give(); _sem->give();
} }
} }

7
libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.h
View File

@ -26,9 +26,10 @@ private:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> i2c_dev); AP_HAL::OwnPtr<AP_HAL::I2CDevice> i2c_dev);
bool measure(void); bool measure(void);
bool collect(uint16_t &distance_cm); bool collect_raw(uint16_t &raw_distance);
bool check_measure(uint16_t raw_distance, uint16_t &distance_cm);
bool init(void);
bool init(void);
void timer(void); void timer(void);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev; AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;