diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 013183b011..878a5e8a03 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -26,15 +26,18 @@ Datasheets: ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf */ -#include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #include "AP_InertialSensor_L3G4200D.h" #include #include -const extern AP_HAL::HAL &hal; + + +const extern AP_HAL::HAL& hal; /////// /// Accelerometer ADXL345 register definitions @@ -46,43 +49,71 @@ const extern AP_HAL::HAL &hal; #define ADXL345_ACCELEROMETER_ADXLREG_DEVID 0x00 #define ADXL345_ACCELEROMETER_ADXLREG_DATAX0 0x32 #define ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL 0x38 -#define ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL_STREAM 0x9F +#define ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL_STREAM 0x9F // 32 sample before triggering #define ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS 0x39 +#define ADXL345_ACCELEROMETER_BW_RATE_0_10HZ 0x00 +#define ADXL345_ACCELEROMETER_BW_RATE_0_20HZ 0x01 +#define ADXL345_ACCELEROMETER_BW_RATE_0_39HZ 0x02 +#define ADXL345_ACCELEROMETER_BW_RATE_0_78HZ 0x03 +#define ADXL345_ACCELEROMETER_BW_RATE_1_56HZ 0x04 +#define ADXL345_ACCELEROMETER_BW_RATE_3_13HZ 0x05 +#define ADXL345_ACCELEROMETER_BW_RATE_6_25HZ 0x06 +#define ADXL345_ACCELEROMETER_BW_RATE_12_5HZ 0x07 +#define ADXL345_ACCELEROMETER_BW_RATE_25HZ 0x08 +#define ADXL345_ACCELEROMETER_BW_RATE_50HZ 0x09 +#define ADXL345_ACCELEROMETER_BW_RATE_100HZ 0x0A +#define ADXL345_ACCELEROMETER_BW_RATE_200HZ 0x0B +#define ADXL345_ACCELEROMETER_BW_RATE_400HZ 0x0C +#define ADXL345_ACCELEROMETER_BW_RATE_800HZ 0x0D +#define ADXL345_ACCELEROMETER_BW_RATE_1600HZ 0x0E +#define ADXL345_ACCELEROMETER_BW_RATE_3200HZ 0x0F + + + +#define ADXL345_ACCELEROMETER_ENABLE 0x08 + +#define ADXL345_ACCELEROMETER_MEASURE_MODE 0x08 + +#define ADXL345_ACCELEROMETER_RANGE_2G 0x00 +#define ADXL345_ACCELEROMETER_RANGE_4G 0x01 +#define ADXL345_ACCELEROMETER_RANGE_8G 0x02 +#define ADXL345_ACCELEROMETER_RANGE_16G 0x03 + + // ADXL345 accelerometer scaling // Result will be scaled to 1m/s/s // ADXL345 in Full resolution mode (any g scaling) is 256 counts/g, so scale by 9.81/256 = 0.038320312 #define ADXL345_ACCELEROMETER_SCALE_M_S (GRAVITY_MSS / 256.0f) /// Gyro ITG3205 register definitions -#define L3G4200D_I2C_ADDRESS 0x69 +#define L3G4200D_I2C_ADDRESS 0x69 -#define L3G4200D_REG_WHO_AM_I 0x0f +#define L3G4200D_REG_WHO_AM_I 0x0f #define L3G4200D_REG_WHO_AM_I_VALUE 0xd3 -#define L3G4200D_REG_CTRL_REG1 0x20 +#define L3G4200D_REG_CTRL_REG1 0x20 #define L3G4200D_REG_CTRL_REG1_DRBW_800_110 0xf0 #define L3G4200D_REG_CTRL_REG1_PD 0x08 #define L3G4200D_REG_CTRL_REG1_XYZ_ENABLE 0x07 -#define L3G4200D_REG_CTRL_REG4 0x23 +#define L3G4200D_REG_CTRL_REG4 0x23 #define L3G4200D_REG_CTRL_REG4_FS_2000 0x30 -#define L3G4200D_REG_CTRL_REG5 0x24 +#define L3G4200D_REG_CTRL_REG5 0x24 #define L3G4200D_REG_CTRL_REG5_FIFO_EN 0x40 - -#define L3G4200D_REG_FIFO_CTL 0x2e +#define L3G4200D_REG_FIFO_CTL 0x2e #define L3G4200D_REG_FIFO_CTL_STREAM 0x40 -#define L3G4200D_REG_FIFO_SRC 0x2f +#define L3G4200D_REG_FIFO_SRC 0x2f #define L3G4200D_REG_FIFO_SRC_ENTRIES_MASK 0x1f #define L3G4200D_REG_FIFO_SRC_EMPTY 0x20 #define L3G4200D_REG_FIFO_SRC_OVERRUN 0x40 -#define L3G4200D_REG_XL 0x28 +#define L3G4200D_REG_XL 0x28 // this bit is ORd into the register to enable auto-increment mode -#define L3G4200D_REG_AUTO_INCREMENT 0x80 +#define L3G4200D_REG_AUTO_INCREMENT 0x80 // L3G4200D Gyroscope scaling // running at 2000 DPS full range, 16 bit signed data, datasheet @@ -91,12 +122,15 @@ const extern AP_HAL::HAL &hal; // constructor AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu, - AP_HAL::OwnPtr dev) + AP_HAL::OwnPtr dev_gyro, + AP_HAL::OwnPtr dev_accel) : AP_InertialSensor_Backend(imu) - , _dev(std::move(dev)) + , _dev_gyro(std::move(dev_gyro)) + , _dev_accel(std::move(dev_accel)) { } + AP_InertialSensor_L3G4200D::~AP_InertialSensor_L3G4200D() { } @@ -105,13 +139,14 @@ AP_InertialSensor_L3G4200D::~AP_InertialSensor_L3G4200D() detect the sensor */ AP_InertialSensor_Backend *AP_InertialSensor_L3G4200D::probe(AP_InertialSensor &imu, - AP_HAL::OwnPtr dev) + AP_HAL::OwnPtr dev_gyro, + AP_HAL::OwnPtr dev_accel) { - if (!dev) { + if ((!dev_accel) || (!dev_gyro)){ return nullptr; } AP_InertialSensor_L3G4200D *sensor - = new AP_InertialSensor_L3G4200D(imu, std::move(dev)); + = new AP_InertialSensor_L3G4200D(imu, std::move(dev_gyro), std::move(dev_accel)); if (!sensor || !sensor->_init_sensor()) { delete sensor; return nullptr; @@ -120,82 +155,92 @@ AP_InertialSensor_Backend *AP_InertialSensor_L3G4200D::probe(AP_InertialSensor & return sensor; } -bool AP_InertialSensor_L3G4200D::_init_sensor(void) + + +bool AP_InertialSensor_L3G4200D::_accel_init() { - _dev->get_semaphore()->take_blocking(); + _dev_accel->get_semaphore()->take_blocking(); // Init the accelerometer uint8_t data = 0; - _dev->read_registers(ADXL345_ACCELEROMETER_ADXLREG_DEVID, &data, 1); + _dev_accel->read_registers(ADXL345_ACCELEROMETER_ADXLREG_DEVID, &data, 1); if (data != ADXL345_ACCELEROMETER_XL345_DEVID) { AP_HAL::panic("AP_InertialSensor_L3G4200D: could not find ADXL345 accelerometer sensor"); } - - _dev->write_register(ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL, 0x00); - hal.scheduler->delay(5); - _dev->write_register(ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL, 0xff); - hal.scheduler->delay(5); - // Measure mode: - _dev->write_register(ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL, 0x08); - hal.scheduler->delay(5); - // Full resolution, 8g: // Caution, this must agree with ADXL345_ACCELEROMETER_SCALE_1G // In full resoution mode, the scale factor need not change - _dev->write_register(ADXL345_ACCELEROMETER_ADXLREG_DATA_FORMAT, 0x08); + _dev_accel->write_register(ADXL345_ACCELEROMETER_ADXLREG_DATA_FORMAT, ADXL345_ACCELEROMETER_RANGE_2G); hal.scheduler->delay(5); // Normal power, 800Hz Output Data Rate, 400Hz bandwidth: - _dev->write_register(ADXL345_ACCELEROMETER_ADXLREG_BW_RATE, 0x0d); + _dev_accel->write_register(ADXL345_ACCELEROMETER_ADXLREG_BW_RATE, ADXL345_ACCELEROMETER_BW_RATE_400HZ); hal.scheduler->delay(5); - // enable FIFO in stream mode. This will allow us to read the accelerometers - // much less frequently - _dev->write_register(ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL, - ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL_STREAM); + // enable FIFO in stream mode. This will allow us to read the gyros much less frequently + _dev_accel->write_register(ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL, + ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL_STREAM); + hal.scheduler->delay(5); + // Measure mode: + _dev_accel->write_register(ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL, + ADXL345_ACCELEROMETER_MEASURE_MODE); + hal.scheduler->delay(5); + + // Set up the filter desired + _set_filter_frequency(_accel_filter_cutoff()); + + _dev_accel->get_semaphore()->give(); + + return true; +} + +bool AP_InertialSensor_L3G4200D::_gyro_init() +{ + uint8_t data = 0; // Init the Gyro - // Expect to read the right 'WHO_AM_I' value - _dev->read_registers(L3G4200D_REG_WHO_AM_I, &data, 1); + + _dev_gyro->get_semaphore()->take_blocking(); + + _dev_gyro->read_registers(L3G4200D_REG_WHO_AM_I, &data, 1); if (data != L3G4200D_REG_WHO_AM_I_VALUE) { AP_HAL::panic("AP_InertialSensor_L3G4200D: could not find L3G4200D gyro sensor"); } - - // setup for 800Hz sampling with 110Hz filter - _dev->write_register(L3G4200D_REG_CTRL_REG1, + + // setup for 800Hz sampling with 110Hz filter + _dev_gyro->write_register(L3G4200D_REG_CTRL_REG1, // CTRL_REG1 400Hz ODR, 20hz filter, run! L3G4200D_REG_CTRL_REG1_DRBW_800_110 | L3G4200D_REG_CTRL_REG1_PD | L3G4200D_REG_CTRL_REG1_XYZ_ENABLE); - hal.scheduler->delay(1); + hal.scheduler->delay(5); - _dev->write_register(L3G4200D_REG_CTRL_REG1, - L3G4200D_REG_CTRL_REG1_DRBW_800_110 | - L3G4200D_REG_CTRL_REG1_PD | - L3G4200D_REG_CTRL_REG1_XYZ_ENABLE); - hal.scheduler->delay(1); - - _dev->write_register(L3G4200D_REG_CTRL_REG1, - L3G4200D_REG_CTRL_REG1_DRBW_800_110 | - L3G4200D_REG_CTRL_REG1_PD | - L3G4200D_REG_CTRL_REG1_XYZ_ENABLE); - hal.scheduler->delay(1); // setup for 2000 degrees/sec full range - _dev->write_register(L3G4200D_REG_CTRL_REG4, + _dev_gyro->write_register(L3G4200D_REG_CTRL_REG4, L3G4200D_REG_CTRL_REG4_FS_2000); - hal.scheduler->delay(1); + hal.scheduler->delay(5); // enable FIFO - _dev->write_register(L3G4200D_REG_CTRL_REG5, + _dev_gyro->write_register(L3G4200D_REG_CTRL_REG5, L3G4200D_REG_CTRL_REG5_FIFO_EN); - hal.scheduler->delay(1); + hal.scheduler->delay(5); // enable FIFO in stream mode. This will allow us to read the gyros much less frequently - _dev->write_register(L3G4200D_REG_FIFO_CTL, + _dev_gyro->write_register(L3G4200D_REG_FIFO_CTL, L3G4200D_REG_FIFO_CTL_STREAM); - hal.scheduler->delay(1); + hal.scheduler->delay(5); - _dev->get_semaphore()->give(); + _dev_gyro->get_semaphore()->give(); + + return true; +} + +bool AP_InertialSensor_L3G4200D::_init_sensor(void) +{ + + _accel_init(); + + _gyro_init(); return true; } @@ -205,13 +250,25 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void) */ void AP_InertialSensor_L3G4200D::start(void) { - _gyro_instance = _imu.register_gyro(800, _dev->get_bus_id_devtype(DEVTYPE_L3G4200D)); - _accel_instance = _imu.register_accel(800, _dev->get_bus_id_devtype(DEVTYPE_L3G4200D)); + _gyro_instance = _imu.register_gyro(800, _dev_gyro->get_bus_id_devtype(DEVTYPE_L3G4200D)); + _accel_instance = _imu.register_accel(800, _dev_accel->get_bus_id_devtype(DEVTYPE_L3G4200D)); // start the timer process to read samples - _dev->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, void)); + _dev_accel->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate_accel, void)); + _dev_gyro->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate_gyro, void)); } + +/* + set the filter frequency + */ +void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz) +{ + _accel_filter.set_cutoff_frequency(800, filter_hz); + _gyro_filter.set_cutoff_frequency(800, filter_hz); +} + + /* copy filtered data to the frontend */ @@ -224,14 +281,14 @@ bool AP_InertialSensor_L3G4200D::update(void) } // Accumulate values from accels and gyros -void AP_InertialSensor_L3G4200D::_accumulate(void) +void AP_InertialSensor_L3G4200D::_accumulate_gyro (void) { uint8_t num_samples_available; uint8_t fifo_status = 0; // Read gyro FIFO status fifo_status = 0; - _dev->read_registers(L3G4200D_REG_FIFO_SRC, &fifo_status, 1); + _dev_gyro->read_registers(L3G4200D_REG_FIFO_SRC, &fifo_status, 1); if (fifo_status & L3G4200D_REG_FIFO_SRC_OVERRUN) { // FIFO is full num_samples_available = 32; @@ -243,33 +300,42 @@ void AP_InertialSensor_L3G4200D::_accumulate(void) num_samples_available = fifo_status & L3G4200D_REG_FIFO_SRC_ENTRIES_MASK; } + // read the samples and apply the filter if (num_samples_available > 0) { // read all the entries in one go, using AUTO_INCREMENT. This saves a lot of time on I2C setup int16_t buffer[num_samples_available][3]; - if (!_dev->read_registers(L3G4200D_REG_XL | L3G4200D_REG_AUTO_INCREMENT, + if (_dev_gyro->read_registers(L3G4200D_REG_XL | L3G4200D_REG_AUTO_INCREMENT, (uint8_t *)&buffer, sizeof(buffer))) { for (uint8_t i=0; i < num_samples_available; i++) { Vector3f gyro = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]); // Adjust for chip scaling to get radians/sec + //hal.console->printf("gyro %f \r\n",gyro.x); gyro *= L3G4200D_GYRO_SCALE_R_S; _rotate_and_correct_gyro(_gyro_instance, gyro); _notify_new_gyro_raw_sample(_gyro_instance, gyro); } } } +} + +void AP_InertialSensor_L3G4200D::_accumulate_accel (void) +{ + uint8_t num_samples_available; + uint8_t fifo_status = 0; // Read accelerometer FIFO to find out how many samples are available - _dev->read_registers(ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS, + _dev_accel->read_registers(ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS, &fifo_status, 1); num_samples_available = fifo_status & 0x3F; // read the samples and apply the filter if (num_samples_available > 0) { int16_t buffer[num_samples_available][3]; - if (!_dev->read_registers_multiple(ADXL345_ACCELEROMETER_ADXLREG_DATAX0, - (uint8_t *)buffer, sizeof(buffer[0]), - num_samples_available)) { - for (uint8_t i=0; iread_registers(ADXL345_ACCELEROMETER_ADXLREG_DATAX0, + (uint8_t *)buffer[i], sizeof(buffer[0]))) + { Vector3f accel = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]); // Adjust for chip scaling to get m/s/s accel *= ADXL345_ACCELEROMETER_SCALE_M_S; @@ -280,4 +346,4 @@ void AP_InertialSensor_L3G4200D::_accumulate(void) } } -#endif +#endif \ No newline at end of file diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index 02da026176..535613571d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -1,3 +1,4 @@ + #pragma once #include @@ -14,26 +15,46 @@ class AP_InertialSensor_L3G4200D : public AP_InertialSensor_Backend { public: AP_InertialSensor_L3G4200D(AP_InertialSensor &imu, - AP_HAL::OwnPtr dev); + AP_HAL::OwnPtr dev_gyro, + AP_HAL::OwnPtr dev_accel); + + + virtual ~AP_InertialSensor_L3G4200D(); // probe the sensor on I2C bus static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, - AP_HAL::OwnPtr dev); + AP_HAL::OwnPtr dev_gyro, + AP_HAL::OwnPtr dev_accel); + + /* update accel and gyro state */ bool update() override; void start(void) override; + private: + bool _accel_init(); + bool _gyro_init(); bool _init_sensor(); - void _accumulate(); + void _accumulate_gyro(); + void _accumulate_accel(); + + AP_HAL::OwnPtr _dev_gyro; + AP_HAL::OwnPtr _dev_accel; - AP_HAL::OwnPtr _dev; + void _set_filter_frequency(uint8_t filter_hz); + + // Low Pass filters for gyro and accel + LowPassFilter2pVector3f _accel_filter; + LowPassFilter2pVector3f _gyro_filter; + + enum Rotation _rotation; // gyro and accel instances uint8_t _gyro_instance; uint8_t _accel_instance; }; -#endif +#endif // __AP_INERTIAL_SENSOR_L3G4200D2_H__ \ No newline at end of file