mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-18 10:30:35 +08:00
sensor accel/gyro message cleanup
- 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
@@ -31,7 +31,10 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(drivers_accelerometer PX4Accelerometer.cpp)
|
||||
px4_add_library(drivers_accelerometer
|
||||
PX4Accelerometer.cpp
|
||||
PX4Accelerometer.hpp
|
||||
)
|
||||
target_link_libraries(drivers_accelerometer
|
||||
PRIVATE
|
||||
drivers__device
|
||||
|
||||
@@ -44,6 +44,7 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
|
||||
ModuleParams(nullptr),
|
||||
_sensor_pub{ORB_ID(sensor_accel), priority},
|
||||
_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
|
||||
_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
|
||||
_sensor_status_pub{ORB_ID(sensor_accel_status), priority},
|
||||
_device_id{device_id},
|
||||
_rotation{rotation}
|
||||
@@ -62,8 +63,7 @@ PX4Accelerometer::~PX4Accelerometer()
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
case ACCELIOCSSCALE: {
|
||||
@@ -85,8 +85,7 @@ PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
PX4Accelerometer::set_device_type(uint8_t devtype)
|
||||
void PX4Accelerometer::set_device_type(uint8_t devtype)
|
||||
{
|
||||
// current DeviceStructure
|
||||
union device::Device::DeviceId device_id;
|
||||
@@ -95,41 +94,36 @@ PX4Accelerometer::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
|
||||
PX4Accelerometer::set_sample_rate(uint16_t rate)
|
||||
void PX4Accelerometer::set_sample_rate(uint16_t rate)
|
||||
{
|
||||
_sample_rate = rate;
|
||||
|
||||
ConfigureFilter(_filter.get_cutoff_freq());
|
||||
}
|
||||
|
||||
void
|
||||
PX4Accelerometer::set_update_rate(uint16_t rate)
|
||||
void PX4Accelerometer::set_update_rate(uint16_t rate)
|
||||
{
|
||||
const uint32_t update_interval = 1000000 / rate;
|
||||
|
||||
_integrator_reset_samples = 4000 / update_interval;
|
||||
}
|
||||
|
||||
void
|
||||
PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
|
||||
void PX4Accelerometer::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_accel_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++;
|
||||
}
|
||||
}
|
||||
@@ -140,62 +134,61 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
|
||||
// Filtered values
|
||||
const Vector3f val_filtered{_filter.apply(val_calibrated)};
|
||||
|
||||
|
||||
// Integrated values
|
||||
Vector3f integrated_value;
|
||||
Vector3f delta_velocity;
|
||||
uint32_t integral_dt = 0;
|
||||
|
||||
if (_integrator_samples == 0) {
|
||||
_integrator_timestamp_sample = timestamp_sample;
|
||||
}
|
||||
|
||||
_integrator_samples++;
|
||||
|
||||
if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
|
||||
if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
|
||||
|
||||
sensor_accel_s report{};
|
||||
report.timestamp = timestamp;
|
||||
// publish control data (filtered)
|
||||
{
|
||||
sensor_accel_s report{};
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.x = val_filtered(0);
|
||||
report.y = val_filtered(1);
|
||||
report.z = val_filtered(2);
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
// fill sensor_accel_integrated and publish
|
||||
sensor_accel_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_velocity.copyTo(report.delta_velocity);
|
||||
report.dt = integral_dt;
|
||||
report.samples = _integrator_samples;
|
||||
report.clip_count = _integrator_clipping;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
// Raw values (ADC units 0 - 65535)
|
||||
report.x_raw = x;
|
||||
report.y_raw = y;
|
||||
report.z_raw = z;
|
||||
_sensor_integrated_pub.publish(report);
|
||||
|
||||
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;
|
||||
|
||||
_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);
|
||||
PublishStatus();
|
||||
}
|
||||
|
||||
void
|
||||
PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
{
|
||||
// filtered data (control)
|
||||
float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
|
||||
@@ -211,45 +204,31 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
|
||||
|
||||
|
||||
// status
|
||||
{
|
||||
sensor_accel_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);
|
||||
}
|
||||
|
||||
|
||||
@@ -284,6 +263,22 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
|
||||
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
|
||||
|
||||
// publish control data (filtered)
|
||||
{
|
||||
sensor_accel_s report{};
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
|
||||
|
||||
// average integrated values to apply calibration
|
||||
@@ -297,37 +292,25 @@ PX4Accelerometer::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).emult(_calibration_scale))};
|
||||
val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore
|
||||
Vector3f delta_velocity{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))};
|
||||
delta_velocity *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore
|
||||
|
||||
// publish
|
||||
sensor_accel_s report{};
|
||||
// fill sensor_accel_integrated and publish
|
||||
sensor_accel_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_velocity.copyTo(report.delta_velocity);
|
||||
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_velocity = val_int_calibrated * (integrator_dt_us * 1.e-6f);
|
||||
UpdateVibrationMetrics(delta_velocity);
|
||||
|
||||
// reset integrator
|
||||
@@ -337,6 +320,7 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
_timestamp_sample_prev = sample.timestamp_sample;
|
||||
}
|
||||
|
||||
// publish sensor fifo
|
||||
sensor_accel_fifo_s fifo{};
|
||||
|
||||
fifo.device_id = _device_id;
|
||||
@@ -351,10 +335,36 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
|
||||
fifo.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(fifo);
|
||||
|
||||
|
||||
PublishStatus();
|
||||
}
|
||||
|
||||
void
|
||||
PX4Accelerometer::ResetIntegrator()
|
||||
void PX4Accelerometer::PublishStatus()
|
||||
{
|
||||
// publish sensor status
|
||||
if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) {
|
||||
sensor_accel_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.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 PX4Accelerometer::ResetIntegrator()
|
||||
{
|
||||
_integrator_samples = 0;
|
||||
_integrator_fifo_samples = 0;
|
||||
@@ -367,8 +377,7 @@ PX4Accelerometer::ResetIntegrator()
|
||||
_timestamp_sample_prev = 0;
|
||||
}
|
||||
|
||||
void
|
||||
PX4Accelerometer::ConfigureFilter(float cutoff_freq)
|
||||
void PX4Accelerometer::ConfigureFilter(float cutoff_freq)
|
||||
{
|
||||
_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
|
||||
|
||||
@@ -377,8 +386,7 @@ PX4Accelerometer::ConfigureFilter(float cutoff_freq)
|
||||
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
|
||||
}
|
||||
|
||||
void
|
||||
PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
|
||||
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;
|
||||
@@ -387,8 +395,7 @@ PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
|
||||
_delta_velocity_prev = delta_velocity;
|
||||
}
|
||||
|
||||
void
|
||||
PX4Accelerometer::print_status()
|
||||
void PX4Accelerometer::print_status()
|
||||
{
|
||||
PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
|
||||
PX4_INFO("sample rate: %d Hz", _sample_rate);
|
||||
@@ -398,5 +405,4 @@ PX4Accelerometer::print_status()
|
||||
(double)_calibration_scale(2));
|
||||
PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
|
||||
(double)_calibration_offset(2));
|
||||
|
||||
}
|
||||
|
||||
@@ -45,11 +45,11 @@
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_accel_fifo.h>
|
||||
#include <uORB/topics/sensor_accel_integrated.h>
|
||||
#include <uORB/topics/sensor_accel_status.h>
|
||||
|
||||
class PX4Accelerometer : public cdev::CDev, public ModuleParams
|
||||
{
|
||||
|
||||
public:
|
||||
PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
|
||||
~PX4Accelerometer() override;
|
||||
@@ -67,7 +67,7 @@ public:
|
||||
void set_temperature(float temperature) { _temperature = temperature; }
|
||||
void set_update_rate(uint16_t rate);
|
||||
|
||||
void update(hrt_abstime timestamp, float x, float y, float z);
|
||||
void update(hrt_abstime timestamp_sample, float x, float y, float z);
|
||||
|
||||
void print_status();
|
||||
|
||||
@@ -89,12 +89,14 @@ public:
|
||||
private:
|
||||
|
||||
void ConfigureFilter(float cutoff_freq);
|
||||
void PublishStatus();
|
||||
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;
|
||||
uORB::PublicationMultiData<sensor_accel_status_s> _sensor_status_pub;
|
||||
uORB::PublicationMulti<sensor_accel_s> _sensor_pub;
|
||||
uORB::PublicationMulti<sensor_accel_fifo_s> _sensor_fifo_pub;
|
||||
uORB::PublicationMulti<sensor_accel_integrated_s> _sensor_integrated_pub;
|
||||
uORB::PublicationMulti<sensor_accel_status_s> _sensor_status_pub;
|
||||
|
||||
math::LowPassFilter2pVector3f _filter{1000, 100};
|
||||
|
||||
@@ -102,6 +104,8 @@ private:
|
||||
math::LowPassFilter2pArray _filterArrayY{4000, 100};
|
||||
math::LowPassFilter2pArray _filterArrayZ{4000, 100};
|
||||
|
||||
hrt_abstime _status_last_publish{0};
|
||||
|
||||
Integrator _integrator{4000, false};
|
||||
|
||||
matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f};
|
||||
@@ -112,9 +116,7 @@ private:
|
||||
|
||||
int _class_device_instance{-1};
|
||||
|
||||
|
||||
uint32_t _device_id{0};
|
||||
|
||||
const enum Rotation _rotation;
|
||||
|
||||
float _range{16.0f * CONSTANTS_ONE_G};
|
||||
@@ -123,6 +125,8 @@ private:
|
||||
|
||||
uint64_t _error_count{0};
|
||||
|
||||
uint32_t _clipping[3] {};
|
||||
|
||||
uint16_t _sample_rate{1000};
|
||||
uint16_t _update_rate{1000};
|
||||
|
||||
@@ -138,5 +142,4 @@ private:
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
|
||||
)
|
||||
|
||||
};
|
||||
|
||||
@@ -31,5 +31,8 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(drivers_gyroscope PX4Gyroscope.cpp)
|
||||
px4_add_library(drivers_gyroscope
|
||||
PX4Gyroscope.cpp
|
||||
PX4Gyroscope.hpp
|
||||
)
|
||||
target_link_libraries(drivers_gyroscope PRIVATE drivers__device)
|
||||
|
||||
@@ -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));
|
||||
|
||||
}
|
||||
|
||||
@@ -44,13 +44,12 @@
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/sensor_gyro_control.h>
|
||||
#include <uORB/topics/sensor_gyro_fifo.h>
|
||||
#include <uORB/topics/sensor_gyro_integrated.h>
|
||||
#include <uORB/topics/sensor_gyro_status.h>
|
||||
|
||||
class PX4Gyroscope : public cdev::CDev, public ModuleParams
|
||||
{
|
||||
|
||||
public:
|
||||
PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
|
||||
~PX4Gyroscope() override;
|
||||
@@ -68,7 +67,7 @@ public:
|
||||
void set_temperature(float temperature) { _temperature = temperature; }
|
||||
void set_update_rate(uint16_t rate);
|
||||
|
||||
void update(hrt_abstime timestamp, float x, float y, float z);
|
||||
void update(hrt_abstime timestamp_sample, float x, float y, float z);
|
||||
|
||||
void print_status();
|
||||
|
||||
@@ -91,18 +90,20 @@ private:
|
||||
|
||||
void ConfigureFilter(float cutoff_freq);
|
||||
void ConfigureNotchFilter(float notch_freq, float bandwidth);
|
||||
void PublishStatus();
|
||||
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;
|
||||
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub;
|
||||
uORB::PublicationMultiData<sensor_gyro_status_s> _sensor_status_pub;
|
||||
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub;
|
||||
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub;
|
||||
uORB::PublicationMulti<sensor_gyro_integrated_s> _sensor_integrated_pub;
|
||||
uORB::PublicationMulti<sensor_gyro_status_s> _sensor_status_pub;
|
||||
|
||||
math::LowPassFilter2pVector3f _filter{1000, 100};
|
||||
math::NotchFilter<matrix::Vector3f> _notch_filter{};
|
||||
|
||||
hrt_abstime _control_last_publish{0};
|
||||
hrt_abstime _status_last_publish{0};
|
||||
|
||||
math::LowPassFilter2pArray _filterArrayX{8000, 100};
|
||||
math::LowPassFilter2pArray _filterArrayY{8000, 100};
|
||||
@@ -118,9 +119,7 @@ private:
|
||||
|
||||
int _class_device_instance{-1};
|
||||
|
||||
|
||||
uint32_t _device_id{0};
|
||||
|
||||
const enum Rotation _rotation;
|
||||
|
||||
float _range{math::radians(2000.0f)};
|
||||
@@ -129,6 +128,8 @@ private:
|
||||
|
||||
uint64_t _error_count{0};
|
||||
|
||||
uint32_t _clipping[3] {};
|
||||
|
||||
uint16_t _sample_rate{1000};
|
||||
uint16_t _update_rate{1000};
|
||||
|
||||
@@ -147,5 +148,4 @@ private:
|
||||
(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
|
||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
|
||||
)
|
||||
|
||||
};
|
||||
|
||||
@@ -54,8 +54,7 @@ PX4Magnetometer::~PX4Magnetometer()
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
case MAGIOCSSCALE: {
|
||||
@@ -91,8 +90,7 @@ PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
PX4Magnetometer::set_device_type(uint8_t devtype)
|
||||
void PX4Magnetometer::set_device_type(uint8_t devtype)
|
||||
{
|
||||
// current DeviceStructure
|
||||
union device::Device::DeviceId device_id;
|
||||
@@ -105,19 +103,15 @@ PX4Magnetometer::set_device_type(uint8_t devtype)
|
||||
_sensor_mag_pub.get().device_id = device_id.devid;
|
||||
}
|
||||
|
||||
void
|
||||
PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
|
||||
void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
|
||||
{
|
||||
sensor_mag_s &report = _sensor_mag_pub.get();
|
||||
report.timestamp = timestamp;
|
||||
report.timestamp = timestamp_sample;
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
float xraw_f = x;
|
||||
float yraw_f = y;
|
||||
float zraw_f = z;
|
||||
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
|
||||
const matrix::Vector3f raw_f{xraw_f, yraw_f, zraw_f};
|
||||
const matrix::Vector3f raw_f{x, y, z};
|
||||
|
||||
// Apply range scale and the calibrating offset/scale
|
||||
const matrix::Vector3f val_calibrated{(((raw_f.emult(_sensitivity) * report.scaling) - _calibration_offset).emult(_calibration_scale))};
|
||||
@@ -134,8 +128,7 @@ PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
|
||||
_sensor_mag_pub.update();
|
||||
}
|
||||
|
||||
void
|
||||
PX4Magnetometer::print_status()
|
||||
void PX4Magnetometer::print_status()
|
||||
{
|
||||
PX4_INFO(MAG_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
|
||||
|
||||
@@ -143,6 +136,4 @@ PX4Magnetometer::print_status()
|
||||
(double)_calibration_scale(2));
|
||||
PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
|
||||
(double)_calibration_offset(2));
|
||||
|
||||
print_message(_sensor_mag_pub.get());
|
||||
}
|
||||
|
||||
@@ -45,7 +45,7 @@ class PX4Magnetometer : public cdev::CDev
|
||||
{
|
||||
|
||||
public:
|
||||
PX4Magnetometer(uint32_t device_id, uint8_t priority, enum Rotation rotation);
|
||||
PX4Magnetometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
|
||||
~PX4Magnetometer() override;
|
||||
|
||||
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
|
||||
@@ -57,7 +57,7 @@ public:
|
||||
void set_external(bool external) { _sensor_mag_pub.get().is_external = external; }
|
||||
void set_sensitivity(float x, float y, float z) { _sensitivity = matrix::Vector3f{x, y, z}; }
|
||||
|
||||
void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
|
||||
void update(hrt_abstime timestamp_sample, float x, float y, float z);
|
||||
|
||||
void print_status();
|
||||
|
||||
|
||||
Reference in New Issue
Block a user