Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

PX4Accelerometer and PX4Gyroscope always publish status #13794

Merged
merged 2 commits into from
Dec 28, 2019
Merged

PX4Accelerometer and PX4Gyroscope always publish status #13794

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b0aa366
PX4Accelerometer and PX4Gyroscope add vibration metrics and always pu…
dagar Dec 27, 2019
f6851e5
Merge branch 'master' into pr-sensors_status
dagar Dec 27, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions .ci/Jenkinsfile-hardware
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"'
Expand Down Expand Up @@ -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"'
Expand Down
2 changes: 2 additions & 0 deletions msg/sensor_accel_status.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 changes: 4 additions & 0 deletions msg/sensor_gyro_status.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 changes: 47 additions & 0 deletions src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))};

Expand All @@ -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{};
Expand All @@ -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
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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()
{
Expand Down
8 changes: 6 additions & 2 deletions src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,9 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams

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;
Expand All @@ -106,6 +107,9 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
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};


Expand Down
52 changes: 52 additions & 0 deletions src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)};

Expand Down Expand Up @@ -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{};
Expand All @@ -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
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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()
{
Expand Down
9 changes: 7 additions & 2 deletions src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,9 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams

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;
Expand All @@ -108,6 +109,10 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams

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};


Expand Down