diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index d7c0d49606..2717ad6e9f 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -62,18 +62,11 @@ FixedwingLandDetector::FixedwingLandDetector() _landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US); } -void FixedwingLandDetector::_initialize_topics() -{ - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); -} - void FixedwingLandDetector::_update_topics() { - _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); - _orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors); - _orb_update(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + _airspeedSub.update(&_airspeed); + _sensor_bias_sub.update(&_sensors); + _local_pos_sub.update(&_local_pos); } void FixedwingLandDetector::_update_params() diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 1e4c35601d..19e23338c6 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -43,6 +43,7 @@ #pragma once #include +#include #include #include #include @@ -60,7 +61,6 @@ public: FixedwingLandDetector(); protected: - void _initialize_topics() override; void _update_params() override; void _update_topics() override; @@ -87,9 +87,9 @@ private: float maxXYAccel; } _params{}; - int _airspeedSub{-1}; - int _sensor_bias_sub{-1}; - int _local_pos_sub{-1}; + uORB::Subscription _airspeedSub{ORB_ID(airspeed)}; + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position}); airspeed_s _airspeed{}; sensor_bias_s _sensors{}; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 96f7d9da58..b0a9c11f5b 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -46,7 +46,7 @@ #include #include #include -#include "uORB/topics/parameter_update.h" +#include using namespace time_literals; @@ -61,18 +61,6 @@ LandDetector::LandDetector() : LandDetector::~LandDetector() { perf_free(_cycle_perf); - - if (_armingSub >= 0) { - orb_unsubscribe(_armingSub); - } - - if (_parameterSub >= 0) { - orb_unsubscribe(_parameterSub); - } - - if (_landDetectedPub) { - orb_unadvertise(_landDetectedPub); - } } int LandDetector::start() @@ -104,18 +92,13 @@ void LandDetector::_cycle() _p_total_flight_time_high = param_find("LND_FLIGHT_T_HI"); _p_total_flight_time_low = param_find("LND_FLIGHT_T_LO"); - // Initialize uORB topics. - _armingSub = orb_subscribe(ORB_ID(actuator_armed)); - _parameterSub = orb_subscribe(ORB_ID(parameter_update)); - _initialize_topics(); - _check_params(true); _object.store(this); } _check_params(false); - _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + _armingSub.update(&_arming); _update_topics(); _update_state(); @@ -184,16 +167,9 @@ void LandDetector::_cycle() } void LandDetector::_check_params(const bool force) { - bool updated; parameter_update_s paramUpdate; - orb_check(_parameterSub, &updated); - - if (updated) { - orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); - } - - if (updated || force) { + if (_parameterSub.update(¶mUpdate) || force) { _update_params(); uint32_t flight_time; param_get(_p_total_flight_time_high, (int32_t *)&flight_time); @@ -231,25 +207,4 @@ void LandDetector::_update_state() } } -bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer) -{ - bool newData = false; - - // check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } - - if (!newData) { - return false; - } - - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } - - return true; -} - - } // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index e6252ca9d2..205ae7a0dd 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -47,8 +47,9 @@ #include #include #include -#include +#include #include +#include #include namespace land_detector @@ -97,10 +98,6 @@ public: int start(); protected: - /** - * Called once to initialize uORB topics. - */ - virtual void _initialize_topics() = 0; /** * Update uORB topics. @@ -142,13 +139,6 @@ protected: */ virtual bool _get_ground_effect_state() { return false; } - /** - * Convenience function for polling uORB subscriptions. - * - * @return true if there was new data and it was successfully copied - */ - static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer); - /** Run main land detector loop at this rate in Hz. */ static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50; @@ -185,9 +175,8 @@ private: bool _previous_arming_state{false}; ///< stores the previous _arming.armed state - int _parameterSub{ -1}; - int _armingSub{ -1}; + uORB::Subscription _parameterSub{ORB_ID(parameter_update)}; + uORB::Subscription _armingSub{ORB_ID(actuator_armed)}; }; - } // namespace land_detector diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 2a82fb4219..f531370ece 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -90,27 +90,15 @@ MulticopterLandDetector::MulticopterLandDetector() _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US); } -void MulticopterLandDetector::_initialize_topics() -{ - // subscribe to position, attitude and velocity changes - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _vehicleLocalPositionSetpointSub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - _attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude)); - _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); - _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); - _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _battery_sub = orb_subscribe(ORB_ID(battery_status)); -} - void MulticopterLandDetector::_update_topics() { - _orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - _orb_update(ORB_ID(vehicle_local_position_setpoint), _vehicleLocalPositionSetpointSub, &_vehicleLocalPositionSetpoint); - _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); - _orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); - _orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors); - _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); - _orb_update(ORB_ID(battery_status), _battery_sub, &_battery); + _vehicleLocalPositionSub.update(&_vehicleLocalPosition); + _vehicleLocalPositionSetpointSub.update(&_vehicleLocalPositionSetpoint); + _attitudeSub.update(&_vehicleAttitude); + _actuatorsSub.update(&_actuators); + _sensor_bias_sub.update(&_sensors); + _vehicle_control_mode_sub.update(&_control_mode); + _battery_sub.update(&_battery); } void MulticopterLandDetector::_update_params() @@ -128,10 +116,8 @@ void MulticopterLandDetector::_update_params() param_get(_paramHandle.altitude_max, &_params.altitude_max); param_get(_paramHandle.landSpeed, &_params.landSpeed); param_get(_paramHandle.low_thrust_threshold, &_params.low_thrust_threshold); - } - bool MulticopterLandDetector::_get_freefall_state() { if (_params.freefall_acc_threshold < 0.1f diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 78e9742ecc..1b825aac60 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -45,6 +45,7 @@ #include "LandDetector.h" #include +#include #include #include #include @@ -68,7 +69,6 @@ public: MulticopterLandDetector(); protected: - void _initialize_topics() override; void _update_params() override; void _update_topics() override; @@ -124,13 +124,13 @@ private: float low_thrust_threshold; } _params{}; - int _vehicleLocalPositionSub{ -1}; - int _vehicleLocalPositionSetpointSub{ -1}; - int _actuatorsSub{ -1}; - int _attitudeSub{ -1}; - int _sensor_bias_sub{ -1}; - int _vehicle_control_mode_sub{ -1}; - int _battery_sub{ -1}; + uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicleLocalPositionSetpointSub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _actuatorsSub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _battery_sub{ORB_ID(battery_status)}; vehicle_local_position_s _vehicleLocalPosition {}; vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint {}; diff --git a/src/modules/land_detector/RoverLandDetector.cpp b/src/modules/land_detector/RoverLandDetector.cpp index ede951f055..4a1ad6f827 100644 --- a/src/modules/land_detector/RoverLandDetector.cpp +++ b/src/modules/land_detector/RoverLandDetector.cpp @@ -46,10 +46,6 @@ namespace land_detector { -void RoverLandDetector::_initialize_topics() -{ -} - void RoverLandDetector::_update_topics() { } diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h index 1d7f4b5ebb..89950bcd2f 100644 --- a/src/modules/land_detector/RoverLandDetector.h +++ b/src/modules/land_detector/RoverLandDetector.h @@ -54,8 +54,6 @@ public: RoverLandDetector() = default; protected: - virtual void _initialize_topics() override; - virtual void _update_params() override; virtual void _update_topics() override; diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 8ab1907abe..fc1596ee2f 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -51,20 +51,12 @@ VtolLandDetector::VtolLandDetector() _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); } -void VtolLandDetector::_initialize_topics() -{ - MulticopterLandDetector::_initialize_topics(); - - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); -} - void VtolLandDetector::_update_topics() { MulticopterLandDetector::_update_topics(); - _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); - _orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + _airspeedSub.update(&_airspeed); + _vehicle_status_sub.update(&_vehicle_status); } bool VtolLandDetector::_get_maybe_landed_state() diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index b3aa67678f..6217a99604 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -41,6 +41,7 @@ #pragma once +#include #include #include @@ -55,7 +56,6 @@ public: VtolLandDetector(); protected: - void _initialize_topics() override; void _update_params() override; void _update_topics() override; bool _get_landed_state() override; @@ -70,8 +70,8 @@ private: float maxAirSpeed; } _params{}; - int _airspeedSub{-1}; - int _vehicle_status_sub{-1}; + uORB::Subscription _airspeedSub{ORB_ID(airspeed)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; airspeed_s _airspeed{}; vehicle_status_s _vehicle_status{};