diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock index 2f064e2c5f..1ed18ec602 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock @@ -34,5 +34,10 @@ param set-default LTEST_MODE 1 param set-default PLD_HACC_RAD 0.1 param set-default RTL_PLD_MD 2 +# Orientation parameters for Gazebo irislock sim +param set-default LTEST_EULR_PHI 0 # ENU -> NED already done in gazeebo +param set-default LTEST_EULR_THETA -1.57 # Camera tilt, which is facing 90 degrees down +param set-default LTEST_EULR_PSI 0.0 + # Start up Landing Target Estimator module landing_target_estimator start diff --git a/msg/IrlockReport.msg b/msg/IrlockReport.msg index 9f23cbf3ca..3212828680 100644 --- a/msg/IrlockReport.msg +++ b/msg/IrlockReport.msg @@ -9,3 +9,5 @@ float32 pos_x # tan(theta), where theta is the angle between the target and the float32 pos_y # tan(theta), where theta is the angle between the target and the camera center of projection in camera y-axis float32 size_x #/** size of target along camera x-axis in units of tan(theta) **/ float32 size_y #/** size of target along camera y-axis in units of tan(theta) **/ + +float32[4] q # Quaternion as recorded by the camera (w, x, y, z order, zero-rotation is 1, 0, 0, 0) diff --git a/msg/LandingTargetPose.msg b/msg/LandingTargetPose.msg index 875920f183..c49e8b2f2d 100644 --- a/msg/LandingTargetPose.msg +++ b/msg/LandingTargetPose.msg @@ -24,3 +24,7 @@ bool abs_pos_valid # Flag showing whether absolute position is valid float32 x_abs # X/north position of target, relative to origin (navigation frame) [meters] float32 y_abs # Y/east position of target, relative to origin (navigation frame) [meters] float32 z_abs # Z/down position of target, relative to origin (navigation frame) [meters] +float32[4] q # Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + +float32 target_yaw +float32 target_yaw_filtered diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 7fd5f15cfb..778545da32 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -63,7 +63,13 @@ LandingTargetEstimator::LandingTargetEstimator() _paramHandle.offset_x = param_find("LTEST_SENS_POS_X"); _paramHandle.offset_y = param_find("LTEST_SENS_POS_Y"); _paramHandle.offset_z = param_find("LTEST_SENS_POS_Z"); + _paramHandle.eulr_phi = param_find("LTEST_EULR_PHI"); + _paramHandle.eulr_theta = param_find("LTEST_EULR_THETA"); + _paramHandle.eulr_psi = param_find("LTEST_EULR_PSI"); + _paramHandle.yaw_alpha = param_find("LTEST_YAW_ALPHA"); + _check_params(true); + _alpha_filter_yaw.reset(NAN); } void LandingTargetEstimator::update() @@ -75,7 +81,7 @@ void LandingTargetEstimator::update() /* predict */ if (_estimator_initialized) { if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) { - PX4_WARN("Timeout"); + PX4_INFO("Target lost"); _estimator_initialized = false; } else { @@ -116,6 +122,9 @@ void LandingTargetEstimator::update() _kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init); _kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init); + const float sample_interval = 1.0f; + _alpha_filter_yaw.setParameters(sample_interval, 0.f); + _estimator_initialized = true; _last_update = hrt_absolute_time(); _last_predict = _last_update; @@ -126,10 +135,13 @@ void LandingTargetEstimator::update() 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); + _alpha_filter_yaw.setAlpha(_params.yaw_alpha); + _alpha_filter_yaw.update(_last_unwrapped_yaw); + if (!update_x || !update_y) { if (!_faulty) { _faulty = true; - PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y"); + PX4_INFO("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y"); } } else { @@ -170,6 +182,14 @@ void LandingTargetEstimator::update() _target_pose.z_abs = _target_position_report.rel_pos_z + _vehicleLocalPosition.z; _target_pose.abs_pos_valid = true; + // q should only be filled when abs_pos_valid is set + const float yaw_filterd_and_wrapped = matrix::wrap_pi(_alpha_filter_yaw.getState()); + matrix::Quatf quaternion(matrix::Eulerf(0.f, 0.f, yaw_filterd_and_wrapped)); + _target_pose.target_yaw_filtered = yaw_filterd_and_wrapped; + _target_pose.target_yaw = _last_unwrapped_yaw; + quaternion.copyTo(_target_pose.q); + _last_unwrapped_yaw = _target_pose.target_yaw; + } else { _target_pose.abs_pos_valid = false; } @@ -242,6 +262,24 @@ void LandingTargetEstimator::_update_topics() return; } + // Calculate target yaw [rad] in NED frame from offboard quaternion + const matrix::Quatf q_in_camera(_irlockReport.q); + const matrix::Quatf rot_camera_to_body = matrix::Eulerf(_params.eulr_phi, _params.eulr_theta, _params.eulr_psi); + const matrix::Quatf rot_body_to_ned = q_att; + const matrix::Quatf q_in_body = rot_camera_to_body * q_in_camera; + const matrix::Quatf q_in_ned = rot_body_to_ned * q_in_body; + const float target_yaw_ned = matrix::Eulerf(q_in_ned).psi(); + + if (PX4_ISFINITE(target_yaw_ned)) { + // _last_unwrapped_yaw = unwrap(_last_unwrapped_yaw, target_yaw_ned, 0.0f, (float)M_PI); + _last_unwrapped_yaw = matrix::unwrap_pi(_last_unwrapped_yaw, target_yaw_ned); + + // Initialize yaw lowpass filter if necessary + if (!PX4_ISFINITE(_alpha_filter_yaw.getState())) { + _alpha_filter_yaw.reset(_last_unwrapped_yaw); + } + } + _dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z; // scale the ray s.t. the z component has length of _uncertainty_scale @@ -302,6 +340,10 @@ void LandingTargetEstimator::_update_params() param_get(_paramHandle.offset_x, &_params.offset_x); param_get(_paramHandle.offset_y, &_params.offset_y); param_get(_paramHandle.offset_z, &_params.offset_z); + param_get(_paramHandle.eulr_phi, &_params.eulr_phi); + param_get(_paramHandle.eulr_theta, &_params.eulr_theta); + param_get(_paramHandle.eulr_psi, &_params.eulr_psi); + param_get(_paramHandle.yaw_alpha, &_params.yaw_alpha); } diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index 521a1e3763..22867e88a2 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -62,6 +62,7 @@ #include #include #include +#include #include "KalmanFilter.h" using namespace time_literals; @@ -126,6 +127,10 @@ private: param_t offset_y; param_t offset_z; param_t sensor_yaw; + param_t eulr_phi; + param_t eulr_theta; + param_t eulr_psi; + param_t yaw_alpha; } _paramHandle; struct { @@ -140,6 +145,10 @@ private: float offset_y; float offset_z; enum Rotation sensor_yaw; + float eulr_phi; + float eulr_theta; + float eulr_psi; + float yaw_alpha; } _params; struct { @@ -147,6 +156,7 @@ private: float rel_pos_x; float rel_pos_y; float rel_pos_z; + float quaternion[4]; } _target_position_report; uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; @@ -177,6 +187,8 @@ private: matrix::Vector2f _rel_pos; KalmanFilter _kalman_filter_x; KalmanFilter _kalman_filter_y; + AlphaFilter _alpha_filter_yaw; // poor man's orientation estimator + float _last_unwrapped_yaw{0.0f}; 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}; diff --git a/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/modules/landing_target_estimator/landing_target_estimator_params.c index 24ee9b772f..7804296a9d 100644 --- a/src/modules/landing_target_estimator/landing_target_estimator_params.c +++ b/src/modules/landing_target_estimator/landing_target_estimator_params.c @@ -187,3 +187,51 @@ PARAM_DEFINE_FLOAT(LTEST_SENS_POS_Y, 0.0f); * */ PARAM_DEFINE_FLOAT(LTEST_SENS_POS_Z, 0.0f); + +/** + * Euler Angle Rotation (3-2-1 intrinsic Tait-Bryan) Phi (X) + * + * Orientation transformation from camera frame that detected + * the landing_target to drone's NED frame + * + * @unit rad + * @decimal 2 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_EULR_PHI, 0.0f); + +/** + * Euler Angle Rotation (3-2-1 intrinsic Tait-Bryan) Theta (Y) + * + * Orientation transformation from camera frame that detected + * the landing_target to drone's NED frame + * + * @unit rad + * @decimal 2 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_EULR_THETA, 0.0f); + +/** + * Euler Angle Rotation (3-2-1 intrinsic Tait-Bryan) Psi (Z) + * + * Orientation transformation from camera frame that detected + * the landing_target to drone's NED frame + * + * @unit rad + * @decimal 2 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_EULR_PSI, -1.57f); + +/** + * Target yaw filter alpha value + * + * @min 0 + * @max 100 + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_YAW_ALPHA, 0.05); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 047d84b49f..be15f9f7a2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2515,6 +2515,14 @@ MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg) "landing target: unsupported coordinate frame {1}", landing_target.frame); } else { + // The following was always a work-around: + // irlock_report message, packaged as landing_target message over mavlink + // See https://github.com/PX4/PX4-Autopilot/issues/14698 + // + // irlock_report needs to be reworked into a message that supports feeding PX4's + // landing_target_estimator from offboard. This should not be limited to + // single-point IR beacons, but should support and form of marker, including tags + // that have an orientation. irlock_report_s irlock_report{}; irlock_report.timestamp = hrt_absolute_time(); @@ -2524,6 +2532,10 @@ MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg) irlock_report.size_x = landing_target.size_x; irlock_report.size_y = landing_target.size_y; + // Copy quaternion + matrix::Quatf q(landing_target.q); + q.copyTo(irlock_report.q); + _irlock_report_pub.publish(irlock_report); } } diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 080491d37c..16fbc36561 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -268,6 +268,10 @@ PrecLand::run_state_horizontal_approach() pos_sp_triplet->current.alt = _approach_alt; pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + // note: abs_pos_valid was already checked as a condition to enter this state + pos_sp_triplet->current.yaw = matrix::Eulerf(matrix::Quaternionf(_target_pose.q)).psi(); + pos_sp_triplet->current.yaw_valid = true; + _navigator->set_position_setpoint_triplet_updated(); } @@ -299,6 +303,9 @@ PrecLand::run_state_descend_above_target() pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; + pos_sp_triplet->current.yaw = matrix::Eulerf(matrix::Quaternionf(_target_pose.q)).psi(); + pos_sp_triplet->current.yaw_valid = true; + _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 71970b9490..d7fc37e12e 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -619,6 +619,9 @@ void SimulatorMavlink::handle_message_landing_target(const mavlink_message_t *ms report.size_x = landing_target_mavlink.size_x; report.size_y = landing_target_mavlink.size_y; + matrix::Quatf q(landing_target_mavlink.q); + q.copyTo(report.q); + _irlock_report_pub.publish(report); } }