Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: simplify sensor backends

Browse Source 
use common code for filtering and update, allowing each sensor driver
to be simpler and more consistent
This commit is contained in:
Andrew Tridgell 2015-11-16 11:05:20 +11:00
parent 36c0beb918
commit 0e4bab74ba
19 changed files with 118 additions and 377 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -368,22 +368,24 @@ AP_InertialSensor *AP_InertialSensor::get_instance()
/*
register a new gyro instance
*/
uint8_t AP_InertialSensor::register_gyro(void)
uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz)
{
if (_gyro_count == INS_MAX_INSTANCES) {
hal.scheduler->panic("Too many gyros");
}
_gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
return _gyro_count++;
}
/*
register a new accel instance
*/
uint8_t AP_InertialSensor::register_accel(void)
uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz)
{
if (_accel_count == INS_MAX_INSTANCES) {
hal.scheduler->panic("Too many accels");
}
_accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
return _accel_count++;
}

17
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -23,6 +23,7 @@
#include <AP_Math/AP_Math.h>
#include "AP_InertialSensor_UserInteract.h"
#include <Filter/LowPassFilter.h>
#include <Filter/LowPassFilter2p.h>
class AP_InertialSensor_Backend;
class AuxiliaryBus;
@ -73,8 +74,8 @@ public:
/// Register a new gyro/accel driver, allocating an instance
/// number
uint8_t register_gyro(void);
uint8_t register_accel(void);
uint8_t register_gyro(uint16_t raw_sample_rate_hz);
uint8_t register_accel(uint16_t raw_sample_rate_hz);
// perform accelerometer calibration including providing user instructions
// and feedback
@ -283,6 +284,14 @@ private:
// time accumulator for delta velocity accumulator
float _delta_velocity_acc_dt[INS_MAX_INSTANCES];
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter[INS_MAX_INSTANCES];
LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
Vector3f _accel_filtered[INS_MAX_INSTANCES];
Vector3f _gyro_filtered[INS_MAX_INSTANCES];
bool _new_accel_data[INS_MAX_INSTANCES];
bool _new_gyro_data[INS_MAX_INSTANCES];
// Most recent gyro reading
Vector3f _gyro[INS_MAX_INSTANCES];
Vector3f _delta_angle[INS_MAX_INSTANCES];
@ -303,8 +312,8 @@ private:
float _accel_max_abs_offsets[INS_MAX_INSTANCES];
// accelerometer and gyro raw sample rate in units of Hz
uint32_t _accel_raw_sample_rates[INS_MAX_INSTANCES];
uint32_t _gyro_raw_sample_rates[INS_MAX_INSTANCES];
uint16_t _accel_raw_sample_rates[INS_MAX_INSTANCES];
uint16_t _gyro_raw_sample_rates[INS_MAX_INSTANCES];
// temperatures for an instance if available
float _temperature[INS_MAX_INSTANCES];

