From 22c787058ee45810c4a5c668f316d6f50f35080e Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Wed, 5 Aug 2015 10:01:52 -0300
Subject: [PATCH] AP_InertialSensor: allow to split detection and
 initialization

This allows backends to have a separate detection and initialization
logic. It doesn't change any backend yet and with the current code
there's no change in behavior either. This only allows
AP_InertialSensor::_detect_backend() to be called earlier so
AP_InertialSensor object can be used by other libraries. If it's not
called, later on AP_InertialSensor::init() will detect and start all
backends.
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 31 ++++++++++++++++---
 .../AP_InertialSensor/AP_InertialSensor.h     |  3 ++
 .../AP_InertialSensor_Backend.h               |  6 ++++
 3 files changed, 35 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 5510f2dee5..41d11d9678 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -321,6 +321,7 @@ AP_InertialSensor::AP_InertialSensor() :
     _hil_mode(false),
     _calibrating(false),
     _log_raw_data(false),
+    _backends_detected(false),
     _dataflash(NULL)
 {
     AP_Param::setup_object_defaults(this, var_info);        
@@ -369,6 +370,24 @@ uint8_t AP_InertialSensor::register_accel(void)
     return _accel_count++;
 }
 
+/*
+ * Start all backends for gyro and accel measurements. It automatically calls
+ * _detect_backends() if it has not been called already.
+ */
+void AP_InertialSensor::_start_backends()
+
+{
+    _detect_backends();
+
+    for (uint8_t i = 0; i < _backend_count; i++) {
+        _backends[i]->start();
+    }
+
+    if (_gyro_count == 0 || _accel_count == 0) {
+        hal.scheduler->panic(PSTR("INS needs at least 1 gyro and 1 accel"));
+    }
+}
+
 void
 AP_InertialSensor::init( Start_style style,
                          Sample_rate sample_rate)
@@ -377,8 +396,7 @@ AP_InertialSensor::init( Start_style style,
     _sample_rate = sample_rate;
 
     if (_gyro_count == 0 && _accel_count == 0) {
-        // detect available backends. Only called once
-        _detect_backends();
+        _start_backends();
     }
 
     // initialise accel scale if need be. This is needed as we can't
@@ -432,6 +450,11 @@ void AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
 void 
 AP_InertialSensor::_detect_backends(void)
 {
+    if (_backends_detected)
+        return;
+
+    _backends_detected = true;
+
     if (_hil_mode) {
         _add_backend(AP_InertialSensor_HIL::detect(*this));
         return;
@@ -458,9 +481,7 @@ AP_InertialSensor::_detect_backends(void)
     #error Unrecognised HAL_INS_TYPE setting
 #endif
 
-    if (_backend_count == 0 ||
-        _gyro_count == 0 ||
-        _accel_count == 0) {
+    if (_backend_count == 0) {
         hal.scheduler->panic(PSTR("No INS backends available"));
     }
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 6b80d1526d..83abb54ab4 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -243,6 +243,7 @@ private:
     // load backend drivers
     void _add_backend(AP_InertialSensor_Backend *backend);
     void _detect_backends(void);
+    void _start_backends();
 
     // gyro initialisation
     void _init_gyro();
@@ -334,6 +335,8 @@ private:
     // should we log raw accel/gyro data?
     bool _log_raw_data:1;
 
+    bool _backends_detected:1;
+
     // the delta time in seconds for the last sample
     float _delta_time;
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index 081b9fc9c0..eadecd2d8d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -52,6 +52,12 @@ public:
      */
     virtual bool gyro_sample_available() = 0;
 
+    /*
+     * Configure and start all sensors. The empty implementation allows
+     * subclasses to already start the sensors when it's detected
+     */
+    virtual void start() { }
+
     /*
       return the product ID
      */