fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08: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
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
34 changed files with 696 additions and 756 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.ci/Jenkinsfile-hardware
View File

@ -698,9 +698,13 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener adc_report"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener battery_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_fifo"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_integrated"'
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_fifo"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro_integrated"'
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 "listener servorail_status"'
@ -743,9 +747,13 @@ void statusSEGGER() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener adc_report"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener battery_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_fifo"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel_integrated"'
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_fifo"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro_integrated"'
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 "listener servorail_status"'

7
msg/CMakeLists.txt
View File

@ -60,8 +60,8 @@ set(msg_files
ekf_gps_position.msg
esc_report.msg
esc_status.msg
estimator_status.msg
estimator_innovations.msg
estimator_status.msg
follow_target.msg
geofence_result.msg
gps_dump.msg
@ -105,14 +105,15 @@ set(msg_files
satellite_info.msg
sensor_accel.msg
sensor_accel_fifo.msg
sensor_accel_integrated.msg
sensor_accel_status.msg
sensor_baro.msg
sensor_bias.msg
sensor_combined.msg
sensor_correction.msg
sensor_gyro.msg
sensor_gyro_control.msg
sensor_gyro_fifo.msg
sensor_gyro_integrated.msg
sensor_gyro_status.msg
sensor_mag.msg
sensor_preflight.msg
@ -156,7 +157,7 @@ set(msg_files
vtol_vehicle_status.msg
wheel_encoders.msg
wind_estimate.msg
)
)
set(deprecated_msgs
ekf2_innovations.msg # 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'.

26
msg/sensor_accel.msg
View File

@ -1,22 +1,10 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint64 error_count
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 x # acceleration in the NED X board axis in m/s^2
float32 y # acceleration in the NED Y board axis in m/s^2
float32 z # acceleration in the NED Z board axis in m/s^2
float32 x # acceleration in the NED X board axis in m/s^2
float32 y # acceleration in the NED Y board axis in m/s^2
float32 z # acceleration in the NED Z board axis in m/s^2
uint32 integral_dt # integration time (microseconds)
uint8 integral_samples # number of samples integrated
float32 x_integral # delta velocity in the NED X board axis in m/s over the integration time frame (integral_dt)
float32 y_integral # delta velocity in the NED Y board axis in m/s over the integration time frame (integral_dt)
float32 z_integral # delta velocity in the NED Z board axis in m/s over the integration time frame (integral_dt)
uint8 integral_clip_count # total clip count per integration period on any axis
float32 temperature # temperature in degrees celsius
float32 scaling # scaling from raw to m/s^2
int16 x_raw
int16 y_raw
int16 z_raw
float32 temperature # temperature in degrees celsius

16
msg/sensor_accel_fifo.msg
View File

@ -1,13 +1,13 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint64 timestamp_sample # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 dt # delta time between samples (microseconds)
float32 dt # delta time between samples (microseconds)
float32 scale
uint8 samples # number of valid samples
uint8 samples # number of valid samples
int16[16] x # acceleration in the NED X board axis in m/s/s
int16[16] y # acceleration in the NED Y board axis in m/s/s
int16[16] z # acceleration in the NED Z board axis in m/s/s
int16[16] x # acceleration in the NED X board axis in m/s/s
int16[16] y # acceleration in the NED Y board axis in m/s/s
int16[16] z # acceleration in the NED Z board axis in m/s/s

13
msg/sensor_accel_integrated.msg Normal file
View File

@ -0,0 +1,13 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 temperature # temperature in degrees celsius
uint64 error_count
float32[3] delta_velocity # delta velocity in the NED board axis in m/s over the integration time frame (dt)
uint16 dt # integration time (microseconds)
uint8 samples # number of samples integrated
uint8 clip_count # total clip count per integration period on any axis

9
msg/sensor_accel_status.msg
View File

@ -1,5 +1,6 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 error_count
@ -7,11 +8,11 @@ float32 temperature
uint8 rotation
uint64[3] clipping # clipping per axis
uint32[3] clipping # clipping per axis
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)
float32 vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s)

3
msg/sensor_bias.msg
View File

@ -7,8 +7,11 @@ uint64 timestamp # time since system start (microseconds)
# In-run bias estimates (subtract from uncorrected data)
uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles
float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s)
uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles
float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2)
uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles
float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss)

4
msg/sensor_correction.msg
View File

@ -55,3 +55,7 @@ uint8 selected_baro_instance # barometric pressure uORB topic instance for the v
uint8[3] gyro_mapping
uint8[3] accel_mapping
uint8[3] baro_mapping
uint32[3] gyro_device_ids
uint32[3] accel_device_ids
uint32[3] baro_device_ids

26
msg/sensor_gyro.msg
View File