64
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -4,6 +4,8 @@
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
const extern AP_HAL::HAL& hal;
AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
_imu(imu),
_product_id(AP_PRODUCT_ID_NONE)
@ -87,6 +89,10 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
// save previous delta angle for coning correction
_imu._last_delta_angle[instance] = delta_angle;
_imu._last_raw_gyro[instance] = gyro;
_imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
_imu._new_gyro_data[instance] = true;
}
/*
@ -123,6 +129,10 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
// delta velocity
_imu._delta_velocity_acc[instance] += accel * dt;
_imu._delta_velocity_acc_dt[instance] += dt;
_imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
_imu._new_accel_data[instance] = true;
}
void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance,
@ -131,24 +141,12 @@ void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance,
_imu._accel_max_abs_offsets[instance] = max_offset;
}
void AP_InertialSensor_Backend::_set_accel_raw_sample_rate(uint8_t instance,
uint32_t rate)
{
_imu._accel_raw_sample_rates[instance] = rate;
}
// set accelerometer error_count
void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count)
{
_imu._accel_error_count[instance] = error_count;
}
void AP_InertialSensor_Backend::_set_gyro_raw_sample_rate(uint8_t instance,
uint32_t rate)
{
_imu._gyro_raw_sample_rates[instance] = rate;
}
// set gyro error_count
void AP_InertialSensor_Backend::_set_gyro_error_count(uint8_t instance, uint32_t error_count)
{
@ -169,3 +167,45 @@ void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float tem
{
_imu._temperature[instance] = temperature;
}
/*
common gyro update function for all backends
*/
void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
{
hal.scheduler->suspend_timer_procs();
if (_imu._new_gyro_data[instance]) {
_publish_gyro(instance, _imu._gyro_filtered[instance]);
_imu._new_gyro_data[instance] = false;
}
// possibly update filter frequency
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
hal.scheduler->resume_timer_procs();
}
/*
common accel update function for all backends
*/
void AP_InertialSensor_Backend::update_accel(uint8_t instance)
{
hal.scheduler->suspend_timer_procs();
if (_imu._new_accel_data[instance]) {
_publish_accel(instance, _imu._accel_filtered[instance]);
_imu._new_accel_data[instance] = false;
}
// possibly update filter frequency
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
hal.scheduler->resume_timer_procs();
}

16
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -101,14 +101,12 @@ protected:
// set accelerometer max absolute offset for calibration
void _set_accel_max_abs_offset(uint8_t instance, float offset);
// set accelerometer raw sample rate
void _set_accel_raw_sample_rate(uint8_t instance, uint32_t rate);
// get accelerometer raw sample rate
uint32_t _accel_raw_sample_rate(uint8_t instance) const {
return _imu._accel_raw_sample_rates[instance];
}
// set gyroscope raw sample rate
void _set_gyro_raw_sample_rate(uint8_t instance, uint32_t rate);
// get gyroscope raw sample rate
uint32_t _gyro_raw_sample_rate(uint8_t instance) const {
return _imu._gyro_raw_sample_rates[instance];
}
@ -142,6 +140,16 @@ protected:
return _imu._log_raw_data? _imu._dataflash : NULL;
}
// common gyro update function for all backends
void update_gyro(uint8_t instance);
// common accel update function for all backends
void update_accel(uint8_t instance);
// support for updating filter at runtime
int8_t _last_accel_filter_hz;
int8_t _last_gyro_filter_hz;
// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor
// driver if the sensor is available

21
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -145,8 +145,8 @@ bool AP_InertialSensor_Flymaple::_init_sensor(void)
// give back i2c semaphore
i2c_sem->give();
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_gyro_instance = _imu.register_gyro(raw_sample_rate_hz);
_accel_instance = _imu.register_accel(raw_sample_rate_hz);
_product_id = AP_PRODUCT_ID_FLYMAPLE;
@ -166,22 +166,11 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
// This takes about 20us to run
bool AP_InertialSensor_Flymaple::update(void)
{
Vector3f accel, gyro;
hal.scheduler->suspend_timer_procs();
accel = _accel_filtered;
gyro = _gyro_filtered;
_have_gyro_sample = false;
_have_accel_sample = false;
hal.scheduler->resume_timer_procs();
_publish_accel(_accel_instance, accel);
_publish_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _accel_filter_cutoff()) {
_set_filter_frequency(_accel_filter_cutoff());
_last_filter_hz = _accel_filter_cutoff();
}
update_accel(_accel_instance);
update_gyro(_gyro_instance);
return true;
}
@ -226,7 +215,6 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
_rotate_and_correct_accel(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel);
_accel_filtered = _accel_filter.apply(accel);
_have_accel_sample = true;
_last_accel_timestamp = now;
}
@ -246,7 +234,6 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_gyro_filtered = _gyro_filter.apply(gyro);
_have_gyro_sample = true;
_last_gyro_timestamp = now;
}

