Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: calculate queue depth based on requested sample rate 

this auto-scales the queue depth for plane, rover and copter
This commit is contained in:
Andrew Tridgell 2015-03-12 10:02:36 +11:00
parent c42a40553a
commit 5d0eb49114
5 changed files with 35 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -48,10 +48,10 @@ public:
// the rate that updates will be available to the application
enum Sample_rate {
RATE_50HZ,
RATE_100HZ,
RATE_200HZ,
RATE_400HZ
RATE_50HZ = 50,
RATE_100HZ = 100,
RATE_200HZ = 200,
RATE_400HZ = 400
};
/// Perform startup initialisation.

7
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -116,3 +116,10 @@ uint8_t AP_InertialSensor_Backend::_default_filter(void) const
}
return 30;
}
// return the requested sample rate in Hz
uint16_t AP_InertialSensor_Backend::get_sample_rate_hz(void) const
{
// enum can be directly cast to Hz
return (uint16_t)_imu._sample_rate;
}

3
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -85,6 +85,9 @@ protected:
// return the default filter frequency in Hz for the sample rate
uint8_t _default_filter(void) const;
// return the requested sample rate in Hz
uint16_t get_sample_rate_hz(void) const;
// 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

23
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -44,6 +44,19 @@ AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu
return sensor;
}
/*
calculate the right queue depth for a device with the given sensor
sample rate
*/
uint8_t AP_InertialSensor_PX4::_queue_depth(uint16_t sensor_sample_rate) const
{
uint16_t requested_sample_rate = get_sample_rate_hz();
uint8_t min_depth = (sensor_sample_rate+requested_sample_rate-1)/requested_sample_rate;
// add 5ms more worth of queue to account for possible timing jitter
uint8_t ret = min_depth + (5 * sensor_sample_rate) / 1000;
return ret;
}
bool AP_InertialSensor_PX4::_init_sensor(void)
{
// assumes max 3 instances
@ -88,8 +101,8 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
ioctl(fd, GYROIOCSHWLOWPASS, 256);
// khz sampling
ioctl(fd, GYROIOCSSAMPLERATE, 1000);
// 10ms queue depth
ioctl(fd, SENSORIOCSQUEUEDEPTH, 10);
// set queue depth
ioctl(fd, SENSORIOCSQUEUEDEPTH, _queue_depth(1000));
break;
case DRV_GYR_DEVTYPE_L3GD20:
// hardware LPF as high as possible
@ -97,7 +110,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
// ~khz sampling
ioctl(fd, GYROIOCSSAMPLERATE, 800);
// 10ms queue depth
ioctl(fd, SENSORIOCSQUEUEDEPTH, 8);
ioctl(fd, SENSORIOCSQUEUEDEPTH, _queue_depth(800));
break;
default:
break;
@ -120,7 +133,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
// khz sampling
ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
// 10ms queue depth
ioctl(fd, SENSORIOCSQUEUEDEPTH, 10);
ioctl(fd, SENSORIOCSQUEUEDEPTH, _queue_depth(1000));
break;
case DRV_ACC_DEVTYPE_LSM303D:
// hardware LPF to ~1/10th sample rate for antialiasing
@ -129,7 +142,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
ioctl(fd, ACCELIOCSSAMPLERATE, 1600);
ioctl(fd,SENSORIOCSPOLLRATE, 1600);
// 10ms queue depth
ioctl(fd, SENSORIOCSQUEUEDEPTH, 16);
ioctl(fd, SENSORIOCSQUEUEDEPTH, _queue_depth(1600));
break;
default:
break;

3
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -50,6 +50,9 @@ private:
bool _get_gyro_sample(uint8_t i, struct gyro_report &gyro_report);
bool _get_accel_sample(uint8_t i, struct accel_report &accel_report);
// calculate right queue depth for a sensor
uint8_t _queue_depth(uint16_t sensor_sample_rate) const;
// support for updating filter at runtime
uint8_t _last_filter_hz;