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

PX4Accelerometer and PX4Gyroscope add vibration metrics and always publish status

This commit is contained in:
Daniel Agar 2019-12-27 15:14:38 -05:00
parent e189733bb9
commit 89e1f478ac
7 changed files with 122 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.ci/Jenkinsfile-hardware
View File

@ -686,8 +686,10 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_baro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_mag"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
@ -725,8 +727,10 @@ void statusSEGGER() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_baro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro_status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_mag"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /dev"'

2
msg/sensor_accel_status.msg
View File

@ -13,3 +13,5 @@ uint16 measure_rate
uint16 sample_rate
float32 full_scale_range
float32 vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s)

4
msg/sensor_gyro_status.msg
View File

@ -13,3 +13,7 @@ uint16 measure_rate
uint16 sample_rate
float32 full_scale_range
float32 vibration_metric # high frequency vibration level in the IMU delta angle data (rad)
float32 coning_vibration # Level of coning vibration in the IMU delta angles (rad^2)

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

@ -123,6 +123,17 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
const Vector3f raw{x, y, z};
// Clipping
sensor_accel_status_s &status = _sensor_status_pub.get();
const float clip_limit = (_range / _scale) * 0.95f;
for (int i = 0; i < 3; i++) {
if (fabsf(raw(i)) > clip_limit) {
status.clipping[i]++;
_integrator_clipping++;
}
}
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
@ -133,6 +144,8 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
Vector3f integrated_value;
uint32_t integral_dt = 0;
_integrator_samples++;
if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
sensor_accel_s report{};
@ -152,12 +165,33 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
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;
_sensor_pub.publish(report);
// reset integrator
ResetIntegrator();
// update vibration metrics
const Vector3f delta_velocity = integrated_value * (integral_dt * 1.e-6f);
UpdateVibrationMetrics(delta_velocity);
}
// 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.timestamp = hrt_absolute_time();
_sensor_status_pub.publish(status);
}
void
@ -292,6 +326,9 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
report.timestamp = _integrator_timestamp_sample;
_sensor_pub.publish(report);
// update vibration metrics
const Vector3f delta_velocity = val_int_calibrated * (integrator_dt_us * 1.e-6f);
UpdateVibrationMetrics(delta_velocity);
// reset integrator
ResetIntegrator();
@ -340,6 +377,16 @@ PX4Accelerometer::ConfigureFilter(float cutoff_freq)
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
}
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()
{

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

@ -88,8 +88,9 @@ public:
private:
void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity);
uORB::PublicationMulti<sensor_accel_s> _sensor_pub; // legacy message
uORB::PublicationMulti<sensor_accel_fifo_s> _sensor_fifo_pub;
@ -106,6 +107,9 @@ private:
matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f};
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};
matrix::Vector3f _delta_velocity_prev{0.0f, 0.0f, 0.0f}; // delta velocity from the previous IMU measurement
float _vibration_metric{0.0f}; // high frequency vibration level in the IMU delta velocity data (m/s)
int _class_device_instance{-1};

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

@ -123,6 +123,17 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
const Vector3f raw{x, y, z};
// Clipping
sensor_gyro_status_s &status = _sensor_status_pub.get();
const float clip_limit = (_range / _scale) * 0.95f;
for (int i = 0; i < 3; i++) {
if (fabsf(raw(i)) > clip_limit) {
status.clipping[i]++;
_integrator_clipping++;
}
}
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
@ -157,6 +168,8 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
Vector3f integrated_value;
uint32_t integral_dt = 0;
_integrator_samples++;
if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
sensor_gyro_s report{};
@ -176,12 +189,34 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
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;
_sensor_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);
}
void
@ -342,6 +377,9 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
report.timestamp = _integrator_timestamp_sample;
_sensor_pub.publish(report);
// update vibration metrics
const Vector3f delta_angle = val_int_calibrated * (integrator_dt_us * 1.e-6f);
UpdateVibrationMetrics(delta_angle);
// reset integrator
ResetIntegrator();
@ -390,6 +428,20 @@ PX4Gyroscope::ConfigureFilter(float cutoff_freq)
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
}
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()
{

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

@ -88,8 +88,9 @@ public:
private:
void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
void ConfigureFilter(float cutoff_freq);
void ResetIntegrator();
void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub; // legacy message
uORB::PublicationMulti<sensor_gyro_control_s> _sensor_control_pub;
@ -108,6 +109,10 @@ private:
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};
matrix::Vector3f _delta_angle_prev{0.0f, 0.0f, 0.0f}; // delta angle from the previous IMU measurement
float _vibration_metric{0.0f}; // high frequency vibration level in the IMU delta angle data (rad)
float _coning_vibration{0.0f}; // Level of coning vibration in the IMU delta angles (rad^2)
int _class_device_instance{-1};