fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 23:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

rotate accel/gyro FIFO before publish and fix angular velocity filter resets

Browse Source 
- rotates accel & gyro FIFO data before publication both to simplify downstream usage (including log review) and fix other issues
     - to best handle int16_t data rotations are now either performed with swaps if possible, otherwise promoted to float, rotated using the full rotation matrix, then rounded back to int16_t
 - fix sensors/vehicle_angular_velocity filter reset both with proper rotation and new calibration uncorrect helper
      - in FIFO case filtering is done before calibration is applied, but we need to handle a possible reset from a completely different sensor (vehicle body angular velocity -> sensor frame uncorrected data)
This commit is contained in:
Daniel Agar
2021-03-22 12:01:12 -04:00
committed by GitHub
parent d4dd019578
commit e57aaaaa5e
13 changed files with 419 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+56 -65
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -40,7 +40,7 @@
using namespace time_literals;
using matrix::Vector3f;
static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
static constexpr int32_t sum(const int16_t samples[], uint8_t len)
{
int32_t sum = 0;
@@ -51,7 +51,7 @@ static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
return sum;
}
static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
static constexpr uint8_t clipping(const int16_t samples[], int16_t clip_limit, uint8_t len)
{
unsigned clip_count = 0;
@@ -94,67 +94,11 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
}
void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
{
// clipping
uint8_t clip_count[3];
clip_count[0] = (fabsf(x) >= _clip_limit);
clip_count[1] = (fabsf(y) >= _clip_limit);
clip_count[2] = (fabsf(z) >= _clip_limit);
// publish
Publish(timestamp_sample, x, y, z, clip_count);
}
void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
{
// publish fifo
sample.device_id = _device_id;
sample.scale = _scale;
sample.rotation = _rotation;
sample.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(sample);
{
// trapezoidal integration (equally spaced, scaled by dt later)
const uint8_t N = sample.samples;
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)),
};
_last_sample[0] = sample.x[N - 1];
_last_sample[1] = sample.y[N - 1];
_last_sample[2] = sample.z[N - 1];
// clipping
uint8_t clip_count[3] {
clipping(sample.x, _clip_limit, N),
clipping(sample.y, _clip_limit, N),
clipping(sample.z, _clip_limit, N),
};
const float x = integral(0) / (float)N;
const float y = integral(1) / (float)N;
const float z = integral(2) / (float)N;
// publish
Publish(sample.timestamp_sample, x, y, z, clip_count, N);
}
}
void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3],
uint8_t samples)
{
// Apply rotation (before scaling)
rotate_3f(_rotation, x, y, z);
float clipping_x = clip_count[0];
float clipping_y = clip_count[1];
float clipping_z = clip_count[2];
rotate_3f(_rotation, clipping_x, clipping_y, clipping_z);
// publish
sensor_accel_s report;
report.timestamp_sample = timestamp_sample;
@@ -164,10 +108,57 @@ void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, flo
report.x = x * _scale;
report.y = y * _scale;
report.z = z * _scale;
report.clip_counter[0] = fabsf(roundf(clipping_x));
report.clip_counter[1] = fabsf(roundf(clipping_y));
report.clip_counter[2] = fabsf(roundf(clipping_z));
report.samples = samples;
report.clip_counter[0] = (fabsf(x) >= _clip_limit);
report.clip_counter[1] = (fabsf(y) >= _clip_limit);
report.clip_counter[2] = (fabsf(z) >= _clip_limit);
report.samples = 1;
report.timestamp = hrt_absolute_time();
_sensor_pub.publish(report);
}
void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
{
// rotate all raw samples and publish fifo
const uint8_t N = sample.samples;
for (int n = 0; n < N; n++) {
rotate_3i(_rotation, sample.x[n], sample.y[n], sample.z[n]);
}
sample.device_id = _device_id;
sample.scale = _scale;
sample.timestamp = hrt_absolute_time();
_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)),
};
_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 / (float)N;
// publish
sensor_accel_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.clip_counter[0] = clipping(sample.x, _clip_limit, N);
report.clip_counter[1] = clipping(sample.y, _clip_limit, N);
report.clip_counter[2] = clipping(sample.z, _clip_limit, N);
report.samples = N;
report.timestamp = hrt_absolute_time();
_sensor_pub.publish(report);
@@ -176,5 +167,5 @@ void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, flo
void PX4Accelerometer::UpdateClipLimit()
{
// 99.9% of potential max
_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
_clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX);
}