diff --git a/msg/VteOrientation.msg b/msg/VteOrientation.msg index 54e6a5848b9..0eb09df558f 100644 --- a/msg/VteOrientation.msg +++ b/msg/VteOrientation.msg @@ -1,7 +1,7 @@ # Vision Target Estimator orientation state, exposing the full yaw filter output with covariances for logging and tuning. # # Published by: vision_target_estimator (VTEOrientation). -# Subscribed by: logger only. The orientation-related fields consumed elsewhere (precision landing) are exposed on landing_target_pose. +# Subscribed by: precision landing (navigator) and the logger. uint64 timestamp # [us] Time since system start diff --git a/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp b/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp index abcb3c2c8aa..708be5c1736 100644 --- a/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp +++ b/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp @@ -72,14 +72,15 @@ void VTEOrientation::resetFilter(hrt_abstime min_sample_time) _min_sample_time = min_sample_time; } - _estimator_initialized = false; - _target_est_yaw.resetHistory(); - - if (min_sample_time != 0) { + // Tell consumers the last published yaw no longer holds. + if (_estimator_initialized || (min_sample_time != 0)) { _orientation_msg = {}; _orientation_msg.timestamp = nowUs(); _target_orientation_pub.publish(_orientation_msg); } + + _estimator_initialized = false; + _target_est_yaw.resetHistory(); } void VTEOrientation::update() diff --git a/src/modules/vision_target_estimator/Orientation/VTEOrientation.h b/src/modules/vision_target_estimator/Orientation/VTEOrientation.h index 52020fded5c..d0702ffabbc 100644 --- a/src/modules/vision_target_estimator/Orientation/VTEOrientation.h +++ b/src/modules/vision_target_estimator/Orientation/VTEOrientation.h @@ -76,7 +76,7 @@ public: */ void update(); - /** Drop the estimator state. A nonzero sample boundary also publishes invalid yaw. */ + /** Drop the estimator state and publish an invalid yaw. A nonzero sample boundary drops older samples. */ void resetFilter(hrt_abstime min_sample_time = 0); /** Timeout after which the whole filter is declared stale (see @c timedOut). */ diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.cpp b/src/modules/vision_target_estimator/Position/VTEPosition.cpp index e4722570d3e..13ed6453605 100644 --- a/src/modules/vision_target_estimator/Position/VTEPosition.cpp +++ b/src/modules/vision_target_estimator/Position/VTEPosition.cpp @@ -97,6 +97,10 @@ void VTEPosition::resetFilter(hrt_abstime min_sample_time) _velocity_offset_ned.valid = false; } + if (_estimator_initialized) { + publishInvalidState(); + } + _estimator_initialized = false; _last_relative_meas_fused_time = 0; _bias.set = false; @@ -962,7 +966,9 @@ bool VTEPosition::processObsVision(TargetObs &obs) return false; } - const matrix::Quaternionf quat_att(report.q); + // The covariance rotation below assumes a unit quaternion. + matrix::Quaternionf quat_att(report.q); + quat_att.normalize(); const Vector3f vision_rel(report.rel_pos); const Vector3f vision_ned = quat_att.rotateVector(vision_rel); const float min_ev_pos_var = sq(fmaxf(_param_vte_ev_pos_noise.get(), kMinObservationNoise)); @@ -1213,6 +1219,16 @@ void VTEPosition::publishAidSource(const TargetObs &target_obs, const AxisResult _vte_aid_pubs[obsIndex(target_obs.type)].publish(target_innov); } +void VTEPosition::publishInvalidState() +{ + // Mark the logged state invalid. landing_target_pose is not published: precision landing keeps + // approaching the last pose it received and judges its age with its own timeout. + _vte_state.timestamp = nowUs(); + _vte_state.rel_pos_valid = false; + _vte_state.rel_vel_valid = false; + _target_estimator_state_pub.publish(_vte_state); +} + void VTEPosition::publishTarget() { _vte_state = {}; @@ -1220,12 +1236,7 @@ void VTEPosition::publishTarget() _target_pose.timestamp = _last_predict; _vte_state.timestamp = _last_predict; - -#if defined(CONFIG_VTEST_MOVING) - _target_pose.is_static = false; -#else - _target_pose.is_static = true; -#endif // CONFIG_VTEST_MOVING + _target_pose.is_static = kTargetIsStatic; Vector3f rel_pos; Vector3f rel_pos_var; diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.h b/src/modules/vision_target_estimator/Position/VTEPosition.h index ea8994a3f9b..05cbd44d3bb 100644 --- a/src/modules/vision_target_estimator/Position/VTEPosition.h +++ b/src/modules/vision_target_estimator/Position/VTEPosition.h @@ -81,7 +81,7 @@ public: */ void update(const matrix::Vector3f &acc_ned); - /** Drop the estimator state. A nonzero sample boundary also invalidates cached NED inputs. */ + /** Drop the estimator state and publish it as invalid. A nonzero sample boundary also invalidates cached NED inputs. */ void resetFilter(hrt_abstime min_sample_time = 0); /** Cache the task-specific absolute reference used to build the mission GNSS observation. */ @@ -185,6 +185,11 @@ private: }; static constexpr size_t kObsTypeCount = static_cast(ObsType::kTypeCount); +#if defined(CONFIG_VTEST_MOVING) + static constexpr bool kTargetIsStatic = false; +#else + static constexpr bool kTargetIsStatic = true; +#endif // CONFIG_VTEST_MOVING static constexpr size_t obsIndex(ObsType type) { return static_cast(type); @@ -354,6 +359,7 @@ private: void publishTarget(); /** Report an observation that seeded the state instead of being fused. */ void publishInitializedAid(const TargetObs &obs); + void publishInvalidState(); void resetObservations(); uORB::Subscription _vehicle_gnss_sub{ORB_ID(vehicle_gnss)}; diff --git a/src/modules/vision_target_estimator/VisionTargetEst.cpp b/src/modules/vision_target_estimator/VisionTargetEst.cpp index d81dbc56992..e651e21ef25 100644 --- a/src/modules/vision_target_estimator/VisionTargetEst.cpp +++ b/src/modules/vision_target_estimator/VisionTargetEst.cpp @@ -134,24 +134,20 @@ void VisionTargetEst::Run() ScheduleClear(); } else { - const uint32_t estimator_update_period_us = static_cast(math::min(kPosUpdatePeriodUs, - kYawUpdatePeriodUs)); - ScheduleOnInterval(estimator_update_period_us); + // Poll at the estimator rate. Without the delay the timer would fire again at once. + ScheduleOnInterval(static_cast(kEstimatorUpdatePeriodUs), static_cast(kEstimatorUpdatePeriodUs)); } } - // Task is running, check if an estimator must be started or re-started - startEstIfNeeded(); - - const bool task_completed = isCurrentTaskComplete(); - - if (task_completed) { + if (isCurrentTaskComplete()) { stopAllEstimators(); _current_task_ptr = nullptr; updateAidMask(); return; } + // Task is running, check if an estimator must be started or re-started + startEstIfNeeded(); restartTimedOutEstimators(); if (noEstRunning()) { @@ -670,23 +666,23 @@ bool VisionTargetEst::pollEstimatorInput(matrix::Vector3f &vehicle_acc_ned, matr vehicle_attitude_s vehicle_attitude{}; vehicle_acceleration_s vehicle_acceleration{}; - const bool vehicle_attitude_updated = _vehicle_attitude_sub.update(&vehicle_attitude); - const bool vehicle_acceleration_updated = _vehicle_acceleration_sub.update(&vehicle_acceleration); + // Keep the acceleration sample even when the attitude has not updated yet. + if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) { + _vehicle_acc_body.xyz = matrix::Vector3f(vehicle_acceleration.xyz); + _vehicle_acc_body.timestamp = vehicle_acceleration.timestamp_sample != 0 + ? vehicle_acceleration.timestamp_sample : vehicle_acceleration.timestamp; + } - if (!vehicle_attitude_updated) { + if (!_vehicle_attitude_sub.update(&vehicle_attitude)) { return false; } // Check the same attitude sample that will rotate this cycle's inputs. handleAttitudeReset(vehicle_attitude); - if (vehicle_acceleration_updated) { - _vehicle_acc_body.xyz = matrix::Vector3f(vehicle_acceleration.xyz); - _vehicle_acc_body.timestamp = vehicle_acceleration.timestamp; - } - _vehicle_acc_body.valid = (_vehicle_acc_body.timestamp != 0) - && !hasTimedOut(_vehicle_acc_body.timestamp, kAccUpdatedTimeoutUs); + && !hasTimedOut(_vehicle_acc_body.timestamp, kAccUpdatedTimeoutUs) + && _vehicle_acc_body.xyz.isAllFinite(); acc_valid = _vehicle_acc_body.valid; /* Transform FRD body acc to NED */ diff --git a/src/modules/vision_target_estimator/test/TEST_VTEOrientation.cpp b/src/modules/vision_target_estimator/test/TEST_VTEOrientation.cpp index e3f92b6eb3a..c337448e511 100644 --- a/src/modules/vision_target_estimator/test/TEST_VTEOrientation.cpp +++ b/src/modules/vision_target_estimator/test/TEST_VTEOrientation.cpp @@ -510,7 +510,9 @@ TEST_F(VTEOrientationTest, ResetsFilterOnStalePredictionGap) // WHEN: the estimator updates after the stale gap and then sees a fresh measurement. _vte->update(); EXPECT_FALSE(_aid_sub->update()); - EXPECT_FALSE(_state_sub->update()); + // THEN: consumers are told the last yaw no longer holds. + ASSERT_TRUE(_state_sub->update()); + EXPECT_FALSE(_state_sub->get().orientation_valid); pushValidYawSample(-0.25f); diff --git a/src/modules/vision_target_estimator/test/TEST_VTEPosition.cpp b/src/modules/vision_target_estimator/test/TEST_VTEPosition.cpp index ae2eba65827..7bce58274fb 100644 --- a/src/modules/vision_target_estimator/test/TEST_VTEPosition.cpp +++ b/src/modules/vision_target_estimator/test/TEST_VTEPosition.cpp @@ -2266,6 +2266,27 @@ TEST_F(VTEPositionTest, BiasSeedPropagatesGnssOverLongVisionLatency) expectBiasNear(_vte_state_sub->get(), kZeroVec, 0.05f); } +// WHY: The reported attitude may not be a unit quaternion, and the covariance rotation assumes one. +// WHAT: A 90 degree yaw quaternion scaled by two rotates the position but must not scale the variances. +TEST_F(VTEPositionTest, VisionQuaternionIsNormalized) +{ + enableMask({FusionMaskOption::kVisionPos}); + setLocalVelocity(kZeroVec, vte_test::advanceMicroseconds(kStepUs)); + const matrix::Vector3f rel_pos(1.f, 2.f, 3.f); + const matrix::Vector3f cov(0.09f, 0.09f, 0.09f); + const float half = std::sqrt(2.f); // 2 * cos(45 deg) and 2 * sin(45 deg) + publishVisionPos(rel_pos, matrix::Quaternionf(half, 0.f, 0.f, half), cov, vte_test::advanceMicroseconds(kStepUs)); + _vte->update(kZeroVec); + + ASSERT_TRUE(_aid_fiducial_sub->update()); + const matrix::Vector3f expected(-rel_pos(1), rel_pos(0), rel_pos(2)); + + for (int axis = 0; axis < 3; ++axis) { + EXPECT_NEAR(_aid_fiducial_sub->get().observation[axis], expected(axis), kTolerance); + EXPECT_NEAR(_aid_fiducial_sub->get().observation_variance[axis], cov(axis), kTolerance); + } +} + #if defined(CONFIG_VTEST_MOVING) // Equal UAV and target speeds leave their relative GNSS position unchanged across vision latency. TEST_F(VTEPositionTest, MovingTargetBiasAlignmentRequiresRelativeVelocity) @@ -2313,7 +2334,9 @@ TEST_F(VTEPositionTest, ResetsFilterOnStalePredictionGap) enableMask({FusionMaskOption::kVisionPos}); _vte->setMeasUpdatedTimeout(500_ms); + const matrix::Vector3f local_pos(10.f, 20.f, -5.f); setLocalVelocity(matrix::Vector3f{}, vte_test::advanceMicroseconds(kStepUs)); + setLocalPosition(local_pos, vte_test::nowUs()); publishVisionPos(initial_rel_pos, vte_test::identityQuat(), kSmallVisionCov, vte_test::advanceMicroseconds(kStepUs)); @@ -2324,6 +2347,8 @@ TEST_F(VTEPositionTest, ResetsFilterOnStalePredictionGap) EXPECT_NEAR(initial_state.rel_pos[0], initial_rel_pos(0), kTolerance); EXPECT_NEAR(initial_state.rel_pos[1], initial_rel_pos(1), kTolerance); EXPECT_NEAR(initial_state.rel_pos[2], initial_rel_pos(2), kTolerance); + ASSERT_TRUE(_target_pose_sub->update()); + EXPECT_TRUE(_target_pose_sub->get().abs_pos_valid); vte_test::flushSubscription(_aid_fiducial_sub); vte_test::flushSubscription(_vte_state_sub); @@ -2332,7 +2357,11 @@ TEST_F(VTEPositionTest, ResetsFilterOnStalePredictionGap) _vte->update(matrix::Vector3f{}); EXPECT_FALSE(_aid_fiducial_sub->update()); - EXPECT_FALSE(_vte_state_sub->update()); + // The logged state is marked invalid. The pose is not republished: precision landing keeps + // the last one it received as the last known target location. + ASSERT_TRUE(_vte_state_sub->update()); + EXPECT_FALSE(_vte_state_sub->get().rel_pos_valid); + EXPECT_FALSE(_target_pose_sub->update()); publishVisionPos(fresh_rel_pos, vte_test::identityQuat(), kSmallVisionCov, vte_test::advanceMicroseconds(kStepUs)); diff --git a/src/modules/vision_target_estimator/test/TEST_VTE_VisionTargetEst.cpp b/src/modules/vision_target_estimator/test/TEST_VTE_VisionTargetEst.cpp index 67eec226bb1..355f519dc87 100644 --- a/src/modules/vision_target_estimator/test/TEST_VTE_VisionTargetEst.cpp +++ b/src/modules/vision_target_estimator/test/TEST_VTE_VisionTargetEst.cpp @@ -297,10 +297,11 @@ protected: ASSERT_TRUE(_attitude_pub->publish(msg)); } - void publishAcceleration(const matrix::Vector3f &accel, hrt_abstime timestamp) + void publishAcceleration(const matrix::Vector3f &accel, hrt_abstime timestamp, hrt_abstime sample_time = 0) { vehicle_acceleration_s msg{}; msg.timestamp = timestamp; + msg.timestamp_sample = sample_time; msg.xyz[0] = accel(0); msg.xyz[1] = accel(1); msg.xyz[2] = accel(2); @@ -571,21 +572,22 @@ TEST_F(VisionTargetEstTest, ComputeGpsVelocityOffsetRequiresOffsetAndData) expectVectorNear(vel_offset, expected_offset); } -// WHY: Attitude is required to transform body-frame data into NED. -// WHAT: Call pollEstimatorInput without attitude and expect it to fail. -TEST_F(VisionTargetEstTest, PollEstimatorInputRequiresAttitude) +// Acceleration arriving before attitude must remain available for the next poll. +TEST_F(VisionTargetEstTest, PollEstimatorInputKeepsAccelerationUntilAttitudeArrives) { - // GIVEN: No vehicle attitude sample has been published. - matrix::Vector3f acc_ned{}; + const hrt_abstime sample_time = vte_test::nowUs(); + const hrt_abstime now = vte_test::advanceMicroseconds(1_ms); + publishAcceleration(matrix::Vector3f(1.f, 2.f, -kGravity), now, sample_time); + matrix::Vector3f acc_ned{}, gps_offset{}, vel_offset{}; matrix::Quaternionf q_att{}; - matrix::Vector3f gps_offset{}; - matrix::Vector3f vel_offset{}; - const bool vel_offset_updated = true; - bool acc_valid = true; + bool acc_valid = false; + EXPECT_FALSE(_vte->pollEstimatorInput(acc_ned, q_att, gps_offset, vel_offset, false, acc_valid)); - // WHEN: The estimator input is polled. - EXPECT_FALSE(_vte->pollEstimatorInput(acc_ned, q_att, gps_offset, vel_offset, - vel_offset_updated, acc_valid)); + publishAttitude(vte_test::identityQuat(), now); + ASSERT_TRUE(_vte->pollEstimatorInput(acc_ned, q_att, gps_offset, vel_offset, false, acc_valid)); + EXPECT_TRUE(acc_valid); + EXPECT_EQ(_vte->_vehicle_acc_body.timestamp, sample_time); + expectVectorNear(acc_ned, matrix::Vector3f(1.f, 2.f, 0.f)); } // WHY: Valid inputs should be rotated into NED and include gravity compensation. @@ -1346,7 +1348,7 @@ TEST_F(VisionTargetEstTest, UpdateEstimatorsDropsStaleAcceleration) const hrt_abstime stale_timestamp = timestamp - (VisionTargetEstTestable::kAccUpdatedTimeoutUs + 1_ms); - publishAcceleration(matrix::Vector3f(0.1f, -0.2f, 9.5f), stale_timestamp); + publishAcceleration(matrix::Vector3f(0.1f, -0.2f, 9.5f), timestamp, stale_timestamp); // WHEN: Estimator updates run with stale acceleration input. _vte->updateEstimators(); @@ -1471,6 +1473,22 @@ TEST_F(VisionTargetEstTest, PollEstimatorInputResetsCachedFrame) } } +// WHY: A non-finite acceleration sample must not reach the prediction, which would otherwise skip a step. +TEST_F(VisionTargetEstTest, PollEstimatorInputRejectsNonFiniteAcceleration) +{ + matrix::Vector3f acceleration{}, position_offset{}, velocity_offset{}; + matrix::Quaternionf attitude{}; + bool acceleration_valid = true; + const hrt_abstime now = vte_test::advanceMicroseconds(kStepUs); + publishAcceleration(matrix::Vector3f(NAN, 0.f, 0.f), now); + publishAttitude(vte_test::identityQuat(), now); + + ASSERT_TRUE(_vte->pollEstimatorInput(acceleration, attitude, position_offset, velocity_offset, false, + acceleration_valid)); + EXPECT_FALSE(acceleration_valid); + EXPECT_TRUE(acceleration.isAllFinite()); +} + TEST_F(VisionTargetEstTest, PollLocalPoseUsesSampleTime) { uORB::Publication publication{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/vision_target_estimator/vision_target_estimator_params.yaml b/src/modules/vision_target_estimator/vision_target_estimator_params.yaml index 219dd6f9753..26bf4123982 100644 --- a/src/modules/vision_target_estimator/vision_target_estimator_params.yaml +++ b/src/modules/vision_target_estimator/vision_target_estimator_params.yaml @@ -53,7 +53,7 @@ parameters: type: float default: 3.0 unit: s - min: 0.0 + min: 0.1 max: 50 decimal: 1 increment: 0.5 @@ -66,7 +66,7 @@ parameters: type: float default: 2.0 unit: s - min: 0.0 + min: 0.1 max: 50 decimal: 2 increment: 0.25 @@ -77,7 +77,7 @@ parameters: type: float default: 1.0 unit: s - min: 0.0 + min: 0.1 max: 10 decimal: 2 increment: 0.05 @@ -88,7 +88,7 @@ parameters: type: float default: 0.1 unit: s - min: 0.0 + min: 0.01 max: 5 decimal: 3 increment: 0.01