diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 8aa0865405..ee50968ca8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -23,6 +23,7 @@ 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; +bool AP_InertialSensor_PX4::_do_averaging; 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 ioctl(_accel_fd, SENSORIOCSQUEUEDEPTH, 10); ioctl(_gyro_fd, SENSORIOCSQUEUEDEPTH, 10); + _do_averaging = true; #else /* * 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_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 // 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; _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_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_count = 0; @@ -177,14 +191,22 @@ void AP_InertialSensor_PX4::_accumulate(void) while (::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); + 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++; _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_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++; _last_gyro_timestamp = gyro_report.timestamp; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index 26d2078c60..ef84f80743 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -40,6 +40,7 @@ private: static uint64_t _last_accel_timestamp; static uint64_t _last_gyro_timestamp; uint8_t _sample_divider; + static bool _do_averaging; // support for updating filter at runtime uint8_t _last_filter_hz;