@ -1,22 +1,10 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint64 error_count
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 x # angular velocity in the NED X board axis in rad/s
float32 y # angular velocity in the NED Y board axis in rad/s
float32 z # angular velocity in the NED Z board axis in rad/s
float32 x # angular velocity in the NED X board axis in rad/s
float32 y # angular velocity in the NED Y board axis in rad/s
float32 z # angular velocity in the NED Z board axis in rad/s
uint32 integral_dt # integration time (microseconds)
uint8 integral_samples # number of samples integrated
float32 x_integral # delta angle in the NED X board axis in rad/s over the integration time frame (integral_dt)
float32 y_integral # delta angle in the NED Y board axis in rad/s over the integration time frame (integral_dt)
float32 z_integral # delta angle in the NED Z board axis in rad/s over the integration time frame (integral_dt)
uint8 integral_clip_count # total clip count per integration period on any axis
float32 temperature # temperature in degrees celsius
float32 scaling # scaling from raw to rad/s
int16 x_raw
int16 y_raw
int16 z_raw
float32 temperature # temperature in degrees celsius

11
msg/sensor_gyro_control.msg
View File

@ -1,11 +0,0 @@
# WARNING: Not recommend for custom development.
# This topic is part of an incremental refactoring of the sensor pipeline and will likely disappear post PX4 release v1.10
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # time the raw data was sampled (microseconds)
float32[3] xyz # filtered angular velocity in the board axis in rad/s

16
msg/sensor_gyro_fifo.msg
View File

@ -1,13 +1,13 @@
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 dt # delta time between samples (microseconds)
float32 dt # delta time between samples (microseconds)
float32 scale
uint8 samples # number of valid samples
uint8 samples # number of valid samples
int16[16] x # angular velocity in the NED X board axis in rad/s
int16[16] y # angular velocity in the NED Y board axis in rad/s
int16[16] z # angular velocity in the NED Z board axis in rad/s
int16[16] x # angular velocity in the NED X board axis in rad/s
int16[16] y # angular velocity in the NED Y board axis in rad/s
int16[16] z # angular velocity in the NED Z board axis in rad/s

13
msg/sensor_gyro_integrated.msg Normal file
View File

@ -0,0 +1,13 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
uint32 device_id # unique device ID for the sensor that does not change between power cycles
float32 temperature # temperature in degrees celsius
uint64 error_count
float32[3] delta_angle # delta angle in the NED board axis in rad/s over the integration time frame (dt)
uint16 dt # integration time (microseconds)
uint8 samples # number of samples integrated
uint8 clip_count # total clip count per integration period on any axis

11
msg/sensor_gyro_status.msg
View File

@ -1,5 +1,6 @@
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 timestamp # time since system start (microseconds)
uint32 device_id # unique device ID for the sensor that does not change between power cycles
uint64 error_count
@ -7,13 +8,13 @@ float32 temperature
uint8 rotation
uint64[3] clipping # clipping per axis
uint32[3] clipping # clipping per axis
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 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)
float32 coning_vibration # Level of coning vibration in the IMU delta angles (rad^2)

6
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -256,8 +256,6 @@ rtps:
id: 112
- msg: vehicle_acceleration
id: 113
- msg: sensor_gyro_control
id: 114
- msg: airspeed_validated
id: 115
- msg: onboard_computer_status
@ -273,6 +271,10 @@ rtps:
id: 120
- msg: sensor_gyro_status
id: 121
- msg: sensor_accel_integrated
id: 122
- msg: sensor_gyro_integrated
id: 123
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 150

5
src/lib/drivers/accelerometer/CMakeLists.txt
View File

@ -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

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

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

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

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

5
src/lib/drivers/gyroscope/CMakeLists.txt
View File

@ -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)

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

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

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

23
src/lib/drivers/magnetometer/PX4Magnetometer.cpp
View File

@ -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());
}

4
src/lib/drivers/magnetometer/PX4Magnetometer.hpp
View File

@ -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();

4
src/modules/ekf2/ekf2_main.cpp
View File

@ -1472,6 +1472,10 @@ void Ekf2::Run()
bias.timestamp = now;
bias.gyro_device_id = _sensor_selection.gyro_device_id;
bias.accel_device_id = _sensor_selection.accel_device_id;
bias.mag_device_id = _sensor_selection.mag_device_id;
// In-run bias estimates
_ekf.get_gyro_bias(bias.gyro_bias);
_ekf.get_accel_bias(bias.accel_bias);

1
src/modules/logger/logged_topics.cpp
View File

@ -69,6 +69,7 @@ void LoggedTopics::add_default_topics()
add_topic("position_setpoint_triplet", 200);
add_topic("radio_status");
add_topic("rate_ctrl_status", 200);
add_topic("sensor_bias", 1000);
add_topic("sensor_combined", 100);
add_topic("sensor_correction", 1000);
add_topic("sensor_preflight", 200);

144
src/modules/mavlink/mavlink_messages.cpp
View File

