AP_RangeFinder: convert MaxbotixI2C driver to new API

This commit is contained in:
Andrew Tridgell 2014-06-27 14:03:47 +10:00
parent 0063d83dbc
commit ed346fd639
3 changed files with 87 additions and 61 deletions

View File

@ -21,71 +21,97 @@
* datasheet: http://www.maxbotix.com/documents/I2CXL-MaxSonar-EZ_Datasheet.pdf * datasheet: http://www.maxbotix.com/documents/I2CXL-MaxSonar-EZ_Datasheet.pdf
* *
* Sensor should be connected to the I2C port * Sensor should be connected to the I2C port
*
* Variables:
* bool healthy : indicates whether last communication with sensor was successful
*
* Methods:
* take_reading(): ask the sonar to take a new distance measurement
* read() : read last distance measured (in cm)
*
*/ */
#if 0
// AVR LibC Includes
#include "AP_RangeFinder_MaxsonarI2CXL.h" #include "AP_RangeFinder_MaxsonarI2CXL.h"
#include <AP_HAL.h> #include <AP_HAL.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// Constructor ////////////////////////////////////////////////////////////// /*
The constructor also initialises the rangefinder. Note that this
AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL( FilterInt16 *filter ) : constructor is not called until detect() returns true, so we
RangeFinder(NULL, filter), already know that we should setup the rangefinder
healthy(true), */
_addr(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)
{ {
min_distance = AP_RANGE_FINDER_MAXSONARI2CXL_MIN_DISTANCE;
max_distance = AP_RANGE_FINDER_MAXSONARI2CXL_MAX_DISTANCE;
} }
// Public Methods ////////////////////////////////////////////////////////////// /*
detect if a Maxbotix rangefinder is connected. We'll detect by
// take_reading - ask sensor to make a range reading trying to take a reading on I2C. If we get a result the sensor is
bool AP_RangeFinder_MaxsonarI2CXL::take_reading() there.
*/
bool AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_ranger, uint8_t instance)
{ {
// take range reading and read back results if (!start_reading()) {
return false;
}
// give time for the sensor to process the request
hal.scheduler->delay(50);
uint16_t reading_cm;
return get_reading(reading_cm);
}
// start_reading() - ask sensor to make a range reading
bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
{
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// exit immediately if we can't take the semaphore
if (i2c_sem == NULL || !i2c_sem->take(1)) {
return false;
}
uint8_t tosend[1] = uint8_t tosend[1] =
{ AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING }; { AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING };
if (hal.i2c->write(_addr, 1, tosend) != 0) {
healthy = false; // send command to take reading
if (hal.i2c->write(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR,
1, tosend) != 0) {
i2c_sem->give();
return false; return false;
}else{
healthy = true;
return true;
} }
// return semaphore
i2c_sem->give();
return true;
} }
// read - return last value measured by sensor // read - return last value measured by sensor
int16_t AP_RangeFinder_MaxsonarI2CXL::read() bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm)
{ {
uint8_t buff[2]; uint8_t buff[2];
int16_t ret_value = 0;
// take range reading and read back results // get pointer to i2c bus semaphore
if (hal.i2c->read(_addr, 2, buff) != 0) { AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
healthy = false;
}else{ // exit immediately if we can't take the semaphore
// combine results into distance if (i2c_sem == NULL || !i2c_sem->take(1)) {
ret_value = buff[0] << 8 | buff[1]; return false;
healthy = true;
} }
// ensure distance is within min and max // take range reading and read back results
ret_value = constrain_float(ret_value, min_distance, max_distance); if (hal.i2c->read(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR, 2, buff) != 0) {
i2c_sem->give();
return false;
}
i2c_sem->give();
ret_value = _mode_filter->apply(ret_value); // combine results into distance
reading_cm = ((uint16_t)buff[0]) << 8 | buff[1];
return ret_value; return true;
}
/*
update the state of the sensor
*/
void AP_RangeFinder_MaxsonarI2CXL::update(void)
{
state.healthy = get_reading(state.distance_cm);
} }
#endif

View File

@ -4,6 +4,7 @@
#define __AP_RANGEFINDER_MAXSONARI2CXL_H__ #define __AP_RANGEFINDER_MAXSONARI2CXL_H__
#include "RangeFinder.h" #include "RangeFinder.h"
#include "RangeFinder_Backend.h"
#define AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR 0x70 #define AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR 0x70
@ -14,28 +15,21 @@
#define AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING 0x51 #define AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING 0x51
class AP_RangeFinder_MaxsonarI2CXL : public RangeFinder class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend
{ {
public: public:
// constructor // constructor
AP_RangeFinder_MaxsonarI2CXL(FilterInt16 *filter); AP_RangeFinder_MaxsonarI2CXL(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
// init - simply sets the i2c address // static detection function
void init(uint8_t address = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR) { _addr = address; } static bool detect(RangeFinder &ranger, uint8_t instance);
// take_reading - ask sensor to make a range reading // update state
bool take_reading(); void update(void);
// read value from sensor and return distance in cm
int16_t read();
// heath
bool healthy;
protected:
uint8_t _addr;
private:
// start a reading
static bool start_reading(void);
static bool get_reading(uint16_t &reading_cm);
}; };
#endif // __AP_RANGEFINDER_MAXSONARI2CXL_H__ #endif // __AP_RANGEFINDER_MAXSONARI2CXL_H__

View File

@ -17,6 +17,7 @@
#include "RangeFinder.h" #include "RangeFinder.h"
#include "AP_RangeFinder_analog.h" #include "AP_RangeFinder_analog.h"
#include "AP_RangeFinder_PulsedLightLRF.h" #include "AP_RangeFinder_PulsedLightLRF.h"
#include "AP_RangeFinder_MaxsonarI2CXL.h"
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = { const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = {
@ -203,6 +204,11 @@ void RangeFinder::detect_instance(uint8_t instance)
state[instance].instance = instance; state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_PulsedLightLRF(*this, instance, state[instance]); drivers[instance] = new AP_RangeFinder_PulsedLightLRF(*this, instance, state[instance]);
} }
} else if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_MBI2C) {
if (AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance)) {
state[instance].instance = instance;
drivers[instance] = new AP_RangeFinder_MaxsonarI2CXL(*this, instance, state[instance]);
}
} else if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_ANALOG) { } else if (_type[instance] == RangeFinder_TYPE_AUTO || _type[instance] == RangeFinder_TYPE_ANALOG) {
// note that analog must be the last to be checked, as it will // note that analog must be the last to be checked, as it will
// always come back as present if the pin is valid // always come back as present if the pin is valid