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

AP_RangeFinder: MaxsonarI2CXL: add some fixes

Browse Source 
- coding style changes and some renames
   - fix bus number
   - use be16toh
This commit is contained in:
Lucas De Marchi 2016-07-12 19:00:01 -03:00
parent f2d5eb9a97
commit 4ab1d7cf41
1 changed files with 20 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
View File

@ -22,11 +22,13 @@
*
* Sensor should be connected to the I2C port
*/
#include "AP_RangeFinder_MaxsonarI2CXL.h"
#include <AP_HAL/AP_HAL.h>
#include <utility>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
extern const AP_HAL::HAL& hal;
/*
@ -34,9 +36,9 @@ extern const AP_HAL::HAL& hal;
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state),
_dev(hal.i2c_mgr->get_device(0, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))
AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state)
: AP_RangeFinder_Backend(_ranger, instance, _state)
, _dev(hal.i2c_mgr->get_device(1, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))
{
}
@ -54,8 +56,10 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_range
delete sensor;
return nullptr;
}
// give time for the sensor to process the request
hal.scheduler->delay(50);
uint16_t reading_cm;
if (!sensor->get_reading(reading_cm)) {
delete sensor;
@ -68,22 +72,24 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_range
// start_reading() - ask sensor to make a range reading
bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
{
if (!_dev->get_semaphore()->take(1)) {
if (!_dev || !_dev->get_semaphore()->take(1)) {
return false;
}
uint8_t tosend[] = {AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING};
uint8_t cmd = AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING;
// send command to take reading
bool ret = _dev->transfer(tosend, sizeof(tosend), nullptr, 0);
bool ret = _dev->transfer(&cmd, sizeof(cmd), nullptr, 0);
_dev->get_semaphore()->give();
return ret;
}
// read - return last value measured by sensor
bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm)
{
uint8_t buff[2];
be16_t val;
// exit immediately if we can't take the semaphore
if (!_dev->get_semaphore()->take(1)) {
@ -91,12 +97,12 @@ bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm)
}
// take range reading and read back results
bool ret = _dev->transfer(nullptr, 0, buff, sizeof(buff));
bool ret = _dev->transfer(nullptr, 0, (uint8_t *) &val, sizeof(val));
_dev->get_semaphore()->give();
if (ret) {
// combine results into distance
reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
reading_cm = be16toh(val);
// trigger a new reading
start_reading();