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) {
return false;
}
if (!measure()) {
dev->get_semaphore()->give();
return false;
@ -92,7 +92,7 @@ bool AP_RangeFinder_TeraRangerI2C::init(void)
hal.scheduler->delay(70);
uint16_t _distance_cm;
if (!collect(_distance_cm)) {
if (!collect_raw(_distance_cm)) {
dev->get_semaphore()->give();
return false;
}
@ -115,23 +115,43 @@ bool AP_RangeFinder_TeraRangerI2C::measure()
}
// 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];
uint16_t distance = 0;
// take range reading and read back results
// Take range reading
if (!dev->transfer(nullptr, 0, d, sizeof(d))) {
return false;
}
// Check for CRC
if (d[2] != crc_crc8(d, 2)) {
// bad CRC
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)
{
// take a reading
uint16_t _distance_cm;
if (collect(_distance_cm) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
accum.sum += _distance_cm;
accum.count++;
// Take a reading
uint16_t _raw_distance = 0;
uint16_t _distance_cm = 0;
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();
}
// and immediately ask for a new reading
measure();
}
/*
update the state of the sensor
*/
@ -166,6 +188,6 @@ void AP_RangeFinder_TeraRangerI2C::update(void)
} else {
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);
bool measure(void);
bool collect(uint16_t &distance_cm);
bool init(void);
bool collect_raw(uint16_t &raw_distance);
bool check_measure(uint16_t raw_distance, uint16_t &distance_cm);
bool init(void);
void timer(void);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;