ardupilot/libraries/AP_InertialSensor/AuxiliaryBus.cpp

103 lines
3.0 KiB
C++

#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "AuxiliaryBus.h"
AuxiliaryBusSlave::AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
uint8_t instance)
: _bus(bus)
, _addr(addr)
, _instance(instance)
{
}
AuxiliaryBusSlave::~AuxiliaryBusSlave()
{
}
AuxiliaryBus::AuxiliaryBus(AP_InertialSensor_Backend &backend, uint8_t max_slaves, uint32_t devid)
: _max_slaves(max_slaves)
, _ins_backend(backend)
, _devid(devid)
{
_slaves = (AuxiliaryBusSlave**) calloc(max_slaves, sizeof(AuxiliaryBusSlave*));
}
AuxiliaryBus::~AuxiliaryBus()
{
for (int i = _n_slaves - 1; i >= 0; i--) {
delete _slaves[i];
}
free(_slaves);
}
/*
* Get the next available slave for the sensor exposing this AuxiliaryBus.
* If a new slave cannot be registered or instantiated, `nullptr` is returned.
* Otherwise a new slave is returned, but it's not registered (and therefore
* not owned by the AuxiliaryBus).
*
* After using the slave, if it's not registered for a periodic read it must
* be destroyed.
*
* @addr: the address of this slave in the bus
*
* Return a new slave if successful or `nullptr` otherwise.
*/
AuxiliaryBusSlave *AuxiliaryBus::request_next_slave(uint8_t addr)
{
if (_n_slaves == _max_slaves)
return nullptr;
AuxiliaryBusSlave *slave = _instantiate_slave(addr, _n_slaves);
if (!slave)
return nullptr;
return slave;
}
/*
* Register a periodic read. This should be called after the slave sensor is
* already configured and the only thing the master needs to do is to copy a
* set of registers from the slave to its own registers.
*
* The sample rate is hard-coded, depending on the sensor that exports this
* AuxiliaryBus.
*
* After this call the AuxiliaryBusSlave is owned by this object and should
* not be destroyed. A typical call chain to use a sensor in an AuxiliaryBus
* is (error checking omitted for brevity):
*
* AuxiliaryBusSlave *slave = bus->request_next_slave(addr);
* slave->passthrough_read(WHO_AM_I, buf, 1);
* slave->passthrough_write(...);
* slave->passthrough_write(...);
* ...
* bus->register_periodic_read(slave, SAMPLE_START_REG, SAMPLE_SIZE);
*
* @slave: the AuxiliaryBusSlave already configured to be in continuous mode
* @reg: the first register of the block to use in each periodic transfer
* @size: the block size, usually the size of the sample multiplied by the
* number of axes in each sample.
*
* Return 0 on success or < 0 on error.
*/
int AuxiliaryBus::register_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg,
uint8_t size)
{
assert(slave->_instance == _n_slaves);
assert(_n_slaves < _max_slaves);
int r = _configure_periodic_read(slave, reg, size);
if (r < 0)
return r;
slave->_sample_reg_start = reg;
slave->_sample_size = size;
slave->_registered = true;
_slaves[_n_slaves++] = slave;
return 0;
}