@ -60,13 +60,14 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/camera_capture.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/estimator_status.h>
@ -75,18 +76,22 @@
#include <uORB/topics/input_rc.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/mount_orientation.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/orbit_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_accel_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@ -97,21 +102,16 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/mount_orientation.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/uORB.h>
using matrix::Vector3f;
using matrix::wrap_2pi;
static uint16_t cm_uint16_from_m_float(float m);
@ -952,8 +952,8 @@ private:
protected:
explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel), 0)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro), 0)),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 0)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 0)),
_raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 0)),
_raw_accel_time(0),
_raw_gyro_time(0),
@ -962,9 +962,9 @@ protected:
bool send(const hrt_abstime t) override
{
sensor_accel_s sensor_accel = {};
sensor_gyro_s sensor_gyro = {};
sensor_mag_s sensor_mag = {};
sensor_accel_integrated_s sensor_accel{};
sensor_gyro_integrated_s sensor_gyro{};
sensor_mag_s sensor_mag{};
bool updated = false;
updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
@ -972,19 +972,27 @@ protected:
updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag);
if (updated) {
mavlink_scaled_imu_t msg = {};
mavlink_scaled_imu_t msg{};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g]
msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g]
msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g]
msg.xgyro = sensor_gyro.x_raw; // [milli rad/s]
msg.ygyro = sensor_gyro.y_raw; // [milli rad/s]
msg.zgyro = sensor_gyro.z_raw; // [milli rad/s]
msg.xmag = sensor_mag.x_raw; // [milli tesla]
msg.ymag = sensor_mag.y_raw; // [milli tesla]
msg.zmag = sensor_mag.z_raw; // [milli tesla]
// Accelerometer in mG
const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
// Gyroscope in mrad/s
const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
msg.xacc = (int16_t)accel(0);
msg.yacc = (int16_t)accel(1);
msg.zacc = (int16_t)accel(2);
msg.xgyro = gyro(0);
msg.ygyro = gyro(1);
msg.zgyro = gyro(2);
msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
mavlink_msg_scaled_imu_send_struct(_mavlink->get_channel(), &msg);
@ -1044,8 +1052,8 @@ private:
protected:
explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel), 1)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro), 1)),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 1)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 1)),
_raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 1)),
_raw_accel_time(0),
_raw_gyro_time(0),
@ -1054,9 +1062,9 @@ protected:
bool send(const hrt_abstime t) override
{
sensor_accel_s sensor_accel = {};
sensor_gyro_s sensor_gyro = {};
sensor_mag_s sensor_mag = {};
sensor_accel_integrated_s sensor_accel{};
sensor_gyro_integrated_s sensor_gyro{};
sensor_mag_s sensor_mag{};
bool updated = false;
updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
@ -1064,19 +1072,27 @@ protected:
updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag);
if (updated) {
mavlink_scaled_imu2_t msg = {};
mavlink_scaled_imu2_t msg{};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g]
msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g]
msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g]
msg.xgyro = sensor_gyro.x_raw; // [milli rad/s]
msg.ygyro = sensor_gyro.y_raw; // [milli rad/s]
msg.zgyro = sensor_gyro.z_raw; // [milli rad/s]
msg.xmag = sensor_mag.x_raw; // [milli tesla]
msg.ymag = sensor_mag.y_raw; // [milli tesla]
msg.zmag = sensor_mag.z_raw; // [milli tesla]
// Accelerometer in mG
const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
// Gyroscope in mrad/s
const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
msg.xacc = (int16_t)accel(0);
msg.yacc = (int16_t)accel(1);
msg.zacc = (int16_t)accel(2);
msg.xgyro = gyro(0);
msg.ygyro = gyro(1);
msg.zgyro = gyro(2);
msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
mavlink_msg_scaled_imu2_send_struct(_mavlink->get_channel(), &msg);
@ -1135,8 +1151,8 @@ private:
protected:
explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel), 2)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro), 2)),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 2)),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 2)),
_raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 2)),
_raw_accel_time(0),
_raw_gyro_time(0),
@ -1145,9 +1161,9 @@ protected:
bool send(const hrt_abstime t) override
{
sensor_accel_s sensor_accel = {};
sensor_gyro_s sensor_gyro = {};
sensor_mag_s sensor_mag = {};
sensor_accel_integrated_s sensor_accel{};
sensor_gyro_integrated_s sensor_gyro{};
sensor_mag_s sensor_mag{};
bool updated = false;
updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
@ -1155,19 +1171,27 @@ protected:
updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag);
if (updated) {
mavlink_scaled_imu3_t msg = {};
mavlink_scaled_imu3_t msg{};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g]
msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g]
msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g]
msg.xgyro = sensor_gyro.x_raw; // [milli rad/s]
msg.ygyro = sensor_gyro.y_raw; // [milli rad/s]
msg.zgyro = sensor_gyro.z_raw; // [milli rad/s]
msg.xmag = sensor_mag.x_raw; // [milli tesla]
msg.ymag = sensor_mag.y_raw; // [milli tesla]
msg.zmag = sensor_mag.z_raw; // [milli tesla]
// Accelerometer in mG
const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
// Gyroscope in mrad/s
const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
msg.xacc = (int16_t)accel(0);
msg.yacc = (int16_t)accel(1);
msg.zacc = (int16_t)accel(2);
msg.xgyro = gyro(0);
msg.ygyro = gyro(1);
msg.zgyro = gyro(2);
msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
mavlink_msg_scaled_imu3_send_struct(_mavlink->get_channel(), &msg);

