fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

landing_target_estimator: update to uORB Subscription and Publication

Browse Source
This commit is contained in:
Daniel Agar 2019-06-29 11:06:32 -04:00
parent 38da0f95aa
commit 6ef61e5c19
2 changed files with 37 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
src/modules/landing_target_estimator/LandingTargetEstimator.cpp
View File

@ -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, &paramUpdate);
parameter_update_s paramUpdate;
_parameterSub.copy(&paramUpdate);
}
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);
}

60
src/modules/landing_target_estimator/LandingTargetEstimator.h
View File

@ -45,7 +45,8 @@
#include <px4_workqueue.h>
#include <drivers/drv_hrt.h>
#include <parameters/param.h>
#include <uORB/uORB.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_bias.h>
@ -75,10 +76,6 @@ public:
void update();
protected:
/*
* Called once to initialize uORB topics.
*/
void _initialize_topics();
/*
* Update uORB topics.
@ -90,23 +87,16 @@ protected:
*/
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<landing_target_pose_s> _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<landing_target_innovations_s> _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 @@ private:
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<float> _R_att;
matrix::Vector<float, 2> _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);