diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index a54275006af..9b35bdf62f6 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include #include diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 124c47fe533..3c308e49bf5 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -56,7 +56,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 637fbf6b2c3..362bdba7160 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, rotation) + _px4_rangefinder(DRV_DIST_DEVTYPE_SF1XX, ORB_PRIO_DEFAULT, rotation) { } diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 37712a3f1f4..9ade4619f9e 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/imu/bmi055/BMI055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp index 6534c6b7aaa..85abe6c7783 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.cpp +++ b/src/drivers/imu/bmi055/BMI055_accel.cpp @@ -47,7 +47,7 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER BMI055_accel::BMI055_accel(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device, enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) : BMI055(DRV_ACC_DEVTYPE_BMI055, "bmi055_accel", path_accel, bus_option, bus, device, spi_mode, bus_frequency, rotation), - _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), + _px4_accel(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "bmi055_accel_read")), _bad_transfers(perf_alloc(PC_COUNT, "bmi055_accel_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "bmi055_accel_bad_registers")), diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp index 3f04a673fad..35c05cbc83f 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.cpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.cpp @@ -52,7 +52,7 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS] BMI055_gyro::BMI055_gyro(I2CSPIBusOption bus_option, int bus, const char *path_gyro, uint32_t device, enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) : BMI055(DRV_GYR_DEVTYPE_BMI055, "bmi055_gyro", path_gyro, bus_option, bus, device, spi_mode, bus_frequency, rotation), - _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), + _px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")), _bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers")) diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp index 623da69890e..4a7d4d2404a 100644 --- a/src/drivers/imu/bmi088/BMI088_accel.cpp +++ b/src/drivers/imu/bmi088/BMI088_accel.cpp @@ -56,7 +56,7 @@ BMI088_accel::BMI088_accel(I2CSPIBusOption bus_option, int bus, const char *path enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) : BMI088("bmi088_accel", path_accel, bus_option, bus, DRV_ACC_DEVTYPE_BMI088, device, spi_mode, bus_frequency, rotation), - _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), + _px4_accel(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")), _bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "bmi088_accel_bad_registers")), diff --git a/src/drivers/imu/bmi088/BMI088_gyro.cpp b/src/drivers/imu/bmi088/BMI088_gyro.cpp index 2dee5d568ed..66daee80e5f 100644 --- a/src/drivers/imu/bmi088/BMI088_gyro.cpp +++ b/src/drivers/imu/bmi088/BMI088_gyro.cpp @@ -60,7 +60,7 @@ BMI088_gyro::BMI088_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) : BMI088("bmi088_gyro", path_gyro, bus_option, bus, DRV_GYR_DEVTYPE_BMI088, device, spi_mode, bus_frequency, rotation), - _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), + _px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")), _bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7bb114c282f..419b13b63dc 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,7 +80,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/drivers/safety_button/SafetyButton.hpp b/src/drivers/safety_button/SafetyButton.hpp index 9eaf6c4a1b6..79177b02656 100644 --- a/src/drivers/safety_button/SafetyButton.hpp +++ b/src/drivers/safety_button/SafetyButton.hpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include #include diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 2660eade750..8ef474d40a5 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -40,7 +40,6 @@ #include #include -#include #include #include diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp index 66b6eae31a5..d06e1cc61f4 100644 --- a/src/drivers/uavcan/uavcan_servers.hpp +++ b/src/drivers/uavcan/uavcan_servers.hpp @@ -38,7 +38,6 @@ #include #include #include -#include #include #include diff --git a/src/lib/avoidance/ObstacleAvoidance.hpp b/src/lib/avoidance/ObstacleAvoidance.hpp index 9ed6491e74a..76d76c45f08 100644 --- a/src/lib/avoidance/ObstacleAvoidance.hpp +++ b/src/lib/avoidance/ObstacleAvoidance.hpp @@ -47,7 +47,6 @@ #include #include -#include #include #include #include diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index b99e449970e..0b79c19894e 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include #include diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 1dab7a09a89..bd6811c131c 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -63,7 +63,7 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit return clip_count; } -PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) : +PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) : CDev(nullptr), _sensor_pub{ORB_ID(sensor_accel), priority}, _sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority}, @@ -72,7 +72,11 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro _device_id{device_id}, _rotation{rotation} { + // register class and advertise immediately to keep instance numbering in sync _class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + _sensor_pub.advertise(); + _sensor_integrated_pub.advertise(); + _sensor_status_pub.advertise(); } PX4Accelerometer::~PX4Accelerometer() @@ -80,6 +84,11 @@ PX4Accelerometer::~PX4Accelerometer() if (_class_device_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _class_device_instance); } + + _sensor_pub.unadvertise(); + _sensor_fifo_pub.unadvertise(); + _sensor_integrated_pub.unadvertise(); + _sensor_status_pub.unadvertise(); } int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index e7205753f31..e19c1ed9da6 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include #include @@ -49,7 +49,7 @@ class PX4Accelerometer : public cdev::CDev { public: - PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); + PX4Accelerometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); ~PX4Accelerometer() override; int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; diff --git a/src/lib/drivers/barometer/PX4Barometer.cpp b/src/lib/drivers/barometer/PX4Barometer.cpp index ac37f086026..1f13bc4e2f1 100644 --- a/src/lib/drivers/barometer/PX4Barometer.cpp +++ b/src/lib/drivers/barometer/PX4Barometer.cpp @@ -36,11 +36,12 @@ #include -PX4Barometer::PX4Barometer(uint32_t device_id, uint8_t priority) : +PX4Barometer::PX4Barometer(uint32_t device_id, ORB_PRIO priority) : CDev(nullptr), _sensor_baro_pub{ORB_ID(sensor_baro), priority} { _class_device_instance = register_class_devname(BARO_BASE_DEVICE_PATH); + _sensor_baro_pub.advertise(); _sensor_baro_pub.get().device_id = device_id; } @@ -50,6 +51,8 @@ PX4Barometer::~PX4Barometer() if (_class_device_instance != -1) { unregister_class_devname(BARO_BASE_DEVICE_PATH, _class_device_instance); } + + _sensor_baro_pub.unadvertise(); } void diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp index a2de6b51087..7589cbd0970 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, uint8_t priority = ORB_PRIO_DEFAULT); + PX4Barometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT); ~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 b969c7ecdbf..dba4784d014 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -63,7 +63,7 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit return clip_count; } -PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) : +PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) : CDev(nullptr), ModuleParams(nullptr), _sensor_pub{ORB_ID(sensor_gyro), priority}, @@ -73,7 +73,11 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r _device_id{device_id}, _rotation{rotation} { + // register class and advertise immediately to keep instance numbering in sync _class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); + _sensor_pub.advertise(); + _sensor_integrated_pub.advertise(); + _sensor_status_pub.advertise(); updateParams(); } @@ -83,6 +87,11 @@ PX4Gyroscope::~PX4Gyroscope() if (_class_device_instance != -1) { unregister_class_devname(GYRO_BASE_DEVICE_PATH, _class_device_instance); } + + _sensor_pub.unadvertise(); + _sensor_fifo_pub.unadvertise(); + _sensor_integrated_pub.unadvertise(); + _sensor_status_pub.unadvertise(); } int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 2377f432d32..a7f05bce792 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include #include @@ -49,7 +49,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams { public: - PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); + PX4Gyroscope(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); ~PX4Gyroscope() override; int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index f8cd38f2d93..11196b7384e 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp @@ -36,13 +36,15 @@ #include -PX4Magnetometer::PX4Magnetometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) : +PX4Magnetometer::PX4Magnetometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) : CDev(nullptr), _sensor_mag_pub{ORB_ID(sensor_mag), priority}, _rotation{rotation} { _class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + _sensor_mag_pub.advertise(); + _sensor_mag_pub.get().device_id = device_id; _sensor_mag_pub.get().scaling = 1.0f; _sensor_mag_pub.get().temperature = NAN; @@ -53,6 +55,8 @@ PX4Magnetometer::~PX4Magnetometer() if (_class_device_instance != -1) { unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_device_instance); } + + _sensor_mag_pub.unadvertise(); } int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 946315c6c5d..563e972ca2a 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -45,7 +45,7 @@ class PX4Magnetometer : public cdev::CDev { public: - PX4Magnetometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); + PX4Magnetometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); ~PX4Magnetometer() override; int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index bb8a98ed0ae..c058d81839e 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -35,13 +35,20 @@ #include -PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t priority, const uint8_t device_orientation) : +PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const ORB_PRIO priority, const uint8_t device_orientation) : _distance_sensor_pub{ORB_ID(distance_sensor), priority} { + _distance_sensor_pub.advertise(); + set_device_id(device_id); set_orientation(device_orientation); } +PX4Rangefinder::~PX4Rangefinder() +{ + _distance_sensor_pub.unadvertise(); +} + void PX4Rangefinder::set_device_type(uint8_t device_type) { // TODO: range finders should have device ids diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index a405bc75e38..f5f8b118113 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -44,9 +44,9 @@ class PX4Rangefinder public: PX4Rangefinder(const uint32_t device_id, - const uint8_t priority = ORB_PRIO_DEFAULT, + const ORB_PRIO priority = ORB_PRIO_DEFAULT, const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - ~PX4Rangefinder() = default; + ~PX4Rangefinder(); void print_status(); diff --git a/src/lib/flight_tasks/FlightTasks.hpp b/src/lib/flight_tasks/FlightTasks.hpp index 54c1af61543..919e7be6103 100644 --- a/src/lib/flight_tasks/FlightTasks.hpp +++ b/src/lib/flight_tasks/FlightTasks.hpp @@ -44,7 +44,7 @@ #include "FlightTask.hpp" #include "FlightTasks_generated.hpp" #include -#include +#include #include #include #include diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index f3523092a4e..f5cb140e1fa 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -35,7 +35,7 @@ #include -#include +#include #include using namespace time_literals; diff --git a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp index a28de767634..cfa65292778 100644 --- a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp +++ b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3b2cf1658d4..b1ab8a8ad14 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -46,7 +46,6 @@ // publications #include -#include #include #include #include diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 579f4584194..50819f2b44a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -556,7 +556,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub if (device_id[cur_accel] != 0) { // Get priority - int32_t prio; + ORB_PRIO prio = ORB_PRIO_UNINITIALIZED; orb_priority(worker_data.subs[cur_accel], &prio); if (prio > device_prio_max) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 044ba15211c..71f39de3b61 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -332,7 +332,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) if (worker_data.device_id[cur_gyro] != 0) { // Get priority - int32_t prio; + ORB_PRIO prio = ORB_PRIO_UNINITIALIZED; orb_priority(worker_data.gyro_sensor_sub[cur_gyro], &prio); if (prio > device_prio_max) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 1a45c10e193..ed149b869e9 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -639,7 +639,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma if (device_ids[cur_mag] != 0) { // Get priority - int32_t prio; + ORB_PRIO prio = ORB_PRIO_UNINITIALIZED; orb_priority(worker_data.sub_mag[cur_mag], &prio); if (prio > device_prio_max) { diff --git a/src/modules/events/send_event.h b/src/modules/events/send_event.h index dcff99775b4..267ab9fc824 100644 --- a/src/modules/events/send_event.h +++ b/src/modules/events/send_event.h @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/modules/events/status_display.h b/src/modules/events/status_display.h index 1324d13cece..be3e6d062bb 100644 --- a/src/modules/events/status_display.h +++ b/src/modules/events/status_display.h @@ -42,7 +42,7 @@ #include -#include +#include #include #include #include diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp index 110cf1097d3..98cef3f957c 100644 --- a/src/modules/load_mon/load_mon.cpp +++ b/src/modules/load_mon/load_mon.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include diff --git a/src/modules/logger/log_writer_mavlink.h b/src/modules/logger/log_writer_mavlink.h index 6b6f3681980..c80a5496231 100644 --- a/src/modules/logger/log_writer_mavlink.h +++ b/src/modules/logger/log_writer_mavlink.h @@ -34,7 +34,7 @@ #pragma once #include -#include +#include #include #include diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 4362ff5b234..35eb1fc028b 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 695a7c97f02..7d2796fd0c8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include "mavlink_receiver.h" #include "mavlink_main.h" diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 09f60d570e2..ed9068cb6d6 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -71,7 +71,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index b185b67e0c8..148154ee110 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -46,7 +46,6 @@ #include "mavlink_bridge_header.h" #include -#include #include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8d803e799f0..c0731403ea3 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -54,7 +54,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mavlink/tune_publisher.h b/src/modules/mavlink/tune_publisher.h index 42095e0da73..4ec2fa3b65c 100644 --- a/src/modules/mavlink/tune_publisher.h +++ b/src/modules/mavlink/tune_publisher.h @@ -33,7 +33,7 @@ #pragma once -#include +#include #include #include diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e82dece0fce..2ad528ec54b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -54,7 +54,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/muorb/test/muorb_test_example.cpp b/src/modules/muorb/test/muorb_test_example.cpp index c5bc6dae891..74731017f45 100644 --- a/src/modules/muorb/test/muorb_test_example.cpp +++ b/src/modules/muorb/test/muorb_test_example.cpp @@ -44,7 +44,6 @@ #include #include #include -#include #include px4::AppState MuorbTestExample::appState; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b44ff7db7c4..5aa9a092143 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index ff7950d3fba..ed78b0a8186 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -154,7 +154,7 @@ void VotedSensorsUpdate::parametersUpdate() _gyro.enabled[driver_index] = (device_enabled == 1); if (!_gyro.enabled[driver_index]) { - _gyro.priority[driver_index] = 0; + _gyro.priority[driver_index] = ORB_PRIO_UNINITIALIZED; } gyro_calibration_s gscale{}; @@ -250,7 +250,7 @@ void VotedSensorsUpdate::parametersUpdate() _accel.enabled[driver_index] = (device_enabled == 1); if (!_accel.enabled[driver_index]) { - _accel.priority[driver_index] = 0; + _accel.priority[driver_index] = ORB_PRIO_UNINITIALIZED; } accel_calibration_s ascale{}; @@ -387,7 +387,7 @@ void VotedSensorsUpdate::parametersUpdate() // the mags that were published after the initial parameterUpdate // would be given the priority even if disabled. Reset it to 0 in this case if (!_mag.enabled[topic_instance]) { - _mag.priority[topic_instance] = 0; + _mag.priority[topic_instance] = ORB_PRIO_UNINITIALIZED; } mag_calibration_s mscale{}; @@ -702,14 +702,16 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); // reduce priority of failed sensor to the minimum - sensor.priority[failover_index] = 1; + sensor.priority[failover_index] = ORB_PRIO_MIN; PX4_ERR("Sensor %s #%i failed. Reconfiguring sensor priorities.", sensor_name, failover_index); int ctr_valid = 0; for (uint8_t i = 0; i < sensor.subscription_count; i++) { - if (sensor.priority[i] > 1) { ctr_valid++; } + if (sensor.priority[i] > ORB_PRIO_MIN) { + ctr_valid++; + } PX4_WARN("Remaining sensors after failover event %u: %s #%u priority: %u", failover_index, sensor_name, i, sensor.priority[i]); diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 782ca94ef81..0b0738c3132 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -54,7 +54,6 @@ #include #include -#include #include #include #include @@ -156,7 +155,7 @@ private: uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */ DataValidatorGroup voter{1}; unsigned int last_failover_count{0}; - uint8_t priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */ + ORB_PRIO priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */ 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, true}; diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index 0269cfb6681..9b04e19377e 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -35,7 +35,7 @@ #include "temperature_calibration/temperature_calibration.h" -#include +#include #include #include diff --git a/src/modules/temperature_compensation/temperature_calibration/task.cpp b/src/modules/temperature_compensation/temperature_calibration/task.cpp index 2bcfc0151f0..a361b7b5939 100644 --- a/src/modules/temperature_compensation/temperature_calibration/task.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/task.cpp @@ -39,7 +39,7 @@ * @author Beat Küng */ -#include +#include #include #include #include diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index f963d0ba2e5..b06add28a68 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -43,8 +43,6 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat ORBSet.hpp Publication.hpp PublicationMulti.hpp - PublicationQueued.hpp - PublicationQueuedMulti.hpp Subscription.cpp Subscription.hpp SubscriptionCallback.hpp diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 6076504fc73..90f98603ba7 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -40,16 +40,47 @@ #include #include + #include +#include "uORBDeviceNode.hpp" +#include namespace uORB { +class PublicationBase +{ +public: + + bool advertised() const { return _handle != nullptr; } + + bool unadvertise() { return (DeviceNode::unadvertise(_handle) == PX4_OK); } + + orb_id_t get_topic() const { return get_orb_meta(_orb_id); } + +protected: + + PublicationBase(ORB_ID id) : _orb_id(id) {} + + ~PublicationBase() + { + if (_handle != nullptr) { + // don't automatically unadvertise queued publications (eg vehicle_command) + if (static_cast(_handle)->get_queue_size() == 1) { + unadvertise(); + } + } + } + + orb_advert_t _handle{nullptr}; + const ORB_ID _orb_id; +}; + /** - * Base publication wrapper class + * uORB publication wrapper class */ -template -class Publication +template +class Publication : public PublicationBase { public: @@ -58,8 +89,17 @@ public: * * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. */ - Publication(const orb_metadata *meta) : _meta(meta) {} - ~Publication() { orb_unadvertise(_handle); } + Publication(ORB_ID id) : PublicationBase(id) {} + Publication(const orb_metadata *meta) : PublicationBase(static_cast(meta->o_id)) {} + + bool advertise() + { + if (!advertised()) { + _handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE); + } + + return advertised(); + } /** * Publish the struct @@ -67,25 +107,12 @@ public: */ bool publish(const T &data) { - if (_handle != nullptr) { - return (orb_publish(_meta, _handle, &data) == PX4_OK); - - } else { - orb_advert_t handle = orb_advertise(_meta, &data); - - if (handle != nullptr) { - _handle = handle; - return true; - } + if (!advertised()) { + advertise(); } - return false; + return (DeviceNode::publish(get_topic(), _handle, &data) == PX4_OK); } - -protected: - const orb_metadata *_meta; - - orb_advert_t _handle{nullptr}; }; /** @@ -100,8 +127,8 @@ public: * * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. */ + PublicationData(ORB_ID id) : Publication(id) {} PublicationData(const orb_metadata *meta) : Publication(meta) {} - ~PublicationData() = default; T &get() { return _data; } void set(const T &data) { _data = data; } @@ -118,4 +145,10 @@ private: T _data{}; }; + +template +using PublicationQueued = Publication; + + + } // namespace uORB diff --git a/src/modules/uORB/PublicationMulti.hpp b/src/modules/uORB/PublicationMulti.hpp index cdd4257a80b..d8d2dfab0f9 100644 --- a/src/modules/uORB/PublicationMulti.hpp +++ b/src/modules/uORB/PublicationMulti.hpp @@ -42,14 +42,16 @@ #include #include +#include "Publication.hpp" + namespace uORB { /** * Base publication multi wrapper class */ -template -class PublicationMulti +template +class PublicationMulti : public PublicationBase { public: @@ -59,12 +61,25 @@ public: * @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(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : - _meta(meta), + PublicationMulti(ORB_ID id, ORB_PRIO priority = ORB_PRIO_DEFAULT) : + PublicationBase(id), _priority(priority) {} - ~PublicationMulti() { orb_unadvertise(_handle); } + PublicationMulti(const orb_metadata *meta, ORB_PRIO priority = ORB_PRIO_DEFAULT) : + PublicationBase(static_cast(meta->o_id)), + _priority(priority) + {} + + bool advertise() + { + if (!advertised()) { + int instance = 0; + _handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, _priority, QSIZE); + } + + return advertised(); + } /** * Publish the struct @@ -72,28 +87,15 @@ public: */ bool publish(const T &data) { - if (_handle != nullptr) { - return (orb_publish(_meta, _handle, &data) == PX4_OK); - - } else { - int instance = 0; - orb_advert_t handle = orb_advertise_multi(_meta, &data, &instance, _priority); - - if (handle != nullptr) { - _handle = handle; - return true; - } + if (!advertised()) { + advertise(); } - return false; + return (orb_publish(get_topic(), _handle, &data) == PX4_OK); } protected: - const orb_metadata *_meta; - - orb_advert_t _handle{nullptr}; - - const uint8_t _priority; + const ORB_PRIO _priority; }; /** @@ -109,12 +111,13 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. * @param priority The priority for multi pub */ - PublicationMultiData(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : + 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() = default; - T &get() { return _data; } void set(const T &data) { _data = data; } @@ -130,4 +133,8 @@ private: T _data{}; }; + +template +using PublicationQueuedMulti = PublicationMulti; + } // namespace uORB diff --git a/src/modules/uORB/PublicationQueued.hpp b/src/modules/uORB/PublicationQueued.hpp deleted file mode 100644 index 62bf00388ec..00000000000 --- a/src/modules/uORB/PublicationQueued.hpp +++ /dev/null @@ -1,94 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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. - * - ****************************************************************************/ - -/** - * @file PublicationQueued.hpp - * - */ - -#pragma once - -#include -#include -#include - -namespace uORB -{ - -/** - * Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH) - */ -template -class PublicationQueued -{ -public: - - /** - * Constructor - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. - */ - PublicationQueued(const orb_metadata *meta) : _meta(meta) {} - ~PublicationQueued() - { - //orb_unadvertise(_handle); - } - - /** - * Publish the struct - * @param data The uORB message struct we are updating. - */ - bool publish(const T &data) - { - if (_handle != nullptr) { - return (orb_publish(_meta, _handle, &data) == PX4_OK); - - } else { - orb_advert_t handle = orb_advertise_queue(_meta, &data, T::ORB_QUEUE_LENGTH); - - if (handle != nullptr) { - _handle = handle; - return true; - } - } - - return false; - } - -protected: - const orb_metadata *_meta; - - orb_advert_t _handle{nullptr}; -}; - -} // namespace uORB diff --git a/src/modules/uORB/PublicationQueuedMulti.hpp b/src/modules/uORB/PublicationQueuedMulti.hpp deleted file mode 100644 index 810277e6d7d..00000000000 --- a/src/modules/uORB/PublicationQueuedMulti.hpp +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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 - * 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. - * - ****************************************************************************/ - -/** - * @file PublicationQueuedMulti.hpp - * - */ - -#pragma once - -#include -#include -#include - -namespace uORB -{ - -/** - * Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH) - */ -template -class PublicationQueuedMulti -{ -public: - - /** - * Constructor - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. - */ - PublicationQueuedMulti(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : - _meta(meta), - _priority(priority) - {} - - ~PublicationQueuedMulti() - { - //orb_unadvertise(_handle); - } - - /** - * Publish the struct - * @param data The uORB message struct we are updating. - */ - bool publish(const T &data) - { - if (_handle != nullptr) { - return (orb_publish(_meta, _handle, &data) == PX4_OK); - - } else { - int instance = 0; - orb_advert_t handle = orb_advertise_multi_queue(_meta, &data, &instance, _priority, T::ORB_QUEUE_LENGTH); - - if (handle != nullptr) { - _handle = handle; - return true; - } - } - - return false; - } - -protected: - const orb_metadata *_meta; - - orb_advert_t _handle{nullptr}; - - const uint8_t _priority; -}; - -} // namespace uORB diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index f4921c85a71..a1c3d23f068 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -50,14 +50,13 @@ 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, - int priority) +orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, ORB_PRIO priority) { return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority); } orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance, - int priority, unsigned int queue_size) + ORB_PRIO priority, unsigned int queue_size) { return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority, queue_size); } @@ -113,7 +112,7 @@ int orb_group_count(const struct orb_metadata *meta) return instance; } -int orb_priority(int handle, int32_t *priority) +int orb_priority(int handle, enum ORB_PRIO *priority) { return uORB::Manager::get_instance()->orb_priority(handle, priority); } diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 462e5edc337..766dc387979 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -152,13 +152,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, - int priority) __EXPORT; + enum ORB_PRIO priority) __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, - int priority, unsigned int queue_size) __EXPORT; + enum ORB_PRIO priority, unsigned int queue_size) __EXPORT; /** * @see uORB::Manager::orb_unadvertise() @@ -179,8 +179,7 @@ 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, - int priority) + int *instance, enum ORB_PRIO priority) { if (!*handle) { *handle = orb_advertise_multi(meta, data, instance, priority); @@ -237,7 +236,7 @@ extern int orb_group_count(const struct orb_metadata *meta) __EXPORT; /** * @see uORB::Manager::orb_priority() */ -extern int orb_priority(int handle, int32_t *priority) __EXPORT; +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 9bb2fe015b7..d9afb719a51 100644 --- a/src/modules/uORB/uORBCommon.hpp +++ b/src/modules/uORB/uORBCommon.hpp @@ -47,7 +47,7 @@ static constexpr unsigned orb_maxpath = 64; struct orb_advertdata { const struct orb_metadata *meta; int *instance; - int priority; + ORB_PRIO priority; }; } diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index 0f3c31fcd0b..accfc074e9d 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -58,8 +58,7 @@ uORB::DeviceMaster::~DeviceMaster() px4_sem_destroy(&_lock); } -int -uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority) +int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, ORB_PRIO priority) { int ret = PX4_ERROR; diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/src/modules/uORB/uORBDeviceMaster.hpp index 203513813ea..90e9c456f01 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, int priority); + int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, ORB_PRIO priority); /** * Public interface for getDeviceNodeLocked(). Takes care of synchronization. diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index 803cb4fc043..a817a39163a 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -55,11 +55,11 @@ uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(cdev::file_t *fil } uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, - uint8_t priority, uint8_t queue_size) : + ORB_PRIO priority, uint8_t queue_size) : CDev(path), _meta(meta), - _instance(instance), _priority(priority), + _instance(instance), _queue_size(queue_size) { } @@ -432,7 +432,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle) } #ifdef ORB_COMMUNICATOR -int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta, int priority) +int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta, ORB_PRIO priority) { uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); @@ -445,7 +445,7 @@ int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta, int priorit /* //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, int priority) +int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta, ORB_PRIO priority) { uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); if (ch != nullptr && meta != nullptr) { diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index e3800c61ea7..22337e5377b 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -55,7 +55,7 @@ class SubscriptionCallback; class uORB::DeviceNode : public cdev::CDev, public ListNode { public: - DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t priority, + DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, ORB_PRIO priority, uint8_t queue_size = 1); virtual ~DeviceNode(); @@ -116,8 +116,8 @@ public: static int unadvertise(orb_advert_t handle); #ifdef ORB_COMMUNICATOR - static int16_t topic_advertised(const orb_metadata *meta, int priority); - //static int16_t topic_unadvertised(const orb_metadata *meta, int priority); + static int16_t topic_advertised(const orb_metadata *meta, ORB_PRIO priority); + //static int16_t topic_unadvertised(const orb_metadata *meta, ORB_PRIO priority); /** * processes a request for add subscription from remote @@ -263,8 +263,8 @@ private: uint32_t _lost_messages = 0; /**< nr of lost messages for all subscribers. If two subscribers lose the same message, it is counted as two. */ + ORB_PRIO _priority; /**< priority of the topic */ const uint8_t _instance; /**< orb multi instance identifier */ - uint8_t _priority; /**< priority of the topic */ bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */ uint8_t _queue_size; /**< maximum number of elements in the queue */ int8_t _subscriber_count{0}; diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index 81e15ecfd61..5d16ee472cb 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, - int priority, unsigned int queue_size) + ORB_PRIO priority, unsigned int queue_size) { #ifdef ORB_USE_PUBLISHER_RULES @@ -227,12 +227,14 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, uORB::DeviceNode::topic_advertised(meta, priority); #endif /* ORB_COMMUNICATOR */ - /* the advertiser must perform an initial publish to initialise the object */ - result = orb_publish(meta, advertiser, data); + /* the advertiser may perform an initial publish to initialise the object */ + if (data != nullptr) { + result = orb_publish(meta, advertiser, data); - if (result == PX4_ERROR) { - PX4_WARN("orb_publish failed"); - return nullptr; + if (result == PX4_ERROR) { + PX4_ERR("orb_publish failed %s", meta->o_name); + return nullptr; + } } return advertiser; @@ -305,7 +307,7 @@ 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, int32_t *priority) +int uORB::Manager::orb_priority(int handle, enum ORB_PRIO *priority) { return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); } @@ -322,23 +324,7 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval) return ret; } -int uORB::Manager::node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority) -{ - int ret = PX4_ERROR; - - if (get_device_master()) { - ret = _device_master->advertise(meta, is_advertiser, instance, priority); - } - - /* it's PX4_OK if it already exists */ - if ((PX4_OK != ret) && (EEXIST == errno)) { - ret = PX4_OK; - } - - return ret; -} - -int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance, int priority) +int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance, ORB_PRIO priority) { char path[orb_maxpath]; int fd = -1; @@ -376,36 +362,32 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i /* we may need to advertise the node... */ if (fd < 0) { - /* try to create the node */ - ret = node_advertise(meta, advertiser, instance, priority); + ret = PX4_ERROR; + + if (get_device_master()) { + ret = _device_master->advertise(meta, advertiser, instance, priority); + } + + /* it's OK if it already exists */ + if ((ret != PX4_OK) && (EEXIST == errno)) { + ret = PX4_OK; + } if (ret == PX4_OK) { - /* update the path, as it might have been updated during the node_advertise call */ + /* update the path, as it might have been updated during the node advertise call */ ret = uORB::Utils::node_mkpath(path, meta, instance); - if (ret != PX4_OK) { + /* on success, try to open again */ + if (ret == PX4_OK) { + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + + } else { errno = -ret; return PX4_ERROR; } } - - /* on success, try to open again */ - if (ret == PX4_OK) { - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); - } } - /* - else if (advertiser) { - * We have a valid fd and are an advertiser. - * This can happen if the topic is already subscribed/published, and orb_advertise() is called, - * where instance==nullptr. - * We would need to set the priority here (via px4_ioctl(fd, ...) and a new IOCTL), but orb_advertise() - * uses ORB_PRIO_DEFAULT, and a subscriber also creates the node with ORB_PRIO_DEFAULT. So we don't need - * to do anything here. - } - */ - if (fd < 0) { errno = EIO; return PX4_ERROR; diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 1ea0f7a82f0..c81b4cc7351 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -161,8 +161,8 @@ 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, - int priority, unsigned int queue_size = 1); + orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, ORB_PRIO priority, + unsigned int queue_size = 1); /** * Unadvertise a topic. @@ -318,7 +318,7 @@ public: * 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, int32_t *priority); + int orb_priority(int handle, enum ORB_PRIO *priority); /** * Set the minimum interval between which updates are seen for a subscription. @@ -376,12 +376,6 @@ public: #endif /* ORB_COMMUNICATOR */ private: // class methods - /** - * Advertise a node; don't consider it an error if the node has - * already been advertised. - */ - int node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance = nullptr, - int priority = ORB_PRIO_DEFAULT); /** * Common implementation for orb_advertise and orb_subscribe. @@ -390,7 +384,7 @@ private: // class methods * advertisers. */ int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr, - int priority = ORB_PRIO_DEFAULT); + ORB_PRIO priority = ORB_PRIO_DEFAULT); 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 d2c7ae97e7f..9f92118d3de 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -386,7 +386,7 @@ int uORBTest::UnitTest::test_multi() } /* test priorities */ - int prio; + ORB_PRIO prio; if (PX4_OK != orb_priority(sfd0, &prio)) { return test_fail("prio #0"); diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index 01b8b79985d..80fcb04ce4c 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -39,7 +39,7 @@ */ #include "input_mavlink.h" -#include +#include #include #include #include diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index b4a6ce3f0d9..7a7fde55798 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -41,7 +41,7 @@ #include "output.h" -#include +#include #include namespace vmount diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 2816ccd0cbd..c8ee3a14563 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -48,7 +48,7 @@ */ #include "vtol_att_control_main.h" #include -#include +#include using namespace matrix; diff --git a/src/systemcmds/led_control/led_control.cpp b/src/systemcmds/led_control/led_control.cpp index f8ea389a7db..f60625889d9 100644 --- a/src/systemcmds/led_control/led_control.cpp +++ b/src/systemcmds/led_control/led_control.cpp @@ -45,7 +45,7 @@ #include #include -#include +#include #include static void usage(); diff --git a/src/systemcmds/motor_test/motor_test.cpp b/src/systemcmds/motor_test/motor_test.cpp index e4205a23637..e52e25987e7 100644 --- a/src/systemcmds/motor_test/motor_test.cpp +++ b/src/systemcmds/motor_test/motor_test.cpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include extern "C" __EXPORT int motor_test_main(int argc, char *argv[]);