Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: removed "have sample" logic from drivers

Browse Source 
the frontend has all the information it needs, so we can simplify the
drivers some more
This commit is contained in:
Andrew Tridgell 2015-11-16 11:38:43 +11:00
parent 0e4bab74ba
commit 75ea8f3dc0
17 changed files with 22 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1394,8 +1394,11 @@ check_sample:
bool accel_available = false;
while (!gyro_available || !accel_available) {
for (uint8_t i=0; i<_backend_count; i++) {
gyro_available |= _backends[i]->gyro_sample_available();
accel_available |= _backends[i]->accel_sample_available();
_backends[i]->accumulate();
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
gyro_available |= _new_gyro_data[i];
accel_available |= _new_accel_data[i];
}
if (!gyro_available || !accel_available) {
hal.scheduler->delay_microseconds(100);

11
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -44,16 +44,9 @@ public:
virtual bool update() = 0;
/*
* return true if at least one accel sample is available in the backend
* since the last call to update()
* optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples
*/
virtual bool accel_sample_available() = 0;
/*
* return true if at least one gyro sample is available in the backend
* since the last call to update()
*/
virtual bool gyro_sample_available() = 0;
virtual void accumulate() {}
/*
* Configure and start all sensors. The empty implementation allows

2
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -186,7 +186,7 @@ bool AP_InertialSensor_Flymaple::update(void)
// operations take too long
// So we are stuck with a suboptimal solution. The results are not so
// good in terms of timing. It may be better with the FIFOs enabled
void AP_InertialSensor_Flymaple::_accumulate(void)
void AP_InertialSensor_Flymaple::accumulate(void)
{
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();

5
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -18,15 +18,14 @@ public:
/* update accel and gyro state */
bool update();
bool gyro_sample_available(void) { _accumulate(); return _have_gyro_sample; }
bool accel_sample_available(void) { _accumulate(); return _have_accel_sample; }
// accumulate samples
void accumulate(void) override;
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private:
bool _init_sensor(void);
void _accumulate(void);
bool _have_gyro_sample;
bool _have_accel_sample;

3
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -13,9 +13,6 @@ public:
/* update accel and gyro state */
bool update();
bool gyro_sample_available(void) { return true; }
bool accel_sample_available(void) { return true; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);

24
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -114,17 +114,13 @@ const extern AP_HAL::HAL& hal;
// constructor
AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_data_idx(0),
_have_gyro_sample(false),
_have_accel_sample(false),
_have_sample(false)
_have_accel_sample(false)
{
pthread_spin_init(&_data_lock, PTHREAD_PROCESS_PRIVATE);
}
AP_InertialSensor_L3G4200D::~AP_InertialSensor_L3G4200D()
{
pthread_spin_destroy(&_data_lock);
}
/*
@ -232,14 +228,13 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
// 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(800);
_accel_instance = _imu.register_accel(800);
_product_id = AP_PRODUCT_ID_L3G4200D;
// start the timer process to read samples
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, void));
return true;
}
@ -248,15 +243,9 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
*/
bool AP_InertialSensor_L3G4200D::update(void)
{
pthread_spin_lock(&_data_lock);
unsigned int idx = !_data_idx;
update_gyro(_gyro_instance);
update_accel(_accel_instance);
_have_sample = false;
pthread_spin_unlock(&_data_lock);
return true;
}
@ -335,11 +324,6 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
if (_have_accel_sample && _have_gyro_sample) {
_have_gyro_sample = false;
_have_accel_sample = false;
pthread_spin_lock(&_data_lock);
_data_idx = !_data_idx;
_have_sample = true;
pthread_spin_unlock(&_data_lock);
}
}

7
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -22,9 +22,6 @@ public:
/* update accel and gyro state */
bool update();
bool gyro_sample_available(void) { return _have_sample; }
bool accel_sample_available(void) { return _have_sample; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
@ -35,12 +32,8 @@ private:
bool _init_sensor(void);
void _accumulate(void);
int _data_idx;
pthread_spinlock_t _data_lock;
bool _have_gyro_sample;
bool _have_accel_sample;
volatile bool _have_sample;
// gyro and accel instances
uint8_t _gyro_instance;

9
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -376,8 +376,6 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
AP_InertialSensor_Backend(imu),
_drdy_pin_a(NULL),
_drdy_pin_g(NULL),
_gyro_sample_available(false),
_accel_sample_available(false),
_drdy_pin_num_a(drdy_pin_num_a),
_drdy_pin_num_g(drdy_pin_num_g)
{
@ -736,9 +734,9 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
Vector3f accel_data(raw_data.x, -raw_data.y, -raw_data.z);
accel_data *= _accel_scale;
_rotate_and_correct_accel(_accel_instance, accel_data);
_notify_new_accel_raw_sample(_accel_instance, accel_data);
_accel_sample_available = true;
}
/*
@ -751,16 +749,13 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
Vector3f gyro_data(raw_data.x, -raw_data.y, -raw_data.z);
gyro_data *= _gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, gyro_data);
_notify_new_gyro_raw_sample(_gyro_instance, gyro_data);
_gyro_sample_available = true;
}
bool AP_InertialSensor_LSM9DS0::update()
{
_accel_sample_available = false;
_gyro_sample_available = false;
update_gyro(_gyro_instance);
update_accel(_accel_instance);

11
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
View File

@ -32,14 +32,6 @@ public:
bool update();
bool gyro_sample_available() {
return _gyro_sample_available;
};
bool accel_sample_available() {
return _accel_sample_available;
};
static AP_InertialSensor_Backend * detect(AP_InertialSensor &imu);
private:
@ -64,9 +56,6 @@ private:
int _drdy_pin_num_g;
AP_HAL::DigitalSource * _drdy_pin_g;
bool _gyro_sample_available;
bool _accel_sample_available;
bool _accel_data_ready();
bool _gyro_data_ready();

2
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -537,8 +537,6 @@ void AP_InertialSensor_MPU6000::start()
_samples = new uint8_t[max_samples * MPU6000_SAMPLE_SIZE];
hal.scheduler->delay(1);
_sample_count = 1;
// disable sensor filtering
_set_filter_register(256);

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -63,9 +63,6 @@ public:
/* update accel and gyro state */
bool update();
bool gyro_sample_available(void) { return _sum_count >= _sample_count; }
bool accel_sample_available(void) { return _sum_count >= _sample_count; }
/*
* Return an AuxiliaryBus if the bus driver allows it
*/
@ -88,7 +85,6 @@ private:
AP_HAL::DigitalSource *_drdy_pin;
bool _init_sensor(void);
bool _sample_available();
void _read_data_transaction();
bool _data_ready();
void _poll_data(void);
@ -108,14 +104,10 @@ private:
void _set_filter_register(uint16_t filter_hz);
// how many hardware samples before we report a sample to the caller
uint8_t _sample_count;
float _temp_filtered;
LowPassFilter2pFloat _temp_filter;
volatile uint16_t _sum_count;
bool _fifo_mode;
uint8_t *_samples = nullptr;
};

6
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -325,8 +325,7 @@ static struct gyro_state_s st = {
* @brief Constructor
*/
AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_have_sample_available(false)
AP_InertialSensor_Backend(imu)
{
}
@ -1076,8 +1075,6 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
gyro *= MPU9150_GYRO_SCALE_2000;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_have_sample_available = true;
}
// give back i2c semaphore
@ -1086,7 +1083,6 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
bool AP_InertialSensor_MPU9150::update(void)
{
_have_sample_available = false;
update_gyro(_gyro_instance);
update_accel(_accel_instance);

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
View File

@ -19,16 +19,12 @@ public:
/* update accel and gyro state */
bool update();
bool gyro_sample_available(void) { return _have_sample_available; }
bool accel_sample_available(void) { return _have_sample_available; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private:
bool _init_sensor();
void _accumulate(void);
bool _have_sample_available;
int16_t mpu_set_gyro_fsr(uint16_t fsr);
int16_t mpu_set_accel_fsr(uint8_t fsr);

6
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -362,7 +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),
_have_sample_available(false),
_bus(bus),
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
_default_rotation(ROTATION_ROLL_180_YAW_270)
@ -451,8 +450,6 @@ bool AP_InertialSensor_MPU9250::_init_sensor()
*/
bool AP_InertialSensor_MPU9250::update( void )
{
_have_sample_available = false;
update_gyro(_gyro_instance);
update_accel(_accel_instance);
@ -508,10 +505,9 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
-int16_val(rx, 6));
gyro *= GYRO_SCALE;
gyro.rotate(_default_rotation);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_have_sample_available = true;
}
/*

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -44,9 +44,6 @@ public:
/* update accel and gyro state */
bool update();
bool gyro_sample_available(void) { return _have_sample_available; }
bool accel_sample_available(void) { return _have_sample_available; }
AuxiliaryBus *get_auxiliary_bus();
static AP_InertialSensor_MPU9250 &from(AP_InertialSensor_Backend &backend) {
@ -71,15 +68,11 @@ private:
uint8_t _register_read( uint8_t reg );
void _register_write( uint8_t reg, uint8_t val );
bool _hardware_init(void);
bool _sample_available();
AP_MPU9250_BusDriver *_bus;
AP_HAL::Semaphore *_bus_sem;
AP_MPU9250_AuxiliaryBus *_auxiliar_bus = nullptr;
// do we currently have a sample pending?
bool _have_sample_available;
// gyro and accel instances
uint8_t _gyro_instance;
uint8_t _accel_instance;

51
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -23,9 +23,7 @@ const extern AP_HAL::HAL& hal;
AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_last_get_sample_timestamp(0),
_last_sample_timestamp(0),
_last_gyro_filter_hz(-1),
_last_accel_filter_hz(-1)
_last_sample_timestamp(0)
{
}
@ -164,9 +162,6 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
_accel_sample_time[i] = 1.0f / samplerate;
}
_set_accel_filter_frequency(_accel_filter_cutoff());
_set_gyro_filter_frequency(_gyro_filter_cutoff());
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
_product_id = AP_PRODUCT_ID_VRBRAIN;
#else
@ -179,28 +174,6 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
return true;
}
/*
set the accel filter frequency
*/
void AP_InertialSensor_PX4::_set_accel_filter_frequency(uint8_t filter_hz)
{
for (uint8_t i=0; i<_num_accel_instances; i++) {
float samplerate = _accel_raw_sample_rate(_accel_instance[i]);
_accel_filter[i].set_cutoff_frequency(samplerate, filter_hz);
}
}
/*
set the gyro filter frequency
*/
void AP_InertialSensor_PX4::_set_gyro_filter_frequency(uint8_t filter_hz)
{
for (uint8_t i=0; i<_num_gyro_instances; i++) {
float samplerate = _gyro_raw_sample_rate(_gyro_instance[i]);
_gyro_filter[i].set_cutoff_frequency(samplerate, filter_hz);
}
}
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
@ -369,27 +342,5 @@ bool AP_InertialSensor_PX4::_get_gyro_sample(uint8_t i, struct gyro_report &gyro
return false;
}
bool AP_InertialSensor_PX4::gyro_sample_available(void)
{
_get_sample();
for (uint8_t i=0; i<_num_gyro_instances; i++) {
if (_last_gyro_timestamp[i] != _last_gyro_update_timestamp[i]) {
return true;
}
}
return false;
}
bool AP_InertialSensor_PX4::accel_sample_available(void)
{
_get_sample();
for (uint8_t i=0; i<_num_accel_instances; i++) {
if (_last_accel_timestamp[i] != _last_accel_update_timestamp[i]) {
return true;
}
}
return false;
}
#endif // CONFIG_HAL_BOARD

5
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -27,13 +27,12 @@ public:
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
bool gyro_sample_available(void);
bool accel_sample_available(void);
// accumulate more samples
void accumulate(void) override { _get_sample(); }
private:
bool _init_sensor(void);
void _get_sample(void);
bool _sample_available(void);
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];