10
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -27,19 +27,9 @@ public:
private:
bool _init_sensor(void);
void _accumulate(void);
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
bool _have_gyro_sample;
bool _have_accel_sample;
// support for updating filter at runtime
uint8_t _last_filter_hz;
void _set_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter;
LowPassFilter2pVector3f _gyro_filter;
uint8_t _gyro_instance;
uint8_t _accel_instance;

4
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -29,8 +29,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu
bool AP_InertialSensor_HIL::_init_sensor(void)
{
// grab the used instances
_imu.register_gyro();
_imu.register_accel();
_imu.register_gyro(1200);
_imu.register_accel(1200);
_product_id = AP_PRODUCT_ID_NONE;
_imu.set_hil_mode();

38
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -117,9 +117,7 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
_data_idx(0),
_have_gyro_sample(false),
_have_accel_sample(false),
_have_sample(false),
_accel_filter(800, 10),
_gyro_filter(800, 10)
_have_sample(false)
{
pthread_spin_init(&_data_lock, PTHREAD_PROCESS_PRIVATE);
}
@ -231,54 +229,34 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
hal.scheduler->delay(1);
// Set up the filter desired
_set_filter_frequency(_accel_filter_cutoff());
// give back i2c semaphore
i2c_sem->give();
// start the timer process to read samples
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, void));
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_gyro_instance = _imu.register_gyro(800);
_accel_instance = _imu.register_accel(800);
_product_id = AP_PRODUCT_ID_L3G4200D;
return true;
}
/*
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
*/
bool AP_InertialSensor_L3G4200D::update(void)
{
Vector3f accel, gyro;
pthread_spin_lock(&_data_lock);
unsigned int idx = !_data_idx;
accel = _data[idx].accel_filtered;
gyro = _data[idx].gyro_filtered;
update_gyro(_gyro_instance);
update_accel(_accel_instance);
_have_sample = false;
pthread_spin_unlock(&_data_lock);
_publish_accel(_accel_instance, accel);
_publish_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _accel_filter_cutoff()) {
_set_filter_frequency(_accel_filter_cutoff());
_last_filter_hz = _accel_filter_cutoff();
}
return true;
}
@ -322,7 +300,6 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
gyro *= L3G4200D_GYRO_SCALE_R_S;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_data[_data_idx].gyro_filtered = _gyro_filter.apply(gyro);
_have_gyro_sample = true;
}
}
@ -347,7 +324,6 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
_rotate_and_correct_accel(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel);
_data[_data_idx].accel_filtered = _accel_filter.apply(accel);
_have_accel_sample = true;
}
}

13
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -35,10 +35,6 @@ private:
bool _init_sensor(void);
void _accumulate(void);
struct {
Vector3f accel_filtered;
Vector3f gyro_filtered;
} _data[2];
int _data_idx;
pthread_spinlock_t _data_lock;
@ -46,15 +42,6 @@ private:
bool _have_accel_sample;
volatile bool _have_sample;
// support for updating filter at runtime
uint8_t _last_filter_hz;
void _set_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter;
LowPassFilter2pVector3f _gyro_filter;
// gyro and accel instances
uint8_t _gyro_instance;
uint8_t _accel_instance;

