AP_InertialSensor: added product_id support

fill in parameter from first backend
This commit is contained in:
Andrew Tridgell 2014-10-16 13:14:56 +11:00
parent 37dea4e367
commit 586fa9a816
11 changed files with 37 additions and 6 deletions

View File

@ -326,6 +326,9 @@ AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
_accel_count == 0) { _accel_count == 0) {
hal.scheduler->panic(PSTR("No INS backends available")); hal.scheduler->panic(PSTR("No INS backends available"));
} }
// set the product ID to the ID of the first backend
_product_id.set(_backends[0]->product_id());
} }
void void

View File

@ -5,7 +5,8 @@
#include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_Backend.h"
AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) : AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
_imu(imu) _imu(imu),
_product_id(AP_PRODUCT_ID_NONE)
{} {}
/* /*

View File

@ -48,6 +48,11 @@ public:
*/ */
virtual bool gyro_sample_available() = 0; virtual bool gyro_sample_available() = 0;
/*
return the product ID
*/
int16_t product_id(void) const { return _product_id; }
protected: protected:
// access to frontend // access to frontend
AP_InertialSensor &_imu; AP_InertialSensor &_imu;
@ -58,6 +63,9 @@ protected:
// rotate accel vector, scale and offset // rotate accel vector, scale and offset
void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now); void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now);
// backend should fill in its product ID from AP_PRODUCT_ID_*
int16_t _product_id;
// note that each backend is also expected to have a static detect() // note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor // function which instantiates an instance of the backend sensor
// driver if the sensor is available // driver if the sensor is available

View File

@ -172,6 +172,8 @@ bool AP_InertialSensor_Flymaple::_init_sensor(AP_InertialSensor::Sample_rate sam
_gyro_instance = _imu.register_gyro(); _gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel(); _accel_instance = _imu.register_accel();
_product_id = AP_PRODUCT_ID_FLYMAPLE;
return true; return true;
} }

View File

@ -51,6 +51,8 @@ bool AP_InertialSensor_HIL::_init_sensor(AP_InertialSensor::Sample_rate sample_r
_imu.register_gyro(); _imu.register_gyro();
_imu.register_accel(); _imu.register_accel();
_product_id = AP_PRODUCT_ID_NONE;
return true; return true;
} }

View File

@ -238,6 +238,8 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(AP_InertialSensor::Sample_rate sam
_gyro_instance = _imu.register_gyro(); _gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel(); _accel_instance = _imu.register_accel();
_product_id = AP_PRODUCT_ID_L3G4200D;
return true; return true;
} }

View File

@ -26,6 +26,9 @@ public:
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate); AP_InertialSensor::Sample_rate sample_rate);
// return product ID
int16_t product_id(void) const { return AP_PRODUCT_ID_L3G4200D; }
private: private:
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate); bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
void _accumulate(void); void _accumulate(void);

View File

@ -525,13 +525,13 @@ bool AP_InertialSensor_MPU6000::_hardware_init(AP_InertialSensor::Sample_rate sa
hal.scheduler->delay(1); hal.scheduler->delay(1);
// read the product ID rev c has 1/2 the sensitivity of rev d // read the product ID rev c has 1/2 the sensitivity of rev d
uint8_t product_id = _register_read(MPUREG_PRODUCT_ID); _product_id = _register_read(MPUREG_PRODUCT_ID);
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
if ((product_id == MPU6000ES_REV_C4) || if ((_product_id == MPU6000ES_REV_C4) ||
(product_id == MPU6000ES_REV_C5) || (_product_id == MPU6000ES_REV_C5) ||
(product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C4) ||
(product_id == MPU6000_REV_C5)) { (_product_id == MPU6000_REV_C5)) {
// Accel scale 8g (4096 LSB/g) // Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D // Rev C has different scaling than rev D
_register_write(MPUREG_ACCEL_CONFIG,1<<3); _register_write(MPUREG_ACCEL_CONFIG,1<<3);

View File

@ -250,6 +250,8 @@ bool AP_InertialSensor_MPU9250::_init_sensor(AP_InertialSensor::Sample_rate samp
_gyro_instance = _imu.register_gyro(); _gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel(); _accel_instance = _imu.register_accel();
_product_id = AP_PRODUCT_ID_MPU9250;
// start the timer process to read samples // start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data)); hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data));

View File

@ -86,6 +86,8 @@ bool AP_InertialSensor_Oilpan::_init_sensor(AP_InertialSensor::Sample_rate sampl
_gyro_instance = _imu.register_gyro(); _gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel(); _accel_instance = _imu.register_accel();
_product_id = AP_PRODUCT_ID_APM1_2560;
return true; return true;
} }

View File

@ -94,6 +94,12 @@ bool AP_InertialSensor_PX4::_init_sensor(AP_InertialSensor::Sample_rate sample_r
_set_filter_frequency(_imu.get_filter()); _set_filter_frequency(_imu.get_filter());
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
_product_id = AP_PRODUCT_ID_PX4_V2;
#else
_product_id = AP_PRODUCT_ID_PX4;
#endif
return true; return true;
} }