From 0a332059095d944688ce371adcc4e9383ee2eff7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 29 Jun 2019 11:06:32 -0400 Subject: [PATCH] landing_target_estimator: update to uORB Subscription and Publication --- .../LandingTargetEstimator.cpp | 80 +++---------------- .../LandingTargetEstimator.h | 60 ++++++-------- 2 files changed, 37 insertions(+), 103 deletions(-) diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index d28524056233..61f48d427841 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -51,18 +51,7 @@ namespace landing_target_estimator { -LandingTargetEstimator::LandingTargetEstimator() : - _targetPosePub(nullptr), - _targetInnovationsPub(nullptr), - _paramHandle(), - _vehicleLocalPosition_valid(false), - _vehicleAttitude_valid(false), - _sensorBias_valid(false), - _new_irlockReport(false), - _estimator_initialized(false), - _faulty(false), - _last_predict(0), - _last_update(0) +LandingTargetEstimator::LandingTargetEstimator() { _paramHandle.acc_unc = param_find("LTEST_ACC_UNC"); _paramHandle.meas_unc = param_find("LTEST_MEAS_UNC"); @@ -72,9 +61,6 @@ LandingTargetEstimator::LandingTargetEstimator() : _paramHandle.scale_x = param_find("LTEST_SCALE_X"); _paramHandle.scale_y = param_find("LTEST_SCALE_Y"); - // Initialize uORB topics. - _initialize_topics(); - _check_params(true); } @@ -220,12 +206,7 @@ void LandingTargetEstimator::update() _target_pose.abs_pos_valid = false; } - if (_targetPosePub == nullptr) { - _targetPosePub = orb_advertise(ORB_ID(landing_target_pose), &_target_pose); - - } else { - orb_publish(ORB_ID(landing_target_pose), _targetPosePub, &_target_pose); - } + _targetPosePub.publish(_target_pose); _last_update = hrt_absolute_time(); _last_predict = _last_update; @@ -241,25 +222,17 @@ void LandingTargetEstimator::update() _target_innovations.innov_y = innov_y; _target_innovations.innov_cov_y = innov_cov_y; - if (_targetInnovationsPub == nullptr) { - _targetInnovationsPub = orb_advertise(ORB_ID(landing_target_innovations), &_target_innovations); - - } else { - orb_publish(ORB_ID(landing_target_innovations), _targetInnovationsPub, &_target_innovations); - } + _targetInnovationsPub.publish(_target_innovations); } - } void LandingTargetEstimator::_check_params(const bool force) { - bool updated; - parameter_update_s paramUpdate; - - orb_check(_parameterSub, &updated); + bool updated = _parameterSub.updated(); if (updated) { - orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + parameter_update_s paramUpdate; + _parameterSub.copy(¶mUpdate); } if (updated || force) { @@ -267,44 +240,13 @@ void LandingTargetEstimator::_check_params(const bool force) } } -void LandingTargetEstimator::_initialize_topics() -{ - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude)); - _sensorBiasSub = orb_subscribe(ORB_ID(sensor_bias)); - _irlockReportSub = orb_subscribe(ORB_ID(irlock_report)); - _parameterSub = orb_subscribe(ORB_ID(parameter_update)); -} - void LandingTargetEstimator::_update_topics() { - _vehicleLocalPosition_valid = _orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, - &_vehicleLocalPosition); - _vehicleAttitude_valid = _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); - _sensorBias_valid = _orb_update(ORB_ID(sensor_bias), _sensorBiasSub, &_sensorBias); + _vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition); + _vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude); + _sensorBias_valid = _sensorBiasSub.update(&_sensorBias); - _new_irlockReport = _orb_update(ORB_ID(irlock_report), _irlockReportSub, &_irlockReport); -} - - -bool LandingTargetEstimator::_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; + _new_irlockReport = _irlockReportSub.update(&_irlockReport); } void LandingTargetEstimator::_update_params() @@ -313,9 +255,11 @@ void LandingTargetEstimator::_update_params() param_get(_paramHandle.meas_unc, &_params.meas_unc); param_get(_paramHandle.pos_unc_init, &_params.pos_unc_init); param_get(_paramHandle.vel_unc_init, &_params.vel_unc_init); + int32_t mode = 0; param_get(_paramHandle.mode, &mode); _params.mode = (TargetMode)mode; + param_get(_paramHandle.scale_x, &_params.scale_x); param_get(_paramHandle.scale_y, &_params.scale_y); } diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index 7a8597951755..45062a24085d 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -45,7 +45,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -75,10 +76,6 @@ class LandingTargetEstimator void update(); protected: - /* - * Called once to initialize uORB topics. - */ - void _initialize_topics(); /* * Update uORB topics. @@ -90,23 +87,16 @@ class LandingTargetEstimator */ void _update_params(); - /* - * 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); - /* timeout after which filter is reset if target not seen */ static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000; - orb_advert_t _targetPosePub; - struct landing_target_pose_s _target_pose; + uORB::Publication _targetPosePub{ORB_ID(landing_target_pose)}; + landing_target_pose_s _target_pose{}; - orb_advert_t _targetInnovationsPub; - struct landing_target_innovations_s _target_innovations; + uORB::Publication _targetInnovationsPub{ORB_ID(landing_target_innovations)}; + landing_target_innovations_s _target_innovations{}; - int _parameterSub; + uORB::Subscription _parameterSub{ORB_ID(parameter_update)}; private: @@ -138,31 +128,31 @@ class LandingTargetEstimator float scale_y; } _params; - int _vehicleLocalPositionSub; - int _attitudeSub; - int _sensorBiasSub; - int _irlockReportSub; + uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _sensorBiasSub{ORB_ID(sensor_bias)}; + uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)}; - struct vehicle_local_position_s _vehicleLocalPosition; - struct vehicle_attitude_s _vehicleAttitude; - struct sensor_bias_s _sensorBias; - struct irlock_report_s _irlockReport; + vehicle_local_position_s _vehicleLocalPosition{}; + vehicle_attitude_s _vehicleAttitude{}; + sensor_bias_s _sensorBias{}; + irlock_report_s _irlockReport{}; // keep track of which topics we have received - bool _vehicleLocalPosition_valid; - bool _vehicleAttitude_valid; - bool _sensorBias_valid; - bool _new_irlockReport; - bool _estimator_initialized; + bool _vehicleLocalPosition_valid{false}; + bool _vehicleAttitude_valid{false}; + bool _sensorBias_valid{false}; + bool _new_irlockReport{false}; + bool _estimator_initialized{false}; // keep track of whether last measurement was rejected - bool _faulty; + bool _faulty{false}; - matrix::Dcm _R_att; - matrix::Vector _rel_pos; + matrix::Dcmf _R_att; + matrix::Vector2f _rel_pos; KalmanFilter _kalman_filter_x; KalmanFilter _kalman_filter_y; - hrt_abstime _last_predict; // timestamp of last filter prediction - hrt_abstime _last_update; // timestamp of last filter update (used to check timeout) + hrt_abstime _last_predict{0}; // timestamp of last filter prediction + hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout) void _check_params(const bool force);