12
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -2088,9 +2088,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
sensor_gyro_s gyro{};
gyro.timestamp = timestamp;
gyro.x_raw = imu.xgyro * 1000.0f;
gyro.y_raw = imu.ygyro * 1000.0f;
gyro.z_raw = imu.zgyro * 1000.0f;
gyro.x = imu.xgyro;
gyro.y = imu.ygyro;
gyro.z = imu.zgyro;
@ -2104,9 +2101,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
sensor_accel_s accel{};
accel.timestamp = timestamp;
accel.x_raw = imu.xacc / (CONSTANTS_ONE_G / 1000.0f);
accel.y_raw = imu.yacc / (CONSTANTS_ONE_G / 1000.0f);
accel.z_raw = imu.zacc / (CONSTANTS_ONE_G / 1000.0f);
accel.x = imu.xacc;
accel.y = imu.yacc;
accel.z = imu.zacc;
@ -2507,9 +2501,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
sensor_accel_s accel{};
accel.timestamp = timestamp;
accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f;
accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f;
accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f;
accel.x = hil_state.xacc;
accel.y = hil_state.yacc;
accel.z = hil_state.zacc;
@ -2523,9 +2514,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
sensor_gyro_s gyro{};
gyro.timestamp = timestamp;
gyro.x_raw = hil_state.rollspeed * 1e3f;
gyro.y_raw = hil_state.pitchspeed * 1e3f;
gyro.z_raw = hil_state.yawspeed * 1e3f;
gyro.x = hil_state.rollspeed;
gyro.y = hil_state.pitchspeed;
gyro.z = hil_state.yawspeed;

1
src/modules/sensors/vehicle_acceleration/CMakeLists.txt
View File

@ -33,5 +33,6 @@
px4_add_library(vehicle_acceleration
VehicleAcceleration.cpp
VehicleAcceleration.hpp
)
target_link_libraries(vehicle_acceleration PRIVATE px4_work_queue)

