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

AP_InertialSensor: disable averaging on FMUv2

Browse Source 
the new filter from Leonard means we shouldn't average values, and
instead just use the last value
This commit is contained in:
Andrew Tridgell 2013-08-04 16:11:17 +10:00
parent a0af499aa5
commit 46c5d18585
2 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -23,6 +23,7 @@ uint64_t AP_InertialSensor_PX4::_last_accel_timestamp;
uint64_t AP_InertialSensor_PX4::_last_gyro_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;
bool AP_InertialSensor_PX4::_do_averaging;
uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
{ {
@ -67,6 +68,7 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
// support this yet, but when it does we want to use it // support this yet, but when it does we want to use it
ioctl(_accel_fd, SENSORIOCSQUEUEDEPTH, 10); ioctl(_accel_fd, SENSORIOCSQUEUEDEPTH, 10);
ioctl(_gyro_fd, SENSORIOCSQUEUEDEPTH, 10); ioctl(_gyro_fd, SENSORIOCSQUEUEDEPTH, 10);
_do_averaging = true;
#else #else
/* /*
* set the accel and gyro sampling rate. Set a fixed rate of 800Hz * set the accel and gyro sampling rate. Set a fixed rate of 800Hz
@ -83,6 +85,10 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
// sample dividers above were setup for 200Hz // sample dividers above were setup for 200Hz
_sample_divider *= 4; _sample_divider *= 4;
// the NuttX driver uses a builtin low pass filter, so asking for
// the latest value is always the right one
_do_averaging = false;
#endif #endif
// register a 1kHz timer to read from PX4 sensor drivers // register a 1kHz timer to read from PX4 sensor drivers
@ -121,11 +127,19 @@ bool AP_InertialSensor_PX4::update(void)
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f; _delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
_last_update_usec = _last_gyro_timestamp; _last_update_usec = _last_gyro_timestamp;
_accel = _accel_sum / _accel_sum_count; if (_do_averaging) {
_accel = _accel_sum / _accel_sum_count;
} else {
_accel = _accel_sum;
}
_accel_sum.zero(); _accel_sum.zero();
_accel_sum_count = 0; _accel_sum_count = 0;
_gyro = _gyro_sum / _gyro_sum_count; if (_do_averaging) {
_gyro = _gyro_sum / _gyro_sum_count;
} else {
_gyro = _gyro_sum;
}
_gyro_sum.zero(); _gyro_sum.zero();
_gyro_sum_count = 0; _gyro_sum_count = 0;
@ -177,14 +191,22 @@ void AP_InertialSensor_PX4::_accumulate(void)
while (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report) && while (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp) { accel_report.timestamp != _last_accel_timestamp) {
_accel_sum += Vector3f(accel_report.x, accel_report.y, accel_report.z); if (_do_averaging) {
_accel_sum += Vector3f(accel_report.x, accel_report.y, accel_report.z);
} else {
_accel_sum = Vector3f(accel_report.x, accel_report.y, accel_report.z);
}
_accel_sum_count++; _accel_sum_count++;
_last_accel_timestamp = accel_report.timestamp; _last_accel_timestamp = accel_report.timestamp;
} }
while (::read(_gyro_fd, &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) && while (::read(_gyro_fd, &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
gyro_report.timestamp != _last_gyro_timestamp) { gyro_report.timestamp != _last_gyro_timestamp) {
_gyro_sum += Vector3f(gyro_report.x, gyro_report.y, gyro_report.z); if (_do_averaging) {
_gyro_sum += Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
} else {
_gyro_sum = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
}
_gyro_sum_count++; _gyro_sum_count++;
_last_gyro_timestamp = gyro_report.timestamp; _last_gyro_timestamp = gyro_report.timestamp;
} }

1
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -40,6 +40,7 @@ private:
static uint64_t _last_accel_timestamp; static uint64_t _last_accel_timestamp;
static uint64_t _last_gyro_timestamp; static uint64_t _last_gyro_timestamp;
uint8_t _sample_divider; uint8_t _sample_divider;
static bool _do_averaging;
// support for updating filter at runtime // support for updating filter at runtime
uint8_t _last_filter_hz; uint8_t _last_filter_hz;