fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 00:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

follow_target: use actual velocity measurement

Browse Source
This commit is contained in:
Alessandro Simovic
2021-05-05 16:13:50 -04:00
committed by Daniel Agar
parent 5d9b3504f7
commit 2d6bc9b6e0
1 changed files with 8 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/follow_target.cpp
+8 -1
View File
@@ -156,7 +156,14 @@ void FollowTarget::on_active()
&(_target_position_delta(0)), &(_target_position_delta(1)));
// update the average velocity of the target based on the position
_est_target_vel = _target_position_delta / (dt_ms / 1000.0f);
if (PX4_ISFINITE(_current_target_motion.vx) && PX4_ISFINITE(_current_target_motion.vy)) {
// No need to estimate target velocity if we can take it from the target
_est_target_vel(0) = _current_target_motion.vx;
_est_target_vel(1) = _current_target_motion.vy;
_est_target_vel(2) = 0.0f;
} else {
_est_target_vel = _target_position_delta / (dt_ms / 1000.0f);
}
// if the target is moving add an offset and rotation
if (_est_target_vel.length() > .5F) {