138
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@ -49,26 +49,18 @@ VehicleAcceleration::~VehicleAcceleration()
Stop();
}
bool
VehicleAcceleration::Start()
bool VehicleAcceleration::Start()
{
// initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset.zero();
_bias.zero();
// force initial updates
ParametersUpdate(true);
SensorBiasUpdate(true);
SensorCorrectionsUpdate(true);
// needed to change the active sensor if the primary stops updating
_sensor_selection_sub.registerCallback();
return SensorCorrectionsUpdate(true);
return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
}
void
VehicleAcceleration::Stop()
void VehicleAcceleration::Stop()
{
Deinit();
@ -80,21 +72,23 @@ VehicleAcceleration::Stop()
_sensor_selection_sub.unregisterCallback();
}
void
VehicleAcceleration::SensorBiasUpdate(bool force)
void VehicleAcceleration::SensorBiasUpdate(bool force)
{
if (_sensor_bias_sub.updated() || force) {
sensor_bias_s bias;
if (_sensor_bias_sub.copy(&bias)) {
// TODO: should be checking device ID
_bias = Vector3f{bias.accel_bias};
if (bias.accel_device_id == _selected_sensor_device_id) {
_bias = Vector3f{bias.accel_bias};
} else {
_bias.zero();
}
}
}
}
bool
VehicleAcceleration::SensorCorrectionsUpdate(bool force)
void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
@ -102,49 +96,82 @@ VehicleAcceleration::SensorCorrectionsUpdate(bool force)
sensor_correction_s corrections{};
_sensor_correction_sub.copy(&corrections);
// TODO: should be checking device ID
if (_selected_sensor == 0) {
// selected sensor has changed, find updated index
if ((_corrections_selected_instance != corrections.selected_accel_instance) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.accel_device_ids[i] == _selected_sensor_device_id) {
_corrections_selected_instance = i;
}
}
}
switch (_corrections_selected_instance) {
case 0:
_offset = Vector3f{corrections.accel_offset_0};
_scale = Vector3f{corrections.accel_scale_0};
} else if (_selected_sensor == 1) {
break;
case 1:
_offset = Vector3f{corrections.accel_offset_1};
_scale = Vector3f{corrections.accel_scale_1};
} else if (_selected_sensor == 2) {
break;
case 2:
_offset = Vector3f{corrections.accel_offset_2};
_scale = Vector3f{corrections.accel_scale_2};
} else {
break;
default:
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
}
}
}
// update the latest sensor selection
if ((_selected_sensor != corrections.selected_accel_instance) || force) {
if (corrections.selected_accel_instance < MAX_SENSOR_COUNT) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
bool VehicleAcceleration::SensorSelectionUpdate(bool force)
{
if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection);
const int sensor_new = corrections.selected_accel_instance;
if (_selected_sensor_device_id != sensor_selection.accel_device_id) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
if (_sensor_sub[sensor_new].registerCallback()) {
PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
_selected_sensor = sensor_new;
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
sensor_accel_s report{};
_sensor_sub[i].copy(&report);
return true;
if ((report.device_id != 0) && (report.device_id == sensor_selection.accel_device_id)) {
if (_sensor_sub[i].registerCallback()) {
PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
// record selected sensor (array index)
_selected_sensor_sub_index = i;
_selected_sensor_device_id = sensor_selection.accel_device_id;
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
return true;
}
}
}
PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.accel_device_id);
_selected_sensor_device_id = 0;
_selected_sensor_sub_index = 0;
}
}
return false;
}
void
VehicleAcceleration::ParametersUpdate(bool force)
void VehicleAcceleration::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_params_sub.updated() || force) {
@ -167,20 +194,19 @@ VehicleAcceleration::ParametersUpdate(bool force)
}
}
void
VehicleAcceleration::Run()
void VehicleAcceleration::Run()
{
// update corrections first to set _selected_sensor
bool sensor_select_update = SensorCorrectionsUpdate();
bool sensor_select_update = SensorSelectionUpdate();
SensorCorrectionsUpdate(sensor_select_update);
SensorBiasUpdate(sensor_select_update);
ParametersUpdate();
if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
sensor_accel_s sensor_data;
_sensor_sub[_selected_sensor].copy(&sensor_data);
_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
ParametersUpdate();
SensorBiasUpdate();
// get the sensor data and correct for thermal errors
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
@ -192,8 +218,10 @@ VehicleAcceleration::Run()
// correct for in-run bias errors
accel -= _bias;
vehicle_acceleration_s out{};
out.timestamp_sample = sensor_data.timestamp;
// publish
vehicle_acceleration_s out;
out.timestamp_sample = sensor_data.timestamp_sample;
accel.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
@ -201,8 +229,10 @@ VehicleAcceleration::Run()
}
}
void
VehicleAcceleration::PrintStatus()
void VehicleAcceleration::PrintStatus()
{
PX4_INFO("selected sensor: %d", _selected_sensor);
PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
}

21
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
View File

@ -56,7 +56,7 @@ class VehicleAcceleration : public ModuleParams, public px4::WorkItem
public:
VehicleAcceleration();
virtual ~VehicleAcceleration();
~VehicleAcceleration() override;
void Run() override;
@ -66,10 +66,10 @@ public:
void PrintStatus();
private:
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
bool SensorCorrectionsUpdate(bool force = false);
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
bool SensorSelectionUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
@ -96,10 +96,11 @@ private:
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Vector3f _offset;
matrix::Vector3f _scale;
matrix::Vector3f _bias;
uint8_t _selected_sensor{0};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
matrix::Vector3f _bias{0.f, 0.f, 0.f};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
int8_t _corrections_selected_instance{-1};
};

1
src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt
View File

@ -33,5 +33,6 @@
px4_add_library(vehicle_angular_velocity
VehicleAngularVelocity.cpp
VehicleAngularVelocity.hpp
)
target_link_libraries(vehicle_angular_velocity PRIVATE px4_work_queue)

