mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 01:13:38 +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
@@ -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