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

PX4Accelerometer/PX4Gyroscope update FIFO case to trapezoidal integration

Browse Source
This commit is contained in:
Daniel Agar
2020-01-03 13:55:00 -05:00
parent 697dbfb9f8
commit 009ba638f5
9 changed files with 215 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+75 -80
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2020 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
@@ -39,6 +39,30 @@
using namespace time_literals;
using matrix::Vector3f;
static inline int32_t sum(const int16_t samples[16], uint8_t len)
{
int32_t sum = 0;
for (int n = 0; n < len; n++) {
sum += samples[n];
}
return sum;
}
static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
{
unsigned clip_count = 0;
for (int n = 0; n < len; n++) {
if (abs(samples[n]) > clip_limit) {
clip_count++;
}
}
return clip_count;
}
PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
CDev(nullptr),
ModuleParams(nullptr),
@@ -47,7 +71,8 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
_sensor_status_pub{ORB_ID(sensor_accel_status), priority},
_device_id{device_id},
_rotation{rotation}
_rotation{rotation},
_rotation_dcm{get_rot_matrix(rotation)}
{
_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
@@ -119,10 +144,8 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
const Vector3f raw{x, y, z};
// Clipping (check unscaled raw values)
const float clip_limit = (_range / _scale) * 0.95f;
for (int i = 0; i < 3; i++) {
if (fabsf(raw(i)) > clip_limit) {
if (fabsf(raw(i)) > _clip_limit) {
_clipping[i]++;
_integrator_clipping++;
}
@@ -139,10 +162,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
Vector3f delta_velocity;
uint32_t integral_dt = 0;
if (_integrator_samples == 0) {
_integrator_timestamp_sample = timestamp_sample;
}
_integrator_samples++;
if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
@@ -165,7 +184,7 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
// fill sensor_accel_integrated and publish
sensor_accel_integrated_s report{};
report.timestamp_sample = _integrator_timestamp_sample;
report.timestamp_sample = timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
delta_velocity.copyTo(report.delta_velocity);
@@ -189,10 +208,13 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
{
const uint8_t N = sample.samples;
const float dt = sample.dt;
// filtered data (control)
float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
float y_filtered = _filterArrayY.apply(sample.y, sample.samples);
float z_filtered = _filterArrayZ.apply(sample.z, sample.samples);
float x_filtered = _filterArrayX.apply(sample.x, N);
float y_filtered = _filterArrayY.apply(sample.y, N);
float z_filtered = _filterArrayZ.apply(sample.z, N);
// Apply rotation (before scaling)
rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
@@ -200,65 +222,41 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
const Vector3f raw{x_filtered, y_filtered, z_filtered};
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
const Vector3f val_calibrated{((raw * _scale) - _calibration_offset).emult(_calibration_scale)};
// clipping
const int16_t clip_limit = (_range / _scale) * 0.95f;
unsigned clip_count_x = clipping(sample.x, _clip_limit, N);
unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
// x clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.x[n]) > clip_limit) {
_clipping[0]++;
_integrator_clipping++;
}
}
// y clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.y[n]) > clip_limit) {
_clipping[1]++;
_integrator_clipping++;
}
}
// z clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.z[n]) > clip_limit) {
_clipping[2]++;
_integrator_clipping++;
}
}
_clipping[0] += clip_count_x;
_clipping[1] += clip_count_y;
_clipping[2] += clip_count_z;
_integrator_clipping += clip_count_x + clip_count_y + clip_count_z;
// integrated data (INS)
{
// reset integrator if previous sample was too long ago
if ((sample.timestamp_sample > _timestamp_sample_prev)
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) {
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {
ResetIntegrator();
}
if (_integrator_samples == 0) {
_integrator_timestamp_sample = sample.timestamp_sample;
}
// integrate
_integrator_samples += 1;
_integrator_fifo_samples += sample.samples;
_integrator_fifo_samples += N;
for (int n = 0; n < sample.samples; n++) {
_integrator_accum[0] += sample.x[n];
}
// trapezoidal integration (equally spaced, scaled by dt later)
_integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1));
_integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1));
_integration_raw(2) += (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];
for (int n = 0; n < sample.samples; n++) {
_integrator_accum[1] += sample.y[n];
}
for (int n = 0; n < sample.samples; n++) {
_integrator_accum[2] += sample.z[n];
}
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
@@ -266,7 +264,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
{
sensor_accel_s report{};
report.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
report.timestamp_sample = sample.timestamp_sample;
report.device_id = _device_id;
report.temperature = _temperature;
report.x = val_calibrated(0);
@@ -277,31 +275,25 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
_sensor_pub.publish(report);
}
// Apply rotation and scale
// integrated in microseconds, convert to seconds
const Vector3f delta_velocity_uncalibrated{_rotation_dcm *_integration_raw * _scale};
const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
// scale calibration offset to number of samples
const Vector3f offset{_calibration_offset * _integrator_fifo_samples};
// average integrated values to apply calibration
float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples;
float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples;
float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples;
// Apply rotation (before scaling)
rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg);
const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
// Apply range scale and the calibrating offset/scale
Vector3f delta_velocity{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))};
delta_velocity *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore
// Apply calibration and scale to seconds
Vector3f delta_velocity{((delta_velocity_uncalibrated - offset).emult(_calibration_scale))};
delta_velocity *= 1e-6f * dt;
// fill sensor_accel_integrated and publish
sensor_accel_integrated_s report{};
report.timestamp_sample = _integrator_timestamp_sample;
report.timestamp_sample = sample.timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
delta_velocity.copyTo(report.delta_velocity);
report.dt = integrator_dt_us;
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
report.samples = _integrator_fifo_samples;
report.clip_count = _integrator_clipping;
@@ -323,13 +315,13 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
fifo.device_id = _device_id;
fifo.timestamp_sample = sample.timestamp_sample;
fifo.dt = sample.dt;
fifo.dt = dt;
fifo.scale = _scale;
fifo.samples = sample.samples;
fifo.samples = N;
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples);
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples);
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples);
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N);
fifo.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(fifo);
@@ -366,12 +358,9 @@ void PX4Accelerometer::ResetIntegrator()
{
_integrator_samples = 0;
_integrator_fifo_samples = 0;
_integrator_accum[0] = 0;
_integrator_accum[1] = 0;
_integrator_accum[2] = 0;
_integration_raw.zero();
_integrator_clipping = 0;
_integrator_timestamp_sample = 0;
_timestamp_sample_prev = 0;
}
@@ -384,6 +373,12 @@ void PX4Accelerometer::ConfigureFilter(float cutoff_freq)
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
}
void PX4Accelerometer::UpdateClipLimit()
{
// 95% of potential max
_clip_limit = (_range / _scale) * 0.95f;
}
void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
{
// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)