Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder_PulsedLightLRF: Fix driver after I2CDevice conversion

Browse Source 
* Fix wrong semaphore->take check in ::get_reading
* Simplify semaphore releasing
* Fix typos and trailing whitespace
This commit is contained in:
Murilo Belluzzo 2016-07-11 19:58:29 -03:00 committed by Lucas De Marchi
parent e1342eb533
commit 0dc33410ff
1 changed files with 14 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
View File

@ -20,8 +20,8 @@
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
*/ */
@ -32,7 +32,7 @@ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_range
{ {
} }
/* /*
detect if a PulsedLight rangefinder is connected. We'll detect by detect if a PulsedLight rangefinder is connected. We'll detect by
trying to take a reading on I2C. If we get a result the sensor is trying to take a reading on I2C. If we get a result the sensor is
there. there.
@ -67,38 +67,32 @@ bool AP_RangeFinder_PulsedLightLRF::start_reading()
} }
// send command to take reading // send command to take reading
if (!_dev->write_register(AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG, bool ret = _dev->write_register(AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG,
AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE)) { AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE);
_dev->get_semaphore()->give();
return false;
}
// return semaphore
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
return ret;
return true;
} }
// read - return last value measured by sensor // read - return last value measured by sensor
bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm) bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm)
{ {
if (_dev->get_semaphore()->take(1)) { uint8_t buff[2];
if (!_dev->get_semaphore()->take(1)) {
return false; return false;
} }
uint8_t buff[2];
// read the high and low byte distance registers // read the high and low byte distance registers
if (!_dev->read_registers(AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, buff, sizeof(buff))) { bool ret = _dev->read_registers(AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, buff, sizeof(buff));
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
if (!ret) {
return false; return false;
} }
// 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();
// kick off another reading for next time // kick off another reading for next time
// To-Do: replace this with continuous mode // To-Do: replace this with continuous mode
hal.scheduler->delay_microseconds(200); hal.scheduler->delay_microseconds(200);
@ -107,7 +101,7 @@ bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm)
return true; return true;
} }
/* /*
update the state of the sensor update the state of the sensor
*/ */
void AP_RangeFinder_PulsedLightLRF::update(void) void AP_RangeFinder_PulsedLightLRF::update(void)