aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RangeFinder_LightWareI2C: Fix driver after I2CDevice conversion

Browse Source 
* Fix semaphore not being released in ::get_reading
* Simplify semaphore releasing logic
* Fix typo
This commit is contained in:
Murilo Belluzzo 2016-07-11 20:03:44 -03:00 committed by Lucas De Marchi
parent 49d1520850
commit 66fdfbb850
1 changed files with 12 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
View File

@ -21,7 +21,7 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
/* /*
The constructor also initialises the rangefinder. Note that this The constructor also initializes the rangefinder. Note that this
constructor is not called until detect() returns true, so we constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder already know that we should setup the rangefinder
*/ */
@ -54,29 +54,26 @@ AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder &_ranger
// read - return last value measured by sensor // read - return last value measured by sensor
bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm) bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
{ {
// exit immediately if we can't take the semaphore uint8_t buff[2];
if (!_dev->get_semaphore()->take(1)) {
return false;
}
if (ranger._address[state.instance] == 0) { if (ranger._address[state.instance] == 0) {
return false; return false;
} }
uint8_t buff[2]; // exit immediately if we can't take the semaphore
// read the high and low byte distance registers if (!_dev->get_semaphore()->take(1)) {
if (!_dev->read(buff, sizeof(buff))) {
_dev->get_semaphore()->give();
return false; return false;
} }
// read the high and low byte distance registers
bool ret = _dev->read(buff, sizeof(buff));
if (ret) {
// combine results into distance // combine results into distance
reading_cm = ((uint16_t)buff[0]) << 8 | buff[1]; reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
}
// return semaphore
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
return ret;
return true;
} }
/* /*