dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors/vehicle_imu: vehicle_imu_status include accel/gyro full raw FIFO sample rate

This commit is contained in:
Daniel Agar 2021-02-24 08:13:53 -05:00 committed by GitHub
parent cec31fd685
commit 5f3e883f2c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 40 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/sensor_accel.msg
View File

@ -13,4 +13,6 @@ uint32 error_count
uint8[3] clip_counter # clip count per axis in the sample period
uint8 samples # number of raw samples that went into this message
uint8 ORB_QUEUE_LENGTH = 8

2
msg/sensor_gyro.msg
View File

@ -11,4 +11,6 @@ float32 temperature # temperature in degrees Celsius
uint32 error_count
uint8 samples # number of raw samples that went into this message
uint8 ORB_QUEUE_LENGTH = 8

3
msg/vehicle_imu_status.msg
View File

@ -11,6 +11,9 @@ uint32 gyro_error_count
float32 accel_rate_hz
float32 gyro_rate_hz
float32 accel_raw_rate_hz # full raw sensor sample rate (Hz)
float32 gyro_raw_rate_hz # full raw sensor sample rate (Hz)
float32 accel_vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s)
float32 gyro_vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s)
float32 gyro_coning_vibration # Level of coning vibration in the IMU delta angles (rad^2)

6
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

@ -140,11 +140,12 @@ void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
const float z = integral(2) / (float)N;
// publish
Publish(sample.timestamp_sample, x, y, z, clip_count);
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])
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);
@ -166,6 +167,7 @@ void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, flo
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.timestamp = hrt_absolute_time();
_sensor_pub.publish(report);

3
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

@ -63,7 +63,8 @@ public:
void updateFIFO(sensor_accel_fifo_s &sample);
private:
void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3]);
void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3],
uint8_t samples = 1);
void UpdateClipLimit();
uORB::PublicationMulti<sensor_accel_s> _sensor_pub{ORB_ID(sensor_accel)};

5
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -114,11 +114,11 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
const float z = integral(2) / (float)N;
// publish
Publish(sample.timestamp_sample, x, y, z);
Publish(sample.timestamp_sample, x, y, z, N);
}
}
void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z)
void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t samples)
{
// Apply rotation (before scaling)
rotate_3f(_rotation, x, y, z);
@ -132,6 +132,7 @@ void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y
report.x = x * _scale;
report.y = y * _scale;
report.z = z * _scale;
report.samples = samples;
report.timestamp = hrt_absolute_time();
_sensor_pub.publish(report);

2
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@ -62,7 +62,7 @@ public:
void updateFIFO(sensor_gyro_fifo_s &sample);
private:
void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z);
void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t samples = 1);
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};

30
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -141,50 +141,54 @@ void VehicleIMU::ParametersUpdate(bool force)
}
}
bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample)
bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample, uint8_t samples)
{
bool updated = false;
// conservative maximum time between samples to reject large gaps and reset averaging
float max_interval_us = 10000; // 100 Hz
float min_interval_us = 100; // 10,000 Hz
uint32_t max_interval_us = 10000; // 100 Hz
uint32_t min_interval_us = 100; // 10,000 Hz
if (intavg.update_interval > 0) {
if (intavg.update_interval > 0.f) {
// if available use previously calculated interval as bounds
max_interval_us = 1.5f * intavg.update_interval;
min_interval_us = 0.5f * intavg.update_interval;
max_interval_us = roundf(1.5f * intavg.update_interval);
min_interval_us = roundf(0.5f * intavg.update_interval);
}
const float interval_us = (timestamp_sample - intavg.timestamp_sample_last);
const uint32_t interval_us = (timestamp_sample - intavg.timestamp_sample_last);
if ((intavg.timestamp_sample_last > 0) && (interval_us < max_interval_us) && (interval_us > min_interval_us)) {
intavg.interval_sum += interval_us;
intavg.interval_samples += samples;
intavg.interval_count++;
// periodically calculate sensor update rate
if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) {
const float sample_interval_avg = intavg.interval_sum / intavg.interval_count;
const float sample_interval_avg = (float)intavg.interval_sum / (float)intavg.interval_count;
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.f)) {
// update if interval has changed by more than 0.5%
if ((fabsf(intavg.update_interval - sample_interval_avg) / intavg.update_interval) > 0.005f) {
intavg.update_interval = sample_interval_avg;
intavg.update_interval_raw = (float)intavg.interval_sum / (float)intavg.interval_samples;
updated = true;
}
}
// reset sample interval accumulator
intavg.interval_sum = 0.f;
intavg.interval_count = 0.f;
intavg.interval_samples = 0;
intavg.interval_count = 0;
}
} else {
// reset
intavg.interval_sum = 0.f;
intavg.interval_count = 0.f;
intavg.interval_samples = 0;
intavg.interval_count = 0;
}
intavg.timestamp_sample_last = timestamp_sample;
@ -222,10 +226,11 @@ void VehicleIMU::Run()
} else {
// collect sample interval average for filters
if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample, gyro.samples)) {
update_integrator_config = true;
publish_status = true;
_status.gyro_rate_hz = 1e6f / _gyro_interval.update_interval;
_status.gyro_raw_rate_hz = 1e6f / _gyro_interval.update_interval_raw;
}
}
@ -268,10 +273,11 @@ void VehicleIMU::Run()
} else {
// collect sample interval average for filters
if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample, accel.samples)) {
update_integrator_config = true;
publish_status = true;
_status.accel_rate_hz = 1e6f / _accel_interval.update_interval;
_status.accel_raw_rate_hz = 1e6f / _accel_interval.update_interval_raw;
}
}

8
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -77,12 +77,14 @@ private:
struct IntervalAverage {
hrt_abstime timestamp_sample_last{0};
float interval_sum{0.f};
float interval_count{0.f};
uint32_t interval_sum{0};
uint32_t interval_samples{0};
uint32_t interval_count{0};
float update_interval{0.f};
float update_interval_raw{0.f};
};
bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample);
bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample, uint8_t samples = 1);
void UpdateIntegratorConfiguration();
void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle);
void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);