40
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -376,10 +376,6 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
AP_InertialSensor_Backend(imu),
_drdy_pin_a(NULL),
_drdy_pin_g(NULL),
_last_accel_filter_hz(-1),
_last_gyro_filter_hz(-1),
_accel_filter(800, 15),
_gyro_filter(760, 15),
_gyro_sample_available(false),
_accel_sample_available(false),
_drdy_pin_num_a(drdy_pin_num_a),
@ -480,8 +476,8 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor()
hal.scheduler->resume_timer_procs();
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_gyro_instance = _imu.register_gyro(760);
_accel_instance = _imu.register_accel(800);
_set_accel_max_abs_offset(_accel_instance, 5.0f);
@ -742,7 +738,6 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
accel_data *= _accel_scale;
_rotate_and_correct_accel(_accel_instance, accel_data);
_notify_new_accel_raw_sample(_accel_instance, accel_data);
_accel_filtered = _accel_filter.apply(accel_data);
_accel_sample_available = true;
}
@ -758,7 +753,6 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
gyro_data *= _gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, gyro_data);
_notify_new_gyro_raw_sample(_gyro_instance, gyro_data);
_gyro_filtered = _gyro_filter.apply(gyro_data);
_gyro_sample_available = true;
}
@ -767,38 +761,12 @@ bool AP_InertialSensor_LSM9DS0::update()
_accel_sample_available = false;
_gyro_sample_available = false;
_publish_gyro(_gyro_instance, _gyro_filtered);
_publish_accel(_accel_instance, _accel_filtered);
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_set_accel_filter(_accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_set_gyro_filter(_gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
update_gyro(_gyro_instance);
update_accel(_accel_instance);
return true;
}
/*
* set the accel filter frequency
*/
void AP_InertialSensor_LSM9DS0::_set_accel_filter(uint8_t filter_hz)
{
_accel_filter.set_cutoff_frequency(800, filter_hz);
}
/*
* set the gyro filter frequency
*/
void AP_InertialSensor_LSM9DS0::_set_gyro_filter(uint8_t filter_hz)
{
_gyro_filter.set_cutoff_frequency(760, filter_hz);
}
#if LSM9DS0_DEBUG
/* dump all config registers - used for debug */
void AP_InertialSensor_LSM9DS0::_dump_registers(void)

15
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
View File

@ -97,21 +97,6 @@ private:
void _read_data_transaction_a();
void _read_data_transaction_g();
/* support for updating filter at runtime */
int16_t _last_gyro_filter_hz;
int16_t _last_accel_filter_hz;
/* change the filter frequency */
void _set_accel_filter(uint8_t filter_hz);
void _set_gyro_filter(uint8_t filter_hz);
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
/* Low Pass filters for gyro and accel */
LowPassFilter2pVector3f _accel_filter;
LowPassFilter2pVector3f _gyro_filter;
#if LSM9DS0_DEBUG
void _dump_registers();
#endif

50
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -439,12 +439,7 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
_drdy_pin(NULL),
_bus(bus),
_bus_sem(NULL),
_last_accel_filter_hz(-1),
_last_gyro_filter_hz(-1),
_accel_filter(1000, 15),
_gyro_filter(1000, 15),
_temp_filter(1000, 1),
_sum_count(0),
_samples(NULL)
{
@ -590,11 +585,8 @@ void AP_InertialSensor_MPU6000::start()
_bus_sem->give();
// grab the used instances
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_set_accel_raw_sample_rate(_accel_instance, 1000);
_set_gyro_raw_sample_rate(_gyro_instance, 1000);
_gyro_instance = _imu.register_gyro(1000);
_accel_instance = _imu.register_accel(1000);
hal.scheduler->resume_timer_procs();
@ -608,38 +600,13 @@ void AP_InertialSensor_MPU6000::start()
*/
bool AP_InertialSensor_MPU6000::update( void )
{
// we have a full set of samples
uint16_t num_samples;
Vector3f accel, gyro;
float temp;
update_accel(_accel_instance);
update_gyro(_gyro_instance);
hal.scheduler->suspend_timer_procs();
gyro = _gyro_filtered;
accel = _accel_filtered;
temp = _temp_filtered;
num_samples = 1;
_sum_count = 0;
hal.scheduler->resume_timer_procs();
gyro /= num_samples;
accel /= num_samples;
temp /= num_samples;
_publish_accel(_accel_instance, accel);
_publish_gyro(_gyro_instance, gyro);
_publish_temperature(_accel_instance, temp);
_publish_temperature(_accel_instance, _temp_filtered);
/* give the temperature to the control loop in order to keep it constant*/
hal.util->set_imu_temp(temp);
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_accel_filter.set_cutoff_frequency(1000, _accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_gyro_filter.set_cutoff_frequency(1000, _gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
hal.util->set_imu_temp(_temp_filtered);
return true;
}
@ -724,10 +691,7 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
_notify_new_accel_raw_sample(_accel_instance, accel);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_accel_filtered = _accel_filter.apply(accel);
_gyro_filtered = _gyro_filter.apply(gyro);
_temp_filtered = _temp_filter.apply(temp);
_sum_count++;
}
}

9
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -106,22 +106,13 @@ private:
static const float _gyro_scale;
// support for updating filter at runtime
int8_t _last_accel_filter_hz;
int8_t _last_gyro_filter_hz;
void _set_filter_register(uint16_t filter_hz);
// how many hardware samples before we report a sample to the caller
uint8_t _sample_count;
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
float _temp_filtered;
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter;
LowPassFilter2pVector3f _gyro_filter;
LowPassFilter2pFloat _temp_filter;
volatile uint16_t _sum_count;

51
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -326,9 +326,7 @@ static struct gyro_state_s st = {
*/
AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_have_sample_available(false),
_accel_filter(800, 10),
_gyro_filter(800, 10)
_have_sample_available(false)
{
}
@ -349,22 +347,6 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &
return sensor;
}
/*
set the accel filter frequency
*/
void AP_InertialSensor_MPU9150::_set_accel_filter_frequency(uint8_t filter_hz)
{
_accel_filter.set_cutoff_frequency(800, filter_hz);
}
/*
set the gyro filter frequency
*/
void AP_InertialSensor_MPU9150::_set_gyro_filter_frequency(uint8_t filter_hz)
{
_gyro_filter.set_cutoff_frequency(800, filter_hz);
}
/**
* Init method
*/
@ -458,15 +440,11 @@ bool AP_InertialSensor_MPU9150::_init_sensor(void)
mpu_set_sensors(sensors);
// Set the filter frecuency
_set_accel_filter_frequency(_accel_filter_cutoff());
_set_gyro_filter_frequency(_gyro_filter_cutoff());
// give back i2c semaphore
i2c_sem->give();
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_gyro_instance = _imu.register_gyro(800);
_accel_instance = _imu.register_accel(800);
// start the timer process to read samples
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9150::_accumulate, void));
@ -1093,13 +1071,11 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
accel *= MPU9150_ACCEL_SCALE_2G;
_rotate_and_correct_accel(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel);
_accel_filtered = _accel_filter.apply(accel);
gyro = Vector3f(gyro_x, gyro_y, gyro_z);
gyro *= MPU9150_GYRO_SCALE_2000;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_gyro_filtered = _gyro_filter.apply(gyro);
_have_sample_available = true;
}
@ -1110,26 +1086,9 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
bool AP_InertialSensor_MPU9150::update(void)
{
Vector3f accel, gyro;
hal.scheduler->suspend_timer_procs();
accel = _accel_filtered;
gyro = _gyro_filtered;
_have_sample_available = false;
hal.scheduler->resume_timer_procs();
_publish_accel(_accel_instance, accel);
_publish_gyro(_gyro_instance, gyro);
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_set_accel_filter_frequency(_accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_set_gyro_filter_frequency(_gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
update_gyro(_gyro_instance);
update_accel(_accel_instance);
return true;
}

13
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
View File

@ -28,14 +28,8 @@ public:
private:
bool _init_sensor();
void _accumulate(void);
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
bool _have_sample_available;
// // support for updating filter at runtime
uint8_t _last_accel_filter_hz;
uint8_t _last_gyro_filter_hz;
int16_t mpu_set_gyro_fsr(uint16_t fsr);
int16_t mpu_set_accel_fsr(uint8_t fsr);
int16_t mpu_set_lpf(uint16_t lpf);
@ -48,13 +42,6 @@ private:
int16_t mpu_set_int_latched(uint8_t enable);
int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t *timestamp, uint8_t *sensors, uint8_t *more);
void _set_accel_filter_frequency(uint8_t filter_hz);
void _set_gyro_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter;
LowPassFilter2pVector3f _gyro_filter;
uint8_t _gyro_instance;
uint8_t _accel_instance;
};

