From 27f23ac290e3fbfc3d28c08bfd535c12eb42eebf Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 16 Aug 2020 22:21:04 -0400 Subject: [PATCH] move initial sensor priority to parameters and purge ORB_PRIORITY - CAL_ACCx_EN -> CAL_ACCx_PRIO - CAL_GYROx_EN -> CAL_GYROx_PRIO - CAL_MAGx_EN -> CAL_MAGx_PRIO --- ROMFS/px4fmu_common/init.d-posix/rcS | 5 -- .../px4fmu_common/init.d/airframes/4250_teal | 14 ++-- .../distance_sensor/cm8jl65/CM8JL65.cpp | 2 +- .../distance_sensor/leddar_one/LeddarOne.cpp | 2 +- .../distance_sensor/ll40ls/LidarLiteI2C.cpp | 2 +- .../ll40ls_pwm/LidarLitePWM.cpp | 2 +- .../distance_sensor/mappydot/MappyDot.cpp | 3 +- src/drivers/distance_sensor/mb12xx/mb12xx.cpp | 2 +- src/drivers/distance_sensor/sf0x/SF0X.cpp | 2 +- src/drivers/distance_sensor/sf1xx/sf1xx.cpp | 2 +- src/drivers/distance_sensor/srf02/SRF02.cpp | 2 +- .../distance_sensor/teraranger/TERARANGER.cpp | 2 +- src/drivers/distance_sensor/tfmini/TFMINI.cpp | 2 +- .../ulanding_radar/AerotennaULanding.cpp | 2 +- .../distance_sensor/vl53l0x/VL53L0X.cpp | 2 +- src/drivers/imu/adis16448/ADIS16448.cpp | 8 +-- src/drivers/imu/adis16477/ADIS16477.cpp | 4 +- src/drivers/imu/adis16497/ADIS16497.cpp | 4 +- src/drivers/imu/bma180/bma180.cpp | 2 +- src/drivers/imu/bmi160/bmi160.cpp | 16 ++--- .../imu/bosch/bmi055/BMI055_Accelerometer.cpp | 2 +- .../imu/bosch/bmi055/BMI055_Gyroscope.cpp | 2 +- .../imu/bosch/bmi088/BMI088_Accelerometer.cpp | 2 +- .../imu/bosch/bmi088/BMI088_Gyroscope.cpp | 2 +- src/drivers/imu/fxas21002c/FXAS21002C.cpp | 2 +- src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp | 4 +- .../imu/invensense/icm20602/ICM20602.cpp | 4 +- .../imu/invensense/icm20608g/ICM20608G.cpp | 4 +- .../imu/invensense/icm20649/ICM20649.cpp | 4 +- .../imu/invensense/icm20689/ICM20689.cpp | 4 +- .../imu/invensense/icm20948/ICM20948.cpp | 4 +- .../invensense/icm20948/ICM20948_AK09916.cpp | 2 +- .../imu/invensense/icm40609d/ICM40609D.cpp | 4 +- .../imu/invensense/icm42688p/ICM42688P.cpp | 4 +- .../imu/invensense/mpu6000/MPU6000.cpp | 4 +- .../imu/invensense/mpu6500/MPU6500.cpp | 4 +- .../imu/invensense/mpu9250/MPU9250.cpp | 4 +- .../imu/invensense/mpu9250/MPU9250_AK8963.cpp | 2 +- src/drivers/imu/l3gd20/L3GD20.cpp | 2 +- src/drivers/imu/lsm303d/LSM303D.cpp | 4 +- src/drivers/imu/mpu6000/MPU6000.cpp | 4 +- src/drivers/imu/mpu9250/MPU9250_mag.cpp | 3 +- src/drivers/imu/mpu9250/mpu9250.cpp | 4 +- src/drivers/imu/st/ism330dlc/ISM330DLC.cpp | 4 +- src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp | 4 +- .../magnetometer/akm/ak09916/AK09916.cpp | 2 +- .../magnetometer/akm/ak8963/AK8963.cpp | 2 +- src/drivers/magnetometer/bmm150/bmm150.cpp | 2 +- src/drivers/magnetometer/bmm150/bmm150.hpp | 1 - src/drivers/magnetometer/hmc5883/HMC5883.cpp | 2 +- .../magnetometer/isentek/ist8308/IST8308.cpp | 2 +- src/drivers/magnetometer/ist8310/ist8310.cpp | 2 +- src/drivers/magnetometer/lis2mdl/lis2mdl.cpp | 2 +- src/drivers/magnetometer/lis3mdl/lis3mdl.cpp | 2 +- .../magnetometer/lsm303agr/LSM303AGR.cpp | 2 +- .../magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp | 2 +- src/drivers/magnetometer/qmc5883/QMC5883.cpp | 2 +- src/drivers/magnetometer/rm3100/rm3100.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 3 +- src/drivers/px4io/px4io.cpp | 2 +- src/drivers/uavcan/sensors/baro.cpp | 2 +- src/drivers/uavcan/sensors/gnss.hpp | 2 +- src/drivers/uavcan/sensors/mag.cpp | 2 +- src/drivers/uavcan/sensors/sensor_bridge.cpp | 2 +- .../fake_magnetometer/FakeMagnetometer.cpp | 2 +- .../fake_magnetometer/FakeMagnetometer.hpp | 1 + src/lib/battery/battery.cpp | 2 +- .../accelerometer/PX4Accelerometer.cpp | 6 +- .../accelerometer/PX4Accelerometer.hpp | 2 +- src/lib/drivers/barometer/PX4Barometer.cpp | 4 +- src/lib/drivers/barometer/PX4Barometer.hpp | 2 +- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 6 +- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 2 +- .../drivers/magnetometer/PX4Magnetometer.cpp | 4 +- .../drivers/magnetometer/PX4Magnetometer.hpp | 2 +- .../drivers/rangefinder/PX4Rangefinder.cpp | 4 +- .../drivers/rangefinder/PX4Rangefinder.hpp | 1 - src/lib/mixer_module/mixer_module.hpp | 4 +- src/lib/parameters/param_translation.cpp | 43 +++++++++-- src/lib/sensor_calibration/Accelerometer.cpp | 23 ++++-- src/lib/sensor_calibration/Accelerometer.hpp | 8 ++- src/lib/sensor_calibration/Gyroscope.cpp | 23 ++++-- src/lib/sensor_calibration/Gyroscope.hpp | 8 ++- src/lib/sensor_calibration/Magnetometer.cpp | 32 +++++++-- src/lib/sensor_calibration/Magnetometer.hpp | 8 ++- .../commander/accelerometer_calibration.cpp | 23 ++---- src/modules/commander/gyro_calibration.cpp | 13 ---- src/modules/mavlink/mavlink_receiver.h | 12 ++-- .../MulticopterRateControl.hpp | 2 +- src/modules/rc_update/rc_update.h | 2 +- src/modules/replay/Replay.cpp | 2 +- src/modules/sensors/sensor_params_acc0.c | 14 ++-- src/modules/sensors/sensor_params_acc1.c | 14 ++-- src/modules/sensors/sensor_params_acc2.c | 14 ++-- src/modules/sensors/sensor_params_accel.c | 40 ----------- src/modules/sensors/sensor_params_gyro.c | 40 ----------- src/modules/sensors/sensor_params_gyro0.c | 14 ++-- src/modules/sensors/sensor_params_gyro1.c | 14 ++-- src/modules/sensors/sensor_params_gyro2.c | 14 ++-- src/modules/sensors/sensor_params_mag.c | 10 +-- src/modules/sensors/sensor_params_mag0.c | 12 +++- src/modules/sensors/sensor_params_mag1.c | 12 +++- src/modules/sensors/sensor_params_mag2.c | 12 +++- src/modules/sensors/sensor_params_mag3.c | 14 ++-- src/modules/sensors/sensors.cpp | 3 +- .../vehicle_air_data/VehicleAirData.cpp | 7 +- .../VehicleMagnetometer.cpp | 20 +++++- src/modules/sensors/voted_sensors_update.cpp | 72 +++++++++++-------- src/modules/sensors/voted_sensors_update.h | 8 ++- src/modules/sih/sih.hpp | 8 +-- src/modules/simulator/simulator.h | 8 +-- src/modules/uORB/PublicationMulti.hpp | 25 ++----- src/modules/uORB/Subscription.hpp | 1 - src/modules/uORB/SubscriptionInterval.hpp | 1 - src/modules/uORB/uORB.cpp | 13 ++-- src/modules/uORB/uORB.h | 29 ++------ src/modules/uORB/uORBCommon.hpp | 1 - src/modules/uORB/uORBDeviceMaster.cpp | 7 +- src/modules/uORB/uORBDeviceMaster.hpp | 2 +- src/modules/uORB/uORBDeviceNode.cpp | 16 ++--- src/modules/uORB/uORBDeviceNode.hpp | 11 +-- src/modules/uORB/uORBManager.cpp | 15 ++-- src/modules/uORB/uORBManager.hpp | 26 ++----- .../uORB/uORB_tests/uORBTest_UnitTest.cpp | 33 ++------- 124 files changed, 463 insertions(+), 509 deletions(-) delete mode 100644 src/modules/sensors/sensor_params_accel.c delete mode 100644 src/modules/sensors/sensor_params_gyro.c diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index a77aac7bf4..6ef02a169b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -122,13 +122,8 @@ then param set BAT_N_CELLS 3 param set CAL_ACC0_ID 1311244 - param set CAL_ACC_PRIME 1311244 - param set CAL_GYRO0_ID 1311244 - param set CAL_GYRO_PRIME 1311244 - param set CAL_MAG0_ID 197388 - param set CAL_MAG_PRIME 197388 param set CBRK_AIRSPD_CHK 0 param set CBRK_SUPPLY_CHK 894281 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4250_teal b/ROMFS/px4fmu_common/init.d/airframes/4250_teal index a91b588630..ba3bac9caf 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4250_teal +++ b/ROMFS/px4fmu_common/init.d/airframes/4250_teal @@ -186,17 +186,11 @@ then param set RTL_RETURN_ALT 25 # calibration - param set CAL_ACC0_EN 1 - param set CAL_ACC1_EN 0 - #mpu6500 - param set CAL_ACC_PRIME 1442826 + param set CAL_ACC0_PRIO 255 + param set CAL_ACC1_PRIO 0 - param set CAL_GYRO0_EN 1 - param set CAL_GYRO1_EN 0 - #mpu6500 - param set CAL_GYRO_PRIME 2360330 - - param set CAL_MAG_PRIME 265738 + param set CAL_GYRO0_PRIO 255 + param set CAL_GYRO1_PRIO 0 fi # Logger mode. Default(1) + estimator replay(2) + thermal calibration(4) diff --git a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp index 11e97021e3..8f384c1340 100644 --- a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp @@ -87,7 +87,7 @@ static constexpr unsigned char crc_lsb_vector[] = { CM8JL65::CM8JL65(const char *port, uint8_t rotation) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _px4_rangefinder(0 /* TODO: device ids */, ORB_PRIO_DEFAULT, rotation) + _px4_rangefinder(0 /* TODO: device ids */, rotation) { // Store the port name. strncpy(_port, port, sizeof(_port) - 1); diff --git a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp b/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp index e0012ea686..5b19ce713a 100644 --- a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp +++ b/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp @@ -39,7 +39,7 @@ LeddarOne::LeddarOne(const char *serial_port, uint8_t device_orientation): ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(serial_port)), - _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, device_orientation) + _px4_rangefinder(0 /* device id not yet used */, device_orientation) { _serial_port = strdup(serial_port); diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index 3b2cbacf14..f4c3d41388 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -45,7 +45,7 @@ LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint const int address) : I2C(DRV_RNG_DEVTYPE_LL40LS, MODULE_NAME, bus, address, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_rangefinder(get_device_id(), rotation) { _px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE); _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE); diff --git a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp index 82b3b52687..9b5148ff05 100644 --- a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp @@ -50,7 +50,7 @@ LidarLitePWM::LidarLitePWM(const uint8_t rotation) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation) + _px4_rangefinder(0 /* device id not yet used */, rotation) { _px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE); _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE); diff --git a/src/drivers/distance_sensor/mappydot/MappyDot.cpp b/src/drivers/distance_sensor/mappydot/MappyDot.cpp index 83ce31b0e5..af17042a56 100644 --- a/src/drivers/distance_sensor/mappydot/MappyDot.cpp +++ b/src/drivers/distance_sensor/mappydot/MappyDot.cpp @@ -275,8 +275,7 @@ MappyDot::collect() report.variance = 0; int instance_id; - orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id, ORB_PRIO_DEFAULT); - + orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id); } perf_end(_sample_perf); diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index 417f052f1a..2c115ce784 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -233,7 +233,7 @@ MB12XX::collect() report.variance = 0.0f; int instance_id; - orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id, ORB_PRIO_DEFAULT); + orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id); // Begin the next measurement. if (measure() != PX4_OK) { diff --git a/src/drivers/distance_sensor/sf0x/SF0X.cpp b/src/drivers/distance_sensor/sf0x/SF0X.cpp index 43ead83d27..9219e9b0f4 100644 --- a/src/drivers/distance_sensor/sf0x/SF0X.cpp +++ b/src/drivers/distance_sensor/sf0x/SF0X.cpp @@ -41,7 +41,7 @@ SF0X::SF0X(const char *port, uint8_t rotation) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation), + _px4_rangefinder(0 /* device id not yet used */, rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 1a483133e9..ed55225a37 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp @@ -117,7 +117,7 @@ private: SF1XX::SF1XX(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) : I2C(DRV_DIST_DEVTYPE_SF1XX, MODULE_NAME, bus, address, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(DRV_DIST_DEVTYPE_SF1XX, ORB_PRIO_DEFAULT, rotation) + _px4_rangefinder(DRV_DIST_DEVTYPE_SF1XX, rotation) { } diff --git a/src/drivers/distance_sensor/srf02/SRF02.cpp b/src/drivers/distance_sensor/srf02/SRF02.cpp index 1b6256755b..3c8d81fbe8 100644 --- a/src/drivers/distance_sensor/srf02/SRF02.cpp +++ b/src/drivers/distance_sensor/srf02/SRF02.cpp @@ -36,7 +36,7 @@ SRF02::SRF02(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) : I2C(DRV_DIST_DEVTYPE_SRF02, MODULE_NAME, bus, address, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_rangefinder(get_device_id(), rotation) { _px4_rangefinder.set_max_distance(SRF02_MAX_DISTANCE); _px4_rangefinder.set_min_distance(SRF02_MIN_DISTANCE); diff --git a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp index ca4d399fde..538011f62a 100644 --- a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp +++ b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp @@ -76,7 +76,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len) TERARANGER::TERARANGER(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency) : I2C(DRV_DIST_DEVTYPE_TERARANGER, MODULE_NAME, bus, TERARANGER_ONE_BASEADDR, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_rangefinder(get_device_id(), rotation) { // up the retries since the device misses the first measure attempts I2C::_retries = 3; diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index 99e0e134d4..4832a4b96c 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -37,7 +37,7 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _px4_rangefinder(0 /* TODO: device id */, ORB_PRIO_DEFAULT, rotation) + _px4_rangefinder(0 /* TODO: device id */, rotation) { // store port name strncpy(_port, port, sizeof(_port) - 1); diff --git a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp index b9da4f6d37..b0704083ff 100644 --- a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp +++ b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp @@ -35,7 +35,7 @@ AerotennaULanding::AerotennaULanding(const char *port, uint8_t rotation) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _px4_rangefinder(0 /* TODO: device ids */, ORB_PRIO_DEFAULT, rotation) + _px4_rangefinder(0 /* TODO: device ids */, rotation) { /* store port name */ strncpy(_port, port, sizeof(_port) - 1); diff --git a/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp b/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp index 0f06ee5306..6ad9db2b8a 100644 --- a/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp +++ b/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp @@ -62,7 +62,7 @@ VL53L0X::VL53L0X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) : I2C(DRV_DIST_DEVTYPE_VL53L0X, MODULE_NAME, bus, address, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation) + _px4_rangefinder(0 /* device id not yet used */, rotation) { // VL53L0X typical range 0-2 meters with 25 degree field of view _px4_rangefinder.set_min_distance(0.f); diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp index a0b3afc6f9..7897ace6d1 100644 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/adis16448/ADIS16448.cpp @@ -41,10 +41,10 @@ ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, int32_t device, enum R spi_mode_e spi_mode) : SPI(DRV_IMU_DEVTYPE_ADIS16448, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), - _px4_baro(get_device_id(), ORB_PRIO_MAX), - _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation), - _px4_mag(get_device_id(), ORB_PRIO_MAX, rotation) + _px4_accel(get_device_id(), rotation), + _px4_baro(get_device_id()), + _px4_gyro(get_device_id(), rotation), + _px4_mag(get_device_id(), rotation) { _px4_accel.set_scale(ADIS16448_ACCEL_SENSITIVITY); _px4_gyro.set_scale(ADIS16448_GYRO_INITIAL_SENSITIVITY); diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index db68a4e40e..ac11eadb8c 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -57,8 +57,8 @@ ADIS16477::ADIS16477(I2CSPIBusOption bus_option, int bus, int32_t device, enum R spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : SPI(DRV_IMU_DEVTYPE_ADIS16477, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation), + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")), _drdy_gpio(drdy_gpio) diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp index f0f30463f2..6d5396fb92 100644 --- a/src/drivers/imu/adis16497/ADIS16497.cpp +++ b/src/drivers/imu/adis16497/ADIS16497.cpp @@ -74,8 +74,8 @@ ADIS16497::ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum R spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : SPI(DRV_IMU_DEVTYPE_ADIS16497, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation), + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")), _drdy_gpio(drdy_gpio) diff --git a/src/drivers/imu/bma180/bma180.cpp b/src/drivers/imu/bma180/bma180.cpp index df1e33276f..36ceace7a8 100644 --- a/src/drivers/imu/bma180/bma180.cpp +++ b/src/drivers/imu/bma180/bma180.cpp @@ -179,7 +179,7 @@ BMA180::BMA180(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio spi_mode_e spi_mode) : SPI(DRV_ACC_DEVTYPE_BMA180, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), + _px4_accel(get_device_id(), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")) { } diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index 89bd001e2a..60ad793baf 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -53,15 +53,15 @@ BMI160::BMI160(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio spi_mode_e spi_mode) : SPI(DRV_IMU_DEVTYPE_BMI160, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation), + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation), _accel_reads(perf_alloc(PC_COUNT, MODULE_NAME": accel read")), - _gyro_reads(perf_alloc(PC_COUNT, MODULE_NAME":gyro read")), - _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME":read")), - _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME":bad transfers")), - _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME":bad registers")), - _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME":good transfers")), - _reset_retries(perf_alloc(PC_COUNT, MODULE_NAME":reset retries")), + _gyro_reads(perf_alloc(PC_COUNT, MODULE_NAME": gyro read")), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")), + _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad registers")), + _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good transfers")), + _reset_retries(perf_alloc(PC_COUNT, MODULE_NAME": reset retries")), _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates")) { } diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp index 14146d06b3..ce3fac6f77 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp @@ -43,7 +43,7 @@ namespace Bosch::BMI055::Accelerometer BMI055_Accelerometer::BMI055_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : BMI055(DRV_ACC_DEVTYPE_BMI055, "BMI055_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_accel(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_accel: DRDY interval"); diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index b8495fb1e5..9fe772ffd4 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -43,7 +43,7 @@ namespace Bosch::BMI055::Gyroscope BMI055_Gyroscope::BMI055_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : BMI055(DRV_GYR_DEVTYPE_BMI055, "BMI055_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_gyro: DRDY interval"); diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp index 8cca81b7fd..acccf47570 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp @@ -43,7 +43,7 @@ namespace Bosch::BMI088::Accelerometer BMI088_Accelerometer::BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : BMI088(DRV_ACC_DEVTYPE_BMI088, "BMI088_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_accel: DRDY interval"); diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index d5c3004eb5..9084bd870b 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -43,7 +43,7 @@ namespace Bosch::BMI088::Gyroscope BMI088_Gyroscope::BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : BMI088(DRV_GYR_DEVTYPE_BMI088, "BMI088_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_gyro: DRDY interval"); diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index a0d5a12da8..0a7e9161ae 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp @@ -69,7 +69,7 @@ FXAS21002C::FXAS21002C(device::Device *interface, I2CSPIBusOption bus_option, in int i2c_address) : I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address), _interface(interface), - _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), + _px4_gyro(_interface->get_device_id(), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad register")), diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp index 1ec894b0a9..ff22e5f242 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp @@ -57,9 +57,9 @@ FXOS8701CQ::FXOS8701CQ(device::Device *interface, I2CSPIBusOption bus_option, in int i2c_address) : I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address), _interface(interface), - _px4_accel(interface->get_device_id(), ORB_PRIO_LOW, rotation), + _px4_accel(interface->get_device_id(), rotation), #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _px4_mag(interface->get_device_id(), ORB_PRIO_LOW, rotation), + _px4_mag(interface->get_device_id(), rotation), _mag_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": mag read")), #endif _accel_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": acc read")), diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index d202588ac7..9c7173d690 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -45,8 +45,8 @@ ICM20602::ICM20602(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro SPI(DRV_IMU_DEVTYPE_ICM20602, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index 609b878de3..01c850ef37 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -45,8 +45,8 @@ ICM20608G::ICM20608G(I2CSPIBusOption bus_option, int bus, uint32_t device, enum SPI(DRV_IMU_DEVTYPE_ICM20608G, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index 563a1e5fb2..ca91e3cca9 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -45,8 +45,8 @@ ICM20649::ICM20649(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro SPI(DRV_IMU_DEVTYPE_ICM20649, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index aa17dbc703..35105132e7 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -45,8 +45,8 @@ ICM20689::ICM20689(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro SPI(DRV_IMU_DEVTYPE_ICM20689, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index f138cc55a7..f8072490d8 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -47,8 +47,8 @@ ICM20948::ICM20948(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro SPI(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp index 13f97d45b1..8abe78bfa7 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp @@ -48,7 +48,7 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) ICM20948_AK09916::ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation) : ScheduledWorkItem("icm20948_ak09916", px4::device_bus_to_wq(icm20948.get_device_id())), _icm20948(icm20948), - _px4_mag(icm20948.get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_mag(icm20948.get_device_id(), rotation) { _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916); _px4_mag.set_external(icm20948.external()); diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 277a366524..965494e21b 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -45,8 +45,8 @@ ICM40609D::ICM40609D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum SPI(DRV_IMU_DEVTYPE_ICM40609D, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index a4cc92ca66..9a4714ac16 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -45,8 +45,8 @@ ICM42688P::ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index 95e9cf5572..e843c088af 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -45,8 +45,8 @@ MPU6000::MPU6000(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota SPI(DRV_IMU_DEVTYPE_MPU6000, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 57a08acfd0..eecb9a526f 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -45,8 +45,8 @@ MPU6500::MPU6500(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota SPI(DRV_IMU_DEVTYPE_MPU6500, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index c5dec7e6ad..0e3b66f83e 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -47,8 +47,8 @@ MPU9250::MPU9250(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota SPI(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp index 5bf62d4ebc..6428f77e09 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp @@ -48,7 +48,7 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) MPU9250_AK8963::MPU9250_AK8963(MPU9250 &mpu9250, enum Rotation rotation) : ScheduledWorkItem("mpu9250_ak8963", px4::device_bus_to_wq(mpu9250.get_device_id())), _mpu9250(mpu9250), - _px4_mag(mpu9250.get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_mag(mpu9250.get_device_id(), rotation) { _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK8963); _px4_mag.set_external(mpu9250.external()); diff --git a/src/drivers/imu/l3gd20/L3GD20.cpp b/src/drivers/imu/l3gd20/L3GD20.cpp index 2ac26277e3..7e3ae88078 100644 --- a/src/drivers/imu/l3gd20/L3GD20.cpp +++ b/src/drivers/imu/l3gd20/L3GD20.cpp @@ -39,7 +39,7 @@ L3GD20::L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotati spi_mode_e spi_mode) : SPI(DRV_GYR_DEVTYPE_L3GD20, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation), + _px4_gyro(get_device_id(), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")), diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp index 414686d2dd..e4887f2835 100644 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ b/src/drivers/imu/lsm303d/LSM303D.cpp @@ -57,8 +57,8 @@ LSM303D::LSM303D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota spi_mode_e spi_mode) : SPI(DRV_IMU_DEVTYPE_LSM303D, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), - _px4_mag(get_device_id(), ORB_PRIO_LOW, rotation), + _px4_accel(get_device_id(), rotation), + _px4_mag(get_device_id(), rotation), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: acc_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: mag_read")), _bad_registers(perf_alloc(PC_COUNT, "lsm303d: bad_reg")), diff --git a/src/drivers/imu/mpu6000/MPU6000.cpp b/src/drivers/imu/mpu6000/MPU6000.cpp index 55551c82e2..3545ae522a 100644 --- a/src/drivers/imu/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/mpu6000/MPU6000.cpp @@ -44,8 +44,8 @@ MPU6000::MPU6000(device::Device *interface, enum Rotation rotation, int device_t I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, 0, device_type), _interface(interface), _device_type(device_type), - _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), - _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), + _px4_accel(_interface->get_device_id(), rotation), + _px4_gyro(_interface->get_device_id(), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6k_read")), _bad_transfers(perf_alloc(PC_COUNT, "mpu6k_bad_trans")), _bad_registers(perf_alloc(PC_COUNT, "mpu6k_bad_reg")), diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.cpp b/src/drivers/imu/mpu9250/MPU9250_mag.cpp index 3c912efad5..3106c45b5b 100644 --- a/src/drivers/imu/mpu9250/MPU9250_mag.cpp +++ b/src/drivers/imu/mpu9250/MPU9250_mag.cpp @@ -53,8 +53,7 @@ // Otherwise, it will passthrough the parent MPU9250 MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation) : _interface(interface), - _px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), - rotation), + _px4_mag(parent->_interface->get_device_id(), rotation), _parent(parent), _mag_overruns(perf_alloc(PC_COUNT, MODULE_NAME": mag overruns")), _mag_overflows(perf_alloc(PC_COUNT, MODULE_NAME": mag overflows")), diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 8928f8794b..b02d256269 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -74,8 +74,8 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, enum I2CSPIBusOption bus_option, int bus) : I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), _interface(interface), - _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), - _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), + _px4_accel(_interface->get_device_id(), rotation), + _px4_gyro(_interface->get_device_id(), rotation), _mag(this, mag_interface, rotation), _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp index 05133fdabc..e97e74d4ac 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp @@ -44,8 +44,8 @@ ISM330DLC::ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum SPI(DRV_IMU_DEVTYPE_ST_ISM330DLC, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { } diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index ec17f04491..d893121ec7 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -42,8 +42,8 @@ LSM9DS1::LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota spi_mode_e spi_mode) : SPI(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) + _px4_accel(get_device_id(), rotation), + _px4_gyro(get_device_id(), rotation) { } diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp index 8f7c774d55..da79675638 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp @@ -43,7 +43,7 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) AK09916::AK09916(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : I2C(DRV_MAG_DEVTYPE_AK09916, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation) + _px4_mag(get_device_id(), rotation) { _px4_mag.set_external(external()); } diff --git a/src/drivers/magnetometer/akm/ak8963/AK8963.cpp b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp index 643b6ee394..b1722c25d9 100644 --- a/src/drivers/magnetometer/akm/ak8963/AK8963.cpp +++ b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp @@ -43,7 +43,7 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) AK8963::AK8963(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : I2C(DRV_MAG_DEVTYPE_AK8963, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation) + _px4_mag(get_device_id(), rotation) { _px4_mag.set_external(external()); } diff --git a/src/drivers/magnetometer/bmm150/bmm150.cpp b/src/drivers/magnetometer/bmm150/bmm150.cpp index 87dfd42159..e2c4acd3d4 100644 --- a/src/drivers/magnetometer/bmm150/bmm150.cpp +++ b/src/drivers/magnetometer/bmm150/bmm150.cpp @@ -43,7 +43,7 @@ BMM150::BMM150(I2CSPIBusOption bus_option, const int bus, int bus_frequency, enum Rotation rotation) : I2C(DRV_MAG_DEVTYPE_BMM150, MODULE_NAME, bus, BMM150_SLAVE_ADDRESS, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _px4_mag(get_device_id(), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")), _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good transfers")), diff --git a/src/drivers/magnetometer/bmm150/bmm150.hpp b/src/drivers/magnetometer/bmm150/bmm150.hpp index 1178ca0caa..44aeb4ed29 100644 --- a/src/drivers/magnetometer/bmm150/bmm150.hpp +++ b/src/drivers/magnetometer/bmm150/bmm150.hpp @@ -39,7 +39,6 @@ #include #include #include -#include #include #define BMM150_SLAVE_ADDRESS 0x10 diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.cpp b/src/drivers/magnetometer/hmc5883/HMC5883.cpp index 539a9d331f..42cdf7708a 100644 --- a/src/drivers/magnetometer/hmc5883/HMC5883.cpp +++ b/src/drivers/magnetometer/hmc5883/HMC5883.cpp @@ -35,7 +35,7 @@ HMC5883::HMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _px4_mag(interface->get_device_id(), rotation), _interface(interface), _range_ga(1.9f), _collect_phase(false), diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp index a036401315..db6b56dc19 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -43,7 +43,7 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) IST8308::IST8308(I2CSPIBusOption bus_option, int bus, enum Rotation rotation, int bus_frequency) : I2C(DRV_MAG_DEVTYPE_IST8308, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation) + _px4_mag(get_device_id(), rotation) { _px4_mag.set_external(external()); } diff --git a/src/drivers/magnetometer/ist8310/ist8310.cpp b/src/drivers/magnetometer/ist8310/ist8310.cpp index 4f17ca8520..4eccc043c6 100644 --- a/src/drivers/magnetometer/ist8310/ist8310.cpp +++ b/src/drivers/magnetometer/ist8310/ist8310.cpp @@ -270,7 +270,7 @@ private: IST8310::IST8310(I2CSPIBusOption bus_option, int bus_number, int address, enum Rotation rotation, int bus_frequency) : I2C(DRV_MAG_DEVTYPE_IST8310, MODULE_NAME, bus_number, address, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus_number, address), - _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _px4_mag(get_device_id(), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")), _range_errors(perf_alloc(PC_COUNT, MODULE_NAME": rng_err")), diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp index d757b2458a..314371e472 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp @@ -44,7 +44,7 @@ LIS2MDL::LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _px4_mag(interface->get_device_id(), rotation), _interface(interface), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")), diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp index 4ba6354862..e82f1d664b 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp @@ -44,7 +44,7 @@ LIS3MDL::LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _px4_mag(interface->get_device_id(), rotation), _interface(interface), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")), diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp index 2472222c7e..7a98895474 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp @@ -65,7 +65,7 @@ LSM303AGR::LSM303AGR(I2CSPIBusOption bus_option, int bus, int device, enum Rotat spi_mode_e spi_mode) : SPI(DRV_MAG_DEVTYPE_LSM303AGR, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _px4_mag(get_device_id(), rotation), _mag_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": mag_read")), _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")), _bad_values(perf_alloc(PC_COUNT, MODULE_NAME": bad_val")) diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp index 98ef5385bf..273fba9320 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp @@ -44,7 +44,7 @@ LSM9DS1_MAG::LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, e int bus_frequency, spi_mode_e spi_mode) : SPI(DRV_MAG_DEVTYPE_ST_LSM9DS1_M, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation) + _px4_mag(get_device_id(), rotation) { _px4_mag.set_external(external()); } diff --git a/src/drivers/magnetometer/qmc5883/QMC5883.cpp b/src/drivers/magnetometer/qmc5883/QMC5883.cpp index e7b43df7dd..9c0bed8b4d 100644 --- a/src/drivers/magnetometer/qmc5883/QMC5883.cpp +++ b/src/drivers/magnetometer/qmc5883/QMC5883.cpp @@ -36,7 +36,7 @@ QMC5883::QMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus, int i2c_address) : I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address), - _px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _px4_mag(interface->get_device_id(), rotation), _interface(interface), _collect_phase(false), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp index 0d98fdeb34..d7a21e80f2 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100.cpp @@ -43,7 +43,7 @@ RM3100::RM3100(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _px4_mag(interface->get_device_id(), rotation), _interface(interface), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")), diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 9ac128cc74..e48d0e150d 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -481,8 +481,7 @@ MK::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); int dummy; - _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &dummy, ORB_PRIO_HIGH); + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &dummy); /* * advertise the blctrl status. diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 84c59ab758..6a668a45b8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2058,7 +2058,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t int PX4IO::print_debug() { -#if defined(CONFIG_ARCH_BOARD_PX4_FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4_FMU_V3) || defined(CONFIG_ARCH_BOARD_HEX_CUBE_ORANGE) +#if defined(CONFIG_ARCH_BOARD_PX4_FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4_FMU_V3) int io_fd = -1; if (io_fd <= 0) { diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp index d070d7b7d4..cdb24d3bad 100644 --- a/src/drivers/uavcan/sensors/baro.cpp +++ b/src/drivers/uavcan/sensors/baro.cpp @@ -117,7 +117,7 @@ int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel) device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN; device_id.devid_s.address = static_cast(channel->node_id); - channel->h_driver = new PX4Barometer(device_id.devid, ORB_PRIO_HIGH); + channel->h_driver = new PX4Barometer(device_id.devid); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 24a934b5df..9bbed94498 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -114,7 +114,7 @@ private: float _last_gnss_auxiliary_hdop{0.0f}; float _last_gnss_auxiliary_vdop{0.0f}; - uORB::PublicationMulti _gps_pub{ORB_ID(vehicle_gps_position), ORB_PRIO_DEFAULT}; + uORB::PublicationMulti _gps_pub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _orb_sub_gnss{ORB_ID(vehicle_gps_position)}; int _receiver_node_id{-1}; diff --git a/src/drivers/uavcan/sensors/mag.cpp b/src/drivers/uavcan/sensors/mag.cpp index f973a7a90c..7b1f3f35dc 100644 --- a/src/drivers/uavcan/sensors/mag.cpp +++ b/src/drivers/uavcan/sensors/mag.cpp @@ -148,7 +148,7 @@ int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel) device_id.devid_s.devtype = DRV_MAG_DEVTYPE_UAVCAN; device_id.devid_s.address = static_cast(channel->node_id); - channel->h_driver = new PX4Magnetometer(device_id.devid, ORB_PRIO_HIGH, ROTATION_NONE); + channel->h_driver = new PX4Magnetometer(device_id.devid, ROTATION_NONE); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index bf1c0f4ede..de81d74556 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -130,7 +130,7 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) channel->class_instance = class_instance; DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance); - channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_VERY_HIGH); + channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance); if (channel->orb_advert == nullptr) { DEVICE_LOG("uORB advertise failed. Out of instances?"); diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.cpp b/src/examples/fake_magnetometer/FakeMagnetometer.cpp index 2c485049b2..12547cd853 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.cpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.cpp @@ -41,7 +41,7 @@ using namespace time_literals; FakeMagnetometer::FakeMagnetometer() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _px4_mag(0, ORB_PRIO_MIN, ROTATION_NONE) + _px4_mag(0, ROTATION_NONE) { _px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM); _px4_mag.set_external(false); diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.hpp b/src/examples/fake_magnetometer/FakeMagnetometer.hpp index b10f53a495..55acc8bbad 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.hpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index ecc15e3bd4..8386286880 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -186,7 +186,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre void Battery::publish() { - orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT); + orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance); } void diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index ef13598e1f..04e712596e 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -63,10 +63,10 @@ static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit, return clip_count; } -PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) : +PX4Accelerometer::PX4Accelerometer(uint32_t device_id, enum Rotation rotation) : ModuleParams(nullptr), - _sensor_pub{ORB_ID(sensor_accel), priority}, - _sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority}, + _sensor_pub{ORB_ID(sensor_accel)}, + _sensor_fifo_pub{ORB_ID(sensor_accel_fifo)}, _device_id{device_id}, _rotation{rotation} { diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 6c96164272..5d39edb353 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -44,7 +44,7 @@ class PX4Accelerometer : public ModuleParams { public: - PX4Accelerometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); + PX4Accelerometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE); ~PX4Accelerometer() override; uint32_t get_device_id() const { return _device_id; } diff --git a/src/lib/drivers/barometer/PX4Barometer.cpp b/src/lib/drivers/barometer/PX4Barometer.cpp index 77d5ee44c1..ae5aee3cf6 100644 --- a/src/lib/drivers/barometer/PX4Barometer.cpp +++ b/src/lib/drivers/barometer/PX4Barometer.cpp @@ -36,9 +36,9 @@ #include -PX4Barometer::PX4Barometer(uint32_t device_id, ORB_PRIO priority) : +PX4Barometer::PX4Barometer(uint32_t device_id) : CDev(nullptr), - _sensor_baro_pub{ORB_ID(sensor_baro), priority} + _sensor_baro_pub{ORB_ID(sensor_baro)} { _class_device_instance = register_class_devname(BARO_BASE_DEVICE_PATH); _sensor_baro_pub.advertise(); diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp index d070aace3a..0f07d3f9f2 100644 --- a/src/lib/drivers/barometer/PX4Barometer.hpp +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -45,7 +45,7 @@ class PX4Barometer : public cdev::CDev { public: - PX4Barometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT); + PX4Barometer(uint32_t device_id); ~PX4Barometer() override; const sensor_baro_s &get() { return _sensor_baro_pub.get(); } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 975db95c9d..8ecfa4624b 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -50,10 +50,10 @@ static constexpr int32_t sum(const int16_t samples[16], uint8_t len) return sum; } -PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) : +PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) : ModuleParams(nullptr), - _sensor_pub{ORB_ID(sensor_gyro), priority}, - _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority}, + _sensor_pub{ORB_ID(sensor_gyro)}, + _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)}, _device_id{device_id}, _rotation{rotation} { diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 9066392844..8f732c7c06 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -43,7 +43,7 @@ class PX4Gyroscope : public ModuleParams { public: - PX4Gyroscope(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); + PX4Gyroscope(uint32_t device_id, enum Rotation rotation = ROTATION_NONE); ~PX4Gyroscope() override; uint32_t get_device_id() const { return _device_id; } diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index cb3350829a..e4154c5935 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp @@ -36,9 +36,9 @@ #include -PX4Magnetometer::PX4Magnetometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) : +PX4Magnetometer::PX4Magnetometer(uint32_t device_id, enum Rotation rotation) : CDev(nullptr), - _sensor_pub{ORB_ID(sensor_mag), priority}, + _sensor_pub{ORB_ID(sensor_mag)}, _device_id{device_id}, _rotation{rotation} { diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index c8f4daa6fa..8d367ed1ea 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -43,7 +43,7 @@ class PX4Magnetometer : public cdev::CDev { public: - PX4Magnetometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); + PX4Magnetometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE); ~PX4Magnetometer() override; bool external() { return _external; } diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index 928e17c5aa..883a3d0d43 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -35,8 +35,8 @@ #include -PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const ORB_PRIO priority, const uint8_t device_orientation) : - _distance_sensor_pub{ORB_ID(distance_sensor), priority} +PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_orientation) : + _distance_sensor_pub{ORB_ID(distance_sensor)} { _distance_sensor_pub.advertise(); diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index 36b7af1110..f8dd776e75 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -44,7 +44,6 @@ class PX4Rangefinder public: PX4Rangefinder(const uint32_t device_id, - const ORB_PRIO priority = ORB_PRIO_DEFAULT, const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); ~PX4Rangefinder(); diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 79c39a7c2a..7766fbc37c 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -238,8 +238,8 @@ private: uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; - uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs), ORB_PRIO_DEFAULT}; - uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits), ORB_PRIO_DEFAULT}; ///< mixer status flags + uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs)}; + uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits)}; ///< mixer status flags actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; actuator_armed_s _armed{}; diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 690dea7b71..79d648cdb1 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -60,6 +60,44 @@ bool param_modify_on_import(bson_node_t node) } } + // 2020-06-29 (v1.11 beta): translate CAL_ACCx_EN/CAL_GYROx_EN/CAL_MAGx_EN -> CAL_ACCx_PRIO/CAL_GYROx_PRIO/CAL_MAGx_PRIO + if (node->type == BSON_INT32) { + + const char *cal_sensor_en_params[] = { + "CAL_ACC0_EN", + "CAL_ACC1_EN", + "CAL_ACC2_EN", + "CAL_GYRO0_EN", + "CAL_GYRO1_EN", + "CAL_GYRO2_EN", + "CAL_MAG0_EN", + "CAL_MAG1_EN", + "CAL_MAG2_EN", + "CAL_MAG3_EN", + }; + + for (int i = 0; i < sizeof(cal_sensor_en_params) / sizeof(cal_sensor_en_params[0]); ++i) { + if (strcmp(cal_sensor_en_params[i], node->name) == 0) { + + char new_parameter_name[17] {}; + strcpy(new_parameter_name, cal_sensor_en_params[i]); + + char *str_replace = strstr(new_parameter_name, "_EN"); + + if (str_replace != nullptr) { + strcpy(str_replace, "_PRIO"); + PX4_INFO("%s -> %s", cal_sensor_en_params[i], new_parameter_name); + strcpy(node->name, new_parameter_name); + } + + // if sensor wasn't disabled, reset to -1 so that it can be set to an appropriate default + if (node->i != 0) { + node->i = -1; // special value to process later + } + } + } + } + // translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10) @@ -87,10 +125,7 @@ bool param_modify_on_import(bson_node_t node) "CAL_MAG2_ID", "TC_A2_ID", "TC_B2_ID", - "TC_G2_ID", - "CAL_ACC_PRIME", - "CAL_GYRO_PRIME", - "CAL_MAG_PRIME", + "TC_G2_ID" }; bool found = false; diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index e3073075df..1221ba2642 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -118,9 +118,15 @@ void Accelerometer::ParametersUpdate() _rotation.setIdentity(); } - // CAL_ACCx_EN - int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index); - _enabled = (enabled == 1); + // CAL_ACCx_PRIO + _priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index); + + if (_priority < 0 || _priority > 100) { + // reset to default + PX4_ERR("%s %d invalid priority %d, resetting to %d", SensorString(), _calibration_index, _priority, DEFAULT_PRIORITY); + SetCalibrationParam(SensorString(), "PRIO", _calibration_index, DEFAULT_PRIORITY); + _priority = DEFAULT_PRIORITY; + } // CAL_ACCx_OFF{X,Y,Z} _offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index); @@ -140,7 +146,7 @@ void Accelerometer::Reset() _scale = Vector3f{1.f, 1.f, 1.f}; _thermal_offset.zero(); - _enabled = true; + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; _calibration_index = -1; } @@ -150,10 +156,17 @@ bool Accelerometer::ParametersSave() if (_calibration_index >= 0) { // save calibration SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0); + SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale); + // if (_external) { + // SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); + + // } else { + // SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); + // } + return true; } diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index b6d85bdec5..59e0a87993 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -47,6 +47,9 @@ class Accelerometer public: static constexpr int MAX_SENSOR_COUNT = 3; + static constexpr uint8_t DEFAULT_PRIORITY = 50; + static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; + static constexpr const char *SensorString() { return "ACC"; } Accelerometer(); @@ -63,8 +66,9 @@ public: void set_scale(const matrix::Vector3f &scale) { _scale = scale; } uint32_t device_id() const { return _device_id; } - bool enabled() const { return _enabled; } + bool enabled() const { return (_priority > 0); } bool external() const { return _external; } + const int32_t &priority() const { return _priority; } const matrix::Dcmf &rotation() const { return _rotation; } // apply offsets and scale @@ -91,8 +95,8 @@ private: int8_t _calibration_index{-1}; uint32_t _device_id{0}; + int32_t _priority{DEFAULT_PRIORITY}; - bool _enabled{true}; bool _external{false}; }; } // namespace calibration diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index c12683b53e..a1f29b3da7 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -118,9 +118,15 @@ void Gyroscope::ParametersUpdate() _rotation.setIdentity(); } - // CAL_GYROx_EN - int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index); - _enabled = (enabled == 1); + // CAL_GYROx_PRIO + _priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index); + + if (_priority < 0 || _priority > 100) { + // reset to default + PX4_ERR("%s %d invalid priority %d, resetting to %d", SensorString(), _calibration_index, _priority, DEFAULT_PRIORITY); + SetCalibrationParam(SensorString(), "PRIO", _calibration_index, DEFAULT_PRIORITY); + _priority = DEFAULT_PRIORITY; + } // CAL_GYROx_OFF{X,Y,Z} _offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index); @@ -136,7 +142,7 @@ void Gyroscope::Reset() _offset.zero(); _thermal_offset.zero(); - _enabled = true; + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; _calibration_index = -1; } @@ -146,9 +152,16 @@ bool Gyroscope::ParametersSave() if (_calibration_index >= 0) { // save calibration SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0); + SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); + // if (_external) { + // SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum); + + // } else { + // SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); + // } + return true; } diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 209aa84173..907a2f5789 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -47,6 +47,9 @@ class Gyroscope public: static constexpr int MAX_SENSOR_COUNT = 3; + static constexpr uint8_t DEFAULT_PRIORITY = 50; + static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; + static constexpr const char *SensorString() { return "GYRO"; } Gyroscope(); @@ -62,8 +65,9 @@ public: void set_offset(const matrix::Vector3f &offset) { _offset = offset; } uint32_t device_id() const { return _device_id; } - bool enabled() const { return _enabled; } + bool enabled() const { return (_priority > 0); } bool external() const { return _external; } + const int32_t &priority() const { return _priority; } const matrix::Dcmf &rotation() const { return _rotation; } // apply offsets and scale @@ -89,8 +93,8 @@ private: int8_t _calibration_index{-1}; uint32_t _device_id{0}; + int32_t _priority{DEFAULT_PRIORITY}; - bool _enabled{true}; bool _external{false}; }; } // namespace calibration diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 7e5aac266b..9e5903adc9 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -64,6 +64,21 @@ void Magnetometer::set_device_id(uint32_t device_id) void Magnetometer::set_external(bool external) { + // update priority default appropriately if not set + if (_calibration_index < 0) { + if ((_priority < 0) || (_priority > 100)) { + _priority = external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + + } else if (!_external && external && (_priority == DEFAULT_PRIORITY)) { + // internal -> external + _priority = DEFAULT_EXTERNAL_PRIORITY; + + } else if (_external && !external && (_priority == DEFAULT_EXTERNAL_PRIORITY)) { + // external -> internal + _priority = DEFAULT_PRIORITY; + } + } + _external = external; } @@ -113,9 +128,16 @@ void Magnetometer::ParametersUpdate() _rotation = get_rot_matrix((enum Rotation)rotation); } - // CAL_MAGx_EN - int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index); - _enabled = (enabled == 1); + // CAL_MAGx_PRIO + _priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index); + + if ((_priority < 0) || (_priority > 100)) { + // reset to default + int32_t new_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; + PX4_ERR("%s %d invalid priority %d, resetting to %d", SensorString(), _calibration_index, _priority, new_priority); + SetCalibrationParam(SensorString(), "PRIO", _calibration_index, new_priority); + _priority = new_priority; + } // CAL_MAGx_OFF{X,Y,Z} _offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index); @@ -151,7 +173,7 @@ void Magnetometer::Reset() _power_compensation.zero(); _power = 0.f; - _enabled = true; + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; _calibration_index = -1; } @@ -161,7 +183,7 @@ bool Magnetometer::ParametersSave() if (_calibration_index >= 0) { // save calibration SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); - SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0); + SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority); SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset); Vector3f scale{_scale.diag()}; diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index aa78d2bedd..615e7e6acf 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -48,6 +48,9 @@ class Magnetometer public: static constexpr int MAX_SENSOR_COUNT = 4; + static constexpr uint8_t DEFAULT_PRIORITY = 50; + static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75; + static constexpr const char *SensorString() { return "MAG"; } Magnetometer(); @@ -66,9 +69,10 @@ public: void set_rotation(Rotation rotation); uint32_t device_id() const { return _device_id; } - bool enabled() const { return _enabled; } + bool enabled() const { return (_priority > 0); } bool external() const { return _external; } const matrix::Vector3f &offset() const { return _offset; } + const int32_t &priority() const { return _priority; } const matrix::Dcmf &rotation() const { return _rotation; } const Rotation &rotation_enum() const { return _rotation_enum; } const matrix::Matrix3f &scale() const { return _scale; } @@ -98,8 +102,8 @@ private: int8_t _calibration_index{-1}; uint32_t _device_id{0}; + int32_t _priority{-1}; - bool _enabled{true}; bool _external{false}; }; } // namespace calibration diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index f399cbb631..6578307570 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -263,8 +263,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {}; - ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED; - int32_t device_id_primary = 0; unsigned active_sensors = 0; for (uint8_t cur_accel = 0; cur_accel < MAX_ACCEL_SENS; cur_accel++) { @@ -274,14 +272,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) calibrations[cur_accel].set_device_id(accel_sub.get().device_id); active_sensors++; - // Get priority - const ORB_PRIO prio = accel_sub.get_priority(); - - if (prio > device_prio_max) { - device_prio_max = prio; - device_id_primary = accel_sub.get().device_id; - } - } else { calibrations[cur_accel].Reset(); } @@ -365,7 +355,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) calibrations[i].ParametersSave(); } - param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary); param_notify_changes(); /* if there is a any preflight-check system response, let the barrage of messages through */ @@ -384,7 +373,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) #if !defined(CONSTRAINED_FLASH) PX4_INFO("Accelerometer quick calibration"); - int32_t device_id_primary = 0; + bool success = false; // sensor thermal corrections (optional) uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; @@ -492,20 +481,16 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) } else { calibration.set_offset(offset); + + success = true; } calibration.ParametersSave(); - - if (device_id_primary == 0) { - device_id_primary = arp.device_id; - } - calibration.PrintStatus(); } } - if (device_id_primary != 0) { - param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary); + if (success) { param_notify_changes(); return PX4_OK; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index bb14f9c020..04afa4c8ac 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -212,9 +212,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) gyro_worker_data_t worker_data{}; worker_data.mavlink_log_pub = mavlink_log_pub; - ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED; - int32_t device_id_primary = 0; - // We should not try to subscribe if the topic doesn't actually exist and can be counted. const unsigned orb_gyro_count = orb_group_count(ORB_ID(sensor_gyro)); @@ -232,14 +229,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) if (gyro_sub.advertised() && (gyro_sub.get().device_id != 0) && (gyro_sub.get().timestamp > 0)) { worker_data.calibrations[cur_gyro].set_device_id(gyro_sub.get().device_id); - - // Get priority - const ORB_PRIO prio = gyro_sub.get_priority(); - - if (prio > device_prio_max) { - device_prio_max = prio; - device_id_primary = gyro_sub.get().device_id; - } } // reset calibration index to match uORB numbering @@ -317,8 +306,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) calibration.ParametersSave(); } - param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary); - if (failed) { calibration_log_critical(mavlink_log_pub, "ERROR: failed to set offset params"); res = PX4_ERROR; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 709b76beee..a3909a05f0 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -262,12 +262,12 @@ private: uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; // ORB publications (multi) - uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; - uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; - uORB::PublicationMulti _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW}; - uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW}; - uORB::PublicationMulti _ping_pub{ORB_ID(ping), ORB_PRIO_LOW}; - uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW}; + uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; + uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor)}; + uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; + uORB::PublicationMulti _ping_pub{ORB_ID(ping)}; + uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status)}; // ORB publications (queue length > 1) uORB::PublicationQueued _gps_inject_data_pub{ORB_ID(gps_inject_data)}; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index b0b612e42c..a3e5ca2d62 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -107,7 +107,7 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; uORB::Publication _actuators_0_pub; - uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index fdd4ef8cac..790bea08ad 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -157,7 +157,7 @@ private: uORB::Publication _rc_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */ uORB::Publication _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; /**< manual control as actuator topic */ - uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH}; /**< manual control signal topic */ + uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; /**< manual control signal topic */ rc_channels_s _rc {}; /**< r/c channel data */ diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 4de85ffe05..cbedc43c39 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -955,7 +955,7 @@ Replay::publishTopic(Subscription &sub, void *data) if (advertised) { int instance; - sub.orb_advert = orb_advertise_multi(sub.orb_meta, data, &instance, ORB_PRIO_DEFAULT); + sub.orb_advert = orb_advertise_multi(sub.orb_meta, data, &instance); published = true; } } diff --git a/src/modules/sensors/sensor_params_acc0.c b/src/modules/sensors/sensor_params_acc0.c index f3a647c5b7..8843fefaba 100644 --- a/src/modules/sensors/sensor_params_acc0.c +++ b/src/modules/sensors/sensor_params_acc0.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -40,13 +40,19 @@ PARAM_DEFINE_INT32(CAL_ACC0_ID, 0); /** - * Accelerometer 0 enabled + * Accelerometer 0 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_ACC0_EN, 1); +PARAM_DEFINE_INT32(CAL_ACC0_PRIO, 50); /** * Accelerometer X-axis offset diff --git a/src/modules/sensors/sensor_params_acc1.c b/src/modules/sensors/sensor_params_acc1.c index 48c342f92b..96130686e2 100644 --- a/src/modules/sensors/sensor_params_acc1.c +++ b/src/modules/sensors/sensor_params_acc1.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -40,13 +40,19 @@ PARAM_DEFINE_INT32(CAL_ACC1_ID, 0); /** - * Accelerometer 1 enabled + * Accelerometer 1 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_ACC1_EN, 1); +PARAM_DEFINE_INT32(CAL_ACC1_PRIO, 50); /** * Accelerometer X-axis offset diff --git a/src/modules/sensors/sensor_params_acc2.c b/src/modules/sensors/sensor_params_acc2.c index 9712b2eebe..da1873ca61 100644 --- a/src/modules/sensors/sensor_params_acc2.c +++ b/src/modules/sensors/sensor_params_acc2.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -40,13 +40,19 @@ PARAM_DEFINE_INT32(CAL_ACC2_ID, 0); /** - * Accelerometer 2 enabled + * Accelerometer 2 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_ACC2_EN, 1); +PARAM_DEFINE_INT32(CAL_ACC2_PRIO, 50); /** * Accelerometer X-axis offset diff --git a/src/modules/sensors/sensor_params_accel.c b/src/modules/sensors/sensor_params_accel.c deleted file mode 100644 index 7930422548..0000000000 --- a/src/modules/sensors/sensor_params_accel.c +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2017 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Primary accel ID - * - * @category system - * @group Sensor Calibration - */ -PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0); diff --git a/src/modules/sensors/sensor_params_gyro.c b/src/modules/sensors/sensor_params_gyro.c deleted file mode 100644 index 5932add16d..0000000000 --- a/src/modules/sensors/sensor_params_gyro.c +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2017 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Primary gyro ID - * - * @category system - * @group Sensor Calibration - */ -PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0); diff --git a/src/modules/sensors/sensor_params_gyro0.c b/src/modules/sensors/sensor_params_gyro0.c index 29b1448c16..20b2c99e0a 100644 --- a/src/modules/sensors/sensor_params_gyro0.c +++ b/src/modules/sensors/sensor_params_gyro0.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -40,13 +40,19 @@ PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0); /** - * Gyro 0 enabled + * Gyro 0 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_GYRO0_EN, 1); +PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, 50); /** * Gyro X-axis offset diff --git a/src/modules/sensors/sensor_params_gyro1.c b/src/modules/sensors/sensor_params_gyro1.c index 584b89d7fc..2b30fa51a7 100644 --- a/src/modules/sensors/sensor_params_gyro1.c +++ b/src/modules/sensors/sensor_params_gyro1.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -40,13 +40,19 @@ PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0); /** - * Gyro 1 enabled + * Gyro 1 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_GYRO1_EN, 1); +PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, 50); /** * Gyro X-axis offset diff --git a/src/modules/sensors/sensor_params_gyro2.c b/src/modules/sensors/sensor_params_gyro2.c index 3471b4272d..d2608942f1 100644 --- a/src/modules/sensors/sensor_params_gyro2.c +++ b/src/modules/sensors/sensor_params_gyro2.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -40,13 +40,19 @@ PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0); /** - * Gyro 2 enabled + * Gyro 2 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_GYRO2_EN, 1); +PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, 50); /** * Gyro X-axis offset diff --git a/src/modules/sensors/sensor_params_mag.c b/src/modules/sensors/sensor_params_mag.c index 6c9d29f30f..82d17b31b6 100644 --- a/src/modules/sensors/sensor_params_mag.c +++ b/src/modules/sensors/sensor_params_mag.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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,14 +31,6 @@ * ****************************************************************************/ -/** - * Primary mag ID - * - * @category system - * @group Sensor Calibration - */ -PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0); - /** * Bitfield selecting mag sides for calibration * diff --git a/src/modules/sensors/sensor_params_mag0.c b/src/modules/sensors/sensor_params_mag0.c index 4638c9e126..8bf3611f3b 100644 --- a/src/modules/sensors/sensor_params_mag0.c +++ b/src/modules/sensors/sensor_params_mag0.c @@ -40,13 +40,19 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); /** - * Mag 0 enabled + * Mag 0 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG0_EN, 1); +PARAM_DEFINE_INT32(CAL_MAG0_PRIO, 50); /** * Rotation of magnetometer 0 relative to airframe. diff --git a/src/modules/sensors/sensor_params_mag1.c b/src/modules/sensors/sensor_params_mag1.c index 3418e2c2ee..405aeb4ded 100644 --- a/src/modules/sensors/sensor_params_mag1.c +++ b/src/modules/sensors/sensor_params_mag1.c @@ -40,13 +40,19 @@ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); /** - * Mag 1 enabled + * Mag 1 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG1_EN, 1); +PARAM_DEFINE_INT32(CAL_MAG1_PRIO, 50); /** * Rotation of magnetometer 1 relative to airframe. diff --git a/src/modules/sensors/sensor_params_mag2.c b/src/modules/sensors/sensor_params_mag2.c index 2f575860ed..e8bb1a0bee 100644 --- a/src/modules/sensors/sensor_params_mag2.c +++ b/src/modules/sensors/sensor_params_mag2.c @@ -40,13 +40,19 @@ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); /** - * Mag 2 enabled + * Mag 2 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG2_EN, 1); +PARAM_DEFINE_INT32(CAL_MAG2_PRIO, 50); /** * Rotation of magnetometer 2 relative to airframe. diff --git a/src/modules/sensors/sensor_params_mag3.c b/src/modules/sensors/sensor_params_mag3.c index c6d979ab21..91ba9cd95a 100644 --- a/src/modules/sensors/sensor_params_mag3.c +++ b/src/modules/sensors/sensor_params_mag3.c @@ -40,16 +40,22 @@ PARAM_DEFINE_INT32(CAL_MAG3_ID, 0); /** - * Mag 3 enabled + * Mag 3 priority. + * + * @value 0 Disabled + * @value 1 Min + * @value 25 Low + * @value 50 Medium (Default) + * @value 75 High + * @value 100 Max * - * @boolean * @category system * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG3_EN, 1); +PARAM_DEFINE_INT32(CAL_MAG3_PRIO, 50); /** - * Rotation of magnetometer 2 relative to airframe. + * Rotation of magnetometer 3 relative to airframe. * * An internal magnetometer will force a value of -1, so a GCS * should only attempt to configure the rotation if the value is diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1df10d7555..d28b1f968c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -340,8 +340,7 @@ void Sensors::diff_pres_poll() /* push data into validator */ float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f }; - _airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, - ORB_PRIO_HIGH); + _airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index aee76514da..8f2bce29e3 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -156,11 +156,6 @@ void VehicleAirData::Run() updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]); if (updated[uorb_index]) { - if (_priority[uorb_index] == 0) { - // set initial priority - _priority[uorb_index] = _sensor_sub[uorb_index].get_priority(); - } - // millibar to Pa const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f; @@ -282,7 +277,7 @@ void VehicleAirData::Run() } // reduce priority of failed sensor to the minimum - _priority[failover_index] = ORB_PRIO_MIN; + _priority[failover_index] = 1; } } diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 7d864be9ba..33a8c991d2 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -122,6 +122,24 @@ void VehicleMagnetometer::ParametersUpdate(bool force) } _mag_comp_type = mag_comp_typ; + + // update mag priority (CAL_MAGx_PRIO) + for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) { + const int32_t priority_old = _calibration[mag].priority(); + _calibration[mag].ParametersUpdate(); + const int32_t priority_new = _calibration[mag].priority(); + + if (priority_old != priority_new) { + if (_priority[mag] == priority_old) { + _priority[mag] = priority_new; + + } else { + // change relative priority to incorporate any sensor faults + int priority_change = priority_new - priority_old; + _priority[mag] = math::constrain(_priority[mag] + priority_change, 1, 100); + } + } + } } } @@ -207,7 +225,7 @@ void VehicleMagnetometer::Run() if (_calibration[uorb_index].device_id() != report.device_id) { _calibration[uorb_index].set_external(report.is_external); _calibration[uorb_index].set_device_id(report.device_id); - _priority[uorb_index] = _sensor_sub[uorb_index].get_priority(); + _priority[uorb_index] = _calibration[uorb_index].priority(); } if (_calibration[uorb_index].enabled()) { diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 72de36fdef..f61376e2f2 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -88,28 +88,46 @@ void VotedSensorsUpdate::parametersUpdate() if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) { - if (_accel.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) { - // find corresponding sensor_accel publication - for (uint8_t i = 0; i < ACCEL_COUNT_MAX; i++) { - uORB::SubscriptionData sensor_accel{ORB_ID(sensor_accel), i}; - sensor_accel.update(); + // find corresponding configured accel priority + int8_t accel_cal_index = calibration::FindCalibrationIndex("ACC", imu.get().accel_device_id); - if (imu.get().accel_device_id == sensor_accel.get().device_id) { - _accel.priority[uorb_index] = sensor_accel.get_priority(); - break; + if (accel_cal_index >= 0) { + // found matching CAL_ACCx_PRIO + int32_t accel_priority_old = _accel.priority_configured[uorb_index]; + + _accel.priority_configured[uorb_index] = calibration::GetCalibrationParam("ACC", "PRIO", accel_cal_index); + + if (accel_priority_old != _accel.priority_configured[uorb_index]) { + if (_accel.priority_configured[uorb_index] == 0) { + // disabled + _accel.priority[uorb_index] = 0; + + } else { + // change relative priority to incorporate any sensor faults + int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old; + _accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, 1, 100); } } } - if (_gyro.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) { - // find corresponding sensor_gyro publication - for (uint8_t i = 0; i < GYRO_COUNT_MAX; i++) { - uORB::SubscriptionData sensor_gyro{ORB_ID(sensor_gyro), i}; - sensor_gyro.update(); + // find corresponding configured gyro priority + int8_t gyro_cal_index = calibration::FindCalibrationIndex("GYRO", imu.get().gyro_device_id); - if (imu.get().gyro_device_id == sensor_gyro.get().device_id) { - _gyro.priority[uorb_index] = sensor_gyro.get_priority(); - break; + if (gyro_cal_index >= 0) { + // found matching CAL_GYROx_PRIO + int32_t gyro_priority_old = _gyro.priority_configured[uorb_index]; + + _gyro.priority_configured[uorb_index] = calibration::GetCalibrationParam("GYRO", "PRIO", gyro_cal_index); + + if (gyro_priority_old != _gyro.priority_configured[uorb_index]) { + if (_gyro.priority_configured[uorb_index] == 0) { + // disabled + _gyro.priority[uorb_index] = 0; + + } else { + // change relative priority to incorporate any sensor faults + int priority_change = _gyro.priority_configured[uorb_index] - gyro_priority_old; + _gyro.priority[uorb_index] = math::constrain(_gyro.priority[uorb_index] + priority_change, 1, 100); } } } @@ -122,21 +140,13 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) for (int uorb_index = 0; uorb_index < 3; uorb_index++) { vehicle_imu_s imu_report; - if (_accel.enabled[uorb_index] && _gyro.enabled[uorb_index] && _vehicle_imu_sub[uorb_index].update(&imu_report)) { + if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0) + && _vehicle_imu_sub[uorb_index].update(&imu_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(); - } - - if (_gyro.priority[uorb_index] == 0) { - _gyro.priority[uorb_index] = _gyro.subscription[uorb_index].get_priority(); - } - _accel_device_id[uorb_index] = imu_report.accel_device_id; _gyro_device_id[uorb_index] = imu_report.gyro_device_id; @@ -250,12 +260,12 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na } // reduce priority of failed sensor to the minimum - sensor.priority[failover_index] = ORB_PRIO_MIN; + sensor.priority[failover_index] = 1; int ctr_valid = 0; for (uint8_t i = 0; i < sensor.subscription_count; i++) { - if (sensor.enabled[i] && (sensor.priority[i] > ORB_PRIO_MIN)) { + if (sensor.priority[i] > 1) { ctr_valid++; } } @@ -300,6 +310,8 @@ void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) { sensor_data.advertised[i] = true; + sensor_data.priority[i] = DEFAULT_PRIORITY; + sensor_data.priority_configured[i] = DEFAULT_PRIORITY; if (i > 0) { /* the first always exists, but for each further sensor, add a new validator */ @@ -366,7 +378,7 @@ void VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_imu_s &preflt) for (int sensor_index = 0; sensor_index < _accel.subscription_count; sensor_index++) { // check that the sensor we are checking against is not the same as the primary - if (_accel.enabled[sensor_index] && (_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) { + if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) { float accel_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison agains the primary @@ -415,7 +427,7 @@ void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_imu_s &preflt) for (int sensor_index = 0; sensor_index < _gyro.subscription_count; sensor_index++) { // check that the sensor we are checking against is not the same as the primary - if (_gyro.enabled[sensor_index] && (_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) { + if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) { float gyro_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index faca22d98b..de4b010248 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -126,6 +126,8 @@ private: static constexpr uint8_t GYRO_COUNT_MAX = 3; static constexpr uint8_t SENSOR_COUNT_MAX = math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX); + static constexpr uint8_t DEFAULT_PRIORITY = 50; + struct SensorData { SensorData() = delete; explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}} {} @@ -133,10 +135,10 @@ private: uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */ DataValidatorGroup voter{1}; unsigned int last_failover_count{0}; - ORB_PRIO priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */ + int32_t priority[SENSOR_COUNT_MAX] {}; + int32_t priority_configured[SENSOR_COUNT_MAX] {}; uint8_t last_best_vote{0}; /**< index of the latest best vote */ uint8_t subscription_count{0}; - bool enabled[SENSOR_COUNT_MAX] {true, true, true}; bool advertised[SENSOR_COUNT_MAX] {false, false, false}; }; diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index dcea127e7e..d27e232612 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -98,10 +98,10 @@ private: void parameters_updated(); // simulated sensor instances - 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 + PX4Accelerometer _px4_accel{1311244}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Gyroscope _px4_gyro{1311244}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 2, TYPE: SIMULATION + PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION // to publish the gps position vehicle_gps_position_s _vehicle_gps_pos{}; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 61e9d76b01..d0b450c726 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -154,10 +154,10 @@ private: static Simulator *_instance; // simulated sensor instances - 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 + PX4Accelerometer _px4_accel{1311244}; // 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Gyroscope _px4_gyro{1311244}; // 2294028: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION perf_counter_t _perf_sim_delay{perf_alloc(PC_ELAPSED, MODULE_NAME": network delay")}; perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")}; diff --git a/src/modules/uORB/PublicationMulti.hpp b/src/modules/uORB/PublicationMulti.hpp index d8d2dfab0f..e54afd7365 100644 --- a/src/modules/uORB/PublicationMulti.hpp +++ b/src/modules/uORB/PublicationMulti.hpp @@ -59,23 +59,20 @@ public: * Constructor * * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. - * @param priority The priority for multi pub/sub, 0 means don't publish as multi */ - PublicationMulti(ORB_ID id, ORB_PRIO priority = ORB_PRIO_DEFAULT) : - PublicationBase(id), - _priority(priority) + PublicationMulti(ORB_ID id) : + PublicationBase(id) {} - PublicationMulti(const orb_metadata *meta, ORB_PRIO priority = ORB_PRIO_DEFAULT) : - PublicationBase(static_cast(meta->o_id)), - _priority(priority) + PublicationMulti(const orb_metadata *meta) : + PublicationBase(static_cast(meta->o_id)) {} bool advertise() { if (!advertised()) { int instance = 0; - _handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, _priority, QSIZE); + _handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE); } return advertised(); @@ -93,9 +90,6 @@ public: return (orb_publish(get_topic(), _handle, &data) == PX4_OK); } - -protected: - const ORB_PRIO _priority; }; /** @@ -109,14 +103,9 @@ public: * Constructor * * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. - * @param priority The priority for multi pub */ - PublicationMultiData(ORB_ID id, ORB_PRIO priority = ORB_PRIO_DEFAULT) : - PublicationMulti(id, priority) - {} - PublicationMultiData(const orb_metadata *meta, ORB_PRIO priority = ORB_PRIO_DEFAULT) : - PublicationMulti(meta, priority) - {} + PublicationMultiData(ORB_ID id) : PublicationMulti(id) {} + PublicationMultiData(const orb_metadata *meta) : PublicationMulti(meta) {} T &get() { return _data; } void set(const T &data) { _data = data; } diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 61ff5c977d..3809429f8c 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -129,7 +129,6 @@ public: 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: diff --git a/src/modules/uORB/SubscriptionInterval.hpp b/src/modules/uORB/SubscriptionInterval.hpp index 97222c8151..7d721a4db0 100644 --- a/src/modules/uORB/SubscriptionInterval.hpp +++ b/src/modules/uORB/SubscriptionInterval.hpp @@ -137,7 +137,6 @@ public: uint8_t get_instance() const { return _subscription.get_instance(); } uint32_t get_interval_us() const { return _interval_us; } unsigned get_last_generation() const { return _subscription.get_last_generation(); } - ORB_PRIO get_priority() { return _subscription.get_priority(); } orb_id_t get_topic() const { return _subscription.get_topic(); } /** diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index a1c3d23f06..23830d4638 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -50,15 +50,15 @@ orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *da return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size); } -orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, ORB_PRIO priority) +orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) { - return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority); + return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance); } orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance, - ORB_PRIO priority, unsigned int queue_size) + unsigned int queue_size) { - return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority, queue_size); + return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, queue_size); } int orb_unadvertise(orb_advert_t handle) @@ -112,11 +112,6 @@ int orb_group_count(const struct orb_metadata *meta) return instance; } -int orb_priority(int handle, enum ORB_PRIO *priority) -{ - return uORB::Manager::get_instance()->orb_priority(handle, priority); -} - int orb_set_interval(int handle, unsigned interval) { return uORB::Manager::get_instance()->orb_set_interval(handle, interval); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 766dc38797..5e8751c5d1 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -62,21 +62,6 @@ typedef const struct orb_metadata *orb_id_t; */ #define ORB_MULTI_MAX_INSTANCES 4 // This must be < 10 (because it's the last char of the node path) -/** - * Topic priority. - * Relevant for multi-topics / topic groups - */ -enum ORB_PRIO { - ORB_PRIO_UNINITIALIZED = 0, - ORB_PRIO_MIN = 1, - ORB_PRIO_VERY_LOW = 25, - ORB_PRIO_LOW = 50, - ORB_PRIO_DEFAULT = 75, - ORB_PRIO_HIGH = 100, - ORB_PRIO_VERY_HIGH = 125, - ORB_PRIO_MAX = 255 -}; - /** * Generates a pointer to the uORB metadata structure for * a given topic. @@ -151,14 +136,13 @@ extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const v /** * @see uORB::Manager::orb_advertise_multi() */ -extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - enum ORB_PRIO priority) __EXPORT; +extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT; /** * @see uORB::Manager::orb_advertise_multi() */ extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance, - enum ORB_PRIO priority, unsigned int queue_size) __EXPORT; + unsigned int queue_size) __EXPORT; /** * @see uORB::Manager::orb_unadvertise() @@ -179,10 +163,10 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con * @see uORB::Manager::orb_advertise_multi() for meaning of the individual parameters */ static inline int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, - int *instance, enum ORB_PRIO priority) + int *instance) { if (!*handle) { - *handle = orb_advertise_multi(meta, data, instance, priority); + *handle = orb_advertise_multi(meta, data, instance); if (*handle) { return 0; @@ -233,11 +217,6 @@ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT; */ extern int orb_group_count(const struct orb_metadata *meta) __EXPORT; -/** - * @see uORB::Manager::orb_priority() - */ -extern int orb_priority(int handle, enum ORB_PRIO *priority) __EXPORT; - /** * @see uORB::Manager::orb_set_interval() */ diff --git a/src/modules/uORB/uORBCommon.hpp b/src/modules/uORB/uORBCommon.hpp index d9afb719a5..7eace8def4 100644 --- a/src/modules/uORB/uORBCommon.hpp +++ b/src/modules/uORB/uORBCommon.hpp @@ -47,7 +47,6 @@ static constexpr unsigned orb_maxpath = 64; struct orb_advertdata { const struct orb_metadata *meta; int *instance; - ORB_PRIO priority; }; } diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index e2506dcef0..8f9cfe2fdc 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -59,7 +59,7 @@ uORB::DeviceMaster::~DeviceMaster() px4_sem_destroy(&_lock); } -int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, ORB_PRIO priority) +int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance) { int ret = PX4_ERROR; @@ -107,7 +107,7 @@ int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_adver } /* construct the new node, passing the ownership of path to it */ - uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath, priority); + uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath); /* if we didn't get a device, that's bad, free the path too */ if (node == nullptr) { @@ -142,7 +142,6 @@ int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_adver if (existing_node != nullptr && (!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) { if (is_advertiser) { - existing_node->set_priority((ORB_PRIO)priority); /* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers * could get the same instance). */ @@ -194,7 +193,7 @@ void uORB::DeviceMaster::printStatistics() return; } - PX4_INFO_RAW("%-*s INST #SUB #Q SIZE PRIO PATH\n", (int)max_topic_name_length - 2, "TOPIC NAME"); + PX4_INFO_RAW("%-*s INST #SUB #Q SIZE PATH\n", (int)max_topic_name_length - 2, "TOPIC NAME"); cur_node = first_node; diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/src/modules/uORB/uORBDeviceMaster.hpp index b696e979e3..cf4f8de4cf 100644 --- a/src/modules/uORB/uORBDeviceMaster.hpp +++ b/src/modules/uORB/uORBDeviceMaster.hpp @@ -65,7 +65,7 @@ class uORB::DeviceMaster { public: - int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, ORB_PRIO priority); + int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance); /** * Public interface for getDeviceNodeLocked(). Takes care of synchronization. diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index 1bc85814f9..bd2afb5e59 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -45,10 +45,9 @@ static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast(filp->f_priv); } uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, - ORB_PRIO priority, uint8_t queue_size) : + uint8_t queue_size) : CDev(path), _meta(meta), - _priority(priority), _instance(instance), _queue_size(queue_size) { @@ -251,10 +250,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) *(uintptr_t *)arg = (uintptr_t)this; return PX4_OK; - case ORBIOCGPRIORITY: - *(int *)arg = get_priority(); - return PX4_OK; - case ORBIOCSETQUEUESIZE: { lock(); int ret = update_queue_size(arg); @@ -351,7 +346,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle) } #ifdef ORB_COMMUNICATOR -int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta, ORB_PRIO priority) +int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta) { uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); @@ -364,7 +359,7 @@ int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta, ORB_PRIO pr /* //TODO: Check if we need this since we only unadvertise when things all shutdown and it doesn't actually remove the device -int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta, ORB_PRIO priority) +int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta) { uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); if (ch != nullptr && meta != nullptr) { @@ -401,14 +396,13 @@ uORB::DeviceNode::print_statistics(int max_topic_length) lock(); const uint8_t instance = get_instance(); - const uint8_t priority = get_priority(); const int8_t sub_count = subscriber_count(); const uint8_t queue_size = get_queue_size(); unlock(); - PX4_INFO_RAW("%-*s %2i %4i %2i %4i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count, - queue_size, get_meta()->o_size, priority, get_devname()); + PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count, + queue_size, get_meta()->o_size, get_devname()); return true; } diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 25070cc983..93f90d4934 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -56,8 +56,7 @@ class SubscriptionCallback; class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode { public: - DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, ORB_PRIO priority, - uint8_t queue_size = 1); + DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1); virtual ~DeviceNode(); // no copy, assignment, move, move assignment @@ -119,8 +118,8 @@ public: static int unadvertise(orb_advert_t handle); #ifdef ORB_COMMUNICATOR - static int16_t topic_advertised(const orb_metadata *meta, ORB_PRIO priority); - //static int16_t topic_unadvertised(const orb_metadata *meta, ORB_PRIO priority); + static int16_t topic_advertised(const orb_metadata *meta); + //static int16_t topic_unadvertised(const orb_metadata *meta); /** * processes a request for add subscription from remote @@ -199,9 +198,6 @@ public: uint8_t get_instance() const { return _instance; } - ORB_PRIO get_priority() const { return (ORB_PRIO)_priority; } - void set_priority(ORB_PRIO priority) { _priority = priority; } - /** * Copies data and the corresponding generation * from a node to the buffer provided. @@ -235,7 +231,6 @@ private: px4::atomic _generation{0}; /**< object generation count */ List _callbacks; - ORB_PRIO _priority; /**< priority of the topic */ const uint8_t _instance; /**< orb multi instance identifier */ bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */ uint8_t _queue_size; /**< maximum number of elements in the queue */ diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index 5d16ee472c..0493c68fd8 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -168,7 +168,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) } orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - ORB_PRIO priority, unsigned int queue_size) + unsigned int queue_size) { #ifdef ORB_USE_PUBLISHER_RULES @@ -195,7 +195,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, #endif /* ORB_USE_PUBLISHER_RULES */ /* open the node as an advertiser */ - int fd = node_open(meta, true, instance, priority); + int fd = node_open(meta, true, instance); if (fd == PX4_ERROR) { PX4_ERR("%s advertise failed (%i)", meta->o_name, errno); @@ -224,7 +224,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, #ifdef ORB_COMMUNICATOR // For remote systems call over and inform them - uORB::DeviceNode::topic_advertised(meta, priority); + uORB::DeviceNode::topic_advertised(meta); #endif /* ORB_COMMUNICATOR */ /* the advertiser may perform an initial publish to initialise the object */ @@ -307,11 +307,6 @@ int uORB::Manager::orb_check(int handle, bool *updated) return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); } -int uORB::Manager::orb_priority(int handle, enum ORB_PRIO *priority) -{ - return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); -} - int uORB::Manager::orb_set_interval(int handle, unsigned interval) { return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); @@ -324,7 +319,7 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval) return ret; } -int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance, ORB_PRIO priority) +int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) { char path[orb_maxpath]; int fd = -1; @@ -365,7 +360,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i ret = PX4_ERROR; if (get_device_master()) { - ret = _device_master->advertise(meta, advertiser, instance, priority); + ret = _device_master->advertise(meta, advertiser, instance); } /* it's OK if it already exists */ diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index c81b4cc735..b6e0ae784f 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -106,8 +106,7 @@ public: * Any number of advertisers may publish to a topic; publications are atomic * but co-ordination between publishers is not provided by the ORB. * - * Internally this will call orb_advertise_multi with an instance of 0 and - * default priority. + * Internally this will call orb_advertise_multi with an instance of 0. * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. @@ -124,7 +123,7 @@ public: */ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1) { - return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT, queue_size); + return orb_advertise_multi(meta, data, nullptr, queue_size); } /** @@ -149,10 +148,6 @@ public: * @param instance Pointer to an integer which will yield the instance ID (0-based) * of the publication. This is an output parameter and will be set to the newly * created instance, ie. 0 for the first advertiser, 1 for the next and so on. - * @param priority The priority of the instance. If a subscriber subscribes multiple - * instances, the priority allows the subscriber to prioritize the best - * data source as long as its available. The subscriber is responsible to check - * and handle different priorities (@see orb_priority()). * @param queue_size Maximum number of buffered elements. If this is 1, no queuing is * used. * @return PX4_ERROR on error, otherwise returns a handle @@ -161,7 +156,7 @@ public: * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return -1 and set errno to ENOENT. */ - orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, ORB_PRIO priority, + orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, unsigned int queue_size = 1); /** @@ -308,18 +303,6 @@ public: */ int orb_exists(const struct orb_metadata *meta, int instance); - /** - * Return the priority of the topic - * - * @param handle A handle returned from orb_subscribe. - * @param priority Returns the priority of this topic. This is only relevant for - * topics which are published by multiple publishers (e.g. mag0, mag1, etc.) - * and allows a subscriber to pick the topic with the highest priority, - * independent of the startup order of the associated publishers. - * @return OK on success, PX4_ERROR otherwise with errno set accordingly. - */ - int orb_priority(int handle, enum ORB_PRIO *priority); - /** * Set the minimum interval between which updates are seen for a subscription. * @@ -383,8 +366,7 @@ private: // class methods * Handles creation of the object and the initial publication for * advertisers. */ - int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr, - ORB_PRIO priority = ORB_PRIO_DEFAULT); + int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr); private: // data members static Manager *_Instance; diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp index 9f92118d3d..8de57e3ad0 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -229,7 +229,7 @@ int uORBTest::UnitTest::test_unadvertise() orb_test_s t{}; for (int i = 0; i < 4; ++i) { - _pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX); + _pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i]); if (instance_test[i] != i) { return test_fail("got wrong instance (should be %i, is %i)", i, instance_test[i]); @@ -335,12 +335,12 @@ int uORBTest::UnitTest::test_multi() orb_test_s u{}; int instance0; - _pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); + _pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0); test_note("advertised"); int instance1; - _pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); + _pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1); if (instance0 != 0) { return test_fail("mult. id0: %d", instance0); @@ -385,25 +385,6 @@ int uORBTest::UnitTest::test_multi() return test_fail("sub #1 val. mismatch: %d", u.val); } - /* test priorities */ - ORB_PRIO prio; - - if (PX4_OK != orb_priority(sfd0, &prio)) { - return test_fail("prio #0"); - } - - if (prio != ORB_PRIO_MAX) { - return test_fail("prio: %d", prio); - } - - if (PX4_OK != orb_priority(sfd1, &prio)) { - return test_fail("prio #1"); - } - - if (prio != ORB_PRIO_MIN) { - return test_fail("prio: %d", prio); - } - if (PX4_OK != latency_test(ORB_ID(orb_test), false)) { return test_fail("latency test failed"); } @@ -431,7 +412,7 @@ int uORBTest::UnitTest::pub_test_multi2_main() orb_advert_t &pub = orb_pub[i]; int idx = i; // PX4_WARN("advertise %i, t=%" PRIu64, i, hrt_absolute_time()); - pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx, ORB_PRIO_DEFAULT); + pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx); if (idx != i) { _thread_should_exit = true; @@ -550,12 +531,10 @@ int uORBTest::UnitTest::test_multi_reversed() t.val = 0; int instance2; - - _pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX); + _pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2); int instance3; - - _pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN); + _pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3); test_note("advertised");