225
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -49,26 +49,18 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
Stop();
}
bool
VehicleAngularVelocity::Start()
bool VehicleAngularVelocity::Start()
{
// initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset.zero();
_bias.zero();
// force initial updates
ParametersUpdate(true);
SensorBiasUpdate(true);
SensorCorrectionsUpdate(true);
// needed to change the active sensor if the primary stops updating
_sensor_selection_sub.registerCallback();
return SensorCorrectionsUpdate(true);
return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
}
void
VehicleAngularVelocity::Stop()
void VehicleAngularVelocity::Stop()
{
Deinit();
@ -80,110 +72,106 @@ VehicleAngularVelocity::Stop()
_sensor_selection_sub.unregisterCallback();
}
void
VehicleAngularVelocity::SensorBiasUpdate(bool force)
void VehicleAngularVelocity::SensorBiasUpdate(bool force)
{
if (_sensor_bias_sub.updated() || force) {
sensor_bias_s bias;
if (_sensor_bias_sub.copy(&bias)) {
// TODO: should be checking device ID
_bias = Vector3f{bias.gyro_bias};
if (bias.gyro_device_id == _selected_sensor_device_id) {
_bias = Vector3f{bias.gyro_bias};
} else {
_bias.zero();
}
}
}
}
bool
VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
{
if (_sensor_selection_sub.updated() || force) {
sensor_selection_s sensor_selection;
if (_sensor_selection_sub.copy(&sensor_selection)) {
if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
_selected_sensor_device_id = sensor_selection.gyro_device_id;
force = true;
}
}
}
// check if the selected sensor has updated
if (_sensor_correction_sub.updated() || force) {
sensor_correction_s corrections{};
_sensor_correction_sub.copy(&corrections);
// TODO: should be checking device ID
if (_selected_sensor == 0) {
// selected sensor has changed, find updated index
if ((_corrections_selected_instance != corrections.selected_gyro_instance) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (corrections.gyro_device_ids[i] == _selected_sensor_device_id) {
_corrections_selected_instance = i;
}
}
}
switch (_corrections_selected_instance) {
case 0:
_offset = Vector3f{corrections.gyro_offset_0};
_scale = Vector3f{corrections.gyro_scale_0};
} else if (_selected_sensor == 1) {
break;
case 1:
_offset = Vector3f{corrections.gyro_offset_1};
_scale = Vector3f{corrections.gyro_scale_1};
} else if (_selected_sensor == 2) {
break;
case 2:
_offset = Vector3f{corrections.gyro_offset_2};
_scale = Vector3f{corrections.gyro_scale_2};
} else {
break;
default:
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
}
}
}
// update the latest sensor selection
if ((_selected_sensor != corrections.selected_gyro_instance) || force) {
if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) {
// clear all registered callbacks
for (auto &sub : _sensor_control_sub) {
sub.unregisterCallback();
}
bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
{
if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
sensor_selection_s sensor_selection{};
_sensor_selection_sub.copy(&sensor_selection);
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
// clear all registered callbacks
for (auto &sub : _sensor_sub) {
sub.unregisterCallback();
}
const int sensor_new = corrections.selected_gyro_instance;
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
sensor_gyro_s report{};
_sensor_sub[i].copy(&report);
// subscribe to sensor_gyro_control if available
// currently not all drivers (eg df_*) provide sensor_gyro_control
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
sensor_gyro_control_s report{};
_sensor_control_sub[i].copy(&report);
if ((report.device_id != 0) && (report.device_id == sensor_selection.gyro_device_id)) {
if (_sensor_sub[i].registerCallback()) {
PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
if ((report.device_id != 0) && (report.device_id == _selected_sensor_device_id)) {
if (_sensor_control_sub[i].registerCallback()) {
PX4_DEBUG("selected sensor (control) changed %d -> %d", _selected_sensor, i);
_selected_sensor_control = i;
// record selected sensor (array index)
_selected_sensor_sub_index = i;
_selected_sensor_device_id = sensor_selection.gyro_device_id;
_sensor_control_available = true;
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
// record selected sensor (sensor_gyro orb index)
_selected_sensor = sensor_new;
return true;
}
return true;
}
}
// otherwise fallback to using sensor_gyro (legacy that will be removed)
_sensor_control_available = false;
if (_sensor_sub[sensor_new].registerCallback()) {
PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
_selected_sensor = sensor_new;
return true;
}
}
PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.gyro_device_id);
_selected_sensor_device_id = 0;
_selected_sensor_sub_index = 0;
}
}
return false;
}
void
VehicleAngularVelocity::ParametersUpdate(bool force)
void VehicleAngularVelocity::ParametersUpdate(bool force)
{
// Check if parameters have changed
if (_params_sub.updated() || force) {
@ -206,78 +194,45 @@ VehicleAngularVelocity::ParametersUpdate(bool force)
}
}
void
VehicleAngularVelocity::Run()
void VehicleAngularVelocity::Run()
{
// update corrections first to set _selected_sensor
bool sensor_select_update = SensorCorrectionsUpdate();
bool sensor_select_update = SensorSelectionUpdate();
SensorCorrectionsUpdate(sensor_select_update);
SensorBiasUpdate(sensor_select_update);
ParametersUpdate();
if (_sensor_control_available) {
// using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control
if (_sensor_control_sub[_selected_sensor].updated() || sensor_select_update) {
sensor_gyro_control_s sensor_data;
_sensor_control_sub[_selected_sensor].copy(&sensor_data);
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
sensor_gyro_s sensor_data;
_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
ParametersUpdate();
SensorBiasUpdate();
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// get the sensor data and correct for thermal errors (apply offsets and scale)
Vector3f rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)};
// apply offsets and scale
Vector3f rates{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// correct for in-run bias errors
rates -= _bias;
// correct for in-run bias errors
rates -= _bias;
vehicle_angular_velocity_s angular_velocity;
angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
rates.copyTo(angular_velocity.xyz);
angular_velocity.timestamp = hrt_absolute_time();
// publish
vehicle_angular_velocity_s out;
_vehicle_angular_velocity_pub.publish(angular_velocity);
}
out.timestamp_sample = sensor_data.timestamp_sample;
rates.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
} else {
// otherwise fallback to using sensor_gyro (legacy that will be removed)
if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
sensor_gyro_s sensor_data;
_sensor_sub[_selected_sensor].copy(&sensor_data);
ParametersUpdate();
SensorBiasUpdate();
// get the sensor data and correct for thermal errors
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
Vector3f rates{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// correct for in-run bias errors
rates -= _bias;
vehicle_angular_velocity_s angular_velocity;
angular_velocity.timestamp_sample = sensor_data.timestamp;
rates.copyTo(angular_velocity.xyz);
angular_velocity.timestamp = hrt_absolute_time();
_vehicle_angular_velocity_pub.publish(angular_velocity);
}
_vehicle_angular_velocity_pub.publish(out);
}
}
void
VehicleAngularVelocity::PrintStatus()
void VehicleAngularVelocity::PrintStatus()
{
PX4_INFO("selected sensor: %d", _selected_sensor);
if (_selected_sensor_device_id != 0) {
PX4_INFO("using sensor_gyro_control: %d (%d)", _selected_sensor_device_id, _selected_sensor_control);
} else {
PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor);
}
PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
}

