Line data    Source code

       1             : #include <assert.h>
       2             : #include <stdlib.h>
       3             : 
       4             : #include "AuxiliaryBus.h"
       5             : 
       6           0 : AuxiliaryBusSlave::AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
       7           0 :                                      uint8_t instance)
       8           0 :     : _bus(bus)
       9           0 :     , _addr(addr)
      10           0 :     , _instance(instance)
      11             : {
      12           0 : }
      13             : 
      14           0 : AuxiliaryBusSlave::~AuxiliaryBusSlave()
      15             : {
      16           0 : }
      17             : 
      18           0 : AuxiliaryBus::AuxiliaryBus(AP_InertialSensor_Backend &backend, uint8_t max_slaves, uint32_t devid)
      19           0 :     : _max_slaves(max_slaves)
      20           0 :     , _ins_backend(backend)
      21           0 :     , _devid(devid)
      22             : {
      23           0 :     _slaves = (AuxiliaryBusSlave**) calloc(max_slaves, sizeof(AuxiliaryBusSlave*));
      24           0 : }
      25             : 
      26           0 : AuxiliaryBus::~AuxiliaryBus()
      27             : {
      28           0 :     for (int i = _n_slaves - 1; i >= 0; i--) {
      29           0 :         delete _slaves[i];
      30             :     }
      31           0 :     free(_slaves);
      32           0 : }
      33             : 
      34             : /*
      35             :  * Get the next available slave for the sensor exposing this AuxiliaryBus.
      36             :  * If a new slave cannot be registered or instantiated, `nullptr` is returned.
      37             :  * Otherwise a new slave is returned, but it's not registered (and therefore
      38             :  * not owned by the AuxiliaryBus).
      39             :  *
      40             :  * After using the slave, if it's not registered for a periodic read it must
      41             :  * be destroyed.
      42             :  *
      43             :  * @addr: the address of this slave in the bus
      44             :  *
      45             :  * Return a new slave if successful or `nullptr` otherwise.
      46             :  */
      47           0 : AuxiliaryBusSlave *AuxiliaryBus::request_next_slave(uint8_t addr)
      48             : {
      49           0 :     if (_n_slaves == _max_slaves)
      50           0 :         return nullptr;
      51             : 
      52           0 :     AuxiliaryBusSlave *slave = _instantiate_slave(addr, _n_slaves);
      53           0 :     if (!slave)
      54           0 :         return nullptr;
      55             : 
      56           0 :     return slave;
      57             : }
      58             : 
      59             : /*
      60             :  * Register a periodic read. This should be called after the slave sensor is
      61             :  * already configured and the only thing the master needs to do is to copy a
      62             :  * set of registers from the slave to its own registers.
      63             :  *
      64             :  * The sample rate is hard-coded, depending on the sensor that exports this
      65             :  * AuxiliaryBus.
      66             :  *
      67             :  * After this call the AuxiliaryBusSlave is owned by this object and should
      68             :  * not be destroyed. A typical call chain to use a sensor in an AuxiliaryBus
      69             :  * is (error checking omitted for brevity):
      70             :  *
      71             :  *      AuxiliaryBusSlave *slave = bus->request_next_slave(addr);
      72             :  *      slave->passthrough_read(WHO_AM_I, buf, 1);
      73             :  *      slave->passthrough_write(...);
      74             :  *      slave->passthrough_write(...);
      75             :  *      ...
      76             :  *      bus->register_periodic_read(slave, SAMPLE_START_REG, SAMPLE_SIZE);
      77             :  *
      78             :  * @slave: the AuxiliaryBusSlave already configured to be in continuous mode
      79             :  * @reg:   the first register of the block to use in each periodic transfer
      80             :  * @size:  the block size, usually the size of the sample multiplied by the
      81             :  *         number of axes in each sample.
      82             :  *
      83             :  * Return 0 on success or < 0 on error.
      84             :  */
      85           0 : int AuxiliaryBus::register_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg,
      86             :                                          uint8_t size)
      87             : {
      88           0 :     int r = _configure_periodic_read(slave, reg, size);
      89           0 :     if (r < 0)
      90           0 :         return r;
      91             : 
      92           0 :     slave->_sample_reg_start = reg;
      93           0 :     slave->_sample_size = size;
      94           0 :     slave->_registered = true;
      95           0 :     _slaves[_n_slaves++] = slave;
      96             : 
      97           0 :     return 0;
      98             : }