diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 44907652c1..22f986ae8b 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 750b5e9004..bad66f888d 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 35f28ac871..4b909062c9 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 4d2fcb03f4..11b65537cc 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 383bdea7f2..5fb4e746df 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 98bbf38c04..55db0bbe33 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 2ddcce038e..3cc221d74c 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();