From bb465ca5b7fbcf4f6c0653befeb971d575c9db4a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 18 Jan 2020 01:15:00 -0500 Subject: [PATCH] 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 --- .ci/Jenkinsfile-hardware | 8 + msg/CMakeLists.txt | 7 +- msg/sensor_accel.msg | 26 +- msg/sensor_accel_fifo.msg | 16 +- msg/sensor_accel_integrated.msg | 13 + msg/sensor_accel_status.msg | 9 +- msg/sensor_bias.msg | 3 + msg/sensor_correction.msg | 4 + msg/sensor_gyro.msg | 26 +- msg/sensor_gyro_control.msg | 11 - msg/sensor_gyro_fifo.msg | 16 +- msg/sensor_gyro_integrated.msg | 13 + msg/sensor_gyro_status.msg | 11 +- msg/tools/uorb_rtps_message_ids.yaml | 6 +- src/lib/drivers/accelerometer/CMakeLists.txt | 5 +- .../accelerometer/PX4Accelerometer.cpp | 242 +++++++-------- .../accelerometer/PX4Accelerometer.hpp | 19 +- src/lib/drivers/gyroscope/CMakeLists.txt | 5 +- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 279 ++++++++---------- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 20 +- .../drivers/magnetometer/PX4Magnetometer.cpp | 23 +- .../drivers/magnetometer/PX4Magnetometer.hpp | 4 +- src/modules/ekf2/ekf2_main.cpp | 4 + src/modules/logger/logged_topics.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 144 +++++---- src/modules/mavlink/mavlink_receiver.cpp | 12 - .../vehicle_acceleration/CMakeLists.txt | 1 + .../VehicleAcceleration.cpp | 138 +++++---- .../VehicleAcceleration.hpp | 21 +- .../vehicle_angular_velocity/CMakeLists.txt | 1 + .../VehicleAngularVelocity.cpp | 225 ++++++-------- .../VehicleAngularVelocity.hpp | 30 +- src/modules/sensors/voted_sensors_update.cpp | 107 ++----- src/modules/sensors/voted_sensors_update.h | 2 + 34 files changed, 696 insertions(+), 756 deletions(-) create mode 100644 msg/sensor_accel_integrated.msg delete mode 100644 msg/sensor_gyro_control.msg create mode 100644 msg/sensor_gyro_integrated.msg diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index c9e287c182..f77796764d 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -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"' diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 51b861a2c2..62ae55e38a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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'. diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index 119fa7fd12..98ad1e9c9a 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -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 diff --git a/msg/sensor_accel_fifo.msg b/msg/sensor_accel_fifo.msg index 78099a5a2f..d6bf95c7aa 100644 --- a/msg/sensor_accel_fifo.msg +++ b/msg/sensor_accel_fifo.msg @@ -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 diff --git a/msg/sensor_accel_integrated.msg b/msg/sensor_accel_integrated.msg new file mode 100644 index 0000000000..ea9a92b64a --- /dev/null +++ b/msg/sensor_accel_integrated.msg @@ -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 diff --git a/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg index 21ab217b95..3e94f1c659 100644 --- a/msg/sensor_accel_status.msg +++ b/msg/sensor_accel_status.msg @@ -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) diff --git a/msg/sensor_bias.msg b/msg/sensor_bias.msg index ce81099c99..2a8debf0f0 100644 --- a/msg/sensor_bias.msg +++ b/msg/sensor_bias.msg @@ -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) diff --git a/msg/sensor_correction.msg b/msg/sensor_correction.msg index 4f2fb10d99..30295adbee 100644 --- a/msg/sensor_correction.msg +++ b/msg/sensor_correction.msg @@ -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 diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index b4ff35c666..ce6079fcdf 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -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 diff --git a/msg/sensor_gyro_control.msg b/msg/sensor_gyro_control.msg deleted file mode 100644 index 5151231c4b..0000000000 --- a/msg/sensor_gyro_control.msg +++ /dev/null @@ -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 diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg index 61111fda80..677d5dba8d 100644 --- a/msg/sensor_gyro_fifo.msg +++ b/msg/sensor_gyro_fifo.msg @@ -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 diff --git a/msg/sensor_gyro_integrated.msg b/msg/sensor_gyro_integrated.msg new file mode 100644 index 0000000000..f090ab1689 --- /dev/null +++ b/msg/sensor_gyro_integrated.msg @@ -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 diff --git a/msg/sensor_gyro_status.msg b/msg/sensor_gyro_status.msg index c8cead091f..6d0e0d4410 100644 --- a/msg/sensor_gyro_status.msg +++ b/msg/sensor_gyro_status.msg @@ -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) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 9b20366d7e..fb046578f4 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -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 diff --git a/src/lib/drivers/accelerometer/CMakeLists.txt b/src/lib/drivers/accelerometer/CMakeLists.txt index fd6ca3efcb..cd364f69b4 100644 --- a/src/lib/drivers/accelerometer/CMakeLists.txt +++ b/src/lib/drivers/accelerometer/CMakeLists.txt @@ -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 diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 53ba25c5cd..756c4854af 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -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)); - } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 63bd208e9c..f4e036ea98 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -45,11 +45,11 @@ #include #include #include +#include #include 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_pub; // legacy message - uORB::PublicationMulti _sensor_fifo_pub; - uORB::PublicationMultiData _sensor_status_pub; + uORB::PublicationMulti _sensor_pub; + uORB::PublicationMulti _sensor_fifo_pub; + uORB::PublicationMulti _sensor_integrated_pub; + uORB::PublicationMulti _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) _param_imu_accel_cutoff ) - }; diff --git a/src/lib/drivers/gyroscope/CMakeLists.txt b/src/lib/drivers/gyroscope/CMakeLists.txt index f221e179c1..c059b95103 100644 --- a/src/lib/drivers/gyroscope/CMakeLists.txt +++ b/src/lib/drivers/gyroscope/CMakeLists.txt @@ -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) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index c65f232147..c00f2451c9 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -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)); - } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index dda7927cb2..e1b28e9ada 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -44,13 +44,12 @@ #include #include #include -#include #include +#include #include 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_pub; // legacy message - uORB::PublicationMulti _sensor_control_pub; - uORB::PublicationMulti _sensor_fifo_pub; - uORB::PublicationMultiData _sensor_status_pub; + uORB::PublicationMulti _sensor_pub; + uORB::PublicationMulti _sensor_fifo_pub; + uORB::PublicationMulti _sensor_integrated_pub; + uORB::PublicationMulti _sensor_status_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; math::NotchFilter _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) _param_imu_gyro_nf_bw, (ParamInt) _param_imu_gyro_rate_max ) - }; diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index 2c8976ec63..db679721ba 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp @@ -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()); } diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index e909602dd8..4d2108eab7 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -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(); diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 3a4b5aa9b4..aa4872816d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -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); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index c061d7ed14..79d321011c 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 70e260a7ec..8b6884fb0c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -60,13 +60,14 @@ #include #include #include -#include #include +#include +#include #include +#include #include #include #include -#include #include #include #include @@ -75,18 +76,22 @@ #include #include #include -#include +#include #include #include #include #include #include +#include #include -#include #include +#include +#include +#include #include #include #include +#include #include #include #include @@ -97,21 +102,16 @@ #include #include #include +#include #include #include #include #include +#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include +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); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 936424c776..65a2114596 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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; diff --git a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt index 812abb7cd6..3867dfeab0 100644 --- a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt +++ b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt @@ -33,5 +33,6 @@ px4_add_library(vehicle_acceleration VehicleAcceleration.cpp + VehicleAcceleration.hpp ) target_link_libraries(vehicle_acceleration PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 043019b4a5..66f9be659e 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -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)); } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 6725a9cdf9..9479910cf2 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -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}; }; diff --git a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt index 340252aa99..df655ded52 100644 --- a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt +++ b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt @@ -33,5 +33,6 @@ px4_add_library(vehicle_angular_velocity VehicleAngularVelocity.cpp + VehicleAngularVelocity.hpp ) target_link_libraries(vehicle_angular_velocity PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 8bf0ded953..0b8ead9a62 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -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)); } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 5f5502bf3e..0060c66241 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -49,7 +49,6 @@ #include #include -#include #include 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}; }; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index c3e98ba332..6a7fdfc884 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -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 report{ORB_ID(sensor_gyro), topic_instance}; + uORB::SubscriptionData 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 report{ORB_ID(sensor_accel), topic_instance}; + uORB::SubscriptionData 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, diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 5e4a0994ed..87c591905f 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -54,9 +54,11 @@ #include #include +#include #include #include #include +#include #include #include #include