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: use thread per bus for LightWareI2C driver

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-01 08:51:37 +11:00
parent 88df9c7029
commit 0b27478d7b
2 changed files with 23 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
View File

@ -30,6 +30,9 @@ AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, u
: AP_RangeFinder_Backend(_ranger, instance, _state)
, _dev(std::move(dev))
{
// call timer() at 20Hz
_dev->register_periodic_callback(50000,
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWareI2C::timer, bool));
}
/*
@ -42,13 +45,21 @@ AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder &_ranger
AP_RangeFinder_LightWareI2C *sensor
= new AP_RangeFinder_LightWareI2C(_ranger, instance, _state, std::move(dev));
uint16_t reading_cm;
if (!sensor || !sensor->get_reading(reading_cm)) {
if (!sensor) {
delete sensor;
return nullptr;
}
if (sensor->_dev->get_semaphore()->take(0)) {
uint16_t reading_cm;
if (!sensor->get_reading(reading_cm)) {
sensor->_dev->get_semaphore()->give();
delete sensor;
return nullptr;
}
sensor->_dev->get_semaphore()->give();
}
return sensor;
}
@ -61,12 +72,6 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
return false;
}
// exit immediately if we can't take the semaphore
if (!_dev || !_dev->get_semaphore()->take(1)) {
return false;
}
// read the high and low byte distance registers
bool ret = _dev->read((uint8_t *) &val, sizeof(val));
if (ret) {
@ -74,8 +79,6 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
reading_cm = be16toh(val);
}
_dev->get_semaphore()->give();
return ret;
}
@ -83,6 +86,11 @@ bool AP_RangeFinder_LightWareI2C::get_reading(uint16_t &reading_cm)
update the state of the sensor
*/
void AP_RangeFinder_LightWareI2C::update(void)
{
// nothing to do - its all done in the timer()
}
bool AP_RangeFinder_LightWareI2C::timer(void)
{
if (get_reading(state.distance_cm)) {
// update range_valid state based on distance measured
@ -90,4 +98,5 @@ void AP_RangeFinder_LightWareI2C::update(void)
} else {
set_status(RangeFinder::RangeFinder_NoData);
}
return true;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h
View File

@ -18,6 +18,8 @@ private:
// constructor
AP_RangeFinder_LightWareI2C(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
bool timer();
// get a reading
bool get_reading(uint16_t &reading_cm);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;