AP_InertialSensor: Add support for auxiliary buses

Add an AuxiliaryBus class that can be derived for specific
implementations in inertial sensor backends. It's an abstract
implementation so other libraries can use the auxiliary bus exported. In
order for this to succeed the backend implementation must split the
initialization of the sensor from the actual sample collecting, like is
done in MPU6000.

When AP_InertialSensor::get_auxiliary_bus() is called it will execute
following steps:
	a) Force the backends to be detected if it's the first time it's
	   being called
	b) Find the backend identified by the id
	c) call get_auxiliary_bus() on the backend so other libraries can
	   that AuxiliaryBus to initialize a slave device

Slave devices can be used by calling AuxiliaryBus::request_next_slave()
and are owned by the caller until AuxiliaryBus::register_periodic_read()
is called. From that time on the AuxiliaryBus object takes its ownership.
This way it's possible to do the necessary cleanup later without
introducing refcounts, that we don't have support to.

Between these 2 functions the caller can configure the slave device by
doing its specific initializations by calling the passthrough_*
functions. After the initial configuration and register_periodic_read()
is called only read() can be called.

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1517,6 +1517,20 @@ void AP_InertialSensor::set_delta_angle(uint8_t instance, const Vector3f &deltaa
     }
 }
 
+/*
+ * Get an AuxiliaryBus on the backend identified by @backend_id
+ */
+AuxiliaryBus *AP_InertialSensor::get_auxiliar_bus(int16_t backend_id)
+{
+    _detect_backends();
+
+    AP_InertialSensor_Backend *backend = _find_backend(backend_id);
+    if (backend == NULL)
+        return NULL;
+
+    return backend->get_auxiliar_bus();
+}
+
 #if INS_VIBRATION_CHECK
 // calculate vibration levels and check for accelerometer clipping (called by a backends)
 void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt)

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -33,6 +33,7 @@
 #include <Filter/LowPassFilter.h>
 
 class AP_InertialSensor_Backend;
+class AuxiliaryBus;
 
 /*
   forward declare DataFlash class. We can't include DataFlash.h
@@ -238,6 +239,8 @@ public:
     void set_delta_velocity(uint8_t instance, float deltavt, const Vector3f &deltav);
     void set_delta_angle(uint8_t instance, const Vector3f &deltaa);
 
+    AuxiliaryBus *get_auxiliar_bus(int16_t backend_id);
+
 private:
 
     // load backend drivers

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -24,6 +24,8 @@
 #ifndef __AP_INERTIALSENSOR_BACKEND_H__
 #define __AP_INERTIALSENSOR_BACKEND_H__
 
+class AuxiliaryBus;
+
 class AP_InertialSensor_Backend
 {
 public:
@@ -58,6 +60,11 @@ public:
      */
     virtual void start() { }
 
+    /*
+     * Return an AuxiliaryBus if backend has another bus it is able to export
+     */
+    virtual AuxiliaryBus *get_auxiliar_bus() { return nullptr; }
+
     /*
       return the product ID
      */

libraries/AP_InertialSensor/AuxiliaryBus.cpp

@@ -0,0 +1,101 @@
+#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)
+    : _max_slaves(max_slaves)
+    , _ins_backend(backend)
+{
+    _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;
+}

libraries/AP_InertialSensor/AuxiliaryBus.h

@@ -0,0 +1,130 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * Copyright (C) 2015  Intel Corporation. All rights reserved.
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <inttypes.h>
+
+class AuxiliaryBus;
+class AP_InertialSensor_Backend;
+
+namespace AP_HAL {
+    class Semaphore;
+}
+
+class AuxiliaryBusSlave
+{
+    friend class AuxiliaryBus;
+
+public:
+    virtual ~AuxiliaryBusSlave();
+
+    /*
+     * Read a block of registers from the slave. This is a one-time read. Must
+     * be implemented by the sensor exposing the AuxiliaryBus.
+     *
+     * This method cannot be called after the periodic read is configured
+     * since the registers for the periodic read may be shared with the
+     * passthrough reads.
+     *
+     * @reg: the first register of the block to use in this one time transfer
+     * @buf: buffer in which to write the values read
+     * @size: the buffer size
+     *
+     * Return the number of bytes read on success or < 0 on error
+     */
+    virtual int passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) = 0;
+
+    /*
+     * Write a single value into a register of this slave. Must be implemented
+     * by the sensor exposing the AuxiliaryBus.
+     *
+     * This method cannot be called after the periodic read is configured
+     * since the registers for the periodic read may be shared with the
+     * passthrough writes.
+     *
+     * @reg: the register to use in this one time transfer
+     * @val: the value to write
+     *
+     * Return the number of bytes written on success or < 0 on error
+     */
+    virtual int passthrough_write(uint8_t reg, uint8_t val) = 0;
+
+    /*
+     * Read the block of registers that were read from the slave on the last
+     * time a periodic read occurred.
+     *
+     * This method must be called after the periodic read is configured and
+     * the buffer must be large enough to accomodate the size configured.
+     *
+     * @buf: buffer in which to write the values read
+     *
+     * Return the number of bytes read on success or < 0 on error
+     */
+    virtual int read(uint8_t *buf) = 0;
+
+protected:
+    /* Only AuxiliaryBus is able to create a slave */
+    AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr, uint8_t instance);
+
+    AuxiliaryBus &_bus;
+    uint8_t _addr = 0;
+    uint8_t _instance = 0;
+
+    uint8_t _sample_reg_start = 0;
+    uint8_t _sample_size = 0;
+
+    bool _registered = false;
+};
+
+class AuxiliaryBus
+{
+    friend class AP_InertialSensor_Backend;
+
+public:
+    AP_InertialSensor_Backend &get_backend() { return _ins_backend; }
+
+    AuxiliaryBusSlave *request_next_slave(uint8_t addr);
+    int register_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg, uint8_t size);
+
+    /*
+     * Get the semaphore needed to call methods on the bus this sensor is on.
+     * Internally no locks are taken and it's the caller's duty to lock and
+     * unlock the bus as needed.
+     *
+     * This method must be implemented by the sensor exposing the
+     * AuxiliaryBus.
+     *
+     * Return the semaphore used protect transfers on the bus
+     */
+    virtual AP_HAL::Semaphore *get_semaphore() = 0;
+
+protected:
+    /* Only AP_InertialSensor_Backend is able to create a bus */
+    AuxiliaryBus(AP_InertialSensor_Backend &backend, uint8_t max_slaves);
+    virtual ~AuxiliaryBus();
+
+    virtual AuxiliaryBusSlave *_instantiate_slave(uint8_t addr, uint8_t instance) = 0;
+    virtual int _configure_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg,
+                                         uint8_t size) = 0;
+
+    uint8_t _n_slaves = 0;
+    const uint8_t _max_slaves;
+    AuxiliaryBusSlave **_slaves;
+    AP_InertialSensor_Backend &_ins_backend;
+};