From 7d9579c5d8d29876f68bffecf5045008c7910f0b Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Sun, 16 Aug 2015 16:16:11 -0300 Subject: [PATCH] AP_InertialSensor: identify backend with ID Identify backend with an id, allowing other libraries to connect to them. This is different from the _product_id member because it identifies the sensor, not the board the sensor is in, which is meaningless for our use case. --- .../AP_InertialSensor/AP_InertialSensor.cpp | 19 +++++++++++++++++++ .../AP_InertialSensor/AP_InertialSensor.h | 1 + .../AP_InertialSensor_Backend.h | 5 +++++ 3 files changed, 25 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 41d11d9678..3f683b72a1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include + #include #include "AP_InertialSensor.h" @@ -388,6 +390,23 @@ void AP_InertialSensor::_start_backends() } } +/* Find a backend that has already been succesfully detected */ +AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id) +{ + assert(_backends_detected); + + for (uint8_t i = 0; i < _backend_count; i++) { + int16_t id = _backends[i]->get_id(); + + if (id < 0 || id != backend_id) + continue; + + return _backends[i]; + } + + return nullptr; +} + void AP_InertialSensor::init( Start_style style, Sample_rate sample_rate) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 83abb54ab4..011ee2c669 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -244,6 +244,7 @@ private: void _add_backend(AP_InertialSensor_Backend *backend); void _detect_backends(void); void _start_backends(); + AP_InertialSensor_Backend *_find_backend(int16_t backend_id); // gyro initialisation void _init_gyro(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index eadecd2d8d..853be5a983 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -63,6 +63,8 @@ public: */ int16_t product_id(void) const { return _product_id; } + int16_t get_id() const { return _id; } + protected: // access to frontend AP_InertialSensor &_imu; @@ -94,6 +96,9 @@ protected: // backend should fill in its product ID from AP_PRODUCT_ID_* int16_t _product_id; + // backend unique identifier or -1 if backend doesn't identify itself + int16_t _id = -1; + // return the default filter frequency in Hz for the sample rate uint8_t _accel_filter_cutoff(void) const { return _imu._accel_filter_cutoff; }