Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 04:06:33 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

move IMU integration out of drivers to sensors hub to handle accel/gyro sync

Browse Source 
- 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
74 changed files with 785 additions and 1197 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.ci/Jenkinsfile-hardware
+2 -8
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 /"'
ROMFS/px4fmu_common/init.d-posix/rcS
+2 -2
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
msg/CMakeLists.txt
+1 -4
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
msg/sensor_accel.msg
+5 -1
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
msg/sensor_accel_integrated.msg
-12
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
msg/sensor_accel_status.msg
-17
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)
msg/sensor_gyro.msg
+3 -1
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
msg/sensor_gyro_integrated.msg
-12
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
msg/sensor_gyro_status.msg
-19
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)
msg/tools/px_generate_uorb_topic_helper.py
+12
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:
msg/tools/uorb_rtps_message_ids.yaml
+15 -21
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
msg/vehicle_imu.msg
+2 -2
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
msg/vehicle_imu_status.msg
+16
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)
src/drivers/drv_sensor.h
+1 -2
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
src/drivers/imu/adis16448/ADIS16448.cpp
-3
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;
src/drivers/imu/adis16477/ADIS16477.cpp
-3
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()
src/drivers/imu/adis16497/ADIS16497.cpp
-3
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()
src/drivers/imu/bma180/bma180.cpp
-1
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()
src/drivers/imu/bmi055/BMI055_accel.cpp
-1
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()
src/drivers/imu/bmi055/BMI055_gyro.cpp
-1
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()
src/drivers/imu/bmi088/BMI088_accel.cpp
-1
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()
src/drivers/imu/bmi088/BMI088_gyro.cpp
-1
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()
src/drivers/imu/bmi160/bmi160.cpp
-1
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()
src/drivers/imu/fxas21002c/FXAS21002C.cpp
-1
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()
src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp
-2
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
src/drivers/imu/invensense/icm20602/ICM20602.cpp
-3
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);
}
src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
-3
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()
src/drivers/imu/invensense/icm20649/ICM20649.cpp
-3
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)
src/drivers/imu/invensense/icm20689/ICM20689.cpp
-3
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()
src/drivers/imu/invensense/icm20948/ICM20948.cpp
-3
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)
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
-3
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);
}
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
-3
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);
}
src/drivers/imu/invensense/mpu6000/MPU6000.cpp
-3
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()
src/drivers/imu/invensense/mpu6500/MPU6500.cpp
-3
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()
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
-3
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()
src/drivers/imu/l3gd20/L3GD20.cpp
-1
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()
src/drivers/imu/lsm303d/LSM303D.cpp
-1
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
src/drivers/imu/mpu6000/MPU6000.cpp
-2
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);
src/drivers/imu/mpu9250/mpu9250.cpp
-2
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);
src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
-2
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()
src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
-2
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()
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+42 -203
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,178 +130,90 @@ 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
{
// publish
sensor_accel_s report;
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();
}
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)
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+2 -34
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
)
};
src/lib/drivers/device/CMakeLists.txt
-1
View File
@@ -56,7 +56,6 @@ endif()
px4_add_library(drivers__device
CDev.cpp
ringbuffer.cpp
integrator.cpp
${SRCS_PLATFORM}
)
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+21 -227
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,24 +116,16 @@ 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
{
// publish
sensor_gyro_s report;
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);
@@ -183,66 +134,41 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
_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();
}
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()
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+4 -38
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
)
};
src/modules/ekf2/ekf2_main.cpp
+3 -5
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;
}
}
src/modules/logger/logged_topics.cpp
+2 -2
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");
src/modules/mavlink/mavlink_messages.cpp
+48 -87
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:
src/modules/mavlink/mavlink_receiver.cpp
+6 -28
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");

Some files were not shown because too many files have changed in this diff Show More