55
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -362,11 +362,6 @@ bool AP_MPU9250_BusDriver_I2C::has_auxiliary_bus()
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_MPU9250_BusDriver *bus) :
AP_InertialSensor_Backend(imu),
_last_accel_filter_hz(-1),
_last_gyro_filter_hz(-1),
_shared_data_idx(0),
_accel_filter(DEFAULT_SAMPLE_RATE, 15),
_gyro_filter(DEFAULT_SAMPLE_RATE, 15),
_have_sample_available(false),
_bus(bus),
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
@ -437,17 +432,14 @@ bool AP_InertialSensor_MPU9250::_init_sensor()
if (!_hardware_init())
return false;
_gyro_instance = _imu.register_gyro();
_accel_instance = _imu.register_accel();
_gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE);
_accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE);
_product_id = AP_PRODUCT_ID_MPU9250;
// start the timer process to read samples
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_poll_data, void));
_set_accel_raw_sample_rate(_accel_instance, DEFAULT_SAMPLE_RATE);
_set_gyro_raw_sample_rate(_gyro_instance, DEFAULT_SAMPLE_RATE);
#if MPU9250_DEBUG
_dump_registers();
#endif
@ -459,25 +451,10 @@ bool AP_InertialSensor_MPU9250::_init_sensor()
*/
bool AP_InertialSensor_MPU9250::update( void )
{
// pull the data from the timer shared data buffer
uint8_t idx = _shared_data_idx;
Vector3f gyro = _shared_data[idx]._gyro_filtered;
Vector3f accel = _shared_data[idx]._accel_filtered;
_have_sample_available = false;
_publish_gyro(_gyro_instance, gyro);
_publish_accel(_accel_instance, accel);
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_set_accel_filter(_accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_set_gyro_filter(_gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
update_gyro(_gyro_instance);
update_accel(_accel_instance);
return true;
}
@ -534,13 +511,6 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
// update the shared buffer
uint8_t idx = _shared_data_idx ^ 1;
_shared_data[idx]._accel_filtered = _accel_filter.apply(accel);
_shared_data[idx]._gyro_filtered = _gyro_filter.apply(gyro);
_shared_data_idx = idx;
_have_sample_available = true;
}
@ -562,23 +532,6 @@ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
_bus->write8(reg, val);
}
/*
set the accel filter frequency
*/
void AP_InertialSensor_MPU9250::_set_accel_filter(uint8_t filter_hz)
{
_accel_filter.set_cutoff_frequency(DEFAULT_SAMPLE_RATE, filter_hz);
}
/*
set the gyro filter frequency
*/
void AP_InertialSensor_MPU9250::_set_gyro_filter(uint8_t filter_hz)
{
_gyro_filter.set_cutoff_frequency(DEFAULT_SAMPLE_RATE, filter_hz);
}
/*
initialise the sensor configuration registers
*/