30
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -49,7 +49,6 @@
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_control.h>
#include <uORB/topics/vehicle_angular_velocity.h>
class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
@ -57,7 +56,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
public:
VehicleAngularVelocity();
virtual ~VehicleAngularVelocity();
~VehicleAngularVelocity() override;
void Run() override;
@ -67,10 +66,10 @@ public:
void PrintStatus();
private:
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
bool SensorCorrectionsUpdate(bool force = false);
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
bool SensorSelectionUpdate(bool force = false);
static constexpr int MAX_SENSOR_COUNT = 3;
@ -89,28 +88,19 @@ private:
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */
{this, ORB_ID(sensor_gyro), 0},
{this, ORB_ID(sensor_gyro), 1},
{this, ORB_ID(sensor_gyro), 2}
};
uORB::SubscriptionCallbackWorkItem _sensor_control_sub[MAX_SENSOR_COUNT] { /**< sensor control data subscription */
{this, ORB_ID(sensor_gyro_control), 0},
{this, ORB_ID(sensor_gyro_control), 1},
{this, ORB_ID(sensor_gyro_control), 2}
};
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Vector3f _offset;
matrix::Vector3f _scale;
matrix::Vector3f _bias;
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor{0};
uint8_t _selected_sensor_control{0};
bool _sensor_control_available{false};
uint8_t _selected_sensor_sub_index{0};
int8_t _corrections_selected_instance{-1};
};

107
src/modules/sensors/voted_sensors_update.cpp
View File

