diff --git a/msg/landing_target_innovations.msg b/msg/landing_target_innovations.msg index 5dd892c560..5f38f80bf6 100644 --- a/msg/landing_target_innovations.msg +++ b/msg/landing_target_innovations.msg @@ -1,4 +1,6 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + # Innovation of landing target position estimator float32 innov_x float32 innov_y diff --git a/msg/landing_target_pose.msg b/msg/landing_target_pose.msg index 875920f183..4a041bf0d4 100644 --- a/msg/landing_target_pose.msg +++ b/msg/landing_target_pose.msg @@ -1,6 +1,7 @@ # Relative position of precision land target in navigation (body fixed, north aligned, NED) and inertial (world fixed, north aligned, NED) frames -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) bool is_static # Flag indicating whether the landing target is static or moving with respect to the ground diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index fd6754a1de..0488e8c880 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -568,6 +568,9 @@ SquareMatrix choleskyInv(const SquareMatrix &A) return L_inv.T() * L_inv; } +using Matrix2f = SquareMatrix; +using Matrix2d = SquareMatrix; + using Matrix3f = SquareMatrix; using Matrix3d = SquareMatrix; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 166a08f1af..6cec996aea 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1515,7 +1515,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) { // velocity of vehicle relative to target has opposite sign to target relative to vehicle auxVelSample auxvel_sample{ - .time_us = landing_target_pose.timestamp, + .time_us = landing_target_pose.timestamp_sample, .vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f}, .velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f}, }; diff --git a/src/modules/landing_target_estimator/CMakeLists.txt b/src/modules/landing_target_estimator/CMakeLists.txt index 879f39e619..97f6529869 100644 --- a/src/modules/landing_target_estimator/CMakeLists.txt +++ b/src/modules/landing_target_estimator/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -36,7 +36,9 @@ px4_add_module( MAIN landing_target_estimator COMPILE_FLAGS SRCS - LandingTargetEstimator.cpp KalmanFilter.cpp + KalmanFilter.hpp + LandingTargetEstimator.cpp + LandingTargetEstimator.hpp ) diff --git a/src/modules/landing_target_estimator/KalmanFilter.cpp b/src/modules/landing_target_estimator/KalmanFilter.cpp index c72ce5faf2..1d5ad23234 100644 --- a/src/modules/landing_target_estimator/KalmanFilter.cpp +++ b/src/modules/landing_target_estimator/KalmanFilter.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,14 +39,14 @@ * */ -#include "KalmanFilter.h" +#include "KalmanFilter.hpp" -KalmanFilter::KalmanFilter(matrix::Vector &initial, matrix::Matrix &covInit) +KalmanFilter::KalmanFilter(matrix::Vector2f &initial, matrix::Matrix2f &covInit) { init(initial, covInit); } -void KalmanFilter::init(matrix::Vector &initial, matrix::Matrix &covInit) +void KalmanFilter::init(matrix::Vector2f &initial, matrix::Matrix2f &covInit) { _x = initial; _covariance = covInit; @@ -54,10 +54,10 @@ void KalmanFilter::init(matrix::Vector &initial, matrix::Matrix initial; + matrix::Vector2f initial; initial(0) = initial0; initial(1) = initial1; - matrix::Matrix covInit; + matrix::Matrix2f covInit; covInit(0, 0) = covInit00; covInit(1, 1) = covInit11; @@ -69,7 +69,7 @@ void KalmanFilter::predict(float dt, float acc, float acc_unc) _x(0) += _x(1) * dt + dt * dt / 2 * acc; _x(1) += acc * dt; - matrix::Matrix A; // propagation matrix + matrix::Matrix2f A; // propagation matrix A(0, 0) = 1; A(1, 1) = 1; A(0, 1) = dt; @@ -78,14 +78,13 @@ void KalmanFilter::predict(float dt, float acc, float acc_unc) G(0, 0) = dt * dt / 2; G(1, 0) = dt; - matrix::Matrix process_noise = G * G.transpose() * acc_unc; + matrix::Matrix2f process_noise = G * G.transpose() * acc_unc; _covariance = A * _covariance * A.transpose() + process_noise; } bool KalmanFilter::update(float meas, float measUnc) { - // H = [1, 0] _residual = meas - _x(0); @@ -100,46 +99,23 @@ bool KalmanFilter::update(float meas, float measUnc) return false; } - matrix::Vector kalmanGain; + matrix::Vector2f kalmanGain; kalmanGain(0) = _covariance(0, 0); kalmanGain(1) = _covariance(1, 0); kalmanGain /= _innovCov; _x += kalmanGain * _residual; - matrix::Matrix identity; + matrix::Matrix2f identity; identity.identity(); - matrix::Matrix KH; // kalmanGain * H + matrix::Matrix2f KH; // kalmanGain * H KH(0, 0) = kalmanGain(0); KH(1, 0) = kalmanGain(1); _covariance = (identity - KH) * _covariance; return true; - -} - -void KalmanFilter::getState(matrix::Vector &state) -{ - state = _x; -} - -void KalmanFilter::getState(float &state0, float &state1) -{ - state0 = _x(0); - state1 = _x(1); -} - -void KalmanFilter::getCovariance(matrix::Matrix &covariance) -{ - covariance = _covariance; -} - -void KalmanFilter::getCovariance(float &cov00, float &cov11) -{ - cov00 = _covariance(0, 0); - cov11 = _covariance(1, 1); } void KalmanFilter::getInnovations(float &innov, float &innovCov) diff --git a/src/modules/landing_target_estimator/KalmanFilter.h b/src/modules/landing_target_estimator/KalmanFilter.hpp similarity index 79% rename from src/modules/landing_target_estimator/KalmanFilter.h rename to src/modules/landing_target_estimator/KalmanFilter.hpp index 09ccbe6a94..a87bfa0f23 100644 --- a/src/modules/landing_target_estimator/KalmanFilter.h +++ b/src/modules/landing_target_estimator/KalmanFilter.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -59,24 +59,24 @@ public: /** * Default constructor, state not initialized */ - KalmanFilter() {}; + KalmanFilter() = default; /** * Constructor, initialize state */ - KalmanFilter(matrix::Vector &initial, matrix::Matrix &covInit); + KalmanFilter(matrix::Vector2f &initial, matrix::Matrix2f &covInit); /** * Default desctructor */ - virtual ~KalmanFilter() {}; + ~KalmanFilter() = default; /** * Initialize filter state * @param initial initial state * @param covInit initial covariance */ - void init(matrix::Vector &initial, matrix::Matrix &covInit); + void init(matrix::Vector2f &initial, matrix::Matrix2f &covInit); /** * Initialize filter state, only specifying diagonal covariance elements @@ -107,27 +107,13 @@ public: * Get the current filter state * @param x1 State */ - void getState(matrix::Vector &state); - - /** - * Get the current filter state - * @param state0 First state - * @param state1 Second state - */ - void getState(float &state0, float &state1); + const matrix::Vector2f &getState() const { return _x; } /** * Get state covariance * @param covariance Covariance of the state */ - void getCovariance(matrix::Matrix &covariance); - - /** - * Get state variances (diagonal elements) - * @param cov00 Variance of first state - * @param cov11 Variance of second state - */ - void getCovariance(float &cov00, float &cov11); + const matrix::Matrix2f &getCovariance() const { return _covariance; } /** * Get measurement innovation and covariance of last update call @@ -137,11 +123,10 @@ public: void getInnovations(float &innov, float &innovCov); private: - matrix::Vector _x; // state + matrix::Vector2f _x{}; // state + matrix::Matrix2f _covariance{}; // state covariance - matrix::Matrix _covariance; // state covariance + float _residual{0.f}; // residual of last measurement update - float _residual{0.0f}; // residual of last measurement update - - float _innovCov{0.0f}; // innovation covariance of last measurement update + float _innovCov{0.f}; // innovation covariance of last measurement update }; diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 8d67f15b68..afbf0a6880 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,293 +31,306 @@ * ****************************************************************************/ -#include -#include -#include - -#include "LandingTargetEstimator.h" - -#define SEC2USEC 1000000.0f +#include "LandingTargetEstimator.hpp" LandingTargetEstimator::LandingTargetEstimator() : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { - _check_params(true); +} + +LandingTargetEstimator::~LandingTargetEstimator() +{ + perf_free(_cycle_perf); +} + +bool LandingTargetEstimator::init() +{ + if (!_vehicle_local_position_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; } void LandingTargetEstimator::Run() { - _check_params(false); - - _update_topics(); - - /* predict */ - if (_estimator_initialized) { - if (hrt_absolute_time() - _last_update > TARGET_UPDATE_TIMEOUT_US) { - PX4_INFO("Target lost"); - _estimator_initialized = false; - - } else { - float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC; - - // predict target position with the help of accel data - matrix::Vector3f a{_vehicle_acceleration.xyz}; - - if (_vehicleAttitude_valid && _vehicle_acceleration_valid) { - matrix::Quaternion q_att(&_vehicleAttitude.q[0]); - _R_att = matrix::Dcm(q_att); - a = _R_att * a; - - } else { - a.zero(); - } - - _kalman_filter_x.predict(dt, -a(0), _param_acc_unc.get()); - _kalman_filter_y.predict(dt, -a(1), _param_acc_unc.get()); - - _last_predict = hrt_absolute_time(); - } - } - - if (!_new_sensorReport) { - // nothing to do + if (should_exit()) { + _vehicle_local_position_sub.unregisterCallback(); + exit_and_cleanup(); return; } - // mark this sensor measurement as consumed - _new_sensorReport = false; + // backup schedule + ScheduleDelayed(100_ms); - if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) { - return; - } - - if (!_estimator_initialized) { - float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f; - float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f; - PX4_INFO("Target acquired %.2f %.2f", (double)vx_init, (double)vy_init); - _kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _param_pos_unc_in.get(), _param_vel_unc_in.get()); - _kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _param_pos_unc_in.get(), _param_vel_unc_in.get()); - - _estimator_initialized = true; - _last_update = hrt_absolute_time(); - _last_predict = _last_update; - - } else { - const float measurement_uncertainty = _param_meas_unc.get() * _dist_z * _dist_z; - bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty); - bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty); - - if (!update_x || !update_y) { - if (!_faulty) { - _faulty = true; - PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y"); - } - - } else { - _faulty = false; - } - - if (!_faulty) { - // only publish if both measurements were good - - _target_pose.timestamp = _target_position_report.timestamp; - - float x, xvel, y, yvel, covx, covx_v, covy, covy_v; - _kalman_filter_x.getState(x, xvel); - _kalman_filter_x.getCovariance(covx, covx_v); - - _kalman_filter_y.getState(y, yvel); - _kalman_filter_y.getCovariance(covy, covy_v); - - _target_pose.is_static = ((TargetMode)_param_mode.get() == TargetMode::Stationary); - - _target_pose.rel_pos_valid = true; - _target_pose.rel_vel_valid = true; - _target_pose.x_rel = x; - _target_pose.y_rel = y; - _target_pose.z_rel = _target_position_report.rel_pos_z ; - _target_pose.vx_rel = xvel; - _target_pose.vy_rel = yvel; - - _target_pose.cov_x_rel = covx; - _target_pose.cov_y_rel = covy; - - _target_pose.cov_vx_rel = covx_v; - _target_pose.cov_vy_rel = covy_v; - - if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) { - _target_pose.x_abs = x + _vehicleLocalPosition.x; - _target_pose.y_abs = y + _vehicleLocalPosition.y; - _target_pose.z_abs = _target_position_report.rel_pos_z + _vehicleLocalPosition.z; - _target_pose.abs_pos_valid = true; - - } else { - _target_pose.abs_pos_valid = false; - } - - _targetPosePub.publish(_target_pose); - - _last_update = hrt_absolute_time(); - _last_predict = _last_update; - } - - float innov_x, innov_cov_x, innov_y, innov_cov_y; - _kalman_filter_x.getInnovations(innov_x, innov_cov_x); - _kalman_filter_y.getInnovations(innov_y, innov_cov_y); - - _target_innovations.timestamp = _target_position_report.timestamp; - _target_innovations.innov_x = innov_x; - _target_innovations.innov_cov_x = innov_cov_x; - _target_innovations.innov_y = innov_y; - _target_innovations.innov_cov_y = innov_cov_y; - - _targetInnovationsPub.publish(_target_innovations); - } -} - -void LandingTargetEstimator::_check_params(const bool force) -{ - if (_parameter_update_sub.updated() || force) { + if (_parameter_update_sub.updated()) { parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); ModuleParams::updateParams(); } -} -void LandingTargetEstimator::_update_topics() -{ - _vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition); - _vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude); - _vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration); + perf_begin(_cycle_perf); + // predict + vehicle_local_position_s vehicle_local_position; - if (_irlockReportSub.update(&_irlockReport)) { // - _new_irlockReport = true; + if (_vehicle_local_position_sub.update(&vehicle_local_position)) { + if (_estimator_initialized) { + const float dt = math::constrain((vehicle_local_position.timestamp_sample - _last_predict) * 1e-6f, 0.001f, 0.1f); - if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) { - // don't have the data needed for an update - return; + // predict target position with the help of accel data + _kalman_filter_x.predict(dt, -vehicle_local_position.ax, _param_ltest_acc_unc.get()); + _kalman_filter_y.predict(dt, -vehicle_local_position.ay, _param_ltest_acc_unc.get()); + + _last_predict = vehicle_local_position.timestamp_sample; } - - if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) { - return; - } - - matrix::Vector sensor_ray; // ray pointing towards target in body frame - sensor_ray(0) = _irlockReport.pos_x * _param_scale_x.get(); // forward - sensor_ray(1) = _irlockReport.pos_y * _param_scale_y.get(); // right - sensor_ray(2) = 1.0f; - - // rotate unit ray according to sensor orientation - _S_att = get_rot_matrix((Rotation)_param_sens_rot.get()); - sensor_ray = _S_att * sensor_ray; - - // rotate the unit ray into the navigation frame - matrix::Quaternion q_att(&_vehicleAttitude.q[0]); - _R_att = matrix::Dcm(q_att); - sensor_ray = _R_att * sensor_ray; - - if (fabsf(sensor_ray(2)) < 1e-6f) { - // z component of measurement unsafe, don't use this measurement - return; - } - - _dist_z = _vehicleLocalPosition.dist_bottom - _param_sens_pos_z.get(); - - // scale the ray s.t. the z component has length of _uncertainty_scale - _target_position_report.timestamp = _irlockReport.timestamp; - _target_position_report.rel_pos_x = sensor_ray(0) / sensor_ray(2) * _dist_z; - _target_position_report.rel_pos_y = sensor_ray(1) / sensor_ray(2) * _dist_z; - _target_position_report.rel_pos_z = _dist_z; - - // Adjust relative position according to sensor offset - _target_position_report.rel_pos_x += _param_sens_pos_x.get(); - _target_position_report.rel_pos_y += _param_sens_pos_y.get(); - - _new_sensorReport = true; - - } else if (_uwbDistanceSub.update(&_uwbDistance)) { - if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) { - // don't have the data needed for an update - PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid); - return; - } - - if (!PX4_ISFINITE((float)_uwbDistance.position[0]) || !PX4_ISFINITE((float)_uwbDistance.position[1]) || - !PX4_ISFINITE((float)_uwbDistance.position[2])) { - PX4_WARN("Position is corrupt!"); - return; - } - - _new_sensorReport = true; - - // The coordinate system is NED (north-east-down) - // the uwb_distance msg contains the Position in NED, Vehicle relative to LP - // The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle. - // To change POV we negate every Axis: - _target_position_report.timestamp = _uwbDistance.timestamp; - _target_position_report.rel_pos_x = -_uwbDistance.position[0]; - _target_position_report.rel_pos_y = -_uwbDistance.position[1]; - _target_position_report.rel_pos_z = -_uwbDistance.position[2]; } -} -int LandingTargetEstimator::start() -{ - ScheduleOnInterval(1000000 / SAMPLE_RATE); - return PX4_OK; + landing_target_pose_s landing_target_pose{}; + + bool update_x = false; + bool update_y = false; + + if (_irlock_report_sub.advertised()) { + // using irlock_report + irlock_report_s irlock_report; + + if (_irlock_report_sub.update(&irlock_report)) { + vehicle_attitude_s vehicle_attitude; + + if (!vehicle_local_position.v_xy_valid || !vehicle_local_position.dist_bottom_valid + || !_vehicle_attitude_sub.update(&vehicle_attitude) + || !PX4_ISFINITE(irlock_report.pos_y) || !PX4_ISFINITE(irlock_report.pos_x)) { + // don't have the data needed for an update + return; + } + + matrix::Vector3f sensor_ray; // ray pointing towards target in body frame + sensor_ray(0) = irlock_report.pos_x * _param_ltest_scale_x.get(); // forward + sensor_ray(1) = irlock_report.pos_y * _param_ltest_scale_y.get(); // right + sensor_ray(2) = 1.f; + + // Orientation of the sensor relative to body frame + const matrix::Dcmf S_att = get_rot_matrix((Rotation)_param_ltest_sens_rot.get()); + // rotate unit ray according to sensor orientation + sensor_ray = S_att * sensor_ray; + + // rotate the unit ray into the navigation frame + const matrix::Quatf q_att(vehicle_attitude.q); + const matrix::Dcmf R_att(q_att); // Orientation of the body frame + sensor_ray = R_att * sensor_ray; + + if (fabsf(sensor_ray(2)) < 1e-6f) { + // z component of measurement unsafe, don't use this measurement + return; + } + + matrix::Vector3f rel_pos{}; + + const float dist_z = vehicle_local_position.dist_bottom - _param_ltest_sens_pos_z.get(); + + // scale the ray s.t. the z component has length of _uncertainty_scale + // - adjust relative position according to sensor offset + rel_pos(0) = (sensor_ray(0) / sensor_ray(2) * dist_z) + _param_ltest_sens_pos_x.get(); + rel_pos(1) = (sensor_ray(1) / sensor_ray(2) * dist_z) + _param_ltest_sens_pos_x.get(); + rel_pos(2) = dist_z; + + if (!_estimator_initialized) { + float vx_init = -vehicle_local_position.vx; + float vy_init = -vehicle_local_position.vy; + + PX4_INFO("IRLock target acquired %.2f %.2f", (double)vx_init, (double)vy_init); + + _kalman_filter_x.init(rel_pos(0), vx_init, _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get()); + _kalman_filter_y.init(rel_pos(1), vy_init, _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get()); + + _estimator_initialized = true; + _last_predict = vehicle_local_position.timestamp_sample; + _last_update = vehicle_local_position.timestamp_sample; + + } else { + const float measurement_uncertainty = _param_ltest_meas_unc.get() * dist_z * dist_z; + + update_x = _kalman_filter_x.update(rel_pos(0), measurement_uncertainty); + update_y = _kalman_filter_y.update(rel_pos(1), measurement_uncertainty); + + if (!update_x || !update_y) { + PX4_DEBUG("IRLock distance measurement rejected (%.3f, %.3f)", (double)rel_pos(0), (double)rel_pos(1)); + } + + landing_target_pose.timestamp_sample = irlock_report.timestamp; + landing_target_pose.z_rel = dist_z; + } + } + + } else if (_uwb_distance_sub.advertised()) { + uwb_distance_s uwb_distance; + + if (_uwb_distance_sub.update(&uwb_distance)) { + + if (!vehicle_local_position.v_xy_valid) { + // don't have the data needed for an update + return; + } + + if (!PX4_ISFINITE((float)uwb_distance.position[0]) + || !PX4_ISFINITE((float)uwb_distance.position[1]) + || !PX4_ISFINITE((float)uwb_distance.position[2])) { + + PX4_WARN("uwb_distance position is corrupt!"); + return; + } + + const matrix::Vector3f position(-uwb_distance.position[0], -uwb_distance.position[1], -uwb_distance.position[2]); + + // The coordinate system is NED (north-east-down) + // the uwb_distance msg contains the Position in NED, Vehicle relative to LP + // The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle. + // To change POV we negate every Axis: + if (!_estimator_initialized) { + float vx_init = -vehicle_local_position.vx; + float vy_init = -vehicle_local_position.vy; + + PX4_INFO("UWB distance target acquired %.2f %.2f", (double)vx_init, (double)vy_init); + + _kalman_filter_x.init(position(0), vx_init, + _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get()); + + _kalman_filter_y.init(position(1), vy_init, + _param_ltest_pos_unc_in.get(), _param_ltest_vel_unc_in.get()); + + _estimator_initialized = true; + _last_predict = vehicle_local_position.timestamp_sample; + _last_update = vehicle_local_position.timestamp_sample; + + } else { + + float measurement_uncertainty = _param_ltest_meas_unc.get(); + + update_x = _kalman_filter_x.update(position(0), measurement_uncertainty); + update_y = _kalman_filter_y.update(position(1), measurement_uncertainty); + + if (!update_x || !update_y) { + PX4_DEBUG("UWB distance measurement rejected (%.3f, %.3f)", (double)position(1), (double)position(1)); + } + + landing_target_pose.timestamp_sample = uwb_distance.timestamp; + landing_target_pose.z_rel = uwb_distance.position[2]; + } + } + } + + if (vehicle_local_position.timestamp_sample > _last_update + TARGET_UPDATE_TIMEOUT_US) { + if (_estimator_initialized) { + PX4_INFO("Target lost"); + _estimator_initialized = false; + } + } + + if (_estimator_initialized) { + // publish landing_target_pose + if (update_x && update_y) { + _last_update = vehicle_local_position.timestamp_sample; + + // only publish if both measurements were good + landing_target_pose.is_static = ((TargetMode)_param_ltest_mode.get() == TargetMode::Stationary); + + landing_target_pose.rel_pos_valid = true; + landing_target_pose.rel_vel_valid = true; + + landing_target_pose.x_rel = _kalman_filter_x.getState()(0); + landing_target_pose.vx_rel = _kalman_filter_x.getState()(1); + landing_target_pose.cov_x_rel = _kalman_filter_x.getCovariance()(0, 0); + landing_target_pose.cov_vx_rel = _kalman_filter_x.getCovariance()(1, 1); + + landing_target_pose.y_rel = _kalman_filter_y.getState()(0); + landing_target_pose.vy_rel = _kalman_filter_y.getState()(1); + landing_target_pose.cov_y_rel = _kalman_filter_y.getCovariance()(0, 0); + landing_target_pose.cov_vy_rel = _kalman_filter_y.getCovariance()(1, 1); + + if (vehicle_local_position.xy_valid) { + landing_target_pose.x_abs = landing_target_pose.x_rel + vehicle_local_position.x; + landing_target_pose.y_abs = landing_target_pose.y_rel + vehicle_local_position.y; + landing_target_pose.z_abs = landing_target_pose.z_rel + vehicle_local_position.z; + landing_target_pose.abs_pos_valid = true; + + } else { + landing_target_pose.x_abs = NAN; + landing_target_pose.y_abs = NAN; + landing_target_pose.z_abs = NAN; + landing_target_pose.abs_pos_valid = false; + } + + landing_target_pose.timestamp = hrt_absolute_time(); + _landing_target_pose_pub.publish(landing_target_pose); + } + + // publish landing_target_innovations + if (landing_target_pose.timestamp_sample != 0) { + + landing_target_innovations_s landing_target_innovations{}; + _kalman_filter_x.getInnovations(landing_target_innovations.innov_x, landing_target_innovations.innov_cov_x); + _kalman_filter_y.getInnovations(landing_target_innovations.innov_y, landing_target_innovations.innov_cov_y); + + landing_target_innovations.timestamp_sample = landing_target_pose.timestamp_sample; + landing_target_innovations.timestamp = hrt_absolute_time(); + _landing_target_innovations_pub.publish(landing_target_innovations); + } + } + + perf_end(_cycle_perf); } int LandingTargetEstimator::task_spawn(int argc, char *argv[]) { LandingTargetEstimator *instance = new LandingTargetEstimator(); - if (!instance) { - PX4_ERR("driver allocation failed"); - return PX4_ERROR; + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); } - _object.store(instance); - _task_id = task_id_is_work_queue; + delete instance; + _object.store(nullptr); + _task_id = -1; - instance->start(); - return 0; + return PX4_ERROR; +} + +int LandingTargetEstimator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); } int LandingTargetEstimator::print_usage(const char *reason) { if (reason) { - printf("%s\n\n", reason); + PX4_WARN("%s\n", reason); } PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Background process running periodically on the LP work queue which filters and estimates a targets position. +Target position estimator using either IRLock or UWB (ultra wide band) data. )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("landing_target_estimator", "system"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_COMMAND("stop"); - PRINT_MODULE_USAGE_COMMAND("status"); + PRINT_MODULE_USAGE_NAME("landing_target_estimator", "estimator"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } -int LandingTargetEstimator::custom_command(int argc, char *argv[]) -{ - if (!is_running()) { - PX4_INFO("not running"); - return PX4_ERROR; - } - - return print_usage("Unrecognized command."); -} - extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[]) { return LandingTargetEstimator::main(argc, argv); diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.hpp similarity index 53% rename from src/modules/landing_target_estimator/LandingTargetEstimator.h rename to src/modules/landing_target_estimator/LandingTargetEstimator.hpp index fdc6dd4a86..fc2ca05010 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,25 +34,27 @@ #pragma once +#include "KalmanFilter.hpp" + +#include +#include +#include +#include +#include #include #include #include -#include #include +#include +#include #include -#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "KalmanFilter.h" using namespace time_literals; @@ -60,90 +62,61 @@ class LandingTargetEstimator : public ModuleBase, public public px4::ScheduledWorkItem { public: - LandingTargetEstimator(); - virtual ~LandingTargetEstimator() = default; - - static int print_usage(const char *reason = nullptr); - static int custom_command(int argc, char *argv[]); + ~LandingTargetEstimator() override; + /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); - int start(); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); private: - void Run() override; - void _check_params(const bool force); - void _update_topics(); - void _update_params(); - enum class TargetMode { Moving = 0, Stationary }; - static constexpr uint32_t TARGET_UPDATE_TIMEOUT_US{2000000}; - static constexpr uint32_t SAMPLE_RATE{50}; // samples per second + static constexpr uint32_t TARGET_UPDATE_TIMEOUT_US{2_s}; - uORB::Publication _targetPosePub{ORB_ID(landing_target_pose)}; - landing_target_pose_s _target_pose{}; - - uORB::Publication _targetInnovationsPub{ORB_ID(landing_target_innovations)}; - landing_target_innovations_s _target_innovations{}; + uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; + uORB::Publication _landing_target_innovations_pub{ORB_ID(landing_target_innovations)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - struct { - hrt_abstime timestamp; - float rel_pos_x; - float rel_pos_y; - float rel_pos_z; - } _target_position_report; + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; - uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)}; - uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _irlock_report_sub{ORB_ID(irlock_report)}; + uORB::Subscription _uwb_distance_sub{ORB_ID(uwb_distance)}; - vehicle_local_position_s _vehicleLocalPosition{}; - vehicle_attitude_s _vehicleAttitude{}; - vehicle_acceleration_s _vehicle_acceleration{}; - irlock_report_s _irlockReport{}; - uwb_distance_s _uwbDistance{}; - - // keep track of which topics we have received - bool _vehicleLocalPosition_valid{false}; - bool _vehicleAttitude_valid{false}; - bool _vehicle_acceleration_valid{false}; - bool _new_irlockReport{false}; - bool _new_sensorReport{false}; bool _estimator_initialized{false}; - // keep track of whether last measurement was rejected - bool _faulty{false}; - matrix::Dcmf _R_att; //Orientation of the body frame - matrix::Dcmf _S_att; //Orientation of the sensor relative to body frame - matrix::Vector2f _rel_pos; - KalmanFilter _kalman_filter_x; - KalmanFilter _kalman_filter_y; + KalmanFilter _kalman_filter_x{}; + KalmanFilter _kalman_filter_y{}; hrt_abstime _last_predict{0}; // timestamp of last filter prediction hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout) - float _dist_z{1.0f}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; DEFINE_PARAMETERS( - (ParamInt) _param_mode, - (ParamFloat) _param_acc_unc, - (ParamFloat) _param_meas_unc, - (ParamFloat) _param_pos_unc_in, - (ParamFloat) _param_vel_unc_in, - (ParamFloat) _param_scale_x, - (ParamFloat) _param_scale_y, - (ParamInt) _param_sens_rot, - (ParamFloat) _param_sens_pos_x, - (ParamFloat) _param_sens_pos_y, - (ParamFloat) _param_sens_pos_z + (ParamInt) _param_ltest_mode, + (ParamFloat) _param_ltest_acc_unc, + (ParamFloat) _param_ltest_meas_unc, + (ParamFloat) _param_ltest_pos_unc_in, + (ParamFloat) _param_ltest_vel_unc_in, + (ParamFloat) _param_ltest_scale_x, + (ParamFloat) _param_ltest_scale_y, + (ParamInt) _param_ltest_sens_rot, + (ParamFloat) _param_ltest_sens_pos_x, + (ParamFloat) _param_ltest_sens_pos_y, + (ParamFloat) _param_ltest_sens_pos_z ) };