diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 477d68e589..2b0ee14062 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -238,7 +238,7 @@ class Graph(object): special_cases_pub = [ ('replay', r'Replay\.cpp$', None, r'^sub\.orb_meta$'), - ('uavcan', r'sensors/.*\.cpp$', r'\bUavcanCDevSensorBridgeBase\([^{]*DEVICE_PATH,([^,)]+)', r'^_orb_topic$'), + ('uavcan', r'sensors/.*\.cpp$', None, r'^_orb_topic$'), ] special_cases_pub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d)) for a,b,c,d in special_cases_pub] diff --git a/src/drivers/barometer/bmp388/bmp388.h b/src/drivers/barometer/bmp388/bmp388.h index a3004619fb..295bae374b 100644 --- a/src/drivers/barometer/bmp388/bmp388.h +++ b/src/drivers/barometer/bmp388/bmp388.h @@ -39,7 +39,6 @@ #pragma once #include -#include #include #include #include diff --git a/src/drivers/distance_sensor/mappydot/MappyDot.cpp b/src/drivers/distance_sensor/mappydot/MappyDot.cpp index af17042a56..efac645092 100644 --- a/src/drivers/distance_sensor/mappydot/MappyDot.cpp +++ b/src/drivers/distance_sensor/mappydot/MappyDot.cpp @@ -43,7 +43,6 @@ #include #include -#include #include #include #include @@ -192,6 +191,8 @@ private: */ int get_sensor_rotation(const size_t index); + static constexpr int RANGE_FINDER_MAX_SENSORS = 12; + px4::Array _sensor_addresses {}; px4::Array _sensor_rotations {}; diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index 75e69d1cc0..a1805fc678 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -57,7 +57,6 @@ #include #include #include -#include #include #include #include @@ -144,6 +143,7 @@ private: */ int measure(); + static constexpr uint8_t RANGE_FINDER_MAX_SENSORS = 4; px4::Array _sensor_addresses {}; px4::Array _sensor_rotations {}; diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h deleted file mode 100644 index c5c6d0525a..0000000000 --- a/src/drivers/drv_baro.h +++ /dev/null @@ -1,55 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 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 drv_baro.h - * - * Barometric pressure sensor driver interface. - */ - -#ifndef _DRV_BARO_H -#define _DRV_BARO_H - -#include -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define BARO_BASE_DEVICE_PATH "/dev/baro" -#define BARO0_DEVICE_PATH "/dev/baro0" - -#include - -#endif /* _DRV_BARO_H */ diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h deleted file mode 100644 index e25a414304..0000000000 --- a/src/drivers/drv_device.h +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 drv_device.h - * - * Generic device / sensor interface. - */ - -#ifndef _DRV_DEVICE_H -#define _DRV_DEVICE_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define _DEVICEIOCBASE (0x100) -#define _DEVICEIOC(_n) (_PX4_IOC(_DEVICEIOCBASE, _n)) - -/** - * Return device ID, to enable matching of configuration parameters - * (such as compass offsets) to specific sensors - */ -#define DEVIOCGDEVICEID _DEVICEIOC(2) - -#endif /* _DRV_DEVICE_H */ diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h deleted file mode 100644 index ae4ad20999..0000000000 --- a/src/drivers/drv_mag.h +++ /dev/null @@ -1,54 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 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 Magnetometer driver interface. - */ - -#ifndef _DRV_MAG_H -#define _DRV_MAG_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define MAG_BASE_DEVICE_PATH "/dev/mag" -#define MAG0_DEVICE_PATH "/dev/mag0" -#define MAG1_DEVICE_PATH "/dev/mag1" -#define MAG2_DEVICE_PATH "/dev/mag2" - -#include - -#endif /* _DRV_MAG_H */ diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h deleted file mode 100644 index c02634c8d3..0000000000 --- a/src/drivers/drv_range_finder.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 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 Rangefinder driver interface. - */ - -#ifndef _DRV_RANGEFINDER_H -#define _DRV_RANGEFINDER_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define RANGE_FINDER_BASE_DEVICE_PATH "/dev/range_finder" -#define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" -#define RANGE_FINDER_MAX_SENSORS 12 // Maximum number of sensors on bus - -#endif /* _DRV_RANGEFINDER_H */ diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 6fd8b3fdf3..c32a1d12bc 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -44,8 +44,6 @@ #include #include -#include "drv_device.h" - /** * Sensor type definitions. * diff --git a/src/drivers/imu/mpu9250/AK8963_I2C.cpp b/src/drivers/imu/mpu9250/AK8963_I2C.cpp index a7c2f225d1..2415d7db96 100644 --- a/src/drivers/imu/mpu9250/AK8963_I2C.cpp +++ b/src/drivers/imu/mpu9250/AK8963_I2C.cpp @@ -39,7 +39,6 @@ #include #include -#include #include "mpu9250.h" #include "MPU9250_mag.h" diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.hpp b/src/drivers/magnetometer/hmc5883/HMC5883.hpp index dff0600a39..d5dc45ce37 100644 --- a/src/drivers/magnetometer/hmc5883/HMC5883.hpp +++ b/src/drivers/magnetometer/hmc5883/HMC5883.hpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include "hmc5883.h" diff --git a/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp b/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp index 8fa6e23cff..628263d163 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp @@ -39,7 +39,6 @@ #include #include -#include #include "hmc5883.h" diff --git a/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp b/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp index d1e1303349..1f3b0175b8 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp @@ -39,7 +39,6 @@ #include #include -#include #include "hmc5883.h" diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp index 7f721da2ec..fff9180378 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp @@ -49,7 +49,6 @@ #include #include -#include #include "board_config.h" #include "lis2mdl.h" diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp index 151a7b0a53..54e5a24b61 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp @@ -49,7 +49,6 @@ #include #include -#include #include "board_config.h" #include "lis2mdl.h" diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp index b39dd5664c..0540d7d8f1 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp @@ -49,7 +49,6 @@ #include #include -#include #include "board_config.h" #include "lis3mdl.h" diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp index 5d6f15cced..af9f99d776 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp @@ -49,7 +49,6 @@ #include #include -#include #include "board_config.h" #include "lis3mdl.h" diff --git a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp index 6c69505b91..e9691ae2ab 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp @@ -49,7 +49,6 @@ #include #include -#include #include "board_config.h" #include "rm3100.h" diff --git a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp index 6aa7dd031a..0ae20b0206 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp @@ -49,7 +49,6 @@ #include #include -#include #include "board_config.h" #include "rm3100.h" diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index 898c4d6e3e..4224875c76 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/src/drivers/optical_flow/px4flow/px4flow.cpp @@ -41,7 +41,6 @@ #include #include -#include #include #include #include @@ -101,8 +100,6 @@ private: uint8_t _sonar_rotation; bool _sensor_ok{false}; bool _collect_phase{false}; - int _class_instance{-1}; - int _orb_class_instance{-1}; uORB::PublicationMulti _px4flow_topic{ORB_ID(optical_flow)}; uORB::PublicationMulti _distance_sensor_topic{ORB_ID(distance_sensor)}; @@ -169,8 +166,6 @@ PX4FLOW::init() return ret; } - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -316,7 +311,7 @@ PX4FLOW::collect() _px4flow_topic.publish(report); /* publish to the distance_sensor topic as well */ - if (_class_instance == CLASS_DEVICE_PRIMARY) { + if (_distance_sensor_topic.get_instance() == 0) { distance_sensor_s distance_report{}; distance_report.timestamp = report.timestamp; distance_report.min_distance = PX4FLOW_MIN_DISTANCE; diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index 6de09c7bdb..e9f170bdfc 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -43,7 +43,7 @@ const char *const UavcanAirspeedBridge::NAME = "airspeed"; UavcanAirspeedBridge::UavcanAirspeedBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_airspeed", "/dev/uavcan/airspeed", AIRSPEED_BASE_DEVICE_PATH, ORB_ID(airspeed)), + UavcanSensorBridgeBase("uavcan_airspeed", ORB_ID(airspeed)), _sub_ias_data(node), _sub_tas_data(node), _sub_oat_data(node) @@ -51,31 +51,25 @@ UavcanAirspeedBridge::UavcanAirspeedBridge(uavcan::INode &node) : int UavcanAirspeedBridge::init() { - int res = device::CDev::init(); - - if (res < 0) { - return res; - } - - res = _sub_ias_data.start(IASCbBinder(this, &UavcanAirspeedBridge::ias_sub_cb)); + int res = _sub_ias_data.start(IASCbBinder(this, &UavcanAirspeedBridge::ias_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); return res; } - res = _sub_tas_data.start(TASCbBinder(this, &UavcanAirspeedBridge::tas_sub_cb)); + int res2 = _sub_tas_data.start(TASCbBinder(this, &UavcanAirspeedBridge::tas_sub_cb)); - if (res < 0) { - DEVICE_LOG("failed to start uavcan sub: %d", res); - return res; + if (res2 < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res2); + return res2; } - res = _sub_oat_data.start(OATCbBinder(this, &UavcanAirspeedBridge::oat_sub_cb)); + int res3 = _sub_oat_data.start(OATCbBinder(this, &UavcanAirspeedBridge::oat_sub_cb)); - if (res < 0) { - DEVICE_LOG("failed to start uavcan sub: %d", res); - return res; + if (res3 < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res3); + return res3; } return 0; diff --git a/src/drivers/uavcan/sensors/airspeed.hpp b/src/drivers/uavcan/sensors/airspeed.hpp index 6c5aa49ea3..c2f142d230 100644 --- a/src/drivers/uavcan/sensors/airspeed.hpp +++ b/src/drivers/uavcan/sensors/airspeed.hpp @@ -45,7 +45,7 @@ #include #include -class UavcanAirspeedBridge : public UavcanCDevSensorBridgeBase +class UavcanAirspeedBridge : public UavcanSensorBridgeBase { public: static const char *const NAME; diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp index cdb24d3bad..9c57d16729 100644 --- a/src/drivers/uavcan/sensors/baro.cpp +++ b/src/drivers/uavcan/sensors/baro.cpp @@ -44,24 +44,15 @@ const char *const UavcanBarometerBridge::NAME = "baro"; -#define UAVCAN_BARO_BASE_DEVICE_PATH "/dev/uavcan/baro" - UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_baro", UAVCAN_BARO_BASE_DEVICE_PATH, UAVCAN_BARO_BASE_DEVICE_PATH, - ORB_ID(sensor_baro)), + UavcanSensorBridgeBase("uavcan_baro", ORB_ID(sensor_baro)), _sub_air_pressure_data(node), _sub_air_temperature_data(node) { } int UavcanBarometerBridge::init() { - int res = device::CDev::init(); - - if (res < 0) { - return res; - } - - res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); + int res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); @@ -78,20 +69,16 @@ int UavcanBarometerBridge::init() return 0; } -void -UavcanBarometerBridge::air_temperature_sub_cb(const +void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg) { last_temperature_kelvin = msg.static_temperature; } -void -UavcanBarometerBridge::air_pressure_sub_cb(const +void UavcanBarometerBridge::air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg) { - lock(); uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); - unlock(); if (channel == nullptr) { // Something went wrong - no channel to publish on; return @@ -125,10 +112,10 @@ int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel) PX4Barometer *baro = (PX4Barometer *)channel->h_driver; - channel->class_instance = baro->get_class_instance(); + channel->instance = baro->get_instance(); - if (channel->class_instance < 0) { - PX4_ERR("UavcanBaro: Unable to get a class instance"); + if (channel->instance < 0) { + PX4_ERR("UavcanBaro: Unable to get an instance"); delete baro; channel->h_driver = nullptr; return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/baro.hpp b/src/drivers/uavcan/sensors/baro.hpp index ec2ffe933e..5600e2f5a7 100644 --- a/src/drivers/uavcan/sensors/baro.hpp +++ b/src/drivers/uavcan/sensors/baro.hpp @@ -38,12 +38,11 @@ #pragma once #include "sensor_bridge.hpp" -#include #include #include -class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase +class UavcanBarometerBridge : public UavcanSensorBridgeBase { public: static const char *const NAME; diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 8a9c279442..0d00fbb6fa 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -39,7 +39,7 @@ const char *const UavcanBatteryBridge::NAME = "battery"; UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_battery", "/dev/uavcan/battery", "/dev/battery", ORB_ID(battery_status)), + UavcanSensorBridgeBase("uavcan_battery", ORB_ID(battery_status)), ModuleParams(nullptr), _sub_battery(node), _warning(battery_status_s::BATTERY_WARNING_NONE), @@ -47,16 +47,9 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) : { } -int -UavcanBatteryBridge::init() +int UavcanBatteryBridge::init() { - int res = device::CDev::init(); - - if (res < 0) { - return res; - } - - res = _sub_battery.start(BatteryInfoCbBinder(this, &UavcanBatteryBridge::battery_sub_cb)); + int res = _sub_battery.start(BatteryInfoCbBinder(this, &UavcanBatteryBridge::battery_sub_cb)); if (res < 0) { PX4_ERR("failed to start uavcan sub: %d", res); diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index 4dfdfd010c..22608154a3 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -43,7 +43,7 @@ #include #include -class UavcanBatteryBridge : public UavcanCDevSensorBridgeBase, public ModuleParams +class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams { public: static const char *const NAME; diff --git a/src/drivers/uavcan/sensors/differential_pressure.cpp b/src/drivers/uavcan/sensors/differential_pressure.cpp index d08b7c3d60..8ce01b2715 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.cpp +++ b/src/drivers/uavcan/sensors/differential_pressure.cpp @@ -46,25 +46,17 @@ const char *const UavcanDifferentialPressureBridge::NAME = "differential_pressure"; UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_differential_pressure", "/dev/uavcan/differential_pressure", - AIRSPEED_BASE_DEVICE_PATH, - ORB_ID(differential_pressure)), + UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure)), _sub_air(node) { } int UavcanDifferentialPressureBridge::init() { - int res = device::CDev::init(); - - if (res < 0) { - return res; - } - // Initialize the calibration offset param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); - res = _sub_air.start(AirCbBinder(this, &UavcanDifferentialPressureBridge::air_sub_cb)); + int res = _sub_air.start(AirCbBinder(this, &UavcanDifferentialPressureBridge::air_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); @@ -74,25 +66,8 @@ int UavcanDifferentialPressureBridge::init() return 0; } -int UavcanDifferentialPressureBridge::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale *)arg; - _diff_pres_offset = s->offset_pa; - return PX4_OK; - } - - default: { - return CDev::ioctl(filp, cmd, arg); - } - } -} - void UavcanDifferentialPressureBridge::air_sub_cb(const - uavcan::ReceivedDataStructure - &msg) + uavcan::ReceivedDataStructure &msg) { _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN; _device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF; diff --git a/src/drivers/uavcan/sensors/differential_pressure.hpp b/src/drivers/uavcan/sensors/differential_pressure.hpp index 7c8ab6fcfd..5364940e17 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.hpp +++ b/src/drivers/uavcan/sensors/differential_pressure.hpp @@ -45,7 +45,7 @@ #include -class UavcanDifferentialPressureBridge : public UavcanCDevSensorBridgeBase +class UavcanDifferentialPressureBridge : public UavcanSensorBridgeBase { public: static const char *const NAME; @@ -57,12 +57,10 @@ public: int init() override; private: - float _diff_pres_offset {0.0f}; + float _diff_pres_offset{0.f}; math::LowPassFilter2p _filter{10.f, 1.1f}; /// Adapted from MS5525 driver - int ioctl(struct file *filp, int cmd, unsigned long arg) override; - void air_sub_cb(const uavcan::ReceivedDataStructure &msg); typedef uavcan::MethodBinder < UavcanDifferentialPressureBridge *, diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp index 166be10a7d..f9cef4c551 100644 --- a/src/drivers/uavcan/sensors/flow.cpp +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -38,7 +38,7 @@ const char *const UavcanFlowBridge::NAME = "flow"; UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_flow", "/dev/uavcan/flow", "/dev/flow", ORB_ID(optical_flow)), + UavcanSensorBridgeBase("uavcan_flow", ORB_ID(optical_flow)), _sub_flow(node) { } @@ -46,13 +46,7 @@ UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node) : int UavcanFlowBridge::init() { - int res = device::CDev::init(); - - if (res < 0) { - return res; - } - - res = _sub_flow.start(FlowCbBinder(this, &UavcanFlowBridge::flow_sub_cb)); + int res = _sub_flow.start(FlowCbBinder(this, &UavcanFlowBridge::flow_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); @@ -62,8 +56,7 @@ UavcanFlowBridge::init() return 0; } -void -UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure &msg) { optical_flow_s flow{}; diff --git a/src/drivers/uavcan/sensors/flow.hpp b/src/drivers/uavcan/sensors/flow.hpp index 40a398a48e..94aec78a0e 100644 --- a/src/drivers/uavcan/sensors/flow.hpp +++ b/src/drivers/uavcan/sensors/flow.hpp @@ -36,13 +36,12 @@ #include "sensor_bridge.hpp" #include -#include #include #include -class UavcanFlowBridge : public UavcanCDevSensorBridgeBase +class UavcanFlowBridge : public UavcanSensorBridgeBase { public: static const char *const NAME; diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 4d51d0b2bd..a4e0f97c5c 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -52,7 +52,7 @@ using namespace time_literals; const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(sensor_gps)), + UavcanSensorBridgeBase("uavcan_gnss", ORB_ID(sensor_gps)), _node(node), _sub_auxiliary(node), _sub_fix(node), @@ -76,13 +76,7 @@ UavcanGnssBridge::~UavcanGnssBridge() int UavcanGnssBridge::init() { - int res = device::CDev::init(); - - if (res < 0) { - return res; - } - - res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb)); + int res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb)); if (res < 0) { PX4_WARN("GNSS auxiliary sub failed %i", res); @@ -502,6 +496,6 @@ bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_l void UavcanGnssBridge::print_status() const { - UavcanCDevSensorBridgeBase::print_status(); + UavcanSensorBridgeBase::print_status(); perf_print_counter(_rtcm_perf); } diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index de4e11fad2..0405738cc9 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -59,7 +59,7 @@ #include "sensor_bridge.hpp" -class UavcanGnssBridge : public UavcanCDevSensorBridgeBase +class UavcanGnssBridge : public UavcanSensorBridgeBase { static constexpr unsigned ORB_TO_UAVCAN_FREQUENCY_HZ = 10; diff --git a/src/drivers/uavcan/sensors/mag.cpp b/src/drivers/uavcan/sensors/mag.cpp index 7b1f3f35dc..a30421cf64 100644 --- a/src/drivers/uavcan/sensors/mag.cpp +++ b/src/drivers/uavcan/sensors/mag.cpp @@ -44,55 +44,34 @@ const char *const UavcanMagnetometerBridge::NAME = "mag"; -#define UAVCAN_MAG_BASE_DEVICE_PATH "/dev/uavcan/mag" - UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_mag", UAVCAN_MAG_BASE_DEVICE_PATH, UAVCAN_MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)), + UavcanSensorBridgeBase("uavcan_mag", ORB_ID(sensor_mag)), _sub_mag(node), _sub_mag2(node) { } -int -UavcanMagnetometerBridge::init() +int UavcanMagnetometerBridge::init() { - int res = device::CDev::init(); - - if (res < 0) { - return res; - } - - res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); + int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { PX4_ERR("failed to start uavcan sub: %d", res); return res; } - res = _sub_mag2.start(Mag2CbBinder(this, &UavcanMagnetometerBridge::mag2_sub_cb)); + int res2 = _sub_mag2.start(Mag2CbBinder(this, &UavcanMagnetometerBridge::mag2_sub_cb)); - if (res < 0) { - PX4_ERR("failed to start uavcan sub2: %d", res); - return res; + if (res2 < 0) { + PX4_ERR("failed to start uavcan sub2: %d", res2); + return res2; } return 0; } -int -UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - default: { - return CDev::ioctl(filp, cmd, arg); - } - } -} - -void -UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure - &msg) +void UavcanMagnetometerBridge::mag_sub_cb(const + uavcan::ReceivedDataStructure &msg) { uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); @@ -121,7 +100,7 @@ UavcanMagnetometerBridge::mag2_sub_cb(const { uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); - if (channel == nullptr || channel->class_instance < 0) { + if (channel == nullptr || channel->instance < 0) { // Something went wrong - no channel to publish on; return return; } @@ -156,10 +135,10 @@ int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel) PX4Magnetometer *mag = (PX4Magnetometer *)channel->h_driver; - channel->class_instance = mag->get_class_instance(); + channel->instance = mag->get_instance(); - if (channel->class_instance < 0) { - PX4_ERR("UavcanMag: Unable to get a class instance"); + if (channel->instance < 0) { + PX4_ERR("UavcanMag: Unable to get an instance"); delete mag; channel->h_driver = nullptr; return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/mag.hpp b/src/drivers/uavcan/sensors/mag.hpp index 51425b9f9b..f668f2e2b3 100644 --- a/src/drivers/uavcan/sensors/mag.hpp +++ b/src/drivers/uavcan/sensors/mag.hpp @@ -39,12 +39,10 @@ #include "sensor_bridge.hpp" -#include - #include #include -class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase +class UavcanMagnetometerBridge : public UavcanSensorBridgeBase { public: static const char *const NAME; @@ -57,8 +55,6 @@ public: private: - int ioctl(struct file *filp, int cmd, unsigned long arg) override; - int init_driver(uavcan_bridge::Channel *channel) override; void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); diff --git a/src/drivers/uavcan/sensors/rangefinder.cpp b/src/drivers/uavcan/sensors/rangefinder.cpp index fce04e9a12..82387e922b 100644 --- a/src/drivers/uavcan/sensors/rangefinder.cpp +++ b/src/drivers/uavcan/sensors/rangefinder.cpp @@ -43,24 +43,17 @@ const char *const UavcanRangefinderBridge::NAME = "rangefinder"; UavcanRangefinderBridge::UavcanRangefinderBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_rangefinder", "/dev/uavcan/rangefinder", RANGE_FINDER_BASE_DEVICE_PATH, - ORB_ID(distance_sensor)), + UavcanSensorBridgeBase("uavcan_rangefinder", ORB_ID(distance_sensor)), _sub_range_data(node) { } int UavcanRangefinderBridge::init() { - int res = device::CDev::init(); - - if (res < 0) { - return res; - } - // Initialize min/max range from params param_get(param_find("UAVCAN_RNG_MIN"), &_range_min_m); param_get(param_find("UAVCAN_RNG_MAX"), &_range_max_m); - res = _sub_range_data.start(RangeCbBinder(this, &UavcanRangefinderBridge::range_sub_cb)); + int res = _sub_range_data.start(RangeCbBinder(this, &UavcanRangefinderBridge::range_sub_cb)); if (res < 0) { DEVICE_LOG("failed to start uavcan sub: %d", res); @@ -70,13 +63,12 @@ int UavcanRangefinderBridge::init() return 0; } -void -UavcanRangefinderBridge::range_sub_cb(const - uavcan::ReceivedDataStructure &msg) +void UavcanRangefinderBridge::range_sub_cb(const + uavcan::ReceivedDataStructure &msg) { uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); - if (channel == nullptr || channel->class_instance < 0) { + if (channel == nullptr || channel->instance < 0) { // Something went wrong - no channel to publish on; return return; } @@ -142,10 +134,10 @@ int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel) PX4Rangefinder *rangefinder = (PX4Rangefinder *)channel->h_driver; - channel->class_instance = rangefinder->get_class_instance(); + channel->instance = rangefinder->get_instance(); - if (channel->class_instance < 0) { - PX4_ERR("UavcanRangefinder: Unable to get a class instance"); + if (channel->instance < 0) { + PX4_ERR("UavcanRangefinder: Unable to get an instance"); delete rangefinder; channel->h_driver = nullptr; return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/rangefinder.hpp b/src/drivers/uavcan/sensors/rangefinder.hpp index c84e7e2aa5..470d23c8af 100644 --- a/src/drivers/uavcan/sensors/rangefinder.hpp +++ b/src/drivers/uavcan/sensors/rangefinder.hpp @@ -38,13 +38,12 @@ #pragma once #include "sensor_bridge.hpp" -#include #include #include #include -class UavcanRangefinderBridge : public UavcanCDevSensorBridgeBase +class UavcanRangefinderBridge : public UavcanSensorBridgeBase { public: static const char *const NAME; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 21bd20b19c..fbd2d070e0 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -63,21 +63,15 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List= 0) { - (void)unregister_class_devname(_class_devname, _channels[i].class_instance); - } - } - delete [] _channels; } void -UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) +UavcanSensorBridgeBase::publish(const int node_id, const void *report) { assert(report != nullptr); @@ -117,32 +111,20 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) // update device id as we now know our device node_id _device_id.devid_s.address = static_cast(node_id); - // Ask the CDev helper which class instance we can take - const int class_instance = register_class_devname(_class_devname); - - if (class_instance < 0 || class_instance >= int(_max_channels)) { - _out_of_channels = true; - DEVICE_LOG("out of class instances"); - (void)unregister_class_devname(_class_devname, class_instance); - return; - } - // Publish to the appropriate topic, abort on failure - channel->node_id = node_id; - 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->instance); - channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance); + channel->node_id = node_id; + DEVICE_LOG("channel %d instance %d ok", channel->node_id, channel->instance); if (channel->orb_advert == nullptr) { DEVICE_LOG("uORB advertise failed. Out of instances?"); - (void)unregister_class_devname(_class_devname, class_instance); *channel = uavcan_bridge::Channel(); _out_of_channels = true; return; } - DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->orb_instance); + DEVICE_LOG("channel %d instance %d ok", channel->node_id, channel->instance); } assert(channel != nullptr); @@ -150,7 +132,7 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) (void)orb_publish(_orb_topic, channel->orb_advert, report); } -uavcan_bridge::Channel *UavcanCDevSensorBridgeBase::get_channel_for_node(int node_id) +uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id) { uavcan_bridge::Channel *channel = nullptr; @@ -200,13 +182,13 @@ uavcan_bridge::Channel *UavcanCDevSensorBridgeBase::get_channel_for_node(int nod return nullptr; } - DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance); + DEVICE_LOG("channel %d instance %d ok", channel->node_id, channel->instance); } return channel; } -unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const +unsigned UavcanSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; @@ -219,7 +201,7 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const return out; } -int8_t UavcanCDevSensorBridgeBase::get_channel_index_for_node(int node_id) +int8_t UavcanSensorBridgeBase::get_channel_index_for_node(int node_id) { int8_t ch = -1; @@ -233,14 +215,14 @@ int8_t UavcanCDevSensorBridgeBase::get_channel_index_for_node(int node_id) return ch; } -void UavcanCDevSensorBridgeBase::print_status() const +void UavcanSensorBridgeBase::print_status() const { - printf("devname: %s\n", _class_devname); + printf("name: %s\n", _name); for (unsigned i = 0; i < _max_channels; i++) { if (_channels[i].node_id >= 0) { - printf("channel %d: node id %d --> class instance %d\n", - i, _channels[i].node_id, _channels[i].class_instance); + printf("channel %d: node id %d --> instance %d\n", + i, _channels[i].node_id, _channels[i].instance); } else { printf("channel %d: empty\n", i); diff --git a/src/drivers/uavcan/sensors/sensor_bridge.hpp b/src/drivers/uavcan/sensors/sensor_bridge.hpp index 2e806564b4..b71a9edcc5 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.hpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.hpp @@ -39,9 +39,8 @@ #include #include -#include #include -#include +#include #include /** @@ -87,21 +86,19 @@ public: namespace uavcan_bridge { struct Channel { - int node_id = -1; - orb_advert_t orb_advert = nullptr; - int class_instance = -1; - int orb_instance = -1; - cdev::CDev *h_driver = nullptr; + int node_id{-1}; + orb_advert_t orb_advert{nullptr}; + int instance{-1}; + void *h_driver{nullptr}; }; -} +} // namespace uavcan_bridge /** * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel. * For example, sensor_mag0, sensor_mag1, etc. */ -class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev +class UavcanSensorBridgeBase : public IUavcanSensorBridge, public device::Device { - const char *const _class_devname; const orb_id_t _orb_topic; uavcan_bridge::Channel *const _channels; bool _out_of_channels = false; @@ -110,17 +107,15 @@ protected: static constexpr unsigned DEFAULT_MAX_CHANNELS = 4; const unsigned _max_channels; - UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t orb_topic_sensor, - const unsigned max_channels = DEFAULT_MAX_CHANNELS) : - device::CDev(name, devname), - _class_devname(class_devname), + UavcanSensorBridgeBase(const char *name, const orb_id_t orb_topic_sensor, + const unsigned max_channels = DEFAULT_MAX_CHANNELS) : + Device(name), _orb_topic(orb_topic_sensor), _channels(new uavcan_bridge::Channel[max_channels]), _max_channels(max_channels) { - _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; - _device_id.devid_s.bus = 0; + set_device_bus_type(DeviceBusType_UAVCAN); + set_device_bus(0); } /** @@ -141,7 +136,7 @@ protected: uavcan_bridge::Channel *get_channel_for_node(int node_id); public: - virtual ~UavcanCDevSensorBridgeBase(); + virtual ~UavcanSensorBridgeBase(); unsigned get_num_redundant_channels() const override; diff --git a/src/lib/cdev/CDev.cpp b/src/lib/cdev/CDev.cpp index 40d9406997..ed668abf0e 100644 --- a/src/lib/cdev/CDev.cpp +++ b/src/lib/cdev/CDev.cpp @@ -42,7 +42,6 @@ #include #include -#include namespace cdev { diff --git a/src/lib/cdev/nuttx/cdev_platform.cpp b/src/lib/cdev/nuttx/cdev_platform.cpp index 87d85dc44b..097b87bacb 100644 --- a/src/lib/cdev/nuttx/cdev_platform.cpp +++ b/src/lib/cdev/nuttx/cdev_platform.cpp @@ -40,7 +40,6 @@ #include "../CDev.hpp" #include -#include "drivers/drv_device.h" #include #ifdef CONFIG_DISABLE_POLL diff --git a/src/lib/cdev/test/cdevtest_example.cpp b/src/lib/cdev/test/cdevtest_example.cpp index 1c75b2469e..51eb3f731c 100644 --- a/src/lib/cdev/test/cdevtest_example.cpp +++ b/src/lib/cdev/test/cdevtest_example.cpp @@ -43,7 +43,6 @@ #include #include -#include #include #include #include diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 7d6518a772..f54feec673 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -65,8 +65,6 @@ static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit, } PX4Accelerometer::PX4Accelerometer(uint32_t device_id, enum Rotation rotation) : - _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 150e2af61c..9fd673ae83 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -66,8 +66,8 @@ private: void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]); void UpdateClipLimit(); - uORB::PublicationMulti _sensor_pub; - uORB::PublicationMulti _sensor_fifo_pub; + uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_accel)}; + uORB::PublicationMulti _sensor_fifo_pub{ORB_ID(sensor_accel_fifo)}; uint32_t _device_id{0}; const enum Rotation _rotation; diff --git a/src/lib/drivers/barometer/PX4Barometer.cpp b/src/lib/drivers/barometer/PX4Barometer.cpp index ae5aee3cf6..652bcad905 100644 --- a/src/lib/drivers/barometer/PX4Barometer.cpp +++ b/src/lib/drivers/barometer/PX4Barometer.cpp @@ -36,11 +36,8 @@ #include -PX4Barometer::PX4Barometer(uint32_t device_id) : - CDev(nullptr), - _sensor_baro_pub{ORB_ID(sensor_baro)} +PX4Barometer::PX4Barometer(uint32_t device_id) { - _class_device_instance = register_class_devname(BARO_BASE_DEVICE_PATH); _sensor_baro_pub.advertise(); _sensor_baro_pub.get().device_id = device_id; @@ -48,10 +45,6 @@ PX4Barometer::PX4Barometer(uint32_t device_id) : PX4Barometer::~PX4Barometer() { - if (_class_device_instance != -1) { - unregister_class_devname(BARO_BASE_DEVICE_PATH, _class_device_instance); - } - _sensor_baro_pub.unadvertise(); } diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp index 0f07d3f9f2..5898a4a2af 100644 --- a/src/lib/drivers/barometer/PX4Barometer.hpp +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -33,20 +33,17 @@ #pragma once -#include #include -#include #include #include #include #include -class PX4Barometer : public cdev::CDev +class PX4Barometer { - public: PX4Barometer(uint32_t device_id); - ~PX4Barometer() override; + ~PX4Barometer(); const sensor_baro_s &get() { return _sensor_baro_pub.get(); } @@ -57,12 +54,9 @@ public: void update(const hrt_abstime ×tamp_sample, float pressure); - int get_class_instance() { return _class_device_instance; }; + int get_instance() { return _sensor_baro_pub.get_instance(); }; private: - uORB::PublicationMultiData _sensor_baro_pub; - - int _class_device_instance{-1}; - + uORB::PublicationMultiData _sensor_baro_pub{ORB_ID(sensor_baro)}; }; diff --git a/src/lib/drivers/device/CDev.cpp b/src/lib/drivers/device/CDev.cpp index 7da14b7c70..f5b9ddb8d5 100644 --- a/src/lib/drivers/device/CDev.cpp +++ b/src/lib/drivers/device/CDev.cpp @@ -42,7 +42,6 @@ #include #include -#include namespace device { @@ -77,21 +76,4 @@ out: return ret; } -int CDev::ioctl(file_t *filep, int cmd, unsigned long arg) -{ - PX4_DEBUG("CDev::ioctl"); - int ret = -ENOTTY; - - switch (cmd) { - case DEVIOCGDEVICEID: - ret = (int)_device_id.devid; - break; - - default: - break; - } - - return ret; -} - } // namespace device diff --git a/src/lib/drivers/device/CDev.hpp b/src/lib/drivers/device/CDev.hpp index 743e2b9fc6..91dd8f1cf7 100644 --- a/src/lib/drivers/device/CDev.hpp +++ b/src/lib/drivers/device/CDev.hpp @@ -75,16 +75,12 @@ public: /** * Perform an ioctl operation on the device. * - * The default implementation handles DEVIOCGDEVICEID, and otherwise - * returns -ENOTTY. Subclasses should call the default implementation - * for any command they do not handle themselves. - * * @param filep Pointer to the NuttX file structure. * @param cmd The ioctl command value. * @param arg The ioctl argument value. * @return OK on success, or -errno otherwise. */ - virtual int ioctl(file_t *filep, int cmd, unsigned long arg); + virtual int ioctl(file_t *filep, int cmd, unsigned long arg) { return -ENOTTY; } }; diff --git a/src/lib/drivers/device/Device.hpp b/src/lib/drivers/device/Device.hpp index 6179437e0d..45a314d070 100644 --- a/src/lib/drivers/device/Device.hpp +++ b/src/lib/drivers/device/Device.hpp @@ -46,7 +46,7 @@ #include #include -#include +#include #define DEVICE_LOG(FMT, ...) PX4_LOG_NAMED(_name, FMT, ##__VA_ARGS__) #define DEVICE_DEBUG(FMT, ...) PX4_LOG_NAMED_COND(_name, _debug_enabled, FMT, ##__VA_ARGS__) @@ -138,15 +138,7 @@ public: * @param arg An argument to the operation. * @return Negative errno on error, OK or positive value on success. */ - virtual int ioctl(unsigned operation, unsigned &arg) - { - switch (operation) { - case DEVIOCGDEVICEID: - return (int)_device_id.devid; - } - - return -ENODEV; - } + virtual int ioctl(unsigned operation, unsigned &arg) { return -ENODEV; } /** Device bus types for DEVID */ enum DeviceBusType { @@ -182,7 +174,8 @@ public: * * @return The bus type */ - DeviceBusType get_device_bus_type() const { return _device_id.devid_s.bus_type; } + DeviceBusType get_device_bus_type() const { return _device_id.devid_s.bus_type; } + void set_device_bus_type(DeviceBusType bus_type) { _device_id.devid_s.bus_type = bus_type; } static const char *get_device_bus_string(DeviceBusType bus) { @@ -211,6 +204,7 @@ public: * @return The bus ID */ uint8_t get_device_bus() const { return _device_id.devid_s.bus; } + void set_device_bus(uint8_t bus) { _device_id.devid_s.bus = bus; } /** * Return the bus address of the device. diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 19a775df48..f70f41c758 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -52,8 +52,6 @@ static constexpr int32_t sum(const int16_t samples[16], uint8_t len) } PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) : - _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 c7746e24d1..6c2bcaeb13 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -64,8 +64,8 @@ public: private: void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z); - uORB::PublicationMulti _sensor_pub; - uORB::PublicationMulti _sensor_fifo_pub; + uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_gyro)}; + uORB::PublicationMulti _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)}; uint32_t _device_id{0}; const enum Rotation _rotation; diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index e4154c5935..3b9c90bfca 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp @@ -37,23 +37,15 @@ #include PX4Magnetometer::PX4Magnetometer(uint32_t device_id, enum Rotation rotation) : - CDev(nullptr), - _sensor_pub{ORB_ID(sensor_mag)}, _device_id{device_id}, _rotation{rotation} { // advertise immediately to keep instance numbering in sync _sensor_pub.advertise(); - - _class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH); } PX4Magnetometer::~PX4Magnetometer() { - if (_class_device_instance != -1) { - unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_device_instance); - } - _sensor_pub.unadvertise(); } diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 8d367ed1ea..62e53f4f2f 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -33,18 +33,16 @@ #pragma once -#include #include -#include #include #include #include -class PX4Magnetometer : public cdev::CDev +class PX4Magnetometer { public: PX4Magnetometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE); - ~PX4Magnetometer() override; + ~PX4Magnetometer(); bool external() { return _external; } @@ -58,10 +56,10 @@ public: void update(const hrt_abstime ×tamp_sample, float x, float y, float z); - int get_class_instance() { return _class_device_instance; }; + int get_instance() { return _sensor_pub.get_instance(); }; private: - uORB::PublicationMulti _sensor_pub; + uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_mag)}; uint32_t _device_id{0}; const enum Rotation _rotation; @@ -71,6 +69,4 @@ private: uint32_t _error_count{0}; bool _external{false}; - - int _class_device_instance{-1}; }; diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index d7a1962e30..7191be4b0c 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -35,12 +35,8 @@ #include -PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_orientation) : - CDev(nullptr), - _distance_sensor_pub{ORB_ID(distance_sensor)} +PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_orientation) { - _class_device_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - _distance_sensor_pub.advertise(); set_device_id(device_id); @@ -49,10 +45,6 @@ PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_or PX4Rangefinder::~PX4Rangefinder() { - if (_class_device_instance != -1) { - unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_device_instance); - } - _distance_sensor_pub.unadvertise(); } diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index d5225d1bd4..c5cfa522b5 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -34,15 +34,12 @@ #pragma once #include -#include -#include #include #include #include -class PX4Rangefinder : public cdev::CDev +class PX4Rangefinder { - public: PX4Rangefinder(const uint32_t device_id, const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); @@ -64,12 +61,8 @@ public: void update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality = -1); - int get_class_instance() { return _class_device_instance; }; + int get_instance() { return _distance_sensor_pub.get_instance(); }; private: - - uORB::PublicationMultiData _distance_sensor_pub; - - int _class_device_instance{-1}; - + uORB::PublicationMultiData _distance_sensor_pub{ORB_ID(distance_sensor)}; }; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index b6a9aec15e..f128fd1e27 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -44,7 +44,6 @@ #include #include -#include #include #include #include @@ -198,8 +197,8 @@ private: uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; - uORB::PublicationMulti *_dist_pubs[RANGE_FINDER_MAX_SENSORS] {}; - uint8_t _dist_sensor_ids[RANGE_FINDER_MAX_SENSORS] {}; + uORB::PublicationMulti *_dist_pubs[ORB_MULTI_MAX_INSTANCES] {}; + uint8_t _dist_sensor_ids[ORB_MULTI_MAX_INSTANCES] {}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};