dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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)
This commit is contained in:
Daniel Agar 2020-05-30 11:07:54 -04:00 committed by GitHub
parent 86cd1d0802
commit e34bdb4be9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
74 changed files with 785 additions and 1197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
.ci/Jenkinsfile-hardware
View File

@ -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 /"'

4
ROMFS/px4fmu_common/init.d-posix/rcS
View File

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

5
msg/CMakeLists.txt
View File

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

6
msg/sensor_accel.msg
View File

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

12
msg/sensor_accel_integrated.msg
View File

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

17
msg/sensor_accel_status.msg
View File

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

4
msg/sensor_gyro.msg
View File

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

12
msg/sensor_gyro_integrated.msg
View File

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

19
msg/sensor_gyro_status.msg
View File

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

12
msg/tools/px_generate_uorb_topic_helper.py
View File

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

36
msg/tools/uorb_rtps_message_ids.yaml
View File

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

4
msg/vehicle_imu.msg
View File

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

16
msg/vehicle_imu_status.msg Normal file
View File

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

3
src/drivers/drv_sensor.h
View File

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

3
src/drivers/imu/adis16448/ADIS16448.cpp
View File

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

3
src/drivers/imu/adis16477/ADIS16477.cpp
View File

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

3
src/drivers/imu/adis16497/ADIS16497.cpp
View File

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

1
src/drivers/imu/bma180/bma180.cpp
View File

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

1
src/drivers/imu/bmi055/BMI055_accel.cpp
View File

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

1
src/drivers/imu/bmi055/BMI055_gyro.cpp
View File

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

1
src/drivers/imu/bmi088/BMI088_accel.cpp
View File

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

1
src/drivers/imu/bmi088/BMI088_gyro.cpp
View File

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

1
src/drivers/imu/bmi160/bmi160.cpp
View File

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

1
src/drivers/imu/fxas21002c/FXAS21002C.cpp
View File

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

2
src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp
View File

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

3
src/drivers/imu/invensense/icm20602/ICM20602.cpp
View File

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

3
src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
View File

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

3
src/drivers/imu/invensense/icm20649/ICM20649.cpp
View File

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

3
src/drivers/imu/invensense/icm20689/ICM20689.cpp
View File

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

3
src/drivers/imu/invensense/icm20948/ICM20948.cpp
View File

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

3
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
View File

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

3
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
View File

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

3
src/drivers/imu/invensense/mpu6000/MPU6000.cpp
View File

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

3
src/drivers/imu/invensense/mpu6500/MPU6500.cpp
View File

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

3
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
View File

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

1
src/drivers/imu/l3gd20/L3GD20.cpp
View File

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

1
src/drivers/imu/lsm303d/LSM303D.cpp
View File

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

2
src/drivers/imu/mpu6000/MPU6000.cpp
View File

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

2
src/drivers/imu/mpu9250/mpu9250.cpp
View File

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

2
src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
View File

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

2
src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
View File

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

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

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

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

@ -37,15 +37,12 @@
#include <drivers/drv_hrt.h>
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
#include <lib/drivers/device/integrator.h>
#include <lib/ecl/geo/geo.h>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_accel_status.h>
class PX4Accelerometer : public cdev::CDev, public ModuleParams
{
@ -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_accel_s> _sensor_pub;
uORB::PublicationMulti<sensor_accel_fifo_s> _sensor_fifo_pub;
uORB::PublicationMulti<sensor_accel_integrated_s> _sensor_integrated_pub;
uORB::PublicationMulti<sensor_accel_status_s> _sensor_status_pub;
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<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
)
};

1
src/lib/drivers/device/CMakeLists.txt
View File

@ -56,7 +56,6 @@ endif()
px4_add_library(drivers__device
CDev.cpp
ringbuffer.cpp
integrator.cpp
${SRCS_PLATFORM}
)

266
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

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

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

@ -37,14 +37,11 @@
#include <drivers/drv_hrt.h>
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
#include <lib/drivers/device/integrator.h>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_gyro_status.h>
class PX4Gyroscope : public cdev::CDev, public ModuleParams
{
@ -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_gyro_s> _sensor_pub;
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub;
uORB::PublicationMulti<sensor_gyro_integrated_s> _sensor_integrated_pub;
uORB::PublicationMulti<sensor_gyro_status_s> _sensor_status_pub;
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<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
)
};

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

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

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

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

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