@ -92,9 +92,9 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
void VotedSensorsUpdate::initializeSensors()
{
initSensorClass(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX);
initSensorClass(ORB_ID(sensor_gyro_integrated), _gyro, GYRO_COUNT_MAX);
initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX);
initSensorClass(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX);
initSensorClass(ORB_ID(sensor_accel_integrated), _accel, ACCEL_COUNT_MAX);
initSensorClass(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX);
}
@ -142,7 +142,7 @@ void VotedSensorsUpdate::parametersUpdate()
/* gyro */
for (uint8_t topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_gyro_s> report{ORB_ID(sensor_gyro), topic_instance};
uORB::SubscriptionData<sensor_gyro_integrated_s> report{ORB_ID(sensor_gyro_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance);
@ -151,10 +151,11 @@ void VotedSensorsUpdate::parametersUpdate()
topic_instance);
_corrections.gyro_mapping[topic_instance] = 0;
_corrections.gyro_device_ids[topic_instance] = 0;
} else {
_corrections.gyro_mapping[topic_instance] = temp;
_corrections.gyro_device_ids[topic_instance] = report.get().device_id;
}
}
@ -162,7 +163,7 @@ void VotedSensorsUpdate::parametersUpdate()
/* accel */
for (uint8_t topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_accel_s> report{ORB_ID(sensor_accel), topic_instance};
uORB::SubscriptionData<sensor_accel_integrated_s> report{ORB_ID(sensor_accel_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance);
@ -171,10 +172,11 @@ void VotedSensorsUpdate::parametersUpdate()
topic_instance);
_corrections.accel_mapping[topic_instance] = 0;
_corrections.accel_device_ids[topic_instance] = 0;
} else {
_corrections.accel_mapping[topic_instance] = temp;
_corrections.accel_device_ids[topic_instance] = report.get().device_id;
}
}
@ -191,10 +193,11 @@ void VotedSensorsUpdate::parametersUpdate()
topic_instance);
_corrections.baro_mapping[topic_instance] = 0;
_corrections.baro_device_ids[topic_instance] = 0;
} else {
_corrections.baro_mapping[topic_instance] = temp;
_corrections.baro_device_ids[topic_instance] = report.get().device_id;
}
}
@ -565,9 +568,9 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
orb_check(_accel.subscription[uorb_index], &accel_updated);
if (accel_updated) {
sensor_accel_s accel_report;
sensor_accel_integrated_s accel_report;
int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report);
int ret = orb_copy(ORB_ID(sensor_accel_integrated), _accel.subscription[uorb_index], &accel_report);
if (ret != PX4_OK || accel_report.timestamp == 0) {
continue; //ignore invalid data
@ -586,40 +589,17 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
_accel_device_id[uorb_index] = accel_report.device_id;
Vector3f accel_data;
/*
* Correct the raw sensor data for scale factor errors
* and offsets due to temperature variation. It is assumed that any filtering of input
* data required is performed in the sensor driver, preferably before downsampling.
*/
if (accel_report.integral_dt != 0) {
/*
* Using data that has been integrated in the driver before downsampling is preferred
* becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors
* and offsets due to temperature variation. It is assumed that any filtering of input
* data required is performed in the sensor driver, preferably before downsampling.
*/
// convert the delta velocities to an equivalent acceleration before application of corrections
const float dt_inv = 1.e6f / (float)accel_report.dt;
Vector3f accel_data = Vector3f{accel_report.delta_velocity} * dt_inv;
// convert the delta velocities to an equivalent acceleration before application of corrections
float dt_inv = 1.e6f / accel_report.integral_dt;
accel_data = Vector3f(accel_report.x_integral * dt_inv,
accel_report.y_integral * dt_inv,
accel_report.z_integral * dt_inv);
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt;
} else {
// using the value instead of the integral (the integral is the prefered choice)
// Correct each sensor for temperature effects
// Filtering and/or downsampling of temperature should be performed in the driver layer
accel_data = Vector3f(accel_report.x, accel_report.y, accel_report.z);
// handle the cse where this is our first output
if (_last_accel_timestamp[uorb_index] == 0) {
_last_accel_timestamp[uorb_index] = accel_report.timestamp - 1000;
}
// approximate the delta time using the difference in accel data time stamps
_last_sensor_data[uorb_index].accelerometer_integral_dt =
(accel_report.timestamp - _last_accel_timestamp[uorb_index]);
}
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
@ -672,9 +652,9 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
orb_check(_gyro.subscription[uorb_index], &gyro_updated);
if (gyro_updated) {
sensor_gyro_s gyro_report;
sensor_gyro_integrated_s gyro_report;
int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report);
int ret = orb_copy(ORB_ID(sensor_gyro_integrated), _gyro.subscription[uorb_index], &gyro_report);
if (ret != PX4_OK || gyro_report.timestamp == 0) {
continue; //ignore invalid data
@ -693,40 +673,17 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
_gyro_device_id[uorb_index] = gyro_report.device_id;
Vector3f gyro_rate;
/*
* Correct the raw sensor data for scale factor errors
* and offsets due to temperature variation. It is assumed that any filtering of input
* data required is performed in the sensor driver, preferably before downsampling.
*/
if (gyro_report.integral_dt != 0) {
/*
* Using data that has been integrated in the driver before downsampling is preferred
* becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors
* and offsets due to temperature variation. It is assumed that any filtering of input
* data required is performed in the sensor driver, preferably before downsampling.
*/
// convert the delta angles to an equivalent angular rate before application of corrections
const float dt_inv = 1.e6f / (float)gyro_report.dt;
Vector3f gyro_rate = Vector3f{gyro_report.delta_angle} * dt_inv;
// convert the delta angles to an equivalent angular rate before application of corrections
float dt_inv = 1.e6f / gyro_report.integral_dt;
gyro_rate = Vector3f(gyro_report.x_integral * dt_inv,
gyro_report.y_integral * dt_inv,
gyro_report.z_integral * dt_inv);
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt;
} else {
//using the value instead of the integral (the integral is the prefered choice)
// Correct each sensor for temperature effects
// Filtering and/or downsampling of temperature should be performed in the driver layer
gyro_rate = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
// handle the case where this is our first output
if (_last_sensor_data[uorb_index].timestamp == 0) {
_last_sensor_data[uorb_index].timestamp = gyro_report.timestamp - 1000;
}
// approximate the delta time using the difference in gyro data time stamps
_last_sensor_data[uorb_index].gyro_integral_dt =
(gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp);
}
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,

2
src/modules/sensors/voted_sensors_update.h
View File

@ -54,9 +54,11 @@
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_magnetometer.h>