From c323a71933226ae52d68675dbf8a238156e6f32a Mon Sep 17 00:00:00 2001
From: Craig Elder <Craig@3DRobotics.com>
Date: Tue, 8 May 2012 19:26:35 -0700
Subject: [PATCH] AP_InertialSensor: Return product ID from sensor
initialization
this exposes the product ID to the IMU Layer
---
libraries/AP_InertialSensor/AP_InertialSensor.h | 2 +-
.../AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 5 +++--
.../AP_InertialSensor/AP_InertialSensor_MPU6000.h | 2 +-
.../AP_InertialSensor/AP_InertialSensor_Oilpan.cpp | 10 +++++++++-
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h | 2 +-
libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp | 4 +++-
libraries/AP_InertialSensor/AP_InertialSensor_Stub.h | 2 +-
7 files changed, 19 insertions(+), 8 deletions(-)
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 44907652c..22f986ae8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -13,7 +13,7 @@ class AP_InertialSensor
public:
AP_InertialSensor() {}
- virtual void init( AP_PeriodicProcess * scheduler ) = 0;
+ virtual uint16_t init( AP_PeriodicProcess * scheduler ) = 0;
/* Update the sensor data, so that getters are nonblocking.
* Returns a bool of whether data was updated or not.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 750b5e900..bad66f888 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -119,14 +119,15 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( uint8_t cs_pin )
_initialised = 0;
}
-void AP_InertialSensor_MPU6000::init( AP_PeriodicProcess * scheduler )
+uint16_t AP_InertialSensor_MPU6000::init( AP_PeriodicProcess * scheduler )
{
- if (_initialised) return;
+ if (_initialised) return _product_id;
_initialised = 1;
scheduler->suspend_timer();
hardware_init();
scheduler->resume_timer();
scheduler->register_process( &AP_InertialSensor_MPU6000::read );
+ return _product_id;
}
// accumulation in ISR - must be read with interrupts disabled
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index 35f28ac87..4b909062c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -16,7 +16,7 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
AP_InertialSensor_MPU6000( uint8_t cs_pin );
- void init( AP_PeriodicProcess * scheduler );
+ uint16_t init( AP_PeriodicProcess * scheduler );
/* Concrete implementation of AP_InertialSensor functions: */
bool update();
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
index 4d2fcb03f..11b65537c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
@@ -48,9 +48,17 @@ AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) :
_accel.z = 0;
}
-void AP_InertialSensor_Oilpan::init( AP_PeriodicProcess * scheduler)
+uint16_t AP_InertialSensor_Oilpan::init( AP_PeriodicProcess * scheduler)
{
_adc->Init(scheduler);
+
+#if defined(DESKTOP_BUILD)
+ return AP_PRODUCT_ID_SITL;
+#elif defined(__AVR_ATmega1280__)
+ return AP_PRODUCT_ID_APM1_1280;
+#else
+ return AP_PRODUCT_ID_APM1_2560;
+#endif
}
bool AP_InertialSensor_Oilpan::update()
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
index 383bdea7f..5fb4e746d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
@@ -17,7 +17,7 @@ class AP_InertialSensor_Oilpan : public AP_InertialSensor
AP_InertialSensor_Oilpan( AP_ADC * adc );
/* Concrete implementation of AP_InertialSensor functions: */
- void init(AP_PeriodicProcess * scheduler);
+ uint16_t init(AP_PeriodicProcess * scheduler);
bool update();
bool new_data_available();
float gx();
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
index 98bbf38c0..55db0bbe3 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
@@ -2,7 +2,9 @@
#include "AP_InertialSensor_Stub.h"
-void AP_InertialSensor_Stub::init( AP_PeriodicProcess * scheduler ) {}
+uint16_t AP_InertialSensor_Stub::init( AP_PeriodicProcess * scheduler ) {
+ return AP_PRODUCT_ID_NONE;
+}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
index 2ddcce038..3cc221d74 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
@@ -15,7 +15,7 @@ class AP_InertialSensor_Stub : public AP_InertialSensor
AP_InertialSensor_Stub() {}
- void init( AP_PeriodicProcess * scheduler );
+ uint16_t init( AP_PeriodicProcess * scheduler );
/* Concrete implementation of AP_InertialSensor functions: */
bool update();
--
GitLab