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: 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

34
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
View File

@ -22,25 +22,27 @@
* *
* Sensor should be connected to the I2C port * Sensor should be connected to the I2C port
*/ */
#include "AP_RangeFinder_MaxsonarI2CXL.h" #include "AP_RangeFinder_MaxsonarI2CXL.h"
#include <AP_HAL/AP_HAL.h>
#include <utility> #include <utility>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
/* /*
The constructor also initializes 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
*/ */
AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state)
AP_RangeFinder_Backend(_ranger, instance, _state), : AP_RangeFinder_Backend(_ranger, instance, _state)
_dev(hal.i2c_mgr->get_device(0, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)) , _dev(hal.i2c_mgr->get_device(1, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))
{ {
} }
/* /*
detect if a Maxbotix rangefinder is connected. We'll detect by detect if a Maxbotix 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.
@ -54,8 +56,10 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_range
delete sensor; delete sensor;
return nullptr; return nullptr;
} }
// give time for the sensor to process the request // give time for the sensor to process the request
hal.scheduler->delay(50); hal.scheduler->delay(50);
uint16_t reading_cm; uint16_t reading_cm;
if (!sensor->get_reading(reading_cm)) { if (!sensor->get_reading(reading_cm)) {
delete sensor; delete sensor;
@ -68,22 +72,24 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_range
// start_reading() - ask sensor to make a range reading // start_reading() - ask sensor to make a range reading
bool AP_RangeFinder_MaxsonarI2CXL::start_reading() bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
{ {
if (!_dev->get_semaphore()->take(1)) { if (!_dev || !_dev->get_semaphore()->take(1)) {
return false; 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 // 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(); _dev->get_semaphore()->give();
return ret; return ret;
} }
// read - return last value measured by sensor // read - return last value measured by sensor
bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm) 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 // exit immediately if we can't take the semaphore
if (!_dev->get_semaphore()->take(1)) { 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 // 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(); _dev->get_semaphore()->give();
if (ret) { if (ret) {
// combine results into distance // combine results into distance
reading_cm = ((uint16_t)buff[0]) << 8 | buff[1]; reading_cm = be16toh(val);
// trigger a new reading // trigger a new reading
start_reading(); start_reading();
@ -106,7 +112,7 @@ bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm)
} }
/* /*
update the state of the sensor update the state of the sensor
*/ */
void AP_RangeFinder_MaxsonarI2CXL::update(void) void AP_RangeFinder_MaxsonarI2CXL::update(void)