From e34bdb4be9ef633a5d4d55eaa47bc0b4bba06bcf Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 30 May 2020 11:07:54 -0400 Subject: [PATCH] move IMU integration out of drivers to sensors hub to handle accel/gyro sync - IMU integration move from drivers (PX4Accelerometer/PX4Gyroscope) to sensors/vehicle_imu - sensors: voted_sensors_update now consumes vehicle_imu - delete sensor_accel_integrated, sensor_gyro_integrated - merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status - sensors status output minor improvements (ordering, whitespace, show selected sensor device id and instance) --- .ci/Jenkinsfile-hardware | 10 +- ROMFS/px4fmu_common/init.d-posix/rcS | 4 +- msg/CMakeLists.txt | 5 +- msg/sensor_accel.msg | 6 +- msg/sensor_accel_integrated.msg | 12 - msg/sensor_accel_status.msg | 17 - msg/sensor_gyro.msg | 4 +- msg/sensor_gyro_integrated.msg | 12 - msg/sensor_gyro_status.msg | 19 -- msg/tools/px_generate_uorb_topic_helper.py | 12 + msg/tools/uorb_rtps_message_ids.yaml | 36 +-- msg/vehicle_imu.msg | 4 +- msg/vehicle_imu_status.msg | 16 + src/drivers/drv_sensor.h | 3 +- src/drivers/imu/adis16448/ADIS16448.cpp | 3 - src/drivers/imu/adis16477/ADIS16477.cpp | 3 - src/drivers/imu/adis16497/ADIS16497.cpp | 3 - src/drivers/imu/bma180/bma180.cpp | 1 - src/drivers/imu/bmi055/BMI055_accel.cpp | 1 - src/drivers/imu/bmi055/BMI055_gyro.cpp | 1 - src/drivers/imu/bmi088/BMI088_accel.cpp | 1 - src/drivers/imu/bmi088/BMI088_gyro.cpp | 1 - src/drivers/imu/bmi160/bmi160.cpp | 1 - src/drivers/imu/fxas21002c/FXAS21002C.cpp | 1 - src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp | 2 - .../imu/invensense/icm20602/ICM20602.cpp | 3 - .../imu/invensense/icm20608g/ICM20608G.cpp | 3 - .../imu/invensense/icm20649/ICM20649.cpp | 3 - .../imu/invensense/icm20689/ICM20689.cpp | 3 - .../imu/invensense/icm20948/ICM20948.cpp | 3 - .../imu/invensense/icm40609d/ICM40609D.cpp | 3 - .../imu/invensense/icm42688p/ICM42688P.cpp | 3 - .../imu/invensense/mpu6000/MPU6000.cpp | 3 - .../imu/invensense/mpu6500/MPU6500.cpp | 3 - .../imu/invensense/mpu9250/MPU9250.cpp | 3 - src/drivers/imu/l3gd20/L3GD20.cpp | 1 - src/drivers/imu/lsm303d/LSM303D.cpp | 1 - src/drivers/imu/mpu6000/MPU6000.cpp | 2 - src/drivers/imu/mpu9250/mpu9250.cpp | 2 - src/drivers/imu/st/ism330dlc/ISM330DLC.cpp | 2 - src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp | 2 - .../accelerometer/PX4Accelerometer.cpp | 263 +++------------- .../accelerometer/PX4Accelerometer.hpp | 36 +-- src/lib/drivers/device/CMakeLists.txt | 1 - src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 266 ++-------------- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 42 +-- src/modules/ekf2/ekf2_main.cpp | 8 +- src/modules/logger/logged_topics.cpp | 4 +- src/modules/mavlink/mavlink_messages.cpp | 135 +++----- src/modules/mavlink/mavlink_receiver.cpp | 34 +- src/modules/sensors/CMakeLists.txt | 1 - .../sensor_corrections/SensorCorrections.hpp | 2 + src/modules/sensors/sensors.cpp | 75 ++--- .../VehicleAcceleration.cpp | 18 +- .../VehicleAcceleration.hpp | 4 +- .../vehicle_air_data/VehicleAirData.cpp | 1 - .../VehicleAngularVelocity.cpp | 18 +- .../VehicleAngularVelocity.hpp | 4 +- .../sensors/vehicle_imu/CMakeLists.txt | 2 + .../sensors/vehicle_imu/Integrator.cpp} | 80 ++--- .../sensors/vehicle_imu/Integrator.hpp} | 74 +++-- .../sensors/vehicle_imu/VehicleIMU.cpp | 292 +++++++++++++++--- .../sensors/vehicle_imu/VehicleIMU.hpp | 53 +++- .../sensors/vehicle_imu/imu_parameters.c | 1 + src/modules/sensors/voted_sensors_update.cpp | 245 ++++++--------- src/modules/sensors/voted_sensors_update.h | 43 +-- src/modules/sih/sih.cpp | 2 - src/modules/sih/sih.hpp | 4 +- src/modules/simulator/simulator.cpp | 2 +- src/modules/simulator/simulator.h | 7 +- src/modules/simulator/simulator_mavlink.cpp | 10 +- src/modules/uORB/Subscription.hpp | 13 +- src/modules/uORB/SubscriptionCallback.hpp | 22 +- src/systemcmds/top/CMakeLists.txt | 2 +- 74 files changed, 785 insertions(+), 1197 deletions(-) delete mode 100644 msg/sensor_accel_integrated.msg delete mode 100644 msg/sensor_accel_status.msg delete mode 100644 msg/sensor_gyro_integrated.msg delete mode 100644 msg/sensor_gyro_status.msg create mode 100644 msg/vehicle_imu_status.msg rename src/{lib/drivers/device/integrator.cpp => modules/sensors/vehicle_imu/Integrator.cpp} (65%) rename src/{lib/drivers/device/integrator.h => modules/sensors/vehicle_imu/Integrator.hpp} (50%) diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index ec5dac06bd..dbfbf50916 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -770,19 +770,16 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener logger_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_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_combined"' 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"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener system_power"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_attitude"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_local_position"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"' @@ -832,19 +829,16 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener logger_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel_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_combined"' 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"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener system_power"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_attitude"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_local_position"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /"' diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index d6980ea2e1..b090031dc9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -123,8 +123,8 @@ then param set CAL_ACC0_ID 1311244 param set CAL_ACC_PRIME 1311244 - param set CAL_GYRO0_ID 2294028 - param set CAL_GYRO_PRIME 2294028 + param set CAL_GYRO0_ID 1311244 + param set CAL_GYRO_PRIME 1311244 param set CAL_MAG0_ID 197388 param set CAL_MAG_PRIME 197388 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 201a6326fd..a836c6c4df 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -112,15 +112,11 @@ 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_combined.msg sensor_correction.msg sensor_gyro.msg sensor_gyro_fifo.msg - sensor_gyro_integrated.msg - sensor_gyro_status.msg sensor_mag.msg sensor_preflight.msg sensor_selection.msg @@ -154,6 +150,7 @@ set(msg_files vehicle_global_position.msg vehicle_gps_position.msg vehicle_imu.msg + vehicle_imu_status.msg vehicle_land_detected.msg vehicle_local_position.msg vehicle_local_position_setpoint.msg diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index 67f99870a2..2a99794027 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -9,4 +9,8 @@ float32 z # acceleration in the NED Z board axis in m/s^2 float32 temperature # temperature in degrees celsius -uint8 ORB_QUEUE_LENGTH = 4 +uint32 error_count + +uint8[3] clip_counter # clip count per axis in the sample period + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/sensor_accel_integrated.msg b/msg/sensor_accel_integrated.msg deleted file mode 100644 index cbc7d35234..0000000000 --- a/msg/sensor_accel_integrated.msg +++ /dev/null @@ -1,12 +0,0 @@ -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 - -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[3] clip_counter # clip count per axis over the integration period diff --git a/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg deleted file mode 100644 index b9c20048d1..0000000000 --- a/msg/sensor_accel_status.msg +++ /dev/null @@ -1,17 +0,0 @@ -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 - -float32 temperature - -uint8 rotation - -uint32[3] clipping # clipping per axis - -uint16 measure_rate_hz - -float32 full_scale_range - -float32 vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s) diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index c3fd3207dd..c6ec512817 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -9,4 +9,6 @@ float32 z # angular velocity in the NED Z board axis in rad/s float32 temperature # temperature in degrees celsius -uint8 ORB_QUEUE_LENGTH = 4 +uint32 error_count + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/sensor_gyro_integrated.msg b/msg/sensor_gyro_integrated.msg deleted file mode 100644 index 35b3944097..0000000000 --- a/msg/sensor_gyro_integrated.msg +++ /dev/null @@ -1,12 +0,0 @@ -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 - -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[3] clip_counter # clip count per axis over the integration period diff --git a/msg/sensor_gyro_status.msg b/msg/sensor_gyro_status.msg deleted file mode 100644 index 5258c260f1..0000000000 --- a/msg/sensor_gyro_status.msg +++ /dev/null @@ -1,19 +0,0 @@ -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 - -float32 temperature - -uint8 rotation - -uint32[3] clipping # clipping per axis - -uint16 measure_rate_hz - -float32 full_scale_range - -float32 vibration_metric # high frequency vibration level in the IMU delta angle data (rad) - -float32 coning_vibration # Level of coning vibration in the IMU delta angles (rad^2) diff --git a/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py index c0bd8fbe96..51668e1463 100644 --- a/msg/tools/px_generate_uorb_topic_helper.py +++ b/msg/tools/px_generate_uorb_topic_helper.py @@ -305,6 +305,18 @@ def print_field(field): print("char device_id_buffer[80];") print("device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), message.device_id);") print("PX4_INFO_RAW(\"\\tdevice_id: %d (%s) \\n\", message.device_id, device_id_buffer);") + elif field.name == 'accel_device_id': + print("char accel_device_id_buffer[80];") + print("device::Device::device_id_print_buffer(accel_device_id_buffer, sizeof(accel_device_id_buffer), message.accel_device_id);") + print("PX4_INFO_RAW(\"\\taccel_device_id: %d (%s) \\n\", message.accel_device_id, accel_device_id_buffer);") + elif field.name == 'gyro_device_id': + print("char gyro_device_id_buffer[80];") + print("device::Device::device_id_print_buffer(gyro_device_id_buffer, sizeof(gyro_device_id_buffer), message.gyro_device_id);") + print("PX4_INFO_RAW(\"\\tgyro_device_id: %d (%s) \\n\", message.gyro_device_id, gyro_device_id_buffer);") + elif field.name == 'baro_device_id': + print("char baro_device_id_buffer[80];") + print("device::Device::device_id_print_buffer(baro_device_id_buffer, sizeof(baro_device_id_buffer), message.baro_device_id);") + print("PX4_INFO_RAW(\"\\tbaro_device_id: %d (%s) \\n\", message.baro_device_id, baro_device_id_buffer);") elif is_array and 'char' in field.type: print(("PX4_INFO_RAW(\"\\t" + field.name + ": \\\"%." + str(array_length) + "s\\\" \\n\", message." + field.name + ");")) else: diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index dcae9393f4..a0031db16a 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -267,40 +267,34 @@ rtps: id: 117 - msg: sensor_accel_fifo id: 118 - - msg: sensor_accel_status - id: 119 - msg: sensor_gyro_fifo - id: 120 - - msg: sensor_gyro_status - id: 121 - - msg: sensor_accel_integrated - id: 122 - - msg: sensor_gyro_integrated - id: 123 + id: 119 - msg: vehicle_imu - id: 124 + id: 120 + - msg: vehicle_imu_status + id: 121 - msg: vehicle_angular_acceleration - id: 125 + id: 122 - msg: logger_status - id: 126 + id: 123 - msg: rpm - id: 127 + id: 124 - msg: hover_thrust_estimate - id: 128 + id: 125 - msg: trajectory_bezier - id: 129 + id: 126 - msg: vehicle_trajectory_bezier - id: 130 + id: 127 - msg: timesync_status - id: 131 + id: 128 - msg: orb_test - id: 132 + id: 129 - msg: orb_test_medium - id: 133 + id: 130 - msg: orb_test_large - id: 134 + id: 131 - msg: yaw_estimator_status - id: 135 + id: 132 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/msg/vehicle_imu.msg b/msg/vehicle_imu.msg index c5ebfdf2ab..f5e8da10a6 100644 --- a/msg/vehicle_imu.msg +++ b/msg/vehicle_imu.msg @@ -8,8 +8,8 @@ uint32 gyro_device_id # Gyroscope unique device ID for the sensor that float32[3] delta_angle # delta angle in the NED board axis in rad/s over the integration time frame (dt) float32[3] delta_velocity # delta velocity in the NED board axis in m/s over the integration time frame (dt) - -uint16 dt # integration period in us +uint16 delta_angle_dt # integration period in us +uint16 delta_velocity_dt # integration period in us uint8 CLIPPING_X = 1 uint8 CLIPPING_Y = 2 diff --git a/msg/vehicle_imu_status.msg b/msg/vehicle_imu_status.msg new file mode 100644 index 0000000000..411f619f8a --- /dev/null +++ b/msg/vehicle_imu_status.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles +uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles + +uint32[3] accel_clipping # clipping per axis + +uint32 accel_error_count +uint32 gyro_error_count + +uint16 accel_rate_hz +uint16 gyro_rate_hz + +float32 accel_vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s) +float32 gyro_vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s) +float32 gyro_coning_vibration # Level of coning vibration in the IMU delta angles (rad^2) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a1582cc11e..6caa0cf7f8 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -68,13 +68,12 @@ #define DRV_IMU_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 #define DRV_ACC_DEVTYPE_MPU6000_LEGACY 0x13 -#define DRV_ACC_DEVTYPE_ACCELSIM 0x14 +#define DRV_IMU_DEVTYPE_SIM 0x14 #define DRV_ACC_DEVTYPE_MPU9250_LEGACY 0x16 #define DRV_IMU_DEVTYPE_BMI160 0x17 #define DRV_IMU_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_L3GD20 0x22 -#define DRV_GYR_DEVTYPE_GYROSIM 0x23 #define DRV_IMU_DEVTYPE_MPU9250 0x24 #define DRV_IMU_DEVTYPE_ICM20649 0x25 #define DRV_IMU_DEVTYPE_ICM42688P 0x26 diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp index 9bfc0d55e0..0555820a45 100644 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/adis16448/ADIS16448.cpp @@ -115,9 +115,6 @@ bool ADIS16448::reset() return false; } - _px4_accel.set_update_rate(_sample_rate); - _px4_gyro.set_update_rate(_sample_rate); - // Set gyroscope scale to default value. //if (!set_gyro_dyn_range(GYRO_INITIAL_SENSITIVITY)) { // return false; diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index 8e1fbd1077..3283158858 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -69,10 +69,7 @@ ADIS16477::ADIS16477(I2CSPIBusOption bus_option, int bus, int32_t device, enum R #endif // GPIO_SPI1_RESET_ADIS16477 _px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB - _px4_accel.set_update_rate(ADIS16477_DEFAULT_RATE); - _px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB - _px4_gyro.set_update_rate(ADIS16477_DEFAULT_RATE); } ADIS16477::~ADIS16477() diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp index e1b6a119e7..38aa2c5864 100644 --- a/src/drivers/imu/adis16497/ADIS16497.cpp +++ b/src/drivers/imu/adis16497/ADIS16497.cpp @@ -84,9 +84,6 @@ ADIS16497::ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum R // Configure hardware reset line px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16497); #endif // GPIO_SPI1_RESET_ADIS16497 - - _px4_accel.set_update_rate(ADIS16497_DEFAULT_RATE); - _px4_gyro.set_update_rate(ADIS16497_DEFAULT_RATE); } ADIS16497::~ADIS16497() diff --git a/src/drivers/imu/bma180/bma180.cpp b/src/drivers/imu/bma180/bma180.cpp index 5fdd698bb9..f829c07a65 100644 --- a/src/drivers/imu/bma180/bma180.cpp +++ b/src/drivers/imu/bma180/bma180.cpp @@ -182,7 +182,6 @@ BMA180::BMA180(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")) { - _px4_accel.set_update_rate(1000); } BMA180::~BMA180() diff --git a/src/drivers/imu/bmi055/BMI055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp index 27ff3e518b..e84029a50b 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.cpp +++ b/src/drivers/imu/bmi055/BMI055_accel.cpp @@ -56,7 +56,6 @@ BMI055_accel::BMI055_accel(I2CSPIBusOption bus_option, int bus, const char *path _duplicates(perf_alloc(PC_COUNT, "bmi055_accel_duplicates")), _got_duplicate(false) { - _px4_accel.set_update_rate(BMI055_ACCEL_DEFAULT_RATE); } BMI055_accel::~BMI055_accel() diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp index 2f795b5b94..fa9398fece 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.cpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.cpp @@ -58,7 +58,6 @@ BMI055_gyro::BMI055_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g _bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers")), _duplicates(perf_alloc(PC_COUNT, "bmi055_gyro_duplicates")) { - _px4_gyro.set_update_rate(BMI055_GYRO_DEFAULT_RATE); } BMI055_gyro::~BMI055_gyro() diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp index 6cf164ef35..68b86583b1 100644 --- a/src/drivers/imu/bmi088/BMI088_accel.cpp +++ b/src/drivers/imu/bmi088/BMI088_accel.cpp @@ -65,7 +65,6 @@ BMI088_accel::BMI088_accel(I2CSPIBusOption bus_option, int bus, const char *path _duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")), _got_duplicate(false) { - _px4_accel.set_update_rate(BMI088_ACCEL_DEFAULT_RATE); } BMI088_accel::~BMI088_accel() diff --git a/src/drivers/imu/bmi088/BMI088_gyro.cpp b/src/drivers/imu/bmi088/BMI088_gyro.cpp index b01f92692a..2485e5f1d5 100644 --- a/src/drivers/imu/bmi088/BMI088_gyro.cpp +++ b/src/drivers/imu/bmi088/BMI088_gyro.cpp @@ -66,7 +66,6 @@ BMI088_gyro::BMI088_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g _bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")), _duplicates(perf_alloc(PC_COUNT, "bmi088_gyro_duplicates")) { - _px4_gyro.set_update_rate(BMI088_GYRO_DEFAULT_RATE); } BMI088_gyro::~BMI088_gyro() diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index b2332ad499..1ef0e3ed01 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -64,7 +64,6 @@ BMI160::BMI160(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio _reset_retries(perf_alloc(PC_COUNT, MODULE_NAME":reset retries")), _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates")) { - _px4_gyro.set_update_rate(BMI160_GYRO_DEFAULT_RATE); } BMI160::~BMI160() diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index 0cb6e10af8..15312cd5fc 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp @@ -75,7 +75,6 @@ FXAS21002C::FXAS21002C(device::Device *interface, I2CSPIBusOption bus_option, in _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad register")), _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicate reading")) { - _px4_gyro.set_update_rate(FXAS21002C_DEFAULT_RATE); } FXAS21002C::~FXAS21002C() diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp index 5d5261a362..1ec894b0a9 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp @@ -66,8 +66,6 @@ FXOS8701CQ::FXOS8701CQ(device::Device *interface, I2CSPIBusOption bus_option, in _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad reg")), _accel_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": acc dupe")) { - _px4_accel.set_update_rate(FXOS8701C_ACCEL_DEFAULT_RATE); - #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) _px4_mag.set_scale(0.001f); #endif diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index d46804fdb4..bb36426e0e 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -316,9 +316,6 @@ void ICM20602::ConfigureSampleRate(int sample_rate) _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); - ConfigureFIFOWatermark(_fifo_gyro_samples); } diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index 98cd276779..d1b8a78729 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -328,9 +328,6 @@ void ICM20608G::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); } bool ICM20608G::Configure() diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index 7b6089f241..1161b3bbc1 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -330,9 +330,6 @@ void ICM20649::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); _fifo_accel_samples = roundf(math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES)); - - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); } void ICM20649::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index 14aad4fb23..3ca7817923 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -328,9 +328,6 @@ void ICM20689::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); } bool ICM20689::Configure() diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index 3b223cba9a..5e10f237b4 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -364,9 +364,6 @@ void ICM20948::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); _fifo_accel_samples = roundf(math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES)); - - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); } void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index c6187ad176..4f05c09c18 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -322,9 +322,6 @@ void ICM40609D::ConfigureSampleRate(int sample_rate) _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); - ConfigureFIFOWatermark(_fifo_gyro_samples); } diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 5716c85b28..27b079e425 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -324,9 +324,6 @@ void ICM42688P::ConfigureSampleRate(int sample_rate) _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); - ConfigureFIFOWatermark(_fifo_gyro_samples); } diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index bf4adfd700..1887ffd4e4 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -323,9 +323,6 @@ void MPU6000::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); } bool MPU6000::Configure() diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 6887da69b3..28c85a7066 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -323,9 +323,6 @@ void MPU6500::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); } bool MPU6500::Configure() diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 11dbc4d176..238cc19e1a 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -356,9 +356,6 @@ void MPU9250::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - - _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); } bool MPU9250::Configure() diff --git a/src/drivers/imu/l3gd20/L3GD20.cpp b/src/drivers/imu/l3gd20/L3GD20.cpp index e4f41610ca..efc5dade3e 100644 --- a/src/drivers/imu/l3gd20/L3GD20.cpp +++ b/src/drivers/imu/l3gd20/L3GD20.cpp @@ -45,7 +45,6 @@ L3GD20::L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotati _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")), _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe")) { - _px4_gyro.set_update_rate(L3GD20_DEFAULT_RATE); } L3GD20::~L3GD20() diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp index 4c81fb1d83..414686d2dd 100644 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ b/src/drivers/imu/lsm303d/LSM303D.cpp @@ -127,7 +127,6 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); - _px4_accel.set_update_rate(LSM303D_ACCEL_DEFAULT_RATE); // we setup the anti-alias on-chip filter as 50Hz. We believe // this operates in the analog domain, and is critical for diff --git a/src/drivers/imu/mpu6000/MPU6000.cpp b/src/drivers/imu/mpu6000/MPU6000.cpp index 55fa623c29..b815a63af1 100644 --- a/src/drivers/imu/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/mpu6000/MPU6000.cpp @@ -160,8 +160,6 @@ int MPU6000::reset() // SAMPLE RATE _set_sample_rate(1000); - _px4_accel.set_update_rate(1000); - _px4_gyro.set_update_rate(1000); px4_usleep(1000); _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 7a1ee69967..efd5670918 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -189,8 +189,6 @@ MPU9250::reset_mpu() // SAMPLE RATE _set_sample_rate(_sample_rate); - _px4_accel.set_update_rate(_sample_rate); - _px4_gyro.set_update_rate(_sample_rate); _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp index 97945b679d..ba44d975a0 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp @@ -47,8 +47,6 @@ ISM330DLC::ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) { - _px4_accel.set_update_rate(1000000 / FIFO_INTERVAL); - _px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL); } ISM330DLC::~ISM330DLC() diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index 747842ff70..5965398fb0 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -45,8 +45,6 @@ LSM9DS1::LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) { - _px4_accel.set_update_rate(1000000 / _fifo_interval); - _px4_gyro.set_update_rate(1000000 / _fifo_interval); } LSM9DS1::~LSM9DS1() diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 3f2542e1bd..16a7fb3163 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -68,21 +68,14 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum R 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} { // register class and advertise immediately to keep instance numbering in sync _class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); _sensor_pub.advertise(); - _sensor_integrated_pub.advertise(); - _sensor_status_pub.advertise(); updateParams(); - - // set reasonable default, driver should be setting real value - set_update_rate(800); } PX4Accelerometer::~PX4Accelerometer() @@ -93,8 +86,6 @@ PX4Accelerometer::~PX4Accelerometer() _sensor_pub.unadvertise(); _sensor_fifo_pub.unadvertise(); - _sensor_integrated_pub.unadvertise(); - _sensor_status_pub.unadvertise(); } int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) @@ -132,25 +123,6 @@ void PX4Accelerometer::set_device_type(uint8_t devtype) _device_id = device_id.devid; } -void PX4Accelerometer::set_update_rate(uint16_t rate) -{ - _update_rate = math::constrain((int)rate, 50, 32000); - - // constrain IMU integration time 1-20 milliseconds (50-1000 Hz) - int32_t imu_integration_rate_hz = math::constrain(_param_imu_integ_rate.get(), 50, 1000); - - if (imu_integration_rate_hz != _param_imu_integ_rate.get()) { - _param_imu_integ_rate.set(imu_integration_rate_hz); - _param_imu_integ_rate.commit_no_notification(); - } - - const float update_interval_us = 1e6f / _update_rate; - const float imu_integration_interval_us = 1e6f / (float)imu_integration_rate_hz; - - _integrator_reset_samples = roundf(imu_integration_interval_us / update_interval_us); - _integrator.set_autoreset_interval(_integrator_reset_samples * update_interval_us); -} - void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z) { // Apply rotation (before scaling) @@ -158,67 +130,32 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl const Vector3f raw{x, y, z}; - // Clipping (check unscaled raw values) - for (int i = 0; i < 3; i++) { - if (fabsf(raw(i)) > _clip_limit) { - _clipping_total[i]++; - _integrator_clipping(i)++; - } - } + // clipping + float clip_count_x = (fabsf(raw(0)) > _clip_limit); + float clip_count_y = (fabsf(raw(1)) > _clip_limit); + float clip_count_z = (fabsf(raw(2)) > _clip_limit); + + rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z); // Apply range scale and the calibrating offset/scale const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))}; - // publish raw data immediately - { - sensor_accel_s report; + // publish + sensor_accel_s report; - report.timestamp_sample = timestamp_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(); + report.timestamp_sample = timestamp_sample; + report.device_id = _device_id; + report.temperature = _temperature; + report.error_count = _error_count; + report.x = val_calibrated(0); + report.y = val_calibrated(1); + report.z = val_calibrated(2); + report.clip_counter[0] = fabsf(roundf(clip_count_x)); + report.clip_counter[1] = fabsf(roundf(clip_count_y)); + report.clip_counter[2] = fabsf(roundf(clip_count_z)); + report.timestamp = hrt_absolute_time(); - _sensor_pub.publish(report); - } - - // Integrated values - Vector3f delta_velocity; - uint32_t integral_dt = 0; - - _integrator_samples++; - - if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) { - - // fill sensor_accel_integrated and publish - sensor_accel_integrated_s report; - - report.timestamp_sample = timestamp_sample; - report.error_count = _error_count; - report.device_id = _device_id; - delta_velocity.copyTo(report.delta_velocity); - report.dt = integral_dt; - report.samples = _integrator_samples; - - for (int i = 0; i < 3; i++) { - report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i))); - } - - report.timestamp = hrt_absolute_time(); - - _sensor_integrated_pub.publish(report); - - - // reset integrator - ResetIntegrator(); - - // update vibration metrics - UpdateVibrationMetrics(delta_velocity); - } - - PublishStatus(); + _sensor_pub.publish(report); } void PX4Accelerometer::updateFIFO(const FIFOSample &sample) @@ -226,110 +163,57 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) const uint8_t N = sample.samples; const float dt = sample.dt; - // publish raw data immediately { - // average - float x = (float)sum(sample.x, N) / (float)N; - float y = (float)sum(sample.y, N) / (float)N; - float z = (float)sum(sample.z, N) / (float)N; + // trapezoidal integration (equally spaced, scaled by dt later) + Vector3f integral{ + (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)), + (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)), + (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)), + }; + + _last_sample[0] = sample.x[N - 1]; + _last_sample[1] = sample.y[N - 1]; + _last_sample[2] = sample.z[N - 1]; + + // clipping + float clip_count_x = clipping(sample.x, _clip_limit, N); + float clip_count_y = clipping(sample.y, _clip_limit, N); + float clip_count_z = clipping(sample.z, _clip_limit, N); + + rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z); + // Apply rotation (before scaling) - rotate_3f(_rotation, x, y, z); + rotate_3f(_rotation, integral(0), integral(1), integral(2)); - // Apply range scale and the calibrating offset/scale + // average + const float x = integral(0) / (float)N; + const float y = integral(1) / (float)N; + const float z = integral(2) / (float)N; + + // Apply range scale and the calibration offset/scale const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)}; + // publish sensor_accel_s report; report.timestamp_sample = sample.timestamp_sample; report.device_id = _device_id; report.temperature = _temperature; + report.error_count = _error_count; report.x = val_calibrated(0); report.y = val_calibrated(1); report.z = val_calibrated(2); + report.clip_counter[0] = fabsf(roundf(clip_count_x)); + report.clip_counter[1] = fabsf(roundf(clip_count_y)); + report.clip_counter[2] = fabsf(roundf(clip_count_z)); report.timestamp = hrt_absolute_time(); _sensor_pub.publish(report); } - // clipping - unsigned clip_count_x = clipping(sample.x, _clip_limit, N); - unsigned clip_count_y = clipping(sample.y, _clip_limit, N); - unsigned clip_count_z = clipping(sample.z, _clip_limit, N); - - _clipping_total[0] += clip_count_x; - _clipping_total[1] += clip_count_y; - _clipping_total[2] += clip_count_z; - - _integrator_clipping(0) += clip_count_x; - _integrator_clipping(1) += clip_count_y; - _integrator_clipping(2) += clip_count_z; - - // integrated data (INS) - { - // reset integrator if previous sample was too long ago - if ((sample.timestamp_sample > _timestamp_sample_prev) - && ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) { - - ResetIntegrator(); - } - - // integrate - _integrator_samples += 1; - _integrator_fifo_samples += N; - - // trapezoidal integration (equally spaced, scaled by dt later) - _integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)); - _integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)); - _integration_raw(2) += (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)); - _last_sample[0] = sample.x[N - 1]; - _last_sample[1] = sample.y[N - 1]; - _last_sample[2] = sample.z[N - 1]; - - - if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) { - - // Apply rotation (before scaling) - rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2)); - - // scale calibration offset to number of samples - const Vector3f offset{_calibration_offset * _integrator_fifo_samples}; - - // Apply calibration and scale to seconds - const Vector3f delta_velocity{((_integration_raw * _scale) - offset).emult(_calibration_scale) * 1e-6f * dt}; - - // fill sensor_accel_integrated and publish - sensor_accel_integrated_s report; - - report.timestamp_sample = sample.timestamp_sample; - report.error_count = _error_count; - report.device_id = _device_id; - delta_velocity.copyTo(report.delta_velocity); - report.dt = _integrator_fifo_samples * dt; // time span in microseconds - report.samples = _integrator_fifo_samples; - - rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2)); - const Vector3f clipping{_integrator_clipping}; - - for (int i = 0; i < 3; i++) { - report.clip_counter[i] = fabsf(roundf(clipping(i))); - } - - report.timestamp = hrt_absolute_time(); - _sensor_integrated_pub.publish(report); - - // update vibration metrics - UpdateVibrationMetrics(delta_velocity); - - // reset integrator - ResetIntegrator(); - } - - _timestamp_sample_prev = sample.timestamp_sample; - } - - // publish sensor fifo + // publish fifo sensor_accel_fifo_s fifo{}; fifo.device_id = _device_id; @@ -344,42 +228,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) fifo.timestamp = hrt_absolute_time(); _sensor_fifo_pub.publish(fifo); - - - PublishStatus(); -} - -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_hz = _update_rate; - status.temperature = _temperature; - status.vibration_metric = _vibration_metric; - status.clipping[0] = _clipping_total[0]; - status.clipping[1] = _clipping_total[1]; - status.clipping[2] = _clipping_total[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; - _integration_raw.zero(); - _integrator_clipping.zero(); - - _timestamp_sample_prev = 0; } void PX4Accelerometer::UpdateClipLimit() @@ -388,15 +236,6 @@ void PX4Accelerometer::UpdateClipLimit() _clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX); } -void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity) -{ - // Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) - const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev; - _vibration_metric = 0.99f * _vibration_metric + 0.01f * delta_velocity_diff.norm(); - - _delta_velocity_prev = delta_velocity; -} - void PX4Accelerometer::print_status() { #if !defined(CONSTRAINED_FLASH) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index c73bb9a46a..1b13d713cc 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -37,15 +37,12 @@ #include #include #include -#include #include #include #include #include #include #include -#include -#include class PX4Accelerometer : public cdev::CDev, public ModuleParams { @@ -64,7 +61,6 @@ public: void set_range(float range) { _range = range; UpdateClipLimit(); } void set_scale(float scale) { _scale = scale; UpdateClipLimit(); } void set_temperature(float temperature) { _temperature = temperature; } - void set_update_rate(uint16_t rate); void update(hrt_abstime timestamp_sample, float x, float y, float z); @@ -86,27 +82,14 @@ public: void updateFIFO(const FIFOSample &sample); private: - - void PublishStatus(); - void ResetIntegrator(); void UpdateClipLimit(); - void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity); uORB::PublicationQueuedMulti _sensor_pub; uORB::PublicationMulti _sensor_fifo_pub; - uORB::PublicationMulti _sensor_integrated_pub; - uORB::PublicationMulti _sensor_status_pub; - - hrt_abstime _status_last_publish{0}; - - Integrator _integrator{5000, false}; // 200 Hz default matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f}; matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f}; - matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement - float _vibration_metric{0.f}; // high frequency vibration level in the IMU delta velocity data (m/s) - int _class_device_instance{-1}; uint32_t _device_id{0}; @@ -116,24 +99,9 @@ private: float _scale{1.f}; float _temperature{0.f}; - int16_t _clip_limit{(int16_t)(_range / _scale)}; + float _clip_limit{_range / _scale}; - uint64_t _error_count{0}; + uint32_t _error_count{0}; - uint32_t _clipping_total[3] {}; - - uint16_t _update_rate{1000}; - - // integrator - hrt_abstime _timestamp_sample_prev{0}; - matrix::Vector3f _integration_raw{}; - matrix::Vector3f _integrator_clipping{}; int16_t _last_sample[3] {}; - uint8_t _integrator_reset_samples{4}; - uint8_t _integrator_samples{0}; - uint8_t _integrator_fifo_samples{0}; - - DEFINE_PARAMETERS( - (ParamInt) _param_imu_integ_rate - ) }; diff --git a/src/lib/drivers/device/CMakeLists.txt b/src/lib/drivers/device/CMakeLists.txt index d1830466fc..cdd1f1c035 100644 --- a/src/lib/drivers/device/CMakeLists.txt +++ b/src/lib/drivers/device/CMakeLists.txt @@ -56,7 +56,6 @@ endif() px4_add_library(drivers__device CDev.cpp ringbuffer.cpp - integrator.cpp ${SRCS_PLATFORM} ) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index b52be75688..44298f6622 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -50,39 +50,19 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len) return sum; } -static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len) -{ - unsigned clip_count = 0; - - for (int n = 0; n < len; n++) { - if (abs(samples[n]) >= clip_limit) { - clip_count++; - } - } - - return clip_count; -} - PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) : CDev(nullptr), ModuleParams(nullptr), _sensor_pub{ORB_ID(sensor_gyro), 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} { // register class and advertise immediately to keep instance numbering in sync _class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); _sensor_pub.advertise(); - _sensor_integrated_pub.advertise(); - _sensor_status_pub.advertise(); updateParams(); - - // set reasonable default, driver should be setting real value - set_update_rate(800); } PX4Gyroscope::~PX4Gyroscope() @@ -93,8 +73,6 @@ PX4Gyroscope::~PX4Gyroscope() _sensor_pub.unadvertise(); _sensor_fifo_pub.unadvertise(); - _sensor_integrated_pub.unadvertise(); - _sensor_status_pub.unadvertise(); } int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) @@ -131,25 +109,6 @@ void PX4Gyroscope::set_device_type(uint8_t devtype) _device_id = device_id.devid; } -void PX4Gyroscope::set_update_rate(uint16_t rate) -{ - _update_rate = math::constrain((int)rate, 50, 32000); - - // constrain IMU integration time 1-20 milliseconds (50-1000 Hz) - int32_t imu_integration_rate_hz = math::constrain(_param_imu_integ_rate.get(), 50, 1000); - - if (imu_integration_rate_hz != _param_imu_integ_rate.get()) { - _param_imu_integ_rate.set(imu_integration_rate_hz); - _param_imu_integ_rate.commit_no_notification(); - } - - const float update_interval_us = 1e6f / _update_rate; - const float imu_integration_interval_us = 1e6f / (float)imu_integration_rate_hz; - - _integrator_reset_samples = roundf(imu_integration_interval_us / update_interval_us); - _integrator.set_autoreset_interval(_integrator_reset_samples * update_interval_us); -} - void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z) { // Apply rotation (before scaling) @@ -157,67 +116,22 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float const Vector3f raw{x, y, z}; - // Clipping (check unscaled raw values) - for (int i = 0; i < 3; i++) { - if (fabsf(raw(i)) > _clip_limit) { - _clipping_total[i]++; - _integrator_clipping(i)++; - } - } - // Apply range scale and the calibrating offset/scale const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)}; - // publish raw data immediately - { - sensor_gyro_s report; + // publish + sensor_gyro_s report; - report.timestamp_sample = timestamp_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(); + report.timestamp_sample = timestamp_sample; + report.device_id = _device_id; + report.temperature = _temperature; + report.error_count = _error_count; + report.x = val_calibrated(0); + report.y = val_calibrated(1); + report.z = val_calibrated(2); + report.timestamp = hrt_absolute_time(); - _sensor_pub.publish(report); - } - - // Integrated values - Vector3f delta_angle; - uint32_t integral_dt = 0; - - _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 = timestamp_sample; - report.error_count = _error_count; - report.device_id = _device_id; - delta_angle.copyTo(report.delta_angle); - report.dt = integral_dt; - report.samples = _integrator_samples; - - for (int i = 0; i < 3; i++) { - report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i))); - } - - report.timestamp = hrt_absolute_time(); - - _sensor_integrated_pub.publish(report); - - - // reset integrator - ResetIntegrator(); - - // update vibration metrics - UpdateVibrationMetrics(delta_angle); - } - - PublishStatus(); + _sensor_pub.publish(report); } void PX4Gyroscope::updateFIFO(const FIFOSample &sample) @@ -225,24 +139,36 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample) const uint8_t N = sample.samples; const float dt = sample.dt; - // publish raw data immediately { - // average - float x = (float)sum(sample.x, N) / (float)N; - float y = (float)sum(sample.y, N) / (float)N; - float z = (float)sum(sample.z, N) / (float)N; + // trapezoidal integration (equally spaced, scaled by dt later) + Vector3f integral{ + (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)), + (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)), + (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)), + }; + + _last_sample[0] = sample.x[N - 1]; + _last_sample[1] = sample.y[N - 1]; + _last_sample[2] = sample.z[N - 1]; // Apply rotation (before scaling) - rotate_3f(_rotation, x, y, z); + rotate_3f(_rotation, integral(0), integral(1), integral(2)); + + // average + const float x = integral(0) / (float)N; + const float y = integral(1) / (float)N; + const float z = integral(2) / (float)N; // Apply range scale and the calibration offset const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset}; + // publish sensor_gyro_s report; report.timestamp_sample = sample.timestamp_sample; report.device_id = _device_id; report.temperature = _temperature; + report.error_count = _error_count; report.x = val_calibrated(0); report.y = val_calibrated(1); report.z = val_calibrated(2); @@ -252,83 +178,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample) } - // clipping - unsigned clip_count_x = clipping(sample.x, _clip_limit, N); - unsigned clip_count_y = clipping(sample.y, _clip_limit, N); - unsigned clip_count_z = clipping(sample.z, _clip_limit, N); - - _clipping_total[0] += clip_count_x; - _clipping_total[1] += clip_count_y; - _clipping_total[2] += clip_count_z; - - _integrator_clipping(0) += clip_count_x; - _integrator_clipping(1) += clip_count_y; - _integrator_clipping(2) += clip_count_z; - - // integrated data (INS) - { - // reset integrator if previous sample was too long ago - if ((sample.timestamp_sample > _timestamp_sample_prev) - && ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) { - - ResetIntegrator(); - } - - // integrate - _integrator_samples += 1; - _integrator_fifo_samples += N; - - // trapezoidal integration (equally spaced, scaled by dt later) - _integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)); - _integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)); - _integration_raw(2) += (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)); - _last_sample[0] = sample.x[N - 1]; - _last_sample[1] = sample.y[N - 1]; - _last_sample[2] = sample.z[N - 1]; - - - if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) { - - // Apply rotation (before scaling) - rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2)); - - // scale calibration offset to number of samples - const Vector3f offset{_calibration_offset * _integrator_fifo_samples}; - - // Apply calibration and scale to seconds - const Vector3f delta_angle{((_integration_raw * _scale) - offset) * 1e-6f * dt}; - - // fill sensor_gyro_integrated and publish - sensor_gyro_integrated_s report; - - report.timestamp_sample = sample.timestamp_sample; - report.error_count = _error_count; - report.device_id = _device_id; - delta_angle.copyTo(report.delta_angle); - report.dt = _integrator_fifo_samples * dt; // time span in microseconds - report.samples = _integrator_fifo_samples; - - rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2)); - const Vector3f clipping{_integrator_clipping}; - - for (int i = 0; i < 3; i++) { - report.clip_counter[i] = fabsf(roundf(clipping(i))); - } - - report.timestamp = hrt_absolute_time(); - _sensor_integrated_pub.publish(report); - - // update vibration metrics - UpdateVibrationMetrics(delta_angle); - - // reset integrator - ResetIntegrator(); - } - - _timestamp_sample_prev = sample.timestamp_sample; - } - - // publish sensor fifo + // publish fifo sensor_gyro_fifo_s fifo{}; fifo.device_id = _device_id; @@ -343,62 +193,6 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample) fifo.timestamp = hrt_absolute_time(); _sensor_fifo_pub.publish(fifo); - - - PublishStatus(); -} - -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_hz = _update_rate; - status.temperature = _temperature; - status.vibration_metric = _vibration_metric; - status.coning_vibration = _coning_vibration; - status.clipping[0] = _clipping_total[0]; - status.clipping[1] = _clipping_total[1]; - status.clipping[2] = _clipping_total[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; - _integration_raw.zero(); - _integrator_clipping.zero(); - - _timestamp_sample_prev = 0; -} - -void PX4Gyroscope::UpdateClipLimit() -{ - // 99.9% of potential max - _clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX); -} - -void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle) -{ - // Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) - const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev; - _vibration_metric = 0.99f * _vibration_metric + 0.01f * delta_angle_diff.norm(); - - // Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) - const Vector3f coning_metric = delta_angle % _delta_angle_prev; - _coning_vibration = 0.99f * _coning_vibration + 0.01f * coning_metric.norm(); - - _delta_angle_prev = delta_angle; } void PX4Gyroscope::print_status() diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index d6159566e2..2b94919c64 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -37,14 +37,11 @@ #include #include #include -#include #include #include #include #include #include -#include -#include class PX4Gyroscope : public cdev::CDev, public ModuleParams { @@ -62,10 +59,9 @@ public: void set_device_type(uint8_t devtype); void set_error_count(uint64_t error_count) { _error_count = error_count; } void increase_error_count() { _error_count++; } - void set_range(float range) { _range = range; UpdateClipLimit(); } - void set_scale(float scale) { _scale = scale; UpdateClipLimit(); } + void set_range(float range) { _range = range; } + void set_scale(float scale) { _scale = scale; } void set_temperature(float temperature) { _temperature = temperature; } - void set_update_rate(uint16_t rate); void update(hrt_abstime timestamp_sample, float x, float y, float z); @@ -87,27 +83,11 @@ public: void updateFIFO(const FIFOSample &sample); private: - - void PublishStatus(); - void ResetIntegrator(); - void UpdateClipLimit(); - void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle); - uORB::PublicationQueuedMulti _sensor_pub; uORB::PublicationMulti _sensor_fifo_pub; - uORB::PublicationMulti _sensor_integrated_pub; - uORB::PublicationMulti _sensor_status_pub; - - hrt_abstime _status_last_publish{0}; - - Integrator _integrator{5000, true}; // 200 Hz default matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f}; - matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement - float _vibration_metric{0.f}; // high frequency vibration level in the IMU delta angle data (rad) - float _coning_vibration{0.f}; // Level of coning vibration in the IMU delta angles (rad^2) - int _class_device_instance{-1}; uint32_t _device_id{0}; @@ -117,25 +97,11 @@ private: float _scale{1.f}; float _temperature{0.f}; - int16_t _clip_limit{(int16_t)(_range / _scale)}; + uint32_t _error_count{0}; - uint64_t _error_count{0}; - - uint32_t _clipping_total[3] {}; - - uint16_t _update_rate{1000}; - - // integrator - hrt_abstime _timestamp_sample_prev{0}; - matrix::Vector3f _integration_raw{}; - matrix::Vector3f _integrator_clipping{}; int16_t _last_sample[3] {}; - uint8_t _integrator_reset_samples{4}; - uint8_t _integrator_samples{0}; - uint8_t _integrator_fifo_samples{0}; DEFINE_PARAMETERS( - (ParamInt) _param_imu_gyro_rate_max, - (ParamInt) _param_imu_integ_rate + (ParamInt) _param_imu_gyro_rate_max ) }; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index a2cdd137b2..acb9f2dd08 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -772,9 +772,9 @@ void Ekf2::Run() updated = _vehicle_imu_subs[_imu_sub_index].update(&imu); imu_sample_new.time_us = imu.timestamp_sample; - imu_sample_new.delta_ang_dt = imu.dt * 1.e-6f; + imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f; imu_sample_new.delta_ang = Vector3f{imu.delta_angle}; - imu_sample_new.delta_vel_dt = imu.dt * 1.e-6f; + imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f; imu_sample_new.delta_vel = Vector3f{imu.delta_velocity}; if (imu.delta_velocity_clipping > 0) { @@ -783,7 +783,7 @@ void Ekf2::Run() imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z; } - imu_dt = imu.dt; + imu_dt = imu.delta_angle_dt; bias.accel_device_id = imu.accel_device_id; bias.gyro_device_id = imu.gyro_device_id; @@ -853,12 +853,10 @@ void Ekf2::Run() if (_imu_sub_index < 0) { if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) { - PX4_WARN("accel id changed, resetting IMU bias"); _imu_bias_reset_request = true; } if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) { - PX4_WARN("gyro id changed, resetting IMU bias"); _imu_bias_reset_request = true; } } diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 56626b890e..05ec409726 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -110,12 +110,12 @@ void LoggedTopics::add_default_topics() add_topic_multi("distance_sensor", 1000); add_topic_multi("optical_flow", 1000); add_topic_multi("sensor_accel", 1000); - add_topic_multi("sensor_accel_status", 1000); add_topic_multi("sensor_baro", 1000); add_topic_multi("sensor_gyro", 1000); - add_topic_multi("sensor_gyro_status", 1000); add_topic_multi("sensor_mag", 1000); add_topic_multi("vehicle_gps_position", 1000); + add_topic_multi("vehicle_imu", 500); + add_topic_multi("vehicle_imu_status", 1000); #ifdef CONFIG_ARCH_BOARD_PX4_SITL add_topic("actuator_controls_virtual_fw"); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0e92dc1177..1180a38394 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -84,12 +84,8 @@ #include #include #include -#include -#include #include #include -#include -#include #include #include #include @@ -106,6 +102,8 @@ #include #include #include +#include +#include #include #include #include @@ -1062,12 +1060,11 @@ public: unsigned get_size() override { - return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _raw_imu_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 0}; - uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 0}; + uORB::Subscription _raw_imu_sub{ORB_ID(vehicle_imu), 0}; uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 0}; // do not allow top copy this class @@ -1080,28 +1077,25 @@ protected: bool send(const hrt_abstime t) override { - if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) { + if (_raw_imu_sub.updated() || _raw_mag_sub.updated()) { - sensor_accel_integrated_s sensor_accel{}; - _raw_accel_sub.copy(&sensor_accel); - - sensor_gyro_integrated_s sensor_gyro{}; - _raw_gyro_sub.copy(&sensor_gyro); + vehicle_imu_s imu{}; + _raw_imu_sub.copy(&imu); sensor_mag_s sensor_mag{}; _raw_mag_sub.copy(&sensor_mag); mavlink_scaled_imu_t msg{}; - msg.time_boot_ms = sensor_accel.timestamp / 1000; + msg.time_boot_ms = imu.timestamp / 1000; // 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; + const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f accel = Vector3f{imu.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; + const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt; + const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f; msg.xacc = (int16_t)accel(0); msg.yacc = (int16_t)accel(1); @@ -1152,12 +1146,11 @@ public: unsigned get_size() override { - return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _raw_imu_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 1}; - uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 1}; + uORB::Subscription _raw_imu_sub{ORB_ID(vehicle_imu), 1}; uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 1}; // do not allow top copy this class @@ -1170,28 +1163,25 @@ protected: bool send(const hrt_abstime t) override { - if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) { + if (_raw_imu_sub.updated() || _raw_mag_sub.updated()) { - sensor_accel_integrated_s sensor_accel{}; - _raw_accel_sub.copy(&sensor_accel); - - sensor_gyro_integrated_s sensor_gyro{}; - _raw_gyro_sub.copy(&sensor_gyro); + vehicle_imu_s imu{}; + _raw_imu_sub.copy(&imu); sensor_mag_s sensor_mag{}; _raw_mag_sub.copy(&sensor_mag); mavlink_scaled_imu2_t msg{}; - msg.time_boot_ms = sensor_accel.timestamp / 1000; + msg.time_boot_ms = imu.timestamp / 1000; // 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; + const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f accel = Vector3f{imu.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; + const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt; + const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f; msg.xacc = (int16_t)accel(0); msg.yacc = (int16_t)accel(1); @@ -1241,12 +1231,11 @@ public: unsigned get_size() override { - return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _raw_imu_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 2}; - uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 2}; + uORB::Subscription _raw_imu_sub{ORB_ID(vehicle_imu), 2}; uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 2}; // do not allow top copy this class @@ -1259,28 +1248,25 @@ protected: bool send(const hrt_abstime t) override { - if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) { + if (_raw_imu_sub.updated() || _raw_mag_sub.updated()) { - sensor_accel_integrated_s sensor_accel{}; - _raw_accel_sub.copy(&sensor_accel); - - sensor_gyro_integrated_s sensor_gyro{}; - _raw_gyro_sub.copy(&sensor_gyro); + vehicle_imu_s imu{}; + _raw_imu_sub.copy(&imu); sensor_mag_s sensor_mag{}; _raw_mag_sub.copy(&sensor_mag); mavlink_scaled_imu3_t msg{}; - msg.time_boot_ms = sensor_accel.timestamp / 1000; + msg.time_boot_ms = imu.timestamp / 1000; // 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; + const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; + const Vector3f accel = Vector3f{imu.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; + const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt; + const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f; msg.xacc = (int16_t)accel(0); msg.yacc = (int16_t)accel(1); @@ -2817,13 +2803,7 @@ public: return size; } - for (auto &x : _sensor_accel_status_sub) { - if (x.advertised()) { - return size; - } - } - - for (auto &x : _sensor_gyro_status_sub) { + for (auto &x : _vehicle_imu_status_sub) { if (x.advertised()) { return size; } @@ -2835,16 +2815,10 @@ public: private: uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; - uORB::Subscription _sensor_accel_status_sub[3] { - {ORB_ID(sensor_accel_status), 0}, - {ORB_ID(sensor_accel_status), 1}, - {ORB_ID(sensor_accel_status), 2}, - }; - - uORB::Subscription _sensor_gyro_status_sub[3] { - {ORB_ID(sensor_gyro_status), 0}, - {ORB_ID(sensor_gyro_status), 1}, - {ORB_ID(sensor_gyro_status), 2}, + uORB::Subscription _vehicle_imu_status_sub[3] { + {ORB_ID(vehicle_imu_status), 0}, + {ORB_ID(vehicle_imu_status), 1}, + {ORB_ID(vehicle_imu_status), 2}, }; /* do not allow top copying this class */ @@ -2859,10 +2833,10 @@ protected: { bool updated = _sensor_selection_sub.updated(); - // check for sensor_accel_status update + // check for vehicle_imu_status update if (!updated) { for (int i = 0; i < 3; i++) { - if (_sensor_accel_status_sub[i].updated() || _sensor_gyro_status_sub[i].updated()) { + if (_vehicle_imu_status_sub[i].updated()) { updated = true; break; } @@ -2882,29 +2856,16 @@ protected: sensor_selection_s sensor_selection{}; _sensor_selection_sub.copy(&sensor_selection); - // primary gyro coning and high frequency vibration metrics - if (sensor_selection.gyro_device_id != 0) { - for (auto &x : _sensor_gyro_status_sub) { - sensor_gyro_status_s status; - - if (x.copy(&status)) { - if (status.device_id == sensor_selection.gyro_device_id) { - msg.vibration_x = status.coning_vibration; - msg.vibration_y = status.vibration_metric; - break; - } - } - } - } - // primary accel high frequency vibration metric if (sensor_selection.accel_device_id != 0) { - for (auto &x : _sensor_accel_status_sub) { - sensor_accel_status_s status; + for (auto &x : _vehicle_imu_status_sub) { + vehicle_imu_status_s status; if (x.copy(&status)) { - if (status.device_id == sensor_selection.accel_device_id) { - msg.vibration_z = status.vibration_metric; + if (status.accel_device_id == sensor_selection.accel_device_id) { + msg.vibration_x = status.gyro_coning_vibration; + msg.vibration_y = status.gyro_vibration_metric; + msg.vibration_z = status.accel_vibration_metric; break; } } @@ -2913,11 +2874,11 @@ protected: // accel 0, 1, 2 cumulative clipping for (int i = 0; i < 3; i++) { - sensor_accel_status_s acc_status; + vehicle_imu_status_s status; - if (_sensor_accel_status_sub[i].copy(&acc_status)) { + if (_vehicle_imu_status_sub[i].copy(&status)) { - const uint32_t clipping = acc_status.clipping[0] + acc_status.clipping[1] + acc_status.clipping[2]; + const uint32_t clipping = status.accel_clipping[0] + status.accel_clipping[1] + status.accel_clipping[2]; switch (i) { case 0: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5e42484ec7..cec9929f6f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2188,15 +2188,8 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* gyro */ { if (_px4_gyro == nullptr) { - // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION - _px4_gyro = new PX4Gyroscope(2294028); - - if (_px4_gyro == nullptr) { - PX4_ERR("PX4Gyroscope alloc failed"); - - } else { - _px4_gyro->set_update_rate(200); // TODO: measure actual - } + // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_gyro = new PX4Gyroscope(1311244); } if (_px4_gyro != nullptr) { @@ -2208,15 +2201,8 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* accelerometer */ { if (_px4_accel == nullptr) { - // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION _px4_accel = new PX4Accelerometer(1311244); - - if (_px4_accel == nullptr) { - PX4_ERR("PX4Accelerometer alloc failed"); - - } else { - _px4_accel->set_update_rate(200); // TODO: measure actual - } } if (_px4_accel != nullptr) { @@ -2230,10 +2216,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) if (_px4_mag == nullptr) { // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION _px4_mag = new PX4Magnetometer(197388); - - if (_px4_mag == nullptr) { - PX4_ERR("PX4Magnetometer alloc failed"); - } } if (_px4_mag != nullptr) { @@ -2247,10 +2229,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) if (_px4_baro == nullptr) { // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION _px4_baro = new PX4Barometer(6620172); - - if (_px4_baro == nullptr) { - PX4_ERR("PX4Barometer alloc failed"); - } } if (_px4_baro != nullptr) { @@ -2629,7 +2607,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* accelerometer */ { if (_px4_accel == nullptr) { - // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION _px4_accel = new PX4Accelerometer(1311244); if (_px4_accel == nullptr) { @@ -2647,8 +2625,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* gyroscope */ { if (_px4_gyro == nullptr) { - // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION - _px4_gyro = new PX4Gyroscope(2294028); + // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_gyro = new PX4Gyroscope(1311244); if (_px4_gyro == nullptr) { PX4_ERR("PX4Gyroscope alloc failed"); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index e4ea12c319..3316d5b1f7 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -41,7 +41,6 @@ add_subdirectory(vehicle_imu) px4_add_module( MODULE modules__sensors MAIN sensors - PRIORITY "SCHED_PRIORITY_MAX-5" SRCS voted_sensors_update.cpp sensors.cpp diff --git a/src/modules/sensors/sensor_corrections/SensorCorrections.hpp b/src/modules/sensors/sensor_corrections/SensorCorrections.hpp index 75881e72f8..88c146b730 100644 --- a/src/modules/sensors/sensor_corrections/SensorCorrections.hpp +++ b/src/modules/sensors/sensor_corrections/SensorCorrections.hpp @@ -69,6 +69,8 @@ public: void ParametersUpdate(); void SensorCorrectionsUpdate(bool force = false); + const matrix::Dcmf &getBoardRotation() const { return _board_rotation; } + private: static constexpr int MAX_SENSOR_COUNT = 3; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 998ea9ce0c..60bc639efa 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -68,6 +68,7 @@ #include #include #include +#include #include #include @@ -120,6 +121,12 @@ private: sensor_combined_s _sensor_combined{}; sensor_preflight_s _sensor_preflight{}; + uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[3] { + {this, ORB_ID(vehicle_imu), 0}, + {this, ORB_ID(vehicle_imu), 1}, + {this, ORB_ID(vehicle_imu), 2} + }; + uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */ uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */ uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ @@ -132,12 +139,6 @@ private: uORB::Publication _sensor_preflight_pub{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */ uORB::Publication _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */ - uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub[GYRO_COUNT_MAX] { - {this, ORB_ID(sensor_gyro_integrated), 0}, - {this, ORB_ID(sensor_gyro_integrated), 1}, - {this, ORB_ID(sensor_gyro_integrated), 2} - }; - enum class MagCompensationType { Disabled = 0, Throttle, @@ -147,9 +148,6 @@ private: MagCompensationType _mag_comp_type{MagCompensationType::Disabled}; - uint32_t _selected_sensor_device_id{0}; - uint8_t _selected_sensor_sub_index{0}; - perf_counter_t _loop_perf; /**< loop performance counter */ DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ @@ -211,7 +209,7 @@ Sensors::Sensors(bool hil_enabled) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers), _hil_enabled(hil_enabled), _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), - _voted_sensors_update(_parameters, hil_enabled) + _voted_sensors_update(_parameters, hil_enabled, _vehicle_imu_sub) { initialize_parameter_handles(_parameter_handles); @@ -227,6 +225,11 @@ Sensors::Sensors(bool hil_enabled) : Sensors::~Sensors() { + // clear all registered callbacks + for (auto &sub : _vehicle_imu_sub) { + sub.unregisterCallback(); + } + _vehicle_acceleration.Stop(); _vehicle_angular_velocity.Stop(); _vehicle_air_data.Stop(); @@ -243,6 +246,8 @@ bool Sensors::init() // initially run manually ScheduleDelayed(10_ms); + _vehicle_imu_sub[0].registerCallback(); + return true; } @@ -417,12 +422,12 @@ void Sensors::InitializeVehicleIMU() for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { if (_vehicle_imu_list[i] == nullptr) { - uORB::Subscription accel_sub{ORB_ID(sensor_accel_integrated), i}; - sensor_accel_integrated_s accel{}; + uORB::Subscription accel_sub{ORB_ID(sensor_accel), i}; + sensor_accel_s accel{}; accel_sub.copy(&accel); - uORB::Subscription gyro_sub{ORB_ID(sensor_gyro_integrated), i}; - sensor_gyro_integrated_s gyro{}; + uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i}; + sensor_gyro_s gyro{}; gyro_sub.copy(&gyro); if (accel.device_id > 0 && gyro.device_id > 0) { @@ -432,11 +437,16 @@ void Sensors::InitializeVehicleIMU() // Start VehicleIMU instance and store if (imu->Start()) { _vehicle_imu_list[i] = imu; + ScheduleNow(); } else { delete imu; } } + + } else { + // abort on first failure, try again later + return; } } } @@ -446,7 +456,7 @@ void Sensors::Run() { if (should_exit()) { // clear all registered callbacks - for (auto &sub : _sensor_gyro_integrated_sub) { + for (auto &sub : _vehicle_imu_sub) { sub.unregisterCallback(); } @@ -465,9 +475,6 @@ void Sensors::Run() // backup schedule as a watchdog timeout ScheduleDelayed(10_ms); - sensor_gyro_integrated_s gyro_integrated; - _sensor_gyro_integrated_sub[_selected_sensor_sub_index].copy(&gyro_integrated); - // check vehicle status for changes to publication state if (_vcontrol_mode_sub.updated()) { vehicle_control_mode_s vcontrol_mode{}; @@ -516,32 +523,6 @@ void Sensors::Run() diff_pres_poll(); - - // check failover and update subscribed sensor (if necessary) - _voted_sensors_update.checkFailover(); - const uint32_t current_device_id = _voted_sensors_update.bestGyroID(); - - if (_selected_sensor_device_id != current_device_id) { - // clear all registered callbacks - for (auto &sub : _sensor_gyro_integrated_sub) { - sub.unregisterCallback(); - } - - for (int i = 0; i < GYRO_COUNT_MAX; i++) { - sensor_gyro_integrated_s report{}; - - if (_sensor_gyro_integrated_sub[i].copy(&report)) { - if ((report.device_id != 0) && (report.device_id == current_device_id)) { - if (_sensor_gyro_integrated_sub[i].registerCallback()) { - // record selected sensor (array index) - _selected_sensor_sub_index = i; - _selected_sensor_device_id = current_device_id; - } - } - } - } - } - if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) { _magnetometer_pub.publish(magnetometer); _magnetometer_prev_timestamp = magnetometer.timestamp; @@ -639,7 +620,9 @@ int Sensors::print_status() _voted_sensors_update.printStatus(); PX4_INFO_RAW("\n"); + _vehicle_air_data.PrintStatus(); + PX4_INFO_RAW("\n"); PX4_INFO("Airspeed status:"); _airspeed_validator.print(); @@ -649,14 +632,10 @@ int Sensors::print_status() PX4_INFO_RAW("\n"); _vehicle_angular_velocity.PrintStatus(); - PX4_INFO_RAW("\n"); - _vehicle_air_data.PrintStatus(); - PX4_INFO_RAW("\n"); for (auto &i : _vehicle_imu_list) { if (i != nullptr) { - PX4_INFO_RAW("\n"); i->PrintStatus(); } } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 87d6940c93..82e31e3044 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -43,7 +43,7 @@ namespace sensors VehicleAcceleration::VehicleAcceleration() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers), _corrections(this, SensorCorrections::SensorType::Accelerometer) { _lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get()); @@ -65,7 +65,10 @@ bool VehicleAcceleration::Start() return false; } - ScheduleNow(); + if (!SensorSelectionUpdate(true)) { + ScheduleDelayed(10_ms); + } + return true; } @@ -198,6 +201,9 @@ void VehicleAcceleration::ParametersUpdate(bool force) void VehicleAcceleration::Run() { + // backup schedule + ScheduleDelayed(10_ms); + // update corrections first to set _selected_sensor bool selection_updated = SensorSelectionUpdate(); @@ -259,11 +265,9 @@ void VehicleAcceleration::Run() void VehicleAcceleration::PrintStatus() { - 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("sample rate: %.3f Hz", (double)_update_rate_hz); - + PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz", + _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz); + PX4_INFO("estimated bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2)); _corrections.PrintStatus(); } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 288cdff45a..0fc52419b4 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include #include @@ -54,7 +54,7 @@ namespace sensors { -class VehicleAcceleration : public ModuleParams, public px4::WorkItem +class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem { public: diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index fffb69fdf9..ec27e568f3 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -278,6 +278,5 @@ void VehicleAirData::PrintStatus() PX4_INFO("selected barometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id, _selected_sensor_sub_index); } - perf_print_counter(_cycle_perf); _voter.print(); } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 946b706c67..9fe653c755 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -43,7 +43,7 @@ namespace sensors VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _corrections(this, SensorCorrections::SensorType::Gyroscope) { _lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get()); @@ -68,7 +68,10 @@ bool VehicleAngularVelocity::Start() return false; } - ScheduleNow(); + if (!SensorSelectionUpdate(true)) { + ScheduleDelayed(10_ms); + } + return true; } @@ -218,6 +221,9 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) void VehicleAngularVelocity::Run() { + // backup schedule + ScheduleDelayed(10_ms); + // update corrections first to set _selected_sensor bool selection_updated = SensorSelectionUpdate(); @@ -311,11 +317,9 @@ void VehicleAngularVelocity::Run() void VehicleAngularVelocity::PrintStatus() { - 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("sample rate: %.3f Hz", (double)_update_rate_hz); - + PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz", + _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz); + PX4_INFO("estimated bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2)); _corrections.PrintStatus(); } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 5886e38837..df524998d9 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,7 +56,7 @@ namespace sensors { -class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem +class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem { public: diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index eafaefa874..7ce66b836b 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -32,6 +32,8 @@ ############################################################################ px4_add_library(vehicle_imu + Integrator.cpp + Integrator.hpp VehicleIMU.cpp VehicleIMU.hpp ) diff --git a/src/lib/drivers/device/integrator.cpp b/src/modules/sensors/vehicle_imu/Integrator.cpp similarity index 65% rename from src/lib/drivers/device/integrator.cpp rename to src/modules/sensors/vehicle_imu/Integrator.cpp index 8cd298d21b..cf79744e42 100644 --- a/src/lib/drivers/device/integrator.cpp +++ b/src/modules/sensors/vehicle_imu/Integrator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,30 +31,15 @@ * ****************************************************************************/ -/** - * @file integrator.cpp - * - * A resettable integrator - * - * @author Lorenz Meier - * @author Julian Oes - */ - -#include "integrator.h" +#include "Integrator.hpp" #include -Integrator::Integrator(uint32_t auto_reset_interval, bool coning_compensation) : - _coning_comp_on(coning_compensation) -{ - set_autoreset_interval(auto_reset_interval); -} +using matrix::Vector3f; -bool -Integrator::put(const hrt_abstime ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral, - uint32_t &integral_dt) +bool Integrator::put(const hrt_abstime ×tamp, const Vector3f &val) { - if (_last_integration_time == 0) { + if ((_last_integration_time == 0) || (timestamp <= _last_integration_time)) { /* this is the first item in the integrator */ _last_integration_time = timestamp; _last_reset_time = timestamp; @@ -63,18 +48,12 @@ Integrator::put(const hrt_abstime ×tamp, const matrix::Vector3f &val, matri return false; } - float dt = 0.0f; - - // Integrate: - // Leave dt at 0 if the integration time does not make sense. - // Without this check the integral is likely to explode. - if (timestamp >= _last_integration_time) { - dt = static_cast(timestamp - _last_integration_time) * 1e-6f; - } - // Use trapezoidal integration to calculate the delta integral + const float dt = static_cast(timestamp - _last_integration_time) * 1e-6f; const matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f; _last_val = val; + _last_integration_time = timestamp; + _integrated_samples++; // Calculate coning corrections if required if (_coning_comp_on) { @@ -83,7 +62,7 @@ Integrator::put(const hrt_abstime ×tamp, const matrix::Vector3f &val, matri // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation // Sourced: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf // Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m - _beta += ((_last_alpha + _last_delta_alpha * (1.0f / 6.0f)) % delta_alpha) * 0.5f; + _beta += ((_last_alpha + _last_delta_alpha * (1.f / 6.f)) % delta_alpha) * 0.5f; _last_delta_alpha = delta_alpha; _last_alpha = _alpha; } @@ -91,37 +70,28 @@ Integrator::put(const hrt_abstime ×tamp, const matrix::Vector3f &val, matri // accumulate delta integrals _alpha += delta_alpha; - _last_integration_time = timestamp; + return true; +} - // Only do auto reset if auto reset interval is not 0. - if (_auto_reset_interval > 0 && (timestamp - _last_reset_time) >= _auto_reset_interval) { +bool Integrator::reset(Vector3f &integral, uint32_t &integral_dt) +{ + if (integral_ready()) { + integral = Vector3f{_alpha}; + _alpha.zero(); + + integral_dt = (_last_integration_time - _last_reset_time); + _last_reset_time = _last_integration_time; + _integrated_samples = 0; // apply coning corrections if required if (_coning_comp_on) { - integral = _alpha + _beta; - - } else { - integral = _alpha; + integral += _beta; + _beta.zero(); + _last_alpha.zero(); } - // reset the integrals and coning corrections - _reset(integral_dt); - return true; - - } else { - return false; } -} - -void -Integrator::_reset(uint32_t &integral_dt) -{ - _alpha.zero(); - _last_alpha.zero(); - _beta.zero(); - - integral_dt = (_last_integration_time - _last_reset_time); - - _last_reset_time = _last_integration_time; + + return false; } diff --git a/src/lib/drivers/device/integrator.h b/src/modules/sensors/vehicle_imu/Integrator.hpp similarity index 50% rename from src/lib/drivers/device/integrator.h rename to src/modules/sensors/vehicle_imu/Integrator.hpp index 805476785d..02c8d2fd05 100644 --- a/src/lib/drivers/device/integrator.h +++ b/src/modules/sensors/vehicle_imu/Integrator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file integrator.h + * @file Integrator.hpp * * A resettable integrator * @@ -48,9 +48,18 @@ class Integrator { public: - Integrator(uint32_t auto_reset_interval = 2500 /* 400 Hz */, bool coning_compensation = false); + Integrator(bool coning_compensation = false) : _coning_comp_on(coning_compensation) {} ~Integrator() = default; + /** + * Put an item into the integral. + * + * @param timestamp Timestamp of the current value. + * @param val Item to put. + * @return true if data was accepted and integrated. + */ + bool put(const uint64_t ×tamp, const matrix::Vector3f &val); + /** * Put an item into the integral. * @@ -61,33 +70,54 @@ public: * @return true if putting the item triggered an integral reset and the integral should be * published. */ - bool put(const uint64_t ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt); + bool put(const uint64_t ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt) + { + return put(timestamp, val) && reset(integral, integral_dt); + } /** - * Set auto reset interval during runtime. This won't reset the integrator. + * Set reset interval during runtime. This won't reset the integrator. * - * @param auto_reset_interval New reset time interval for the integrator (+- 10%). + * @param reset_interval New reset time interval for the integrator. */ - void set_autoreset_interval(uint32_t auto_reset_interval) { _auto_reset_interval = 0.90f * auto_reset_interval; } + void set_reset_interval(uint32_t reset_interval) { _reset_interval_min = reset_interval; } -private: - uint32_t _auto_reset_interval{0}; /**< the interval after which the content will be published - and the integrator reset, 0 if no auto-reset */ + /** + * Set required samples for reset. This won't reset the integrator. + * + * @param reset_samples New reset time interval for the integrator. + */ + void set_reset_samples(uint8_t reset_samples) { _reset_samples_min = reset_samples; } + uint8_t get_reset_samples() const { return _reset_samples_min; } - uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */ - uint64_t _last_reset_time{0}; /**< last auto-announcement of integral value */ + /** + * Is the Integrator ready to reset? + * + * @return true if integrator has sufficient data (minimum interval & samples satisfied) to reset. + */ + bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_last_integration_time >= (_last_reset_time + _reset_interval_min)); } - matrix::Vector3f _alpha{0.0f, 0.0f, 0.0f}; /**< integrated value before coning corrections are applied */ - matrix::Vector3f _last_alpha{0.0f, 0.0f, 0.0f}; /**< previous value of _alpha */ - matrix::Vector3f _beta{0.0f, 0.0f, 0.0f}; /**< accumulated coning corrections */ - matrix::Vector3f _last_val{0.0f, 0.0f, 0.0f}; /**< previous input */ - matrix::Vector3f _last_delta_alpha{0.0f, 0.0f, 0.0f}; /**< integral from previous previous sampling interval */ - - bool _coning_comp_on{false}; /**< true to turn on coning corrections */ - - /* Do a reset. + /* Reset integrator and return current integral & integration time * * @param integral_dt Get the dt in us of the current integration. + * @return true if integral valid */ - void _reset(uint32_t &integral_dt); + bool reset(matrix::Vector3f &integral, uint32_t &integral_dt); + +private: + uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */ + uint64_t _last_reset_time{0}; /**< last auto-announcement of integral value */ + + matrix::Vector3f _alpha{0.f, 0.f, 0.f}; /**< integrated value before coning corrections are applied */ + matrix::Vector3f _last_alpha{0.f, 0.f, 0.f}; /**< previous value of _alpha */ + matrix::Vector3f _beta{0.f, 0.f, 0.f}; /**< accumulated coning corrections */ + matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */ + matrix::Vector3f _last_delta_alpha{0.f, 0.f, 0.f}; /**< integral from previous previous sampling interval */ + + uint32_t _reset_interval_min{1}; /**< the interval after which the content will be published and the integrator reset */ + + uint8_t _integrated_samples{0}; + uint8_t _reset_samples_min{1}; + + const bool _coning_comp_on{false}; /**< true to turn on coning corrections */ }; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 05bd15d11b..0dc6fbe38d 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -35,20 +35,37 @@ #include +#include + using namespace matrix; using namespace time_literals; +using math::constrain; + namespace sensors { VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers), - _sensor_accel_integrated_sub(this, ORB_ID(sensor_accel_integrated), accel_index), - _sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers), + _sensor_accel_sub(this, ORB_ID(sensor_accel), accel_index), + _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index), _accel_corrections(this, SensorCorrections::SensorType::Accelerometer), _gyro_corrections(this, SensorCorrections::SensorType::Gyroscope) { + const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); + + _accel_integrator.set_reset_interval(configured_interval_us); + _accel_integrator.set_reset_samples(sensor_accel_s::ORB_QUEUE_LENGTH); + _sensor_accel_sub.set_required_updates(1); + + _gyro_integrator.set_reset_interval(configured_interval_us); + _gyro_integrator.set_reset_samples(sensor_gyro_s::ORB_QUEUE_LENGTH); + _sensor_gyro_sub.set_required_updates(1); + + // advertise immediately to ensure consistent ordering + _vehicle_imu_pub.advertise(); + _vehicle_imu_status_pub.advertise(); } VehicleIMU::~VehicleIMU() @@ -61,14 +78,14 @@ bool VehicleIMU::Start() // force initial updates ParametersUpdate(true); - return _sensor_accel_integrated_sub.registerCallback() && _sensor_gyro_integrated_sub.registerCallback(); + return _sensor_gyro_sub.registerCallback(); } void VehicleIMU::Stop() { // clear all registered callbacks - _sensor_accel_integrated_sub.unregisterCallback(); - _sensor_gyro_integrated_sub.unregisterCallback(); + _sensor_accel_sub.unregisterCallback(); + _sensor_gyro_sub.unregisterCallback(); Deinit(); } @@ -85,52 +102,251 @@ void VehicleIMU::ParametersUpdate(bool force) _accel_corrections.ParametersUpdate(); _gyro_corrections.ParametersUpdate(); + + // constrain IMU integration time 1-20 milliseconds (50-1000 Hz) + int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), 50, 1000); + + if (imu_integration_rate_hz != _param_imu_integ_rate.get()) { + _param_imu_integ_rate.set(imu_integration_rate_hz); + _param_imu_integ_rate.commit_no_notification(); + } } } +bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample) +{ + bool updated = false; + + if ((intavg.timestamp_sample_last > 0) && (timestamp_sample > intavg.timestamp_sample_last)) { + intavg.interval_sum += (timestamp_sample - intavg.timestamp_sample_last); + intavg.interval_count++; + + } else { + intavg.interval_sum = 0.f; + intavg.interval_count = 0.f; + } + + intavg.timestamp_sample_last = timestamp_sample; + + // periodically calculate sensor update rate + if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) { + + const float sample_interval_avg = intavg.interval_sum / intavg.interval_count; + + if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.f)) { + // update if interval has changed by more than 1% + if ((fabsf(intavg.update_interval - sample_interval_avg) / intavg.update_interval) > 0.01f) { + + intavg.update_interval = sample_interval_avg; + updated = true; + } + } + + // reset sample interval accumulator + intavg.timestamp_sample_last = 0; + } + + return updated; +} + void VehicleIMU::Run() { - if (_sensor_accel_integrated_sub.updated() && _sensor_gyro_integrated_sub.updated()) { - sensor_accel_integrated_s accel; - _sensor_accel_integrated_sub.copy(&accel); - _accel_corrections.set_device_id(accel.device_id); + // backup schedule + ScheduleDelayed(10_ms); - sensor_gyro_integrated_s gyro; - _sensor_gyro_integrated_sub.copy(&gyro); + ParametersUpdate(); + _accel_corrections.SensorCorrectionsUpdate(); + _gyro_corrections.SensorCorrectionsUpdate(); + + bool update_integrator_config = false; + + // integrate queued gyro + sensor_gyro_s gyro; + + while (!_gyro_integrator.integral_ready() && _sensor_gyro_sub.update(&gyro)) { _gyro_corrections.set_device_id(gyro.device_id); + _gyro_error_count = gyro.error_count; - ParametersUpdate(); - _accel_corrections.SensorCorrectionsUpdate(); - _gyro_corrections.SensorCorrectionsUpdate(); + const Vector3f gyro_corrected{_gyro_corrections.Correct(Vector3f{gyro.x, gyro.y, gyro.z})}; + _gyro_integrator.put(gyro.timestamp_sample, gyro_corrected); + _last_timestamp_sample_gyro = gyro.timestamp_sample; - // delta angle: apply offsets, scale, and board rotation - const float gyro_dt = 1.e-6f * gyro.dt; - Vector3f delta_angle = _gyro_corrections.Correct(Vector3f{gyro.delta_angle} * gyro_dt) / gyro_dt; - - // delta velocity: apply offsets, scale, and board rotation - const float accel_dt = 1.e-6f * accel.dt; - Vector3f delta_velocity = _accel_corrections.Correct(Vector3f{accel.delta_velocity} * accel_dt) / accel_dt; - - // publich vehicle_imu - vehicle_imu_s imu; - imu.timestamp_sample = accel.timestamp_sample; - imu.accel_device_id = accel.device_id; - imu.gyro_device_id = gyro.device_id; - - delta_angle.copyTo(imu.delta_angle); - delta_velocity.copyTo(imu.delta_velocity); - - imu.dt = accel.dt; - //imu.clip_count = accel.clip_count; - imu.timestamp = hrt_absolute_time(); - - _vehicle_imu_pub.publish(imu); + // collect sample interval average for filters + if (UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) { + update_integrator_config = true; + } } + + // update accel, stopping once caught up to the last gyro sample + sensor_accel_s accel; + + while (_sensor_accel_sub.update(&accel)) { + _accel_corrections.set_device_id(accel.device_id); + _accel_error_count = accel.error_count; + + const Vector3f accel_corrected{_accel_corrections.Correct(Vector3f{accel.x, accel.y, accel.z})}; + _accel_integrator.put(accel.timestamp_sample, accel_corrected); + _last_timestamp_sample_accel = accel.timestamp_sample; + + // collect sample interval average for filters + if (UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) { + update_integrator_config = true; + } + + if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) { + + // rotate sensor clip counts into vehicle body frame + const Vector3f clipping{_accel_corrections.getBoardRotation() * + Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}}; + + // round to get reasonble clip counts per axis (after board rotation) + const uint8_t clip_x = roundf(fabsf(clipping(0))); + const uint8_t clip_y = roundf(fabsf(clipping(1))); + const uint8_t clip_z = roundf(fabsf(clipping(2))); + + _delta_velocity_clipping_total[0] += clip_x; + _delta_velocity_clipping_total[1] += clip_y; + _delta_velocity_clipping_total[2] += clip_z; + + if (clip_x > 0) { + _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X; + } + + if (clip_y > 0) { + _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y; + } + + if (clip_z > 0) { + _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z; + } + } + + // break once caught up to gyro + if (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval)) { + break; + } + } + + // reconfigure integrators if calculated sensor intervals have changed + if (update_integrator_config) { + UpdateIntegratorConfiguration(); + } + + // publish if both accel & gyro integrators are ready + if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { + + uint32_t accel_integral_dt; + uint32_t gyro_integral_dt; + Vector3f delta_angle; + Vector3f delta_velocity; + + if (_accel_integrator.reset(delta_velocity, accel_integral_dt) + && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) { + + UpdateAccelVibrationMetrics(delta_velocity); + UpdateGyroVibrationMetrics(delta_angle); + + // vehicle_imu_status + // publish first so that error counts are available synchronously if needed + vehicle_imu_status_s status; + status.accel_device_id = _accel_corrections.get_device_id(); + status.gyro_device_id = _gyro_corrections.get_device_id(); + status.accel_error_count = _accel_error_count; + status.gyro_error_count = _gyro_error_count; + status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval); + status.gyro_rate_hz = round(1e6f / _gyro_interval.update_interval); + status.accel_vibration_metric = _accel_vibration_metric; + status.gyro_vibration_metric = _gyro_vibration_metric; + status.gyro_coning_vibration = _gyro_coning_vibration; + status.accel_clipping[0] = _delta_velocity_clipping_total[0]; + status.accel_clipping[1] = _delta_velocity_clipping_total[1]; + status.accel_clipping[2] = _delta_velocity_clipping_total[2]; + status.timestamp = hrt_absolute_time(); + _vehicle_imu_status_pub.publish(status); + + + // publish vehicle_imu + vehicle_imu_s imu; + imu.timestamp_sample = _last_timestamp_sample_gyro; + imu.accel_device_id = _accel_corrections.get_device_id(); + imu.gyro_device_id = _gyro_corrections.get_device_id(); + delta_angle.copyTo(imu.delta_angle); + delta_velocity.copyTo(imu.delta_velocity); + imu.delta_angle_dt = gyro_integral_dt; + imu.delta_velocity_dt = accel_integral_dt; + imu.delta_velocity_clipping = _delta_velocity_clipping; + imu.timestamp = hrt_absolute_time(); + _vehicle_imu_pub.publish(imu); + + // reset clip counts + _delta_velocity_clipping = 0; + + return; + } + } +} + +void VehicleIMU::UpdateIntegratorConfiguration() +{ + if ((_accel_interval.update_interval > 0) && (_gyro_interval.update_interval > 0)) { + + const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); + + // determine number of sensor samples that will get closest to the desired integration interval + const uint8_t accel_integral_samples = constrain(roundf(configured_interval_us / _accel_interval.update_interval), + 1.f, (float)sensor_accel_s::ORB_QUEUE_LENGTH); + + const uint8_t gyro_integral_samples = constrain(roundf(configured_interval_us / _gyro_interval.update_interval), + 1.f, (float)sensor_gyro_s::ORB_QUEUE_LENGTH); + + // let the gyro set the configuration and scheduling + // accel integrator will be forced to reset when gyro integrator is ready + _gyro_integrator.set_reset_samples(gyro_integral_samples); + _accel_integrator.set_reset_samples(1); + + // relaxed minimum integration time required + _accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * _accel_interval.update_interval)); + _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval.update_interval)); + + _sensor_accel_sub.set_required_updates(accel_integral_samples); + _sensor_gyro_sub.set_required_updates(gyro_integral_samples); + + // run when there are enough new gyro samples, unregister accel + _sensor_accel_sub.unregisterCallback(); + + PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d", + _accel_corrections.get_device_id(), _gyro_corrections.get_device_id(), accel_integral_samples, gyro_integral_samples); + } +} + +void VehicleIMU::UpdateAccelVibrationMetrics(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; + _accel_vibration_metric = 0.99f * _accel_vibration_metric + 0.01f * delta_velocity_diff.norm(); + + _delta_velocity_prev = delta_velocity; +} + +void VehicleIMU::UpdateGyroVibrationMetrics(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; + _gyro_vibration_metric = 0.99f * _gyro_vibration_metric + 0.01f * delta_angle_diff.norm(); + + // Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) + const Vector3f coning_metric = delta_angle % _delta_angle_prev; + _gyro_coning_vibration = 0.99f * _gyro_coning_vibration + 0.01f * coning_metric.norm(); + + _delta_angle_prev = delta_angle; } void VehicleIMU::PrintStatus() { - PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_corrections.get_device_id(), _gyro_corrections.get_device_id()); + PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", + _accel_corrections.get_device_id(), (double)_accel_interval.update_interval, + _gyro_corrections.get_device_id(), (double)_gyro_interval.update_interval); + _accel_corrections.PrintStatus(); _gyro_corrections.PrintStatus(); } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index cd9ec741fb..e73e0466ff 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -33,6 +33,8 @@ #pragma once +#include "Integrator.hpp" + #include #include @@ -40,19 +42,20 @@ #include #include #include -#include +#include #include #include #include #include -#include -#include +#include +#include #include +#include namespace sensors { -class VehicleIMU : public ModuleParams, public px4::WorkItem +class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem { public: VehicleIMU() = delete; @@ -69,13 +72,51 @@ private: void ParametersUpdate(bool force = false); void Run() override; + struct IntervalAverage { + hrt_abstime timestamp_sample_last{0}; + float interval_sum{0.f}; + float interval_count{0.f}; + float update_interval{0.f}; + }; + + bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample); + void UpdateIntegratorConfiguration(); + void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); + void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity); + uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; + uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; uORB::Subscription _params_sub{ORB_ID(parameter_update)}; - uORB::SubscriptionCallbackWorkItem _sensor_accel_integrated_sub; - uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub; + uORB::SubscriptionCallbackWorkItem _sensor_accel_sub; + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub; SensorCorrections _accel_corrections; SensorCorrections _gyro_corrections; + + Integrator _accel_integrator{}; // 200 Hz default + Integrator _gyro_integrator{true}; // 200 Hz default, coning compensation enabled + + hrt_abstime _last_timestamp_sample_accel{0}; + hrt_abstime _last_timestamp_sample_gyro{0}; + + IntervalAverage _accel_interval{}; + IntervalAverage _gyro_interval{}; + + uint32_t _accel_error_count{0}; + uint32_t _gyro_error_count{0}; + + matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement + matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement + float _accel_vibration_metric{0.f}; // high frequency vibration level in the IMU delta velocity data (m/s) + float _gyro_vibration_metric{0.f}; // high frequency vibration level in the IMU delta angle data (rad) + float _gyro_coning_vibration{0.f}; // Level of coning vibration in the IMU delta angles (rad^2) + + uint8_t _delta_velocity_clipping{0}; + uint32_t _delta_velocity_clipping_total[3] {}; + + DEFINE_PARAMETERS( + (ParamInt) _param_imu_integ_rate + ) }; } // namespace sensors diff --git a/src/modules/sensors/vehicle_imu/imu_parameters.c b/src/modules/sensors/vehicle_imu/imu_parameters.c index 33ac69f03a..00064a8808 100644 --- a/src/modules/sensors/vehicle_imu/imu_parameters.c +++ b/src/modules/sensors/vehicle_imu/imu_parameters.c @@ -35,6 +35,7 @@ * IMU integration rate. * * The rate at which raw IMU data is integrated to produce delta angles and delta velocities. +* Recommended to set this to a multiple of the estimator update period (currently 10 ms for ekf2). * * @min 100 * @max 1000 diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 5572a0c93b..fbaf8e958e 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -52,18 +52,14 @@ using namespace sensors; using namespace matrix; using math::radians; -VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled) - : ModuleParams(nullptr), _parameters(parameters), _hil_enabled(hil_enabled), _mag_compensator(this) +VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled, + uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) : + ModuleParams(nullptr), + _vehicle_imu_sub(vehicle_imu_sub), + _parameters(parameters), + _hil_enabled(hil_enabled), + _mag_compensator(this) { - for (unsigned i = 0; i < 3; i++) { - _corrections.gyro_scale_0[i] = 1.0f; - _corrections.accel_scale_0[i] = 1.0f; - _corrections.gyro_scale_1[i] = 1.0f; - _corrections.accel_scale_1[i] = 1.0f; - _corrections.gyro_scale_2[i] = 1.0f; - _corrections.accel_scale_2[i] = 1.0f; - } - _mag.voter.set_timeout(300000); _mag.voter.set_equal_value_threshold(1000); @@ -463,162 +459,104 @@ void VotedSensorsUpdate::parametersUpdate() } -void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw) +void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) { - float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 }; - float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 }; + for (int uorb_index = 0; uorb_index < 3; uorb_index++) { + vehicle_imu_s imu_report; - for (int uorb_index = 0; uorb_index < _accel.subscription_count; uorb_index++) { + if (_accel.enabled[uorb_index] && _gyro.enabled[uorb_index] && _vehicle_imu_sub[uorb_index].update(&imu_report)) { - sensor_accel_integrated_s accel_report; - - if (_accel.enabled[uorb_index] && _accel.subscription[uorb_index].update(&accel_report)) { + // copy corresponding vehicle_imu_status for accel & gyro error counts + vehicle_imu_status_s imu_status{}; + _vehicle_imu_status_sub[uorb_index].copy(&imu_status); // First publication with data if (_accel.priority[uorb_index] == 0) { _accel.priority[uorb_index] = _accel.subscription[uorb_index].get_priority(); } - _accel_device_id[uorb_index] = accel_report.device_id; - - /* - * 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; - - _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt; - - // apply temperature compensation - accel_data(0) = (accel_data(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X - accel_data(1) = (accel_data(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y - accel_data(2) = (accel_data(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z - - // rotate corrected measurements from sensor to body frame - accel_data = _board_rotation * accel_data; - - _last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0); - _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); - _last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2); - - // record if there's any clipping per axis - _last_sensor_data[uorb_index].accelerometer_clipping = 0; - - if (accel_report.clip_counter[0] > 0 || accel_report.clip_counter[1] > 0 || accel_report.clip_counter[2] > 0) { - - const Vector3f sensor_clip_count{(float)accel_report.clip_counter[0], (float)accel_report.clip_counter[1], (float)accel_report.clip_counter[2]}; - const Vector3f clipping{_board_rotation * sensor_clip_count}; - static constexpr float CLIP_COUNT_THRESHOLD = 1.f; - - if (fabsf(clipping(0)) >= CLIP_COUNT_THRESHOLD) { - _last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_X; - } - - if (fabsf(clipping(1)) >= CLIP_COUNT_THRESHOLD) { - _last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_Y; - } - - if (fabsf(clipping(2)) >= CLIP_COUNT_THRESHOLD) { - _last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_Z; - } - } - - _last_accel_timestamp[uorb_index] = accel_report.timestamp; - _accel.voter.put(uorb_index, accel_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, - accel_report.error_count, _accel.priority[uorb_index]); - } - } - - // find the best sensor - int best_index; - _accel.voter.get_best(hrt_absolute_time(), &best_index); - - // write the best sensor data to the output variables - if (best_index >= 0) { - raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt; - memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[best_index].accelerometer_m_s2, sizeof(raw.accelerometer_m_s2)); - - raw.accelerometer_clipping = _last_sensor_data[best_index].accelerometer_clipping; - - if (best_index != _accel.last_best_vote) { - _accel.last_best_vote = (uint8_t)best_index; - } - - if (_selection.accel_device_id != _accel_device_id[best_index]) { - _selection_changed = true; - _selection.accel_device_id = _accel_device_id[best_index]; - } - } -} - -void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw) -{ - float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 }; - float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 }; - - for (int uorb_index = 0; uorb_index < _gyro.subscription_count; uorb_index++) { - sensor_gyro_integrated_s gyro_report; - - if (_gyro.enabled[uorb_index] && _gyro.subscription[uorb_index].update(&gyro_report)) { - - // First publication with data if (_gyro.priority[uorb_index] == 0) { _gyro.priority[uorb_index] = _gyro.subscription[uorb_index].get_priority(); } - _gyro_device_id[uorb_index] = gyro_report.device_id; + _accel_device_id[uorb_index] = imu_report.accel_device_id; + _gyro_device_id[uorb_index] = imu_report.gyro_device_id; - /* - * 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 + const float accel_dt_inv = 1.e6f / (float)imu_report.delta_velocity_dt; + Vector3f accel_data = Vector3f{imu_report.delta_velocity} * accel_dt_inv; - // 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; - _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt; - - // apply temperature compensation - gyro_rate(0) = (gyro_rate(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X - gyro_rate(1) = (gyro_rate(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y - gyro_rate(2) = (gyro_rate(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z - - // rotate corrected measurements from sensor to body frame - gyro_rate = _board_rotation * gyro_rate; + // convert the delta angles to an equivalent angular rate + const float gyro_dt_inv = 1.e6f / (float)imu_report.delta_angle_dt; + Vector3f gyro_rate = Vector3f{imu_report.delta_angle} * gyro_dt_inv; + _last_sensor_data[uorb_index].timestamp = imu_report.timestamp_sample; + _last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0); + _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); + _last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2); + _last_sensor_data[uorb_index].accelerometer_integral_dt = imu_report.delta_velocity_dt; + _last_sensor_data[uorb_index].accelerometer_clipping = imu_report.delta_velocity_clipping; _last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0); _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); + _last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt; - _last_sensor_data[uorb_index].timestamp = gyro_report.timestamp_sample; - _gyro.voter.put(uorb_index, gyro_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, - gyro_report.error_count, _gyro.priority[uorb_index]); + + _last_accel_timestamp[uorb_index] = imu_report.timestamp_sample; + + _accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, + imu_status.accel_error_count, _accel.priority[uorb_index]); + + _gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, + imu_status.gyro_error_count, _gyro.priority[uorb_index]); } } // find the best sensor - int best_index; - _gyro.voter.get_best(hrt_absolute_time(), &best_index); + int accel_best_index; + int gyro_best_index; + _accel.voter.get_best(hrt_absolute_time(), &accel_best_index); + _gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index); + + checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC); + checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO); // write data for the best sensor to output variables - if (best_index >= 0) { - raw.timestamp = _last_sensor_data[best_index].timestamp; - raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt; - memcpy(&raw.gyro_rad, &_last_sensor_data[best_index].gyro_rad, sizeof(raw.gyro_rad)); + if ((accel_best_index >= 0) && (gyro_best_index >= 0)) { + raw.timestamp = _last_sensor_data[gyro_best_index].timestamp; + memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2, + sizeof(raw.accelerometer_m_s2)); + memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad)); + raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt; + raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt; + raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping; - if (_gyro.last_best_vote != best_index) { - _gyro.last_best_vote = (uint8_t)best_index; + if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) { + _accel.last_best_vote = (uint8_t)accel_best_index; + _selection.accel_device_id = _accel_device_id[accel_best_index]; + _selection_changed = true; } - if (_selection.gyro_device_id != _gyro_device_id[best_index]) { + if ((_gyro.last_best_vote != gyro_best_index) || (_selection.gyro_device_id != _gyro_device_id[gyro_best_index])) { + _gyro.last_best_vote = (uint8_t)gyro_best_index; + _selection.gyro_device_id = _gyro_device_id[gyro_best_index]; _selection_changed = true; - _selection.gyro_device_id = _gyro_device_id[best_index]; + + // clear all registered callbacks + for (auto &sub : _vehicle_imu_sub) { + sub.unregisterCallback(); + } + + for (int i = 0; i < GYRO_COUNT_MAX; i++) { + vehicle_imu_s report{}; + + if (_vehicle_imu_sub[i].copy(&report)) { + if ((report.gyro_device_id != 0) && (report.gyro_device_id == _gyro_device_id[gyro_best_index])) { + _vehicle_imu_sub[i].registerCallback(); + } + } + } } } } @@ -655,6 +593,8 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer) int best_index; _mag.voter.get_best(hrt_absolute_time(), &best_index); + checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG); + if (best_index >= 0) { magnetometer = _last_magnetometer[best_index]; _mag.last_best_vote = (uint8_t)best_index; @@ -769,39 +709,34 @@ void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor void VotedSensorsUpdate::printStatus() { - PX4_INFO("gyro status:"); + PX4_INFO("selected gyro: %d (%d)", _selection.gyro_device_id, _gyro.last_best_vote); _gyro.voter.print(); - PX4_INFO("accel status:"); + + PX4_INFO_RAW("\n"); + PX4_INFO("selected accel: %d (%d)", _selection.accel_device_id, _accel.last_best_vote); _accel.voter.print(); - PX4_INFO("mag status:"); + + PX4_INFO_RAW("\n"); + PX4_INFO("selected mag: %d (%d)", _selection.mag_device_id, _mag.last_best_vote); _mag.voter.print(); } void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer) { - _corrections_sub.update(&_corrections); - - accelPoll(raw); - gyroPoll(raw); + imuPoll(raw); magPoll(magnetometer); // publish sensor selection if changed if (_selection_changed) { - _selection.timestamp = hrt_absolute_time(); - - _sensor_selection_pub.publish(_selection); - - _selection_changed = false; + // don't publish until selected IDs are valid + if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) { + _selection.timestamp = hrt_absolute_time(); + _sensor_selection_pub.publish(_selection); + _selection_changed = false; + } } } -void VotedSensorsUpdate::checkFailover() -{ - checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC); - checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO); - checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG); -} - void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) { if (_last_accel_timestamp[_accel.last_best_vote]) { diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 0b0738c313..6e27762e80 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -55,12 +55,12 @@ #include #include -#include +#include #include #include -#include -#include #include +#include +#include #include #include @@ -81,7 +81,8 @@ public: * @param parameters parameter values. These do not have to be initialized when constructing this object. * Only when calling init(), they have to be initialized. */ - VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled); + VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled, + uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]); /** * initialize subscriptions etc. @@ -113,13 +114,6 @@ public: */ void setRelativeTimestamps(sensor_combined_s &raw); - /** - * check if a failover event occured. if so, report it. - */ - void checkFailover(); - - int bestGyroID() const { return _gyro_device_id[_gyro.last_best_vote]; } - /** * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor */ @@ -166,20 +160,12 @@ private: void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max); /** - * Poll the accelerometer for updated data. + * Poll IMU for updated data. * * @param raw Combined sensor data structure into which * data should be returned. */ - void accelPoll(sensor_combined_s &raw); - - /** - * Poll the gyro for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void gyroPoll(sensor_combined_s &raw); + void imuPoll(sensor_combined_s &raw); /** * Poll the magnetometer for updated data. @@ -195,8 +181,8 @@ private: */ bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type); - SensorData _accel{ORB_ID::sensor_accel_integrated}; - SensorData _gyro{ORB_ID::sensor_gyro_integrated}; + SensorData _accel{ORB_ID::sensor_accel}; + SensorData _gyro{ORB_ID::sensor_gyro}; SensorData _mag{ORB_ID::sensor_mag}; orb_advert_t _mavlink_log_pub{nullptr}; @@ -204,8 +190,12 @@ private: uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ uORB::PublicationQueued _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */ - /* sensor thermal compensation */ - uORB::Subscription _corrections_sub{ORB_ID(sensor_correction)}; + uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[3]; + uORB::Subscription _vehicle_imu_status_sub[ACCEL_COUNT_MAX] { + {ORB_ID(vehicle_imu_status), 0}, + {ORB_ID(vehicle_imu_status), 1}, + {ORB_ID(vehicle_imu_status), 2}, + }; sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ @@ -216,7 +206,7 @@ private: const Parameters &_parameters; const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ - bool _selection_changed{false}; /**< true when a sensor selection has changed and not been published */ + bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */ float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */ float _gyro_diff[3][2] {}; /**< filtered gyro differences between IMU uinits (rad/s) */ @@ -231,7 +221,6 @@ private: uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */ - sensor_correction_s _corrections {}; /**< struct containing the sensor corrections to be published to the uORB */ sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */ subsystem_info_s _info {}; /**< subsystem info publication */ }; diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index a4b667dc33..e08dc0aeef 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -95,8 +95,6 @@ Sih::Sih() : _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": execution")), _sampling_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sampling")) { - _px4_accel.set_update_rate(LOOP_INTERVAL); - _px4_gyro.set_update_rate(LOOP_INTERVAL); } void Sih::run() diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 1595241602..dcea127e7e 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -98,8 +98,8 @@ private: void parameters_updated(); // simulated sensor instances - PX4Accelerometer _px4_accel{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - PX4Gyroscope _px4_gyro{ 2294028, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION + PX4Accelerometer _px4_accel{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Gyroscope _px4_gyro{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION PX4Magnetometer _px4_mag{ 197388, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION PX4Barometer _px4_baro{ 6620172, ORB_PRIO_DEFAULT }; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 4e5fcfd40e..2e7dd35132 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -128,7 +128,7 @@ int simulator_main(int argc, char *argv[]) g_sim_task = px4_task_spawn_cmd("simulator", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, + SCHED_PRIORITY_MAX, 1500, Simulator::start, argv); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 4c5583adf0..5c8c8b5830 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -135,9 +135,6 @@ public: private: Simulator() : ModuleParams(nullptr) { - // current default - _px4_accel.set_update_rate(250); - _px4_gyro.set_update_rate(250); } ~Simulator() @@ -162,8 +159,8 @@ private: static Simulator *_instance; // simulated sensor instances - PX4Accelerometer _px4_accel{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - PX4Gyroscope _px4_gyro{2294028, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION + PX4Accelerometer _px4_accel{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Gyroscope _px4_gyro{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION PX4Magnetometer _px4_mag{197388, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION PX4Barometer _px4_baro{6620172, ORB_PRIO_DEFAULT}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 47ed1f5dbc..6cb59f37b1 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -205,16 +205,16 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor } } - // gyro - if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) { - _px4_gyro.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro); - } - // accel if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_param_sim_accel_block.get()) { _px4_accel.update(time, sensors.xacc, sensors.yacc, sensors.zacc); } + // gyro + if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) { + _px4_gyro.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro); + } + // magnetometer if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_param_sim_mag_block.get()) { _px4_mag.update(time, sensors.xmag, sensors.ymag, sensors.zmag); diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 00a9477e4b..b8f65dc140 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -42,6 +42,7 @@ #include #include +#include #include "uORBDeviceNode.hpp" #include "uORBManager.hpp" @@ -124,17 +125,19 @@ public: */ bool copy(void *dst) { return advertised() ? _node->copy(dst, _last_generation) : false; } - uint8_t get_instance() const { return _instance; } - orb_id_t get_topic() const { return get_orb_meta(_orb_id); } - ORB_PRIO get_priority() { return advertised() ? _node->get_priority() : ORB_PRIO_UNINITIALIZED; } + uint8_t get_instance() const { return _instance; } + unsigned get_last_generation() const { return _last_generation; } + ORB_PRIO get_priority() { return advertised() ? _node->get_priority() : ORB_PRIO_UNINITIALIZED; } + orb_id_t get_topic() const { return get_orb_meta(_orb_id); } protected: friend class SubscriptionCallback; + friend class SubscriptionCallbackWorkItem; - DeviceNode *get_node() { return _node; } + DeviceNode *get_node() { return _node; } - DeviceNode *_node{nullptr}; + DeviceNode *_node{nullptr}; unsigned _last_generation{0}; /**< last generation the subscriber has seen */ diff --git a/src/modules/uORB/SubscriptionCallback.hpp b/src/modules/uORB/SubscriptionCallback.hpp index f58f6e84d7..5ed54c8ec2 100644 --- a/src/modules/uORB/SubscriptionCallback.hpp +++ b/src/modules/uORB/SubscriptionCallback.hpp @@ -127,14 +127,30 @@ public: void call() override { - // schedule immediately if no interval, otherwise check time elapsed - if ((_interval_us == 0) || (hrt_elapsed_time_atomic(&_last_update) >= _interval_us)) { - _work_item->ScheduleNow(); + // schedule immediately if updated (queue depth or subscription interval) + if ((_required_updates == 0) + || (_subscription.get_node()->published_message_count() >= (_subscription.get_last_generation() + _required_updates))) { + if (updated()) { + _work_item->ScheduleNow(); + } } } + /** + * Optionally limit callback until more samples are available. + * + * @param required_updates Number of queued updates required before a callback can be called. + */ + void set_required_updates(uint8_t required_updates) + { + // TODO: constrain to queue depth? + _required_updates = required_updates; + } + private: px4::WorkItem *_work_item; + + uint8_t _required_updates{0}; }; } // namespace uORB diff --git a/src/systemcmds/top/CMakeLists.txt b/src/systemcmds/top/CMakeLists.txt index db2111cb47..781e561f06 100644 --- a/src/systemcmds/top/CMakeLists.txt +++ b/src/systemcmds/top/CMakeLists.txt @@ -34,7 +34,7 @@ px4_add_module( MODULE systemcmds__top MAIN top PRIORITY - "SCHED_PRIORITY_MAX - 7" # max priority below sensor WQ threads + "SCHED_PRIORITY_MAX - 16" # max priority below high priority WQ threads SRCS top.c DEPENDS