From 44e3bd6c1c0a0d19b3089163b92120b78ada0b3e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Mar 2018 16:02:43 -0400 Subject: [PATCH] navigator follow_target move to matrix lib --- src/modules/navigator/follow_target.cpp | 4 ---- src/modules/navigator/follow_target.h | 16 ++++++++-------- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 12af5ce22d..aab2c2f878 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -97,7 +97,6 @@ void FollowTarget::on_activation() void FollowTarget::on_active() { struct map_projection_reference_s target_ref; - math::Vector<3> target_reported_velocity(0, 0, 0); follow_target_s target_motion_with_offset = {}; uint64_t current_time = hrt_absolute_time(); bool _radius_entered = false; @@ -128,9 +127,6 @@ void FollowTarget::on_active() _current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)( 1 - _responsiveness); - target_reported_velocity(0) = _current_target_motion.vx; - target_reported_velocity(1) = _current_target_motion.vy; - } else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) { reset_target_validity(); } diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 742f391246..51f1e39d94 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -125,13 +125,13 @@ private: uint64_t _target_updates{0}; uint64_t _last_update_time{0}; - math::Vector<3> _current_vel; - math::Vector<3> _step_vel; - math::Vector<3> _est_target_vel; - math::Vector<3> _target_distance; - math::Vector<3> _target_position_offset; - math::Vector<3> _target_position_delta; - math::Vector<3> _filtered_target_position_delta; + matrix::Vector3f _current_vel; + matrix::Vector3f _step_vel; + matrix::Vector3f _est_target_vel; + matrix::Vector3f _target_distance; + matrix::Vector3f _target_position_offset; + matrix::Vector3f _target_position_delta; + matrix::Vector3f _filtered_target_position_delta; follow_target_s _current_target_motion{}; follow_target_s _previous_target_motion{}; @@ -148,7 +148,7 @@ private: ATT_RATES = 3 }; - math::Matrix<3, 3> _rot_matrix; + matrix::Dcmf _rot_matrix; void track_target_position(); void track_target_velocity();