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

more clean up

Browse Source
This commit is contained in:
Jimmy Johnson
2016-05-27 12:55:34 -07:00
parent f397d40f09
commit 38acd15ec6
2 changed files with 11 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/follow_target.cpp
+11 -15
View File
@@ -53,7 +53,6 @@
#include <uORB/topics/follow_target.h> #include <uORB/topics/follow_target.h>
#include <lib/geo/geo.h> #include <lib/geo/geo.h>
#include <lib/mathlib/math/Limits.hpp> #include <lib/mathlib/math/Limits.hpp>
#include <lib/mathlib/math/Quaternion.hpp>
#include "navigator.h" #include "navigator.h"
@@ -74,7 +73,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
_last_update_time(0), _last_update_time(0),
_current_target_motion({}), _current_target_motion({}),
_previous_target_motion({}), _previous_target_motion({}),
_avg_cos_ratio(0.0F),
_yaw_rate(0.0F), _yaw_rate(0.0F),
_responsiveness(0.0F), _responsiveness(0.0F),
_yaw_auto_max(0.0F), _yaw_auto_max(0.0F),
@@ -87,7 +85,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
_target_distance.zero(); _target_distance.zero();
_target_position_offset.zero(); _target_position_offset.zero();
_target_position_delta.zero(); _target_position_delta.zero();
_rot_vector.zero();
} }
FollowTarget::~FollowTarget() 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", // 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(0),
(double) _step_vel(1), // (double) _step_vel(1),
(double) _current_vel(0), // (double) _current_vel(0),
(double) _current_vel(1), // (double) _current_vel(1),
(double) _est_target_vel(0), // (double) _est_target_vel(0),
(double) _est_target_vel(1), // (double) _est_target_vel(1),
(double) (_target_distance).length(), // (double) (_target_distance).length(),
(double) (_target_position_offset + _target_distance).length(), // (double) (_target_position_offset + _target_distance).length(),
_follow_target_state, // _follow_target_state,
(double)_avg_cos_ratio, (double) _yaw_rate); // (double)_avg_cos_ratio, (double) _yaw_rate);
} }
if(target_position_valid()) { 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() void FollowTarget::reset_target_validity()
{ {
_yaw_rate = NAN; _yaw_rate = NAN;
_avg_cos_ratio = 0.0F;
_previous_target_motion = {}; _previous_target_motion = {};
_current_target_motion = {}; _current_target_motion = {};
_target_updates = 0; _target_updates = 0;
src/modules/navigator/follow_target.h
-4
View File
@@ -121,15 +121,11 @@ private:
math::Vector<3> _est_target_vel; math::Vector<3> _est_target_vel;
math::Vector<3> _target_distance; math::Vector<3> _target_distance;
math::Vector<3> _target_position_offset; math::Vector<3> _target_position_offset;
math::Vector<3> _rot_vector;
math::Vector<3> _target_position_delta; math::Vector<3> _target_position_delta;
math::Vector<3> _filtered_target_position_delta; math::Vector<3> _filtered_target_position_delta;
follow_target_s _current_target_motion; follow_target_s _current_target_motion;
follow_target_s _previous_target_motion; follow_target_s _previous_target_motion;
float _avg_cos_ratio;
double _filtered_target_lat;
double _filtered_target_lon;
float _yaw_rate; float _yaw_rate;
float _responsiveness; float _responsiveness;
float _yaw_auto_max; float _yaw_auto_max;