forked from Archive/PX4-Autopilot
sensors/vehicle_imu: vehicle_imu_status include accel/gyro full raw FIFO sample rate
This commit is contained in:
parent
cec31fd685
commit
5f3e883f2c
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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 ×tamp_sample, float x, float y, float z, uint8_t clip_count[3])
|
||||
void PX4Accelerometer::Publish(const hrt_abstime ×tamp_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 ×tamp_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);
|
||||
|
|
|
@ -63,7 +63,8 @@ public:
|
|||
void updateFIFO(sensor_accel_fifo_s &sample);
|
||||
|
||||
private:
|
||||
void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]);
|
||||
void Publish(const hrt_abstime ×tamp_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)};
|
||||
|
|
|
@ -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 ×tamp_sample, float x, float y, float z)
|
||||
void PX4Gyroscope::Publish(const hrt_abstime ×tamp_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 ×tamp_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);
|
||||
|
|
|
@ -62,7 +62,7 @@ public:
|
|||
void updateFIFO(sensor_gyro_fifo_s &sample);
|
||||
|
||||
private:
|
||||
void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z);
|
||||
void Publish(const hrt_abstime ×tamp_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)};
|
||||
|
|
|
@ -141,50 +141,54 @@ void VehicleIMU::ParametersUpdate(bool force)
|
|||
}
|
||||
}
|
||||
|
||||
bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample)
|
||||
bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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 ×tamp_sample);
|
||||
bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample, uint8_t samples = 1);
|
||||
void UpdateIntegratorConfiguration();
|
||||
void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle);
|
||||
void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);
|
||||
|
|
Loading…
Reference in New Issue