diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 3f683b72a1..3d5123d6ff 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/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) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 011ee2c669..b3561c6ef5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -33,6 +33,7 @@ #include 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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 853be5a983..4938cec233 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/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 */ diff --git a/libraries/AP_InertialSensor/AuxiliaryBus.cpp b/libraries/AP_InertialSensor/AuxiliaryBus.cpp new file mode 100644 index 0000000000..ed0ce2b3a0 --- /dev/null +++ b/libraries/AP_InertialSensor/AuxiliaryBus.cpp @@ -0,0 +1,101 @@ +#include +#include +#include + +#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; +} diff --git a/libraries/AP_InertialSensor/AuxiliaryBus.h b/libraries/AP_InertialSensor/AuxiliaryBus.h new file mode 100644 index 0000000000..2e25fe486a --- /dev/null +++ b/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 . + */ + +#pragma once + +#include + +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; +};