forked from Archive/PX4-Autopilot
df_mpu9250_wrapper: publish imu based on integrator dt, not fixed count
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
parent
f3c12bc7c7
commit
835275f1a4
|
@ -75,7 +75,7 @@
|
|||
#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
|
||||
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
|
||||
|
||||
#define MPU9250_PUB_RATE 250
|
||||
#define MPU9250_PUB_RATE 280
|
||||
|
||||
|
||||
extern "C" { __EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[]); }
|
||||
|
@ -165,8 +165,6 @@ private:
|
|||
math::LowPassFilter2p _gyro_filter_y;
|
||||
math::LowPassFilter2p _gyro_filter_z;
|
||||
|
||||
unsigned _publish_count;
|
||||
|
||||
perf_counter_t _read_counter;
|
||||
perf_counter_t _error_counter;
|
||||
perf_counter_t _fifo_overflow_counter;
|
||||
|
@ -205,7 +203,6 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation) :
|
|||
_gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
|
||||
_gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
|
||||
_gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
|
||||
_publish_count(0),
|
||||
_read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")),
|
||||
_error_counter(perf_alloc(PC_COUNT, "mpu9250_errors")),
|
||||
_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_overflows")),
|
||||
|
@ -680,27 +677,19 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
|
|||
matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
|
||||
matrix::Vector3f gval_integrated;
|
||||
|
||||
_gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
|
||||
bool sensor_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
|
||||
gyro_report.x_integral = gval_integrated(0);
|
||||
gyro_report.y_integral = gval_integrated(1);
|
||||
gyro_report.z_integral = gval_integrated(2);
|
||||
|
||||
// If we are not receiving the last sample from the FIFO buffer yet, let's stop here
|
||||
// and wait for more packets.
|
||||
if (!data.is_last_fifo_sample) {
|
||||
|
||||
// if gyro integrator did not return a sample we can return here
|
||||
// Note: the accel integrator receives the same timestamp as the gyro integrator
|
||||
// so we do not need to handle it seperately
|
||||
if (!sensor_notify) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
|
||||
// Therefore, only publish every forth time.
|
||||
++_publish_count;
|
||||
|
||||
if (_publish_count < 4) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
_publish_count = 0;
|
||||
|
||||
// Update all the counters.
|
||||
perf_set_count(_read_counter, data.read_counter);
|
||||
perf_set_count(_error_counter, data.error_counter);
|
||||
|
@ -750,7 +739,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
|
|||
}
|
||||
|
||||
// TODO: when is this ever blocked?
|
||||
if (!(m_pub_blocked)) {
|
||||
if (!(m_pub_blocked) && sensor_notify) {
|
||||
|
||||
if (_gyro_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
|
||||
|
|
Loading…
Reference in New Issue