mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-31 10:26:52 +08:00
move IMU integration out of drivers to sensors hub to handle accel/gyro sync
- IMU integration move from drivers (PX4Accelerometer/PX4Gyroscope) to sensors/vehicle_imu - sensors: voted_sensors_update now consumes vehicle_imu - delete sensor_accel_integrated, sensor_gyro_integrated - merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status - sensors status output minor improvements (ordering, whitespace, show selected sensor device id and instance)
This commit is contained in:
Daniel Agar
GitHub
@@ -68,21 +68,14 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum R
|
||||
ModuleParams(nullptr),
|
||||
_sensor_pub{ORB_ID(sensor_accel), priority},
|
||||
_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
|
||||
_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
|
||||
_sensor_status_pub{ORB_ID(sensor_accel_status), priority},
|
||||
_device_id{device_id},
|
||||
_rotation{rotation}
|
||||
{
|
||||
// register class and advertise immediately to keep instance numbering in sync
|
||||
_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
|
||||
_sensor_pub.advertise();
|
||||
_sensor_integrated_pub.advertise();
|
||||
_sensor_status_pub.advertise();
|
||||
|
||||
updateParams();
|
||||
|
||||
// set reasonable default, driver should be setting real value
|
||||
set_update_rate(800);
|
||||
}
|
||||
|
||||
PX4Accelerometer::~PX4Accelerometer()
|
||||
@@ -93,8 +86,6 @@ PX4Accelerometer::~PX4Accelerometer()
|
||||
|
||||
_sensor_pub.unadvertise();
|
||||
_sensor_fifo_pub.unadvertise();
|
||||
_sensor_integrated_pub.unadvertise();
|
||||
_sensor_status_pub.unadvertise();
|
||||
}
|
||||
|
||||
int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
@@ -132,25 +123,6 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
|
||||
_device_id = device_id.devid;
|
||||
}
|
||||
|
||||
void PX4Accelerometer::set_update_rate(uint16_t rate)
|
||||
{
|
||||
_update_rate = math::constrain((int)rate, 50, 32000);
|
||||
|
||||
// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
|
||||
int32_t imu_integration_rate_hz = math::constrain(_param_imu_integ_rate.get(), 50, 1000);
|
||||
|
||||
if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
|
||||
_param_imu_integ_rate.set(imu_integration_rate_hz);
|
||||
_param_imu_integ_rate.commit_no_notification();
|
||||
}
|
||||
|
||||
const float update_interval_us = 1e6f / _update_rate;
|
||||
const float imu_integration_interval_us = 1e6f / (float)imu_integration_rate_hz;
|
||||
|
||||
_integrator_reset_samples = roundf(imu_integration_interval_us / update_interval_us);
|
||||
_integrator.set_autoreset_interval(_integrator_reset_samples * update_interval_us);
|
||||
}
|
||||
|
||||
void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
|
||||
{
|
||||
// Apply rotation (before scaling)
|
||||
@@ -158,67 +130,32 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
|
||||
|
||||
const Vector3f raw{x, y, z};
|
||||
|
||||
// Clipping (check unscaled raw values)
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (fabsf(raw(i)) > _clip_limit) {
|
||||
_clipping_total[i]++;
|
||||
_integrator_clipping(i)++;
|
||||
}
|
||||
}
|
||||
// clipping
|
||||
float clip_count_x = (fabsf(raw(0)) > _clip_limit);
|
||||
float clip_count_y = (fabsf(raw(1)) > _clip_limit);
|
||||
float clip_count_z = (fabsf(raw(2)) > _clip_limit);
|
||||
|
||||
rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z);
|
||||
|
||||
// Apply range scale and the calibrating offset/scale
|
||||
const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
|
||||
|
||||
// publish raw data immediately
|
||||
{
|
||||
sensor_accel_s report;
|
||||
// publish
|
||||
sensor_accel_s report;
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.x = val_calibrated(0);
|
||||
report.y = val_calibrated(1);
|
||||
report.z = val_calibrated(2);
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.error_count = _error_count;
|
||||
report.x = val_calibrated(0);
|
||||
report.y = val_calibrated(1);
|
||||
report.z = val_calibrated(2);
|
||||
report.clip_counter[0] = fabsf(roundf(clip_count_x));
|
||||
report.clip_counter[1] = fabsf(roundf(clip_count_y));
|
||||
report.clip_counter[2] = fabsf(roundf(clip_count_z));
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
// Integrated values
|
||||
Vector3f delta_velocity;
|
||||
uint32_t integral_dt = 0;
|
||||
|
||||
_integrator_samples++;
|
||||
|
||||
if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
|
||||
|
||||
// fill sensor_accel_integrated and publish
|
||||
sensor_accel_integrated_s report;
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.error_count = _error_count;
|
||||
report.device_id = _device_id;
|
||||
delta_velocity.copyTo(report.delta_velocity);
|
||||
report.dt = integral_dt;
|
||||
report.samples = _integrator_samples;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i)));
|
||||
}
|
||||
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_integrated_pub.publish(report);
|
||||
|
||||
|
||||
// reset integrator
|
||||
ResetIntegrator();
|
||||
|
||||
// update vibration metrics
|
||||
UpdateVibrationMetrics(delta_velocity);
|
||||
}
|
||||
|
||||
PublishStatus();
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
@@ -226,110 +163,57 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
const uint8_t N = sample.samples;
|
||||
const float dt = sample.dt;
|
||||
|
||||
// publish raw data immediately
|
||||
{
|
||||
// average
|
||||
float x = (float)sum(sample.x, N) / (float)N;
|
||||
float y = (float)sum(sample.y, N) / (float)N;
|
||||
float z = (float)sum(sample.z, N) / (float)N;
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
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
|
||||
float clip_count_x = clipping(sample.x, _clip_limit, N);
|
||||
float clip_count_y = clipping(sample.y, _clip_limit, N);
|
||||
float clip_count_z = clipping(sample.z, _clip_limit, N);
|
||||
|
||||
rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z);
|
||||
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
rotate_3f(_rotation, integral(0), integral(1), integral(2));
|
||||
|
||||
// Apply range scale and the calibrating offset/scale
|
||||
// average
|
||||
const float x = integral(0) / (float)N;
|
||||
const float y = integral(1) / (float)N;
|
||||
const float z = integral(2) / (float)N;
|
||||
|
||||
// Apply range scale and the calibration offset/scale
|
||||
const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)};
|
||||
|
||||
// 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 = val_calibrated(0);
|
||||
report.y = val_calibrated(1);
|
||||
report.z = val_calibrated(2);
|
||||
report.clip_counter[0] = fabsf(roundf(clip_count_x));
|
||||
report.clip_counter[1] = fabsf(roundf(clip_count_y));
|
||||
report.clip_counter[2] = fabsf(roundf(clip_count_z));
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
|
||||
// clipping
|
||||
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);
|
||||
|
||||
_clipping_total[0] += clip_count_x;
|
||||
_clipping_total[1] += clip_count_y;
|
||||
_clipping_total[2] += clip_count_z;
|
||||
|
||||
_integrator_clipping(0) += clip_count_x;
|
||||
_integrator_clipping(1) += clip_count_y;
|
||||
_integrator_clipping(2) += 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) > (N * dt * 2.0f))) {
|
||||
|
||||
ResetIntegrator();
|
||||
}
|
||||
|
||||
// integrate
|
||||
_integrator_samples += 1;
|
||||
_integrator_fifo_samples += 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];
|
||||
|
||||
|
||||
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2));
|
||||
|
||||
// scale calibration offset to number of samples
|
||||
const Vector3f offset{_calibration_offset * _integrator_fifo_samples};
|
||||
|
||||
// Apply calibration and scale to seconds
|
||||
const Vector3f delta_velocity{((_integration_raw * _scale) - offset).emult(_calibration_scale) * 1e-6f * dt};
|
||||
|
||||
// fill sensor_accel_integrated and publish
|
||||
sensor_accel_integrated_s report;
|
||||
|
||||
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_fifo_samples * dt; // time span in microseconds
|
||||
report.samples = _integrator_fifo_samples;
|
||||
|
||||
rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2));
|
||||
const Vector3f clipping{_integrator_clipping};
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
report.clip_counter[i] = fabsf(roundf(clipping(i)));
|
||||
}
|
||||
|
||||
report.timestamp = hrt_absolute_time();
|
||||
_sensor_integrated_pub.publish(report);
|
||||
|
||||
// update vibration metrics
|
||||
UpdateVibrationMetrics(delta_velocity);
|
||||
|
||||
// reset integrator
|
||||
ResetIntegrator();
|
||||
}
|
||||
|
||||
_timestamp_sample_prev = sample.timestamp_sample;
|
||||
}
|
||||
|
||||
// publish sensor fifo
|
||||
// publish fifo
|
||||
sensor_accel_fifo_s fifo{};
|
||||
|
||||
fifo.device_id = _device_id;
|
||||
@@ -344,42 +228,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
|
||||
fifo.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(fifo);
|
||||
|
||||
|
||||
PublishStatus();
|
||||
}
|
||||
|
||||
void PX4Accelerometer::PublishStatus()
|
||||
{
|
||||
// publish sensor status
|
||||
if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) {
|
||||
sensor_accel_status_s status;
|
||||
|
||||
status.device_id = _device_id;
|
||||
status.error_count = _error_count;
|
||||
status.full_scale_range = _range;
|
||||
status.rotation = _rotation;
|
||||
status.measure_rate_hz = _update_rate;
|
||||
status.temperature = _temperature;
|
||||
status.vibration_metric = _vibration_metric;
|
||||
status.clipping[0] = _clipping_total[0];
|
||||
status.clipping[1] = _clipping_total[1];
|
||||
status.clipping[2] = _clipping_total[2];
|
||||
status.timestamp = hrt_absolute_time();
|
||||
_sensor_status_pub.publish(status);
|
||||
|
||||
_status_last_publish = status.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Accelerometer::ResetIntegrator()
|
||||
{
|
||||
_integrator_samples = 0;
|
||||
_integrator_fifo_samples = 0;
|
||||
_integration_raw.zero();
|
||||
_integrator_clipping.zero();
|
||||
|
||||
_timestamp_sample_prev = 0;
|
||||
}
|
||||
|
||||
void PX4Accelerometer::UpdateClipLimit()
|
||||
@@ -388,15 +236,6 @@ void PX4Accelerometer::UpdateClipLimit()
|
||||
_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
|
||||
}
|
||||
|
||||
void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
|
||||
{
|
||||
// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
|
||||
const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev;
|
||||
_vibration_metric = 0.99f * _vibration_metric + 0.01f * delta_velocity_diff.norm();
|
||||
|
||||
_delta_velocity_prev = delta_velocity;
|
||||
}
|
||||
|
||||
void PX4Accelerometer::print_status()
|
||||
{
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
|
||||
@@ -37,15 +37,12 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/cdev/CDev.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <lib/drivers/device/integrator.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_accel_fifo.h>
|
||||
#include <uORB/topics/sensor_accel_integrated.h>
|
||||
#include <uORB/topics/sensor_accel_status.h>
|
||||
|
||||
class PX4Accelerometer : public cdev::CDev, public ModuleParams
|
||||
{
|
||||
@@ -64,7 +61,6 @@ public:
|
||||
void set_range(float range) { _range = range; UpdateClipLimit(); }
|
||||
void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
|
||||
void set_temperature(float temperature) { _temperature = temperature; }
|
||||
void set_update_rate(uint16_t rate);
|
||||
|
||||
void update(hrt_abstime timestamp_sample, float x, float y, float z);
|
||||
|
||||
@@ -86,27 +82,14 @@ public:
|
||||
void updateFIFO(const FIFOSample &sample);
|
||||
|
||||
private:
|
||||
|
||||
void PublishStatus();
|
||||
void ResetIntegrator();
|
||||
void UpdateClipLimit();
|
||||
void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity);
|
||||
|
||||
uORB::PublicationQueuedMulti<sensor_accel_s> _sensor_pub;
|
||||
uORB::PublicationMulti<sensor_accel_fifo_s> _sensor_fifo_pub;
|
||||
uORB::PublicationMulti<sensor_accel_integrated_s> _sensor_integrated_pub;
|
||||
uORB::PublicationMulti<sensor_accel_status_s> _sensor_status_pub;
|
||||
|
||||
hrt_abstime _status_last_publish{0};
|
||||
|
||||
Integrator _integrator{5000, false}; // 200 Hz default
|
||||
|
||||
matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f};
|
||||
matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
|
||||
|
||||
matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement
|
||||
float _vibration_metric{0.f}; // high frequency vibration level in the IMU delta velocity data (m/s)
|
||||
|
||||
int _class_device_instance{-1};
|
||||
|
||||
uint32_t _device_id{0};
|
||||
@@ -116,24 +99,9 @@ private:
|
||||
float _scale{1.f};
|
||||
float _temperature{0.f};
|
||||
|
||||
int16_t _clip_limit{(int16_t)(_range / _scale)};
|
||||
float _clip_limit{_range / _scale};
|
||||
|
||||
uint64_t _error_count{0};
|
||||
uint32_t _error_count{0};
|
||||
|
||||
uint32_t _clipping_total[3] {};
|
||||
|
||||
uint16_t _update_rate{1000};
|
||||
|
||||
// integrator
|
||||
hrt_abstime _timestamp_sample_prev{0};
|
||||
matrix::Vector3f _integration_raw{};
|
||||
matrix::Vector3f _integrator_clipping{};
|
||||
int16_t _last_sample[3] {};
|
||||
uint8_t _integrator_reset_samples{4};
|
||||
uint8_t _integrator_samples{0};
|
||||
uint8_t _integrator_fifo_samples{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
|
||||
)
|
||||
};
|
||||
|
||||
@@ -56,7 +56,6 @@ endif()
|
||||
px4_add_library(drivers__device
|
||||
CDev.cpp
|
||||
ringbuffer.cpp
|
||||
integrator.cpp
|
||||
${SRCS_PLATFORM}
|
||||
)
|
||||
|
||||
|
||||
@@ -1,127 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2016 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file integrator.cpp
|
||||
*
|
||||
* A resettable integrator
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include "integrator.h"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
Integrator::Integrator(uint32_t auto_reset_interval, bool coning_compensation) :
|
||||
_coning_comp_on(coning_compensation)
|
||||
{
|
||||
set_autoreset_interval(auto_reset_interval);
|
||||
}
|
||||
|
||||
bool
|
||||
Integrator::put(const hrt_abstime ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral,
|
||||
uint32_t &integral_dt)
|
||||
{
|
||||
if (_last_integration_time == 0) {
|
||||
/* this is the first item in the integrator */
|
||||
_last_integration_time = timestamp;
|
||||
_last_reset_time = timestamp;
|
||||
_last_val = val;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
float dt = 0.0f;
|
||||
|
||||
// Integrate:
|
||||
// Leave dt at 0 if the integration time does not make sense.
|
||||
// Without this check the integral is likely to explode.
|
||||
if (timestamp >= _last_integration_time) {
|
||||
dt = static_cast<float>(timestamp - _last_integration_time) * 1e-6f;
|
||||
}
|
||||
|
||||
// Use trapezoidal integration to calculate the delta integral
|
||||
const matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f;
|
||||
_last_val = val;
|
||||
|
||||
// Calculate coning corrections if required
|
||||
if (_coning_comp_on) {
|
||||
// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
|
||||
// following:
|
||||
// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
|
||||
// Sourced: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
|
||||
// Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
|
||||
_beta += ((_last_alpha + _last_delta_alpha * (1.0f / 6.0f)) % delta_alpha) * 0.5f;
|
||||
_last_delta_alpha = delta_alpha;
|
||||
_last_alpha = _alpha;
|
||||
}
|
||||
|
||||
// accumulate delta integrals
|
||||
_alpha += delta_alpha;
|
||||
|
||||
_last_integration_time = timestamp;
|
||||
|
||||
// Only do auto reset if auto reset interval is not 0.
|
||||
if (_auto_reset_interval > 0 && (timestamp - _last_reset_time) >= _auto_reset_interval) {
|
||||
|
||||
// apply coning corrections if required
|
||||
if (_coning_comp_on) {
|
||||
integral = _alpha + _beta;
|
||||
|
||||
} else {
|
||||
integral = _alpha;
|
||||
}
|
||||
|
||||
// reset the integrals and coning corrections
|
||||
_reset(integral_dt);
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Integrator::_reset(uint32_t &integral_dt)
|
||||
{
|
||||
_alpha.zero();
|
||||
_last_alpha.zero();
|
||||
_beta.zero();
|
||||
|
||||
integral_dt = (_last_integration_time - _last_reset_time);
|
||||
|
||||
_last_reset_time = _last_integration_time;
|
||||
}
|
||||
@@ -1,93 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2018 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file integrator.h
|
||||
*
|
||||
* A resettable integrator
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
class Integrator
|
||||
{
|
||||
public:
|
||||
Integrator(uint32_t auto_reset_interval = 2500 /* 400 Hz */, bool coning_compensation = false);
|
||||
~Integrator() = default;
|
||||
|
||||
/**
|
||||
* Put an item into the integral.
|
||||
*
|
||||
* @param timestamp Timestamp of the current value.
|
||||
* @param val Item to put.
|
||||
* @param integral Current integral in case the integrator did reset, else the value will not be modified
|
||||
* @param integral_dt Get the dt in us of the current integration (only if reset).
|
||||
* @return true if putting the item triggered an integral reset and the integral should be
|
||||
* published.
|
||||
*/
|
||||
bool put(const uint64_t ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt);
|
||||
|
||||
/**
|
||||
* Set auto reset interval during runtime. This won't reset the integrator.
|
||||
*
|
||||
* @param auto_reset_interval New reset time interval for the integrator (+- 10%).
|
||||
*/
|
||||
void set_autoreset_interval(uint32_t auto_reset_interval) { _auto_reset_interval = 0.90f * auto_reset_interval; }
|
||||
|
||||
private:
|
||||
uint32_t _auto_reset_interval{0}; /**< the interval after which the content will be published
|
||||
and the integrator reset, 0 if no auto-reset */
|
||||
|
||||
uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */
|
||||
uint64_t _last_reset_time{0}; /**< last auto-announcement of integral value */
|
||||
|
||||
matrix::Vector3f _alpha{0.0f, 0.0f, 0.0f}; /**< integrated value before coning corrections are applied */
|
||||
matrix::Vector3f _last_alpha{0.0f, 0.0f, 0.0f}; /**< previous value of _alpha */
|
||||
matrix::Vector3f _beta{0.0f, 0.0f, 0.0f}; /**< accumulated coning corrections */
|
||||
matrix::Vector3f _last_val{0.0f, 0.0f, 0.0f}; /**< previous input */
|
||||
matrix::Vector3f _last_delta_alpha{0.0f, 0.0f, 0.0f}; /**< integral from previous previous sampling interval */
|
||||
|
||||
bool _coning_comp_on{false}; /**< true to turn on coning corrections */
|
||||
|
||||
/* Do a reset.
|
||||
*
|
||||
* @param integral_dt Get the dt in us of the current integration.
|
||||
*/
|
||||
void _reset(uint32_t &integral_dt);
|
||||
};
|
||||
@@ -50,39 +50,19 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
|
||||
return sum;
|
||||
}
|
||||
|
||||
static constexpr 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;
|
||||
}
|
||||
|
||||
PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
|
||||
CDev(nullptr),
|
||||
ModuleParams(nullptr),
|
||||
_sensor_pub{ORB_ID(sensor_gyro), priority},
|
||||
_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
|
||||
_sensor_integrated_pub{ORB_ID(sensor_gyro_integrated), priority},
|
||||
_sensor_status_pub{ORB_ID(sensor_gyro_status), priority},
|
||||
_device_id{device_id},
|
||||
_rotation{rotation}
|
||||
{
|
||||
// register class and advertise immediately to keep instance numbering in sync
|
||||
_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
|
||||
_sensor_pub.advertise();
|
||||
_sensor_integrated_pub.advertise();
|
||||
_sensor_status_pub.advertise();
|
||||
|
||||
updateParams();
|
||||
|
||||
// set reasonable default, driver should be setting real value
|
||||
set_update_rate(800);
|
||||
}
|
||||
|
||||
PX4Gyroscope::~PX4Gyroscope()
|
||||
@@ -93,8 +73,6 @@ PX4Gyroscope::~PX4Gyroscope()
|
||||
|
||||
_sensor_pub.unadvertise();
|
||||
_sensor_fifo_pub.unadvertise();
|
||||
_sensor_integrated_pub.unadvertise();
|
||||
_sensor_status_pub.unadvertise();
|
||||
}
|
||||
|
||||
int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
@@ -131,25 +109,6 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
|
||||
_device_id = device_id.devid;
|
||||
}
|
||||
|
||||
void PX4Gyroscope::set_update_rate(uint16_t rate)
|
||||
{
|
||||
_update_rate = math::constrain((int)rate, 50, 32000);
|
||||
|
||||
// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
|
||||
int32_t imu_integration_rate_hz = math::constrain(_param_imu_integ_rate.get(), 50, 1000);
|
||||
|
||||
if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
|
||||
_param_imu_integ_rate.set(imu_integration_rate_hz);
|
||||
_param_imu_integ_rate.commit_no_notification();
|
||||
}
|
||||
|
||||
const float update_interval_us = 1e6f / _update_rate;
|
||||
const float imu_integration_interval_us = 1e6f / (float)imu_integration_rate_hz;
|
||||
|
||||
_integrator_reset_samples = roundf(imu_integration_interval_us / update_interval_us);
|
||||
_integrator.set_autoreset_interval(_integrator_reset_samples * update_interval_us);
|
||||
}
|
||||
|
||||
void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)
|
||||
{
|
||||
// Apply rotation (before scaling)
|
||||
@@ -157,67 +116,22 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
|
||||
|
||||
const Vector3f raw{x, y, z};
|
||||
|
||||
// Clipping (check unscaled raw values)
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (fabsf(raw(i)) > _clip_limit) {
|
||||
_clipping_total[i]++;
|
||||
_integrator_clipping(i)++;
|
||||
}
|
||||
}
|
||||
|
||||
// Apply range scale and the calibrating offset/scale
|
||||
const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
|
||||
|
||||
// publish raw data immediately
|
||||
{
|
||||
sensor_gyro_s report;
|
||||
// publish
|
||||
sensor_gyro_s report;
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.x = val_calibrated(0);
|
||||
report.y = val_calibrated(1);
|
||||
report.z = val_calibrated(2);
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.error_count = _error_count;
|
||||
report.x = val_calibrated(0);
|
||||
report.y = val_calibrated(1);
|
||||
report.z = val_calibrated(2);
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
// Integrated values
|
||||
Vector3f delta_angle;
|
||||
uint32_t integral_dt = 0;
|
||||
|
||||
_integrator_samples++;
|
||||
|
||||
if (_integrator.put(timestamp_sample, val_calibrated, delta_angle, integral_dt)) {
|
||||
|
||||
// fill sensor_gyro_integrated and publish
|
||||
sensor_gyro_integrated_s report;
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.error_count = _error_count;
|
||||
report.device_id = _device_id;
|
||||
delta_angle.copyTo(report.delta_angle);
|
||||
report.dt = integral_dt;
|
||||
report.samples = _integrator_samples;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i)));
|
||||
}
|
||||
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_integrated_pub.publish(report);
|
||||
|
||||
|
||||
// reset integrator
|
||||
ResetIntegrator();
|
||||
|
||||
// update vibration metrics
|
||||
UpdateVibrationMetrics(delta_angle);
|
||||
}
|
||||
|
||||
PublishStatus();
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
|
||||
@@ -225,24 +139,36 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
|
||||
const uint8_t N = sample.samples;
|
||||
const float dt = sample.dt;
|
||||
|
||||
// publish raw data immediately
|
||||
{
|
||||
// average
|
||||
float x = (float)sum(sample.x, N) / (float)N;
|
||||
float y = (float)sum(sample.y, N) / (float)N;
|
||||
float z = (float)sum(sample.z, N) / (float)N;
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
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];
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
rotate_3f(_rotation, integral(0), integral(1), integral(2));
|
||||
|
||||
// average
|
||||
const float x = integral(0) / (float)N;
|
||||
const float y = integral(1) / (float)N;
|
||||
const float z = integral(2) / (float)N;
|
||||
|
||||
// Apply range scale and the calibration offset
|
||||
const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset};
|
||||
|
||||
// publish
|
||||
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 = val_calibrated(0);
|
||||
report.y = val_calibrated(1);
|
||||
report.z = val_calibrated(2);
|
||||
@@ -252,83 +178,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
|
||||
}
|
||||
|
||||
|
||||
// clipping
|
||||
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);
|
||||
|
||||
_clipping_total[0] += clip_count_x;
|
||||
_clipping_total[1] += clip_count_y;
|
||||
_clipping_total[2] += clip_count_z;
|
||||
|
||||
_integrator_clipping(0) += clip_count_x;
|
||||
_integrator_clipping(1) += clip_count_y;
|
||||
_integrator_clipping(2) += 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) > (N * dt * 2.0f))) {
|
||||
|
||||
ResetIntegrator();
|
||||
}
|
||||
|
||||
// integrate
|
||||
_integrator_samples += 1;
|
||||
_integrator_fifo_samples += 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];
|
||||
|
||||
|
||||
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2));
|
||||
|
||||
// scale calibration offset to number of samples
|
||||
const Vector3f offset{_calibration_offset * _integrator_fifo_samples};
|
||||
|
||||
// Apply calibration and scale to seconds
|
||||
const Vector3f delta_angle{((_integration_raw * _scale) - offset) * 1e-6f * dt};
|
||||
|
||||
// fill sensor_gyro_integrated and publish
|
||||
sensor_gyro_integrated_s report;
|
||||
|
||||
report.timestamp_sample = sample.timestamp_sample;
|
||||
report.error_count = _error_count;
|
||||
report.device_id = _device_id;
|
||||
delta_angle.copyTo(report.delta_angle);
|
||||
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
|
||||
report.samples = _integrator_fifo_samples;
|
||||
|
||||
rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2));
|
||||
const Vector3f clipping{_integrator_clipping};
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
report.clip_counter[i] = fabsf(roundf(clipping(i)));
|
||||
}
|
||||
|
||||
report.timestamp = hrt_absolute_time();
|
||||
_sensor_integrated_pub.publish(report);
|
||||
|
||||
// update vibration metrics
|
||||
UpdateVibrationMetrics(delta_angle);
|
||||
|
||||
// reset integrator
|
||||
ResetIntegrator();
|
||||
}
|
||||
|
||||
_timestamp_sample_prev = sample.timestamp_sample;
|
||||
}
|
||||
|
||||
// publish sensor fifo
|
||||
// publish fifo
|
||||
sensor_gyro_fifo_s fifo{};
|
||||
|
||||
fifo.device_id = _device_id;
|
||||
@@ -343,62 +193,6 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
|
||||
|
||||
fifo.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(fifo);
|
||||
|
||||
|
||||
PublishStatus();
|
||||
}
|
||||
|
||||
void PX4Gyroscope::PublishStatus()
|
||||
{
|
||||
// publish sensor status
|
||||
if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) {
|
||||
sensor_gyro_status_s status;
|
||||
|
||||
status.device_id = _device_id;
|
||||
status.error_count = _error_count;
|
||||
status.full_scale_range = _range;
|
||||
status.rotation = _rotation;
|
||||
status.measure_rate_hz = _update_rate;
|
||||
status.temperature = _temperature;
|
||||
status.vibration_metric = _vibration_metric;
|
||||
status.coning_vibration = _coning_vibration;
|
||||
status.clipping[0] = _clipping_total[0];
|
||||
status.clipping[1] = _clipping_total[1];
|
||||
status.clipping[2] = _clipping_total[2];
|
||||
status.timestamp = hrt_absolute_time();
|
||||
_sensor_status_pub.publish(status);
|
||||
|
||||
_status_last_publish = status.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Gyroscope::ResetIntegrator()
|
||||
{
|
||||
_integrator_samples = 0;
|
||||
_integrator_fifo_samples = 0;
|
||||
_integration_raw.zero();
|
||||
_integrator_clipping.zero();
|
||||
|
||||
_timestamp_sample_prev = 0;
|
||||
}
|
||||
|
||||
void PX4Gyroscope::UpdateClipLimit()
|
||||
{
|
||||
// 99.9% of potential max
|
||||
_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
|
||||
}
|
||||
|
||||
void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
|
||||
{
|
||||
// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
|
||||
const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev;
|
||||
_vibration_metric = 0.99f * _vibration_metric + 0.01f * delta_angle_diff.norm();
|
||||
|
||||
// Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
|
||||
const Vector3f coning_metric = delta_angle % _delta_angle_prev;
|
||||
_coning_vibration = 0.99f * _coning_vibration + 0.01f * coning_metric.norm();
|
||||
|
||||
_delta_angle_prev = delta_angle;
|
||||
}
|
||||
|
||||
void PX4Gyroscope::print_status()
|
||||
|
||||
@@ -37,14 +37,11 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/cdev/CDev.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <lib/drivers/device/integrator.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/sensor_gyro_fifo.h>
|
||||
#include <uORB/topics/sensor_gyro_integrated.h>
|
||||
#include <uORB/topics/sensor_gyro_status.h>
|
||||
|
||||
class PX4Gyroscope : public cdev::CDev, public ModuleParams
|
||||
{
|
||||
@@ -62,10 +59,9 @@ public:
|
||||
void set_device_type(uint8_t devtype);
|
||||
void set_error_count(uint64_t error_count) { _error_count = error_count; }
|
||||
void increase_error_count() { _error_count++; }
|
||||
void set_range(float range) { _range = range; UpdateClipLimit(); }
|
||||
void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
|
||||
void set_range(float range) { _range = range; }
|
||||
void set_scale(float scale) { _scale = scale; }
|
||||
void set_temperature(float temperature) { _temperature = temperature; }
|
||||
void set_update_rate(uint16_t rate);
|
||||
|
||||
void update(hrt_abstime timestamp_sample, float x, float y, float z);
|
||||
|
||||
@@ -87,27 +83,11 @@ public:
|
||||
void updateFIFO(const FIFOSample &sample);
|
||||
|
||||
private:
|
||||
|
||||
void PublishStatus();
|
||||
void ResetIntegrator();
|
||||
void UpdateClipLimit();
|
||||
void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);
|
||||
|
||||
uORB::PublicationQueuedMulti<sensor_gyro_s> _sensor_pub;
|
||||
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub;
|
||||
uORB::PublicationMulti<sensor_gyro_integrated_s> _sensor_integrated_pub;
|
||||
uORB::PublicationMulti<sensor_gyro_status_s> _sensor_status_pub;
|
||||
|
||||
hrt_abstime _status_last_publish{0};
|
||||
|
||||
Integrator _integrator{5000, true}; // 200 Hz default
|
||||
|
||||
matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
|
||||
|
||||
matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement
|
||||
float _vibration_metric{0.f}; // high frequency vibration level in the IMU delta angle data (rad)
|
||||
float _coning_vibration{0.f}; // Level of coning vibration in the IMU delta angles (rad^2)
|
||||
|
||||
int _class_device_instance{-1};
|
||||
|
||||
uint32_t _device_id{0};
|
||||
@@ -117,25 +97,11 @@ private:
|
||||
float _scale{1.f};
|
||||
float _temperature{0.f};
|
||||
|
||||
int16_t _clip_limit{(int16_t)(_range / _scale)};
|
||||
uint32_t _error_count{0};
|
||||
|
||||
uint64_t _error_count{0};
|
||||
|
||||
uint32_t _clipping_total[3] {};
|
||||
|
||||
uint16_t _update_rate{1000};
|
||||
|
||||
// integrator
|
||||
hrt_abstime _timestamp_sample_prev{0};
|
||||
matrix::Vector3f _integration_raw{};
|
||||
matrix::Vector3f _integrator_clipping{};
|
||||
int16_t _last_sample[3] {};
|
||||
uint8_t _integrator_reset_samples{4};
|
||||
uint8_t _integrator_samples{0};
|
||||
uint8_t _integrator_fifo_samples{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
|
||||
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
|
||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
|
||||
)
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user