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

AP_InertialSensor: switch PX4 driver to fixed time per sample model

Browse Source 
this makes the driver much simpler, and does away with the need for an
accumulate function
This commit is contained in:
Andrew Tridgell 2013-08-06 16:31:18 +10:00
parent 1137de1002
commit e5ad9dbd15
2 changed files with 36 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -13,37 +13,28 @@ const extern AP_HAL::HAL& hal;
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
Vector3f AP_InertialSensor_PX4::_accel_in;
uint32_t AP_InertialSensor_PX4::_accel_count;
Vector3f AP_InertialSensor_PX4::_gyro_in;
uint32_t AP_InertialSensor_PX4::_gyro_count;
volatile bool AP_InertialSensor_PX4::_in_accumulate;
uint64_t AP_InertialSensor_PX4::_last_accel_timestamp;
uint64_t AP_InertialSensor_PX4::_last_gyro_timestamp;
int AP_InertialSensor_PX4::_accel_fd;
int AP_InertialSensor_PX4::_gyro_fd;
#include <drivers/drv_hrt.h>
uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
{
uint32_t msec_per_sample;
switch (sample_rate) {
case RATE_50HZ:
msec_per_sample = 20;
_default_filter_hz = 15;
_sample_time_usec = 20000;
break;
case RATE_100HZ:
msec_per_sample = 10;
_default_filter_hz = 30;
_sample_time_usec = 10000;
break;
case RATE_200HZ:
default:
msec_per_sample = 5;
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
}
_delta_time = _sample_time_usec * 1.0e-6f;
// init accelerometers
_accel_fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (_accel_fd < 0) {
@ -69,15 +60,10 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
ioctl(_gyro_fd, GYROIOCSSAMPLERATE, driver_rate);
ioctl(_gyro_fd, SENSORIOCSPOLLRATE, driver_rate);
// ask for a 20 sample buffer
ioctl(_accel_fd, SENSORIOCSQUEUEDEPTH, 20);
ioctl(_gyro_fd, SENSORIOCSQUEUEDEPTH, 20);
// setup sample divider
_sample_divider = (driver_rate * msec_per_sample) / 1000;
// register a 1kHz timer to read from PX4 sensor drivers
hal.scheduler->register_timer_process(_ins_timer);
// ask for a 1 sample buffer. We're really only interested in the
// latest sample as the driver is doing the filtering for us
ioctl(_accel_fd, SENSORIOCSQUEUEDEPTH, 1);
ioctl(_gyro_fd, SENSORIOCSQUEUEDEPTH, 1);
_set_filter_frequency(_mpu6000_filter);
@ -100,25 +86,13 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
bool AP_InertialSensor_PX4::update(void)
{
while (num_samples_available() == 0) {
hal.scheduler->delay(1);
}
Vector3f accel_scale = _accel_scale.get();
hal.scheduler->suspend_timer_procs();
// base the time on the gyro timestamp, as that is what is
// multiplied by time to integrate in DCM
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
_last_update_usec = _last_gyro_timestamp;
// get the latest sample from the sensor drivers
_get_sample();
_accel = _accel_in;
_accel_count = 0;
_gyro = _gyro_in;
_gyro_count = 0;
hal.scheduler->resume_timer_procs();
_gyro = _gyro_in;
// add offsets and rotation
_accel.rotate(_board_orientation);
@ -135,61 +109,48 @@ bool AP_InertialSensor_PX4::update(void)
_last_filter_hz = _mpu6000_filter;
}
_num_samples_available = 0;
return true;
}
float AP_InertialSensor_PX4::get_delta_time(void)
float AP_InertialSensor_PX4::get_delta_time(void)
{
return _delta_time;
}
uint32_t AP_InertialSensor_PX4::get_last_sample_time_micros(void)
{
return _last_update_usec;
}
float AP_InertialSensor_PX4::get_gyro_drift_rate(void)
{
// 0.5 degrees/second/minute
return ToRad(0.5/60);
}
void AP_InertialSensor_PX4::_accumulate(void)
void AP_InertialSensor_PX4::_get_sample(void)
{
struct accel_report accel_report;
struct gyro_report gyro_report;
if (_in_accumulate) {
return;
}
_in_accumulate = true;
while (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp) {
_accel_in = Vector3f(accel_report.x, accel_report.y, accel_report.z);
_accel_count++;
_last_accel_timestamp = accel_report.timestamp;
}
while (::read(_gyro_fd, &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
gyro_report.timestamp != _last_gyro_timestamp) {
_gyro_in = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
_gyro_count++;
_last_gyro_timestamp = gyro_report.timestamp;
}
_in_accumulate = false;
}
void AP_InertialSensor_PX4::_ins_timer(uint32_t now)
{
_accumulate();
}
uint16_t AP_InertialSensor_PX4::num_samples_available(void)
{
_accumulate();
return min(_accel_count, _gyro_count) / _sample_divider;
uint64_t tnow = hrt_absolute_time();
if (tnow - _last_sample_timestamp > _sample_time_usec) {
_num_samples_available++;
_last_sample_timestamp = tnow;
}
return _num_samples_available;
}
#endif // CONFIG_HAL_BOARD

29
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -22,24 +22,21 @@ public:
/* Concrete implementation of AP_InertialSensor functions: */
bool update();
float get_delta_time();
uint32_t get_last_sample_time_micros();
float get_gyro_drift_rate();
uint16_t num_samples_available();
private:
uint16_t _init_sensor( Sample_rate sample_rate );
static void _ins_timer(uint32_t now);
static void _accumulate(void);
uint64_t _last_update_usec;
float _delta_time;
static Vector3f _accel_in;
static uint32_t _accel_count;
static Vector3f _gyro_in;
static uint32_t _gyro_count;
static volatile bool _in_accumulate;
static uint64_t _last_accel_timestamp;
static uint64_t _last_gyro_timestamp;
uint8_t _sample_divider;
uint16_t _init_sensor( Sample_rate sample_rate );
void _get_sample(void);
uint64_t _last_update_usec;
float _delta_time;
Vector3f _accel_in;
Vector3f _gyro_in;
uint64_t _last_accel_timestamp;
uint64_t _last_gyro_timestamp;
uint64_t _last_sample_timestamp;
uint16_t _num_samples_available;
uint32_t _sample_time_usec;
// support for updating filter at runtime
uint8_t _last_filter_hz;
@ -48,8 +45,8 @@ private:
void _set_filter_frequency(uint8_t filter_hz);
// accelerometer and gyro driver handles
static int _accel_fd;
static int _gyro_fd;
int _accel_fd;
int _gyro_fd;
};
#endif
#endif // __AP_INERTIAL_SENSOR_PX4_H__