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

more clean up

Browse Source
This commit is contained in:
Jimmy Johnson 2016-05-26 15:35:03 -07:00
parent f397d40f09
commit 38acd15ec6
2 changed files with 11 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/navigator/follow_target.cpp
View File

@ -53,7 +53,6 @@
#include <uORB/topics/follow_target.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/mathlib/math/Quaternion.hpp>
#include "navigator.h"
@ -74,7 +73,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
_last_update_time(0),
_current_target_motion({}),
_previous_target_motion({}),
_avg_cos_ratio(0.0F),
_yaw_rate(0.0F),
_responsiveness(0.0F),
_yaw_auto_max(0.0F),
@ -87,7 +85,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
_target_distance.zero();
_target_position_offset.zero();
_target_position_delta.zero();
_rot_vector.zero();
}
FollowTarget::~FollowTarget()
@ -247,17 +244,17 @@ void FollowTarget::on_active()
}
}
warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f",
(double) _step_vel(0),
(double) _step_vel(1),
(double) _current_vel(0),
(double) _current_vel(1),
(double) _est_target_vel(0),
(double) _est_target_vel(1),
(double) (_target_distance).length(),
(double) (_target_position_offset + _target_distance).length(),
_follow_target_state,
(double)_avg_cos_ratio, (double) _yaw_rate);
// warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f",
// (double) _step_vel(0),
// (double) _step_vel(1),
// (double) _current_vel(0),
// (double) _current_vel(1),
// (double) _est_target_vel(0),
// (double) _est_target_vel(1),
// (double) (_target_distance).length(),
// (double) (_target_position_offset + _target_distance).length(),
// _follow_target_state,
// (double)_avg_cos_ratio, (double) _yaw_rate);
}
if(target_position_valid()) {
@ -374,7 +371,6 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
void FollowTarget::reset_target_validity()
{
_yaw_rate = NAN;
_avg_cos_ratio = 0.0F;
_previous_target_motion = {};
_current_target_motion = {};
_target_updates = 0;

4
src/modules/navigator/follow_target.h
View File

@ -121,15 +121,11 @@ private:
math::Vector<3> _est_target_vel;
math::Vector<3> _target_distance;
math::Vector<3> _target_position_offset;
math::Vector<3> _rot_vector;
math::Vector<3> _target_position_delta;
math::Vector<3> _filtered_target_position_delta;
follow_target_s _current_target_motion;
follow_target_s _previous_target_motion;
float _avg_cos_ratio;
double _filtered_target_lat;
double _filtered_target_lon;
float _yaw_rate;
float _responsiveness;
float _yaw_auto_max;