@ -84,12 +84,8 @@
#include <uORB/topics/orbit_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_accel_status.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_gyro_status.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/tecs_status.h>
@ -106,6 +102,8 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@ -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:

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

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

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

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

2
src/modules/sensors/sensor_corrections/SensorCorrections.hpp
View File

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

75
src/modules/sensors/sensors.cpp
View File

@ -68,6 +68,7 @@
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/battery_status.h>
@ -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_s> _sensor_preflight_pub{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */
uORB::Publication<vehicle_magnetometer_s> _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();
}
}

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

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

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

@ -41,7 +41,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
@ -54,7 +54,7 @@
namespace sensors
{
class VehicleAcceleration : public ModuleParams, public px4::WorkItem
class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem
{
public:

1
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
View File

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

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

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

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

@ -42,7 +42,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
@ -56,7 +56,7 @@
namespace sensors
{
class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem
{
public:

2
src/modules/sensors/vehicle_imu/CMakeLists.txt
View File

@ -32,6 +32,8 @@
############################################################################
px4_add_library(vehicle_imu
Integrator.cpp
Integrator.hpp
VehicleIMU.cpp
VehicleIMU.hpp
)

80
src/lib/drivers/device/integrator.cpp → src/modules/sensors/vehicle_imu/Integrator.cpp
View File

@ -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 <lorenz@px4.io>
* @author Julian Oes <julian@oes.ch>
*/
#include "integrator.h"
#include "Integrator.hpp"
#include <drivers/drv_hrt.h>
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 &timestamp, const matrix::Vector3f &val, matrix::Vector3f &integral,
uint32_t &integral_dt)
bool Integrator::put(const hrt_abstime &timestamp, 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 &timestamp, 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<float>(timestamp - _last_integration_time) * 1e-6f;
}
// Use trapezoidal integration to calculate the delta integral
const float dt = static_cast<float>(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 &timestamp, 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 &timestamp, 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;
}

74
src/lib/drivers/device/integrator.h → src/modules/sensors/vehicle_imu/Integrator.hpp
View File

@ -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 &timestamp, 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 &timestamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt);
bool put(const uint64_t &timestamp, 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 */
};

292
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -35,20 +35,37 @@
#include <px4_platform_common/log.h>
#include <float.h>
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 &timestamp_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();
}

53
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -33,6 +33,8 @@
#pragma once
#include "Integrator.hpp"
#include <sensor_corrections/SensorCorrections.hpp>
#include <lib/mathlib/math/Limits.hpp>
@ -40,19 +42,20 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
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 &timestamp_sample);
void UpdateIntegratorConfiguration();
void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle);
void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);
uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
uORB::PublicationMulti<vehicle_imu_status_s> _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<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
)
};
} // namespace sensors

1
src/modules/sensors/vehicle_imu/imu_parameters.c
View File

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

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

@ -52,18 +52,14 @@ using namespace sensors;
using namespace matrix;
using math::radians;
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
: ModuleParams(nullptr), _parameters(parameters), _hil_enabled(hil_enabled), _mag_compensator(this)
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, 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]) {

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

@ -55,12 +55,12 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro_integrated.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/subsystem_info.h>
@ -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 &parameters, bool hil_enabled);
VotedSensorsUpdate(const Parameters &parameters, 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_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
uORB::PublicationQueued<subsystem_info_s> _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 */
};

2
src/modules/sih/sih.cpp
View File

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

4
src/modules/sih/sih.hpp
View File

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

2
src/modules/simulator/simulator.cpp
View File

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

7
src/modules/simulator/simulator.h
View File

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

10
src/modules/simulator/simulator_mavlink.cpp
View File

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

13
src/modules/uORB/Subscription.hpp
View File

@ -42,6 +42,7 @@
#include <uORB/topics/uORBTopics.hpp>
#include <px4_platform_common/defines.h>
#include <lib/mathlib/mathlib.h>
#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 */

22
src/modules/uORB/SubscriptionCallback.hpp
View File

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

2
src/systemcmds/top/CMakeLists.txt
View File

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