mirror of https://github.com/ArduPilot/ardupilot
103 lines
3.0 KiB
C++
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;
|
|
}
|