22
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -77,28 +77,6 @@ private:
AP_HAL::Semaphore *_bus_sem;
AP_MPU9250_AuxiliaryBus *_auxiliar_bus = nullptr;
// support for updating filter at runtime
int16_t _last_gyro_filter_hz;
int16_t _last_accel_filter_hz;
// change the filter frequency
void _set_accel_filter(uint8_t filter_hz);
void _set_gyro_filter(uint8_t filter_hz);
// This structure is used to pass data from the timer which reads
// the sensor to the main thread. The _shared_data_idx is used to
// prevent race conditions by ensuring the data is fully updated
// before being used by the consumer
struct {
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
} _shared_data[2];
volatile uint8_t _shared_data_idx;
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter;
LowPassFilter2pVector3f _gyro_filter;
// do we currently have a sample pending?
bool _have_sample_available;

38
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -73,11 +73,9 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_fd[i] >= 0) {
_num_accel_instances = i+1;
_accel_instance[i] = _imu.register_accel();
}
if (_gyro_fd[i] >= 0) {
_num_gyro_instances = i+1;
_gyro_instance[i] = _imu.register_gyro();
}
}
if (_num_accel_instances == 0) {
@ -122,7 +120,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
if (samplerate < 100 || samplerate > 10000) {
hal.scheduler->panic("Invalid gyro sample rate");
}
_set_gyro_raw_sample_rate(_gyro_instance[i], (uint32_t)samplerate);
_gyro_instance[i] = _imu.register_gyro(samplerate);
_gyro_sample_time[i] = 1.0f / samplerate;
}
@ -162,7 +160,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
if (samplerate < 100 || samplerate > 10000) {
hal.scheduler->panic("Invalid accel sample rate");
}
_set_accel_raw_sample_rate(_accel_instance[i], (uint32_t) samplerate);
_accel_instance[i] = _imu.register_accel(samplerate);
_accel_sample_time[i] = 1.0f / samplerate;
}
@ -209,33 +207,11 @@ bool AP_InertialSensor_PX4::update(void)
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) {
Vector3f accel = _accel_in[k];
// calling _publish_accel sets the sensor healthy,
// so we only want to do this if we have new data from it
if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
_publish_accel(_accel_instance[k], accel);
_last_accel_update_timestamp[k] = _last_accel_timestamp[k];
}
update_accel(_accel_instance[k]);
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
Vector3f gyro = _gyro_in[k];
// calling _publish_accel sets the sensor healthy,
// so we only want to do this if we have new data from it
if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) {
_publish_gyro(_gyro_instance[k], gyro);
_last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
}
}
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_set_accel_filter_frequency(_accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_set_gyro_filter_frequency(_gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
update_gyro(_gyro_instance[k]);
}
return true;
@ -250,9 +226,6 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
_rotate_and_correct_accel(frontend_instance, accel);
_notify_new_accel_raw_sample(frontend_instance, accel);
// apply filter for control path
_accel_in[i] = _accel_filter[i].apply(accel);
// save last timestamp
_last_accel_timestamp[i] = accel_report.timestamp;
@ -291,9 +264,6 @@ void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report
_rotate_and_correct_gyro(frontend_instance, gyro);
_notify_new_gyro_raw_sample(frontend_instance, gyro);
// apply filter for control path
_gyro_in[i] = _gyro_filter[i].apply(gyro);
// save last timestamp
_last_gyro_timestamp[i] = gyro_report.timestamp;

13
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -34,8 +34,6 @@ private:
bool _init_sensor(void);
void _get_sample(void);
bool _sample_available(void);
Vector3f _accel_in[INS_MAX_INSTANCES];
Vector3f _gyro_in[INS_MAX_INSTANCES];
uint64_t _last_accel_timestamp[INS_MAX_INSTANCES];
uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES];
uint64_t _last_accel_update_timestamp[INS_MAX_INSTANCES];
@ -54,13 +52,6 @@ private:
// calculate right queue depth for a sensor
uint8_t _queue_depth(uint16_t sensor_sample_rate) const;
// support for updating filter at runtime (-1 means unset)
int8_t _last_gyro_filter_hz;
int8_t _last_accel_filter_hz;
void _set_gyro_filter_frequency(uint8_t filter_hz);
void _set_accel_filter_frequency(uint8_t filter_hz);
// accelerometer and gyro driver handles
uint8_t _num_accel_instances;
uint8_t _num_gyro_instances;
@ -73,10 +64,6 @@ private:
uint8_t _accel_instance[INS_MAX_INSTANCES];
uint8_t _gyro_instance[INS_MAX_INSTANCES];
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter[INS_MAX_INSTANCES];
LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
#ifdef AP_INERTIALSENSOR_PX4_DEBUG
uint32_t _gyro_meas_count[INS_MAX_INSTANCES];
uint32_t _accel_meas_count[INS_MAX_INSTANCES];