42 #include <px4_platform_common/px4_config.h> 43 #include <px4_platform_common/defines.h> 48 #define SEC2USEC 1000000.0f 124 sensor_ray(2) = 1.0f;
129 sensor_ray =
_R_att * sensor_ray;
131 if (fabsf(sensor_ray(2)) < 1e-6
f) {
139 _rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist;
140 _rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist;
158 if (!update_x || !update_y) {
161 PX4_WARN(
"Landing target measurement rejected:%s%s", update_x ?
"" :
" x", update_y ?
"" :
" y");
173 float x, xvel, y, yvel, covx, covx_v, covy, covy_v;
212 float innov_x, innov_cov_x, innov_y, innov_cov_y;
235 if (updated || force) {
KalmanFilter _kalman_filter_y
uORB::Publication< landing_target_pose_s > _targetPosePub
void predict(float dt, float acc, float acc_unc)
Predict the state with an external acceleration estimate.
KalmanFilter _kalman_filter_x
bool _vehicleLocalPosition_valid
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
void init(matrix::Vector< float, 2 > &initial, matrix::Matrix< float, 2, 2 > &covInit)
Initialize filter state.
vehicle_attitude_s _vehicleAttitude
bool update(float meas, float measUnc)
Update the state estimate with a measurement.
uORB::Subscription _vehicle_acceleration_sub
matrix::Vector2f _rel_pos
High-resolution timer with callouts and timekeeping.
void getInnovations(float &innov, float &innovCov)
Get measurement innovation and covariance of last update call.
uORB::Subscription _vehicleLocalPositionSub
bool publish(const T &data)
Publish the struct.
struct landing_target_estimator::LandingTargetEstimator::@100 _paramHandle
Handles for parameters.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void getState(matrix::Vector< float, 2 > &state)
Get the current filter state.
hrt_abstime _last_predict
uORB::Subscription _irlockReportSub
static constexpr uint32_t landing_target_estimator_TIMEOUT_US
bool updated()
Check if there is a new update.
vehicle_acceleration_s _vehicle_acceleration
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
uORB::Subscription _attitudeSub
vehicle_local_position_s _vehicleLocalPosition
struct landing_target_estimator::LandingTargetEstimator::@101 _params
uORB::Subscription _parameterSub
void getCovariance(matrix::Matrix< float, 2, 2 > &covariance)
Get state covariance.
void _check_params(const bool force)
landing_target_pose_s _target_pose
irlock_report_s _irlockReport
bool update(void *dst)
Update the struct.
bool _vehicle_acceleration_valid
bool _estimator_initialized
landing_target_innovations_s _target_innovations
uORB::Publication< landing_target_innovations_s > _targetInnovationsPub
bool copy(void *dst)
Copy the struct.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
bool _vehicleAttitude_valid