fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 00:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

lib/drivers/{accelerometer,gyroscope} skip obselete Vector3f construction

Browse Source 
- FIFO clip count only check if value is INT16_MIN/MAX rather than abs() call
This commit is contained in:
Daniel Agar
2021-10-08 17:33:27 -04:00
parent fd39d5b9a1
commit 089f96f800
2 changed files with 28 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+12 -19
View File
@@ -38,7 +38,6 @@
#include <lib/parameters/param.h>
using namespace time_literals;
using matrix::Vector3f;
static constexpr int32_t sum(const int16_t samples[], uint8_t len)
{
@@ -129,29 +128,23 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
_sensor_fifo_pub.publish(sample);
// trapezoidal integration (equally spaced, scaled by dt later)
const Vector3f integral{
(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
};
// publish
sensor_gyro_s report;
report.timestamp_sample = sample.timestamp_sample;
report.device_id = _device_id;
report.temperature = _temperature;
report.error_count = _error_count;
// trapezoidal integration (equally spaced)
const float scale = _scale / (float)N;
report.x = (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)) * scale;
report.y = (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)) * scale;
report.z = (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)) * scale;
_last_sample[0] = sample.x[N - 1];
_last_sample[1] = sample.y[N - 1];
_last_sample[2] = sample.z[N - 1];
const float scale = _scale / N;
sensor_gyro_s report;
report.timestamp_sample = sample.timestamp_sample;
report.device_id = _device_id;
report.temperature = _temperature;
report.error_count = _error_count;
report.x = integral(0) * scale;
report.y = integral(1) * scale;
report.z = integral(2) * scale;
report.samples = N;
report.timestamp = hrt_absolute_time();