Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

landing_target_estimator: update to uORB Subscription and Publication #12369

Merged
merged 1 commit into from
Jul 1, 2019
Merged

landing_target_estimator: update to uORB Subscription and Publication #12369

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
80 changes: 12 additions & 68 deletions src/modules/landing_target_estimator/LandingTargetEstimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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);
}

Expand Down Expand Up @@ -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;
Expand All @@ -241,70 +222,31 @@ 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) {
_update_params();
}
}

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()
Expand All @@ -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);
}
Expand Down
60 changes: 25 additions & 35 deletions src/modules/landing_target_estimator/LandingTargetEstimator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -75,10 +76,6 @@ class LandingTargetEstimator
void update();

protected:
/*
* Called once to initialize uORB topics.
*/
void _initialize_topics();

/*
* Update uORB topics.
Expand All @@ -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<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:

Expand Down Expand Up @@ -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<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);

Expand Down