fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 09:33:35 +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-07-01 10:35:41 -04:00
parent 38da0f95aa
commit 6ef61e5c19
2 changed files with 37 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/landing_target_estimator/LandingTargetEstimator.cpp
+12 -68
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);
}