yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

sensor accel/gyro message cleanup

Browse Source 
- split out integrated data into new standalone messages (sensor_accel_integrated and sensor_gyro_integrated)
 - publish sensor_gyro at full rate and remove sensor_gyro_control
 - limit sensor status publications to 10 Hz
 - remove unused accel/gyro raw ADC fields
 - add device IDs to sensor_bias and sensor_correction
    - vehicle_angular_velocity/vehicle_acceleration: check device ids before using bias and corrections
This commit is contained in:
Daniel Agar
2020-01-18 01:15:00 -05:00
committed by GitHub
parent 1d932f6ec9
commit bb465ca5b7
34 changed files with 696 additions and 756 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -43,8 +43,8 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
CDev(nullptr),
ModuleParams(nullptr),
_sensor_pub{ORB_ID(sensor_gyro), priority},
_sensor_control_pub{ORB_ID(sensor_gyro_control), 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}
@@ -64,8 +64,7 @@ PX4Gyroscope::~PX4Gyroscope()
}
}
int
PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case GYROIOCSSCALE: {
@@ -86,8 +85,7 @@ PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
}
}
void
PX4Gyroscope::set_device_type(uint8_t devtype)
void PX4Gyroscope::set_device_type(uint8_t devtype)
{
// current DeviceStructure
union device::Device::DeviceId device_id;
@@ -96,12 +94,11 @@ PX4Gyroscope::set_device_type(uint8_t devtype)
// update to new device type
device_id.devid_s.devtype = devtype;
// copy back to report
// copy back
_device_id = device_id.devid;
}
void
PX4Gyroscope::set_sample_rate(uint16_t rate)
void PX4Gyroscope::set_sample_rate(uint16_t rate)
{
_sample_rate = rate;
@@ -109,29 +106,25 @@ PX4Gyroscope::set_sample_rate(uint16_t rate)
ConfigureNotchFilter(_notch_filter.getNotchFreq(), _notch_filter.getBandwidth());
}
void
PX4Gyroscope::set_update_rate(uint16_t rate)
void PX4Gyroscope::set_update_rate(uint16_t rate)
{
const uint32_t update_interval = 1000000 / rate;
_integrator_reset_samples = 4000 / update_interval;
}
void
PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)
{
// Apply rotation (before scaling)
rotate_3f(_rotation, x, y, z);
const Vector3f raw{x, y, z};
// Clipping
sensor_gyro_status_s &status = _sensor_status_pub.get();
// 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) {
status.clipping[i]++;
_clipping[i]++;
_integrator_clipping++;
}
}
@@ -143,86 +136,61 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
Vector3f val_filtered{_notch_filter.apply(val_calibrated)};
val_filtered = _filter.apply(val_filtered);
// publish control data (filtered) immediately
bool publish_control = true;
sensor_gyro_control_s control{};
if (_param_imu_gyro_rate_max.get() > 0) {
const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
if (hrt_elapsed_time(&_control_last_publish) < interval) {
publish_control = false;
}
}
if (publish_control) {
control.timestamp_sample = timestamp;
control.device_id = _device_id;
val_filtered.copyTo(control.xyz);
control.timestamp = hrt_absolute_time();
_sensor_control_pub.publish(control);
_control_last_publish = control.timestamp_sample;
}
// Integrated values
Vector3f integrated_value;
uint32_t integral_dt = 0;
_integrator_samples++;
if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
{
sensor_gyro_s report{};
report.timestamp = timestamp;
report.timestamp_sample = timestamp_sample;
report.device_id = _device_id;
report.temperature = _temperature;
report.scaling = _scale;
report.error_count = _error_count;
// Raw values (ADC units 0 - 65535)
report.x_raw = x;
report.y_raw = y;
report.z_raw = z;
report.x = val_filtered(0);
report.y = val_filtered(1);
report.z = val_filtered(2);
report.integral_dt = integral_dt;
report.integral_samples = _integrator_samples;
report.x_integral = integrated_value(0);
report.y_integral = integrated_value(1);
report.z_integral = integrated_value(2);
report.integral_clip_count = _integrator_clipping;
report.timestamp = hrt_absolute_time();
_sensor_pub.publish(report);
}
// Integrated values
Vector3f delta_angle;
uint32_t integral_dt = 0;
if (_integrator_samples == 0) {
_integrator_timestamp_sample = timestamp_sample;
}
_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 = _integrator_timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
report.temperature = _temperature;
delta_angle.copyTo(report.delta_angle);
report.dt = integral_dt;
report.samples = _integrator_samples;
report.clip_count = _integrator_clipping;
report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);
// reset integrator
ResetIntegrator();
// update vibration metrics
const Vector3f delta_angle = integrated_value * (integral_dt * 1.e-6f);
UpdateVibrationMetrics(delta_angle);
}
// publish status
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.sample_rate = _sample_rate;
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.coning_vibration = _coning_vibration;
status.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);
PublishStatus();
}
void
PX4Gyroscope::updateFIFO(const FIFOSample &sample)
void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
{
// filtered data (control)
float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
@@ -238,11 +206,9 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
const Vector3f val_calibrated{(raw * _scale) - _calibration_offset};
// control
// publish control data (filtered) immediately
{
// publish control data (filtered) immediately
bool publish_control = true;
sensor_gyro_control_s control{};
if (_param_imu_gyro_rate_max.get() > 0) {
const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
@@ -253,56 +219,48 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
}
if (publish_control) {
control.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
control.device_id = _device_id;
val_calibrated.copyTo(control.xyz);
control.timestamp = hrt_absolute_time();
_sensor_control_pub.publish(control);
sensor_gyro_s report{};
_control_last_publish = control.timestamp_sample;
report.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last 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();
_sensor_pub.publish(report);
_control_last_publish = report.timestamp_sample;
}
}
// status
{
sensor_gyro_status_s &status = _sensor_status_pub.get();
// clipping
const int16_t clip_limit = (_range / _scale) * 0.95f;
const int16_t clip_limit = (_range / _scale) * 0.95f;
// x clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.x[n]) > clip_limit) {
status.clipping[0]++;
_integrator_clipping++;
}
// 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) {
status.clipping[1]++;
_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) {
status.clipping[2]++;
_integrator_clipping++;
}
// z clipping
for (int n = 0; n < sample.samples; n++) {
if (abs(sample.z[n]) > clip_limit) {
_clipping[2]++;
_integrator_clipping++;
}
status.device_id = _device_id;
status.error_count = _error_count;
status.full_scale_range = _range;
status.rotation = _rotation;
status.measure_rate = _update_rate;
status.sample_rate = _sample_rate;
status.temperature = _temperature;
status.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);
}
@@ -350,37 +308,25 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
// Apply range scale and the calibrating offset/scale
Vector3f val_int_calibrated{(raw_int * _scale) - _calibration_offset};
val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore
Vector3f delta_angle{(raw_int * _scale) - _calibration_offset};
delta_angle *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore
// publish
sensor_gyro_s report{};
// fill sensor_gyro_integrated and publish
sensor_gyro_integrated_s report{};
report.timestamp_sample = _integrator_timestamp_sample;
report.error_count = _error_count;
report.device_id = _device_id;
report.temperature = _temperature;
report.scaling = _scale;
report.error_count = _error_count;
delta_angle.copyTo(report.delta_angle);
report.dt = integrator_dt_us;
report.samples = _integrator_fifo_samples;
report.clip_count = _integrator_clipping;
// Raw values (ADC units 0 - 65535)
report.x_raw = sample.x[0];
report.y_raw = sample.y[0];
report.z_raw = sample.z[0];
report.x = val_calibrated(0);
report.y = val_calibrated(1);
report.z = val_calibrated(2);
report.integral_dt = integrator_dt_us;
report.integral_samples = _integrator_fifo_samples;
report.x_integral = val_int_calibrated(0);
report.y_integral = val_int_calibrated(1);
report.z_integral = val_int_calibrated(2);
report.integral_clip_count = _integrator_clipping;
report.timestamp = _integrator_timestamp_sample;
_sensor_pub.publish(report);
report.timestamp = hrt_absolute_time();
_sensor_integrated_pub.publish(report);
// update vibration metrics
const Vector3f delta_angle = val_int_calibrated * (integrator_dt_us * 1.e-6f);
UpdateVibrationMetrics(delta_angle);
// reset integrator
@@ -390,6 +336,7 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
_timestamp_sample_prev = sample.timestamp_sample;
}
// publish sensor fifo
sensor_gyro_fifo_s fifo{};
fifo.device_id = _device_id;
@@ -404,10 +351,37 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
fifo.timestamp = hrt_absolute_time();
_sensor_fifo_pub.publish(fifo);
PublishStatus();
}
void
PX4Gyroscope::ResetIntegrator()
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 = _update_rate;
status.sample_rate = _sample_rate;
status.temperature = _temperature;
status.vibration_metric = _vibration_metric;
status.coning_vibration = _coning_vibration;
status.clipping[0] = _clipping[0];
status.clipping[1] = _clipping[1];
status.clipping[2] = _clipping[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;
@@ -420,8 +394,7 @@ PX4Gyroscope::ResetIntegrator()
_timestamp_sample_prev = 0;
}
void
PX4Gyroscope::ConfigureFilter(float cutoff_freq)
void PX4Gyroscope::ConfigureFilter(float cutoff_freq)
{
_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
@@ -430,14 +403,12 @@ PX4Gyroscope::ConfigureFilter(float cutoff_freq)
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
}
void
PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth)
void PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth)
{
_notch_filter.setParameters(_sample_rate, notch_freq, bandwidth);
}
void
PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
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;
@@ -450,8 +421,7 @@ PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
_delta_angle_prev = delta_angle;
}
void
PX4Gyroscope::print_status()
void PX4Gyroscope::print_status()
{
PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
PX4_INFO("sample rate: %d Hz", _sample_rate);
@@ -461,5 +431,4 @@ PX4Gyroscope::print_status()
PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
(double)_calibration_offset(2));
}