zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: update L3G4200D driver

zr-v5.1
mhefny 5 years ago committed by Lucas De Marchi
parent
1200d0e2f8
commit
02daa4c3ec
2 changed files with 164 additions and 77 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 184
      libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
  2. 31
      libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h

184
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
Unescape View File

@ -26,15 +26,18 @@ Datasheets:
ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf 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 L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf
*/ */
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include "AP_InertialSensor_L3G4200D.h" #include "AP_InertialSensor_L3G4200D.h"
#include <inttypes.h> #include <inttypes.h>
#include <utility> #include <utility>
const extern AP_HAL::HAL &hal;
const extern AP_HAL::HAL& hal;
/////// ///////
/// Accelerometer ADXL345 register definitions /// Accelerometer ADXL345 register definitions
@ -46,9 +49,38 @@ const extern AP_HAL::HAL &hal;
#define ADXL345_ACCELEROMETER_ADXLREG_DEVID 0x00 #define ADXL345_ACCELEROMETER_ADXLREG_DEVID 0x00
#define ADXL345_ACCELEROMETER_ADXLREG_DATAX0 0x32 #define ADXL345_ACCELEROMETER_ADXLREG_DATAX0 0x32
#define ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL 0x38 #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_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 // ADXL345 accelerometer scaling
// Result will be scaled to 1m/s/s // 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 // ADXL345 in Full resolution mode (any g scaling) is 256 counts/g, so scale by 9.81/256 = 0.038320312
@ -70,7 +102,6 @@ const extern AP_HAL::HAL &hal;
#define L3G4200D_REG_CTRL_REG5 0x24 #define L3G4200D_REG_CTRL_REG5 0x24
#define L3G4200D_REG_CTRL_REG5_FIFO_EN 0x40 #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_CTL_STREAM 0x40
@ -91,12 +122,15 @@ const extern AP_HAL::HAL &hal;
// constructor // constructor
AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu, AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev) AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_accel)
: AP_InertialSensor_Backend(imu) : 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() AP_InertialSensor_L3G4200D::~AP_InertialSensor_L3G4200D()
{ {
} }
@ -105,13 +139,14 @@ AP_InertialSensor_L3G4200D::~AP_InertialSensor_L3G4200D()
detect the sensor detect the sensor
*/ */
AP_InertialSensor_Backend *AP_InertialSensor_L3G4200D::probe(AP_InertialSensor &imu, AP_InertialSensor_Backend *AP_InertialSensor_L3G4200D::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev) AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_accel)
{ {
if (!dev) { if ((!dev_accel) || (!dev_gyro)){
return nullptr; return nullptr;
} }
AP_InertialSensor_L3G4200D *sensor 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()) { if (!sensor || !sensor->_init_sensor()) {
delete sensor; delete sensor;
return nullptr; return nullptr;
@ -120,82 +155,92 @@ AP_InertialSensor_Backend *AP_InertialSensor_L3G4200D::probe(AP_InertialSensor &
return sensor; 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 // Init the accelerometer
uint8_t data = 0; 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) { if (data != ADXL345_ACCELEROMETER_XL345_DEVID) {
AP_HAL::panic("AP_InertialSensor_L3G4200D: could not find ADXL345 accelerometer sensor"); 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: // Full resolution, 8g:
// Caution, this must agree with ADXL345_ACCELEROMETER_SCALE_1G // Caution, this must agree with ADXL345_ACCELEROMETER_SCALE_1G
// In full resoution mode, the scale factor need not change // 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); hal.scheduler->delay(5);
// Normal power, 800Hz Output Data Rate, 400Hz bandwidth: // 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); hal.scheduler->delay(5);
// enable FIFO in stream mode. This will allow us to read the accelerometers // enable FIFO in stream mode. This will allow us to read the gyros much less frequently
// much less frequently _dev_accel->write_register(ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL,
_dev->write_register(ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL,
ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL_STREAM); 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 // 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) { if (data != L3G4200D_REG_WHO_AM_I_VALUE) {
AP_HAL::panic("AP_InertialSensor_L3G4200D: could not find L3G4200D gyro sensor"); AP_HAL::panic("AP_InertialSensor_L3G4200D: could not find L3G4200D gyro sensor");
} }
// setup for 800Hz sampling with 110Hz filter // setup for 800Hz sampling with 110Hz filter
_dev->write_register(L3G4200D_REG_CTRL_REG1, _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_DRBW_800_110 |
L3G4200D_REG_CTRL_REG1_PD | L3G4200D_REG_CTRL_REG1_PD |
L3G4200D_REG_CTRL_REG1_XYZ_ENABLE); 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 // 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); L3G4200D_REG_CTRL_REG4_FS_2000);
hal.scheduler->delay(1); hal.scheduler->delay(5);
// enable FIFO // enable FIFO
_dev->write_register(L3G4200D_REG_CTRL_REG5, _dev_gyro->write_register(L3G4200D_REG_CTRL_REG5,
L3G4200D_REG_CTRL_REG5_FIFO_EN); 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 // 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); L3G4200D_REG_FIFO_CTL_STREAM);
hal.scheduler->delay(1); hal.scheduler->delay(5);
_dev_gyro->get_semaphore()->give();
return true;
}
_dev->get_semaphore()->give(); bool AP_InertialSensor_L3G4200D::_init_sensor(void)
{
_accel_init();
_gyro_init();
return true; return true;
} }
@ -205,13 +250,25 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
*/ */
void AP_InertialSensor_L3G4200D::start(void) void AP_InertialSensor_L3G4200D::start(void)
{ {
_gyro_instance = _imu.register_gyro(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->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 // 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 copy filtered data to the frontend
*/ */
@ -224,14 +281,14 @@ bool AP_InertialSensor_L3G4200D::update(void)
} }
// Accumulate values from accels and gyros // 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 num_samples_available;
uint8_t fifo_status = 0; uint8_t fifo_status = 0;
// Read gyro FIFO status // Read gyro FIFO status
fifo_status = 0; 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) { if (fifo_status & L3G4200D_REG_FIFO_SRC_OVERRUN) {
// FIFO is full // FIFO is full
num_samples_available = 32; 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; num_samples_available = fifo_status & L3G4200D_REG_FIFO_SRC_ENTRIES_MASK;
} }
// read the samples and apply the filter
if (num_samples_available > 0) { if (num_samples_available > 0) {
// read all the entries in one go, using AUTO_INCREMENT. This saves a lot of time on I2C setup // 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]; 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))) { (uint8_t *)&buffer, sizeof(buffer))) {
for (uint8_t i=0; i < num_samples_available; i++) { for (uint8_t i=0; i < num_samples_available; i++) {
Vector3f gyro = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]); Vector3f gyro = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
// Adjust for chip scaling to get radians/sec // Adjust for chip scaling to get radians/sec
//hal.console->printf("gyro %f \r\n",gyro.x);
gyro *= L3G4200D_GYRO_SCALE_R_S; gyro *= L3G4200D_GYRO_SCALE_R_S;
_rotate_and_correct_gyro(_gyro_instance, gyro); _rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_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 // 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); &fifo_status, 1);
num_samples_available = fifo_status & 0x3F; num_samples_available = fifo_status & 0x3F;
// read the samples and apply the filter // read the samples and apply the filter
if (num_samples_available > 0) { if (num_samples_available > 0) {
int16_t buffer[num_samples_available][3]; int16_t buffer[num_samples_available][3];
if (!_dev->read_registers_multiple(ADXL345_ACCELEROMETER_ADXLREG_DATAX0, for (uint8_t i=0; i<num_samples_available; i++)
(uint8_t *)buffer, sizeof(buffer[0]), {
num_samples_available)) { if (_dev_accel->read_registers(ADXL345_ACCELEROMETER_ADXLREG_DATAX0,
for (uint8_t i=0; i<num_samples_available; i++) { (uint8_t *)buffer[i], sizeof(buffer[0])))
{
Vector3f accel = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]); Vector3f accel = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
// Adjust for chip scaling to get m/s/s // Adjust for chip scaling to get m/s/s
accel *= ADXL345_ACCELEROMETER_SCALE_M_S; accel *= ADXL345_ACCELEROMETER_SCALE_M_S;

31
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
Unescape View File

@ -1,3 +1,4 @@
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
@ -14,26 +15,46 @@ class AP_InertialSensor_L3G4200D : public AP_InertialSensor_Backend
{ {
public: public:
AP_InertialSensor_L3G4200D(AP_InertialSensor &imu, AP_InertialSensor_L3G4200D(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev); AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_accel);
virtual ~AP_InertialSensor_L3G4200D(); virtual ~AP_InertialSensor_L3G4200D();
// probe the sensor on I2C bus // probe the sensor on I2C bus
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev); AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_accel);
/* update accel and gyro state */ /* update accel and gyro state */
bool update() override; bool update() override;
void start(void) override; void start(void) override;
private: private:
bool _accel_init();
bool _gyro_init();
bool _init_sensor(); bool _init_sensor();
void _accumulate(); void _accumulate_gyro();
void _accumulate_accel();
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev_gyro;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev_accel;
void _set_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter;
LowPassFilter2pVector3f _gyro_filter;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev; enum Rotation _rotation;
// gyro and accel instances // gyro and accel instances
uint8_t _gyro_instance; uint8_t _gyro_instance;
uint8_t _accel_instance; uint8_t _accel_instance;
}; };
#endif #endif // __AP_INERTIAL_SENSOR_L3G4200D2_H__
Write Preview
Loading…
Cancel
Save