Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added timer for accumulating samples for PX4

Browse Source 
this makes the driver much more tolerant of sketch timing errors
This commit is contained in:
Andrew Tridgell 2013-01-21 21:51:32 +11:00
parent 3d0cb755d2
commit f60d657f72
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -19,7 +19,8 @@ uint32_t AP_InertialSensor_PX4::_accel_sum_count;
Vector3f AP_InertialSensor_PX4::_gyro_sum; Vector3f AP_InertialSensor_PX4::_gyro_sum;
uint32_t AP_InertialSensor_PX4::_gyro_sum_count; uint32_t AP_InertialSensor_PX4::_gyro_sum_count;
volatile bool AP_InertialSensor_PX4::_in_accumulate; volatile bool AP_InertialSensor_PX4::_in_accumulate;
uint64_t AP_InertialSensor_PX4::_last_timestamp; 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::_accel_fd;
int AP_InertialSensor_PX4::_gyro_fd; int AP_InertialSensor_PX4::_gyro_fd;
@ -80,10 +81,10 @@ bool AP_InertialSensor_PX4::update(void)
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
// base the time on the number of gyro samples, as that is what is // base the time on the gyro timestamp, as that is what is
// multiplied by time to integrate in DCM // multiplied by time to integrate in DCM
_delta_time = _gyro_sum_count / 200.0; _delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
_last_update_usec = (uint32_t)_last_timestamp; _last_update_usec = _last_gyro_timestamp;
_accel = _accel_sum / _accel_sum_count; _accel = _accel_sum / _accel_sum_count;
_accel_sum.zero(); _accel_sum.zero();
@ -145,16 +146,20 @@ void AP_InertialSensor_PX4::_accumulate(void)
} }
_in_accumulate = true; _in_accumulate = true;
if (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report)) { if (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp) {
_accel_sum += Vector3f(accel_report.x, accel_report.y, accel_report.z); _accel_sum += Vector3f(accel_report.x, accel_report.y, accel_report.z);
_accel_sum_count++; _accel_sum_count++;
_last_accel_timestamp = accel_report.timestamp;
} }
if (::read(_gyro_fd, &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report)) { if (::read(_gyro_fd, &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
gyro_report.timestamp != _last_gyro_timestamp) {
_gyro_sum += Vector3f(gyro_report.x, gyro_report.y, gyro_report.z); _gyro_sum += Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
_gyro_sum_count++; _gyro_sum_count++;
_last_timestamp = gyro_report.timestamp; _last_gyro_timestamp = gyro_report.timestamp;
} }
_in_accumulate = false; _in_accumulate = false;
} }

5
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -32,14 +32,15 @@ private:
uint16_t _init_sensor( Sample_rate sample_rate ); uint16_t _init_sensor( Sample_rate sample_rate );
static void _ins_timer(uint32_t now); static void _ins_timer(uint32_t now);
static void _accumulate(void); static void _accumulate(void);
uint32_t _last_update_usec; uint64_t _last_update_usec;
float _delta_time; float _delta_time;
static Vector3f _accel_sum; static Vector3f _accel_sum;
static uint32_t _accel_sum_count; static uint32_t _accel_sum_count;
static Vector3f _gyro_sum; static Vector3f _gyro_sum;
static uint32_t _gyro_sum_count; static uint32_t _gyro_sum_count;
static volatile bool _in_accumulate; static volatile bool _in_accumulate;
static uint64_t _last_timestamp; static uint64_t _last_accel_timestamp;
static uint64_t _last_gyro_timestamp;
uint8_t _sample_divider; uint8_t _sample_divider;
// accelerometer and gyro driver handles // accelerometer and gyro driver handles