dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

lib/drivers/{accelerometer,gyroscope}: ensure sane monotonically increasing timestamp_sample

This commit is contained in:
Daniel Agar 2021-10-13 14:41:36 -04:00
parent e8a064af02
commit 7f99cf5f49
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
4 changed files with 24 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

@ -113,10 +113,11 @@ void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, floa
// Apply rotation (before scaling)
rotate_3f(_rotation, x, y, z);
// publish
sensor_accel_s report;
report.timestamp_sample = timestamp_sample;
// ensure monotonically increasing timestamp_sample within reasonable range
report.timestamp_sample = math::constrain(timestamp_sample, _timestamp_prev, hrt_absolute_time());
report.device_id = _device_id;
report.temperature = _temperature;
report.error_count = _error_count;
@ -130,10 +131,15 @@ void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, floa
report.timestamp = hrt_absolute_time();
_sensor_pub.publish(report);
_timestamp_prev = report.timestamp;
}
void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
{
// ensure monotonically increasing timestamp_sample within reasonable range
sample.timestamp_sample = math::constrain(sample.timestamp_sample, _timestamp_prev, hrt_absolute_time());
// rotate all raw samples and publish fifo
const uint8_t N = sample.samples;
@ -146,6 +152,8 @@ void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
sample.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(sample);
_timestamp_prev = sample.timestamp;
// publish
sensor_accel_s report;

2
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

@ -70,6 +70,8 @@ private:
uORB::PublicationMulti<sensor_accel_s> _sensor_pub{ORB_ID(sensor_accel)};
uORB::PublicationMulti<sensor_accel_fifo_s> _sensor_fifo_pub{ORB_ID(sensor_accel_fifo)};
hrt_abstime _timestamp_prev{0};
uint32_t _device_id{0};
const enum Rotation _rotation;

11
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -100,7 +100,9 @@ void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y,
sensor_gyro_s report;
report.timestamp_sample = timestamp_sample;
// ensure monotonically increasing timestamp_sample within reasonable range
report.timestamp_sample = math::constrain(timestamp_sample, _timestamp_prev, hrt_absolute_time());
report.device_id = _device_id;
report.temperature = _temperature;
report.error_count = _error_count;
@ -111,10 +113,15 @@ void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y,
report.timestamp = hrt_absolute_time();
_sensor_pub.publish(report);
_timestamp_prev = report.timestamp;
}
void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
{
// ensure monotonically increasing timestamp_sample within reasonable range
sample.timestamp_sample = math::constrain(sample.timestamp_sample, _timestamp_prev, hrt_absolute_time());
// rotate all raw samples and publish fifo
const uint8_t N = sample.samples;
@ -127,6 +134,8 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
sample.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(sample);
_timestamp_prev = sample.timestamp;
// publish
sensor_gyro_s report;

3
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@ -1,4 +1,3 @@
/****************************************************************************
*
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
@ -68,6 +67,8 @@ private:
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};
hrt_abstime _timestamp_prev{0};
uint32_t _device_id{0};
const enum Rotation _rotation;