aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: ported L3G4200D driver to new API

Browse Source
This commit is contained in:
Andrew Tridgell 2014-10-16 10:55:18 +11:00
parent deafcd6ddc
commit 2d47a07480
4 changed files with 70 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -217,11 +217,11 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
};
AP_InertialSensor::AP_InertialSensor() :
_gyro_count(0),
_accel_count(0),
_accel(),
_gyro(),
_board_orientation(ROTATION_NONE),
_gyro_count(0),
_accel_count(0),
_hil_mode(false)
{
AP_Param::setup_object_defaults(this, var_info);

1
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -276,6 +276,7 @@ private:
#include "AP_InertialSensor_PX4.h"
#include "AP_InertialSensor_Oilpan.h"
#include "AP_InertialSensor_MPU9250.h"
#include "AP_InertialSensor_L3G4200D.h"
#include "AP_InertialSensor_HIL.h"
#include "AP_InertialSensor_UserInteract_Stream.h"
#include "AP_InertialSensor_UserInteract_MAVLink.h"

135
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -16,9 +16,17 @@
/*
This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer.
This combination is available as a cheap 10DOF sensor on ebay
*/
// 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
This sensor driver is an example only - it should not be used in any
serious autopilot as the latencies on I2C prevent good timing at
high sample rates. It is useful when doing an initial port of
ardupilot to a board where only I2C is available, and a cheap sensor
can be used.
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 <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
@ -103,8 +111,10 @@ const extern AP_HAL::HAL& hal;
#define L3G4200D_GYRO_SCALE_R_S (DEG_TO_RAD * 70.0f * 0.001f)
// constructor
AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() :
AP_InertialSensor(),
AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_have_gyro_sample(false),
_have_accel_sample(false),
_accel_filter_x(800, 10),
_accel_filter_y(800, 10),
_accel_filter_z(800, 10),
@ -113,26 +123,24 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() :
_gyro_filter_z(800, 10)
{}
uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate )
bool AP_InertialSensor_L3G4200D::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
{
switch (sample_rate) {
case RATE_50HZ:
case AP_InertialSensor::RATE_50HZ:
_default_filter_hz = 10;
_sample_period_usec = (1000*1000) / 50;
_gyro_samples_needed = 16;
break;
case RATE_100HZ:
case AP_InertialSensor::RATE_100HZ:
_default_filter_hz = 20;
_sample_period_usec = (1000*1000) / 100;
_gyro_samples_needed = 8;
break;
case RATE_200HZ:
case AP_InertialSensor::RATE_200HZ:
_default_filter_hz = 30;
break;
case AP_InertialSensor::RATE_400HZ:
_default_filter_hz = 30;
break;
default:
_default_filter_hz = 20;
_sample_period_usec = (1000*1000) / 200;
_gyro_samples_needed = 4;
break;
return false;
}
// get pointer to i2c bus semaphore
@ -219,7 +227,7 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate )
// Set up the filter desired
_set_filter_frequency(_mpu6000_filter);
_set_filter_frequency(_imu.get_filter());
// give back i2c semaphore
i2c_sem->give();
@ -227,15 +235,10 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate )
// start the timer process to read samples
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3G4200D::_accumulate));
clock_gettime(CLOCK_MONOTONIC, &_next_sample_ts);
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_next_sample_ts.tv_nsec += _sample_period_usec * 1000UL;
if (_next_sample_ts.tv_nsec >= 1.0e9) {
_next_sample_ts.tv_nsec -= 1.0e9;
_next_sample_ts.tv_sec++;
}
return AP_PRODUCT_ID_L3G4200D;
return true;
}
/*
@ -254,58 +257,37 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
_gyro_filter_z.set_cutoff_frequency(800, filter_hz);
}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
/*
copy filtered data to the frontend
*/
bool AP_InertialSensor_L3G4200D::update(void)
{
Vector3f accel_scale = _accel_scale[0].get();
_previous_accel[0] = _accel[0];
Vector3f accel, gyro;
uint32_t now = hal.scheduler->micros();
hal.scheduler->suspend_timer_procs();
_accel[0] = _accel_filtered;
_gyro[0] = _gyro_filtered;
_delta_time = _gyro_samples_available * (1.0f/800);
_gyro_samples_available = 0;
accel = _accel_filtered;
gyro = _gyro_filtered;
_have_gyro_sample = false;
_have_accel_sample = false;
hal.scheduler->resume_timer_procs();
// add offsets and rotation
_accel[0].rotate(_board_orientation);
// Adjust for chip scaling to get m/s/s
_accel[0] *= ADXL345_ACCELEROMETER_SCALE_M_S;
// Now the calibration scale factor
_accel[0].x *= accel_scale.x;
_accel[0].y *= accel_scale.y;
_accel[0].z *= accel_scale.z;
_accel[0] -= _accel_offset[0];
_gyro[0].rotate(_board_orientation);
accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
_rotate_and_offset_accel(_accel_instance, accel, now);
// Adjust for chip scaling to get radians/sec
_gyro[0] *= L3G4200D_GYRO_SCALE_R_S;
_gyro[0] -= _gyro_offset[0];
gyro *= L3G4200D_GYRO_SCALE_R_S;
_rotate_and_offset_gyro(_gyro_instance, gyro, now);
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
}
return true;
}
float AP_InertialSensor_L3G4200D::get_delta_time(void) const
{
return _delta_time;
}
float AP_InertialSensor_L3G4200D::get_gyro_drift_rate(void)
{
// 0.5 degrees/second/minute (a guess)
return ToRad(0.5/60);
}
// Accumulate values from accels and gyros
void AP_InertialSensor_L3G4200D::_accumulate(void)
{
@ -344,19 +326,17 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
_gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]),
_gyro_filter_y.apply(-buffer[i][1]),
_gyro_filter_z.apply(-buffer[i][2]));
_gyro_samples_available++;
_have_gyro_sample = true;
}
}
}
// Read accelerometer FIFO to find out how many samples are available
hal.i2c->readRegister(ADXL345_ACCELEROMETER_ADDRESS,
ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS,
&fifo_status);
num_samples_available = fifo_status & 0x3F;
#if 1
// read the samples and apply the filter
if (num_samples_available > 0) {
int16_t buffer[num_samples_available][3];
@ -368,35 +348,14 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
_accel_filtered = Vector3f(_accel_filter_x.apply(buffer[i][0]),
_accel_filter_y.apply(-buffer[i][1]),
_accel_filter_z.apply(-buffer[i][2]));
_have_accel_sample = true;
}
}
}
#endif
// give back i2c semaphore
i2c_sem->give();
}
bool AP_InertialSensor_L3G4200D::_sample_available(void)
{
return (_gyro_samples_available >= _gyro_samples_needed);
}
bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms)
{
uint32_t start_us = hal.scheduler->micros();
while (clock_nanosleep(CLOCK_MONOTONIC,
TIMER_ABSTIME, &_next_sample_ts, NULL) == -1 && errno == EINTR) ;
_next_sample_ts.tv_nsec += _sample_period_usec * 1000UL;
if (_next_sample_ts.tv_nsec >= 1.0e9) {
_next_sample_ts.tv_nsec -= 1.0e9;
_next_sample_ts.tv_sec++;
}
//_accumulate();
return true;
}
#endif // CONFIG_HAL_BOARD

39
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -6,40 +6,37 @@
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <AP_Progmem.h>
#include "AP_InertialSensor.h"
#include <Filter.h>
#include <LowPassFilter2p.h>
class AP_InertialSensor_L3G4200D : public AP_InertialSensor
class AP_InertialSensor_L3G4200D : public AP_InertialSensor_Backend
{
public:
AP_InertialSensor_L3G4200D();
AP_InertialSensor_L3G4200D(AP_InertialSensor &imu);
/* Concrete implementation of AP_InertialSensor functions: */
bool update();
float get_delta_time() const;
float get_gyro_drift_rate();
bool wait_for_sample(uint16_t timeout_ms);
/* update accel and gyro state */
bool update();
bool gyro_sample_available(void) { return _have_gyro_sample; }
bool accel_sample_available(void) { return _have_accel_sample; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
private:
uint16_t _init_sensor( Sample_rate sample_rate );
void _accumulate(void);
bool _sample_available();
uint64_t _last_update_usec;
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
void _accumulate(void);
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
uint32_t _sample_period_usec;
uint32_t _last_sample_time;
volatile uint32_t _gyro_samples_available;
volatile uint8_t _gyro_samples_needed;
float _delta_time;
struct timespec _next_sample_ts;
volatile bool _have_gyro_sample;
volatile bool _have_accel_sample;
// support for updating filter at runtime
uint8_t _last_filter_hz;
uint8_t _default_filter_hz;
uint8_t _default_filter_hz;
void _set_filter_frequency(uint8_t filter_hz);
@ -50,6 +47,10 @@ private:
LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
// gyro and accel instances
uint8_t _gyro_instance;
uint8_t _accel_instance;
};
#endif
#endif // __AP_INERTIAL_SENSOR_L3G4200D_H__