Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

WPNav: make get_stopping_point method public

Browse Source
This commit is contained in:
Randy Mackay 2013-05-11 16:05:42 +09:00
parent 80bd458f29
commit 61288fcb90
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -93,8 +93,8 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM
/// simple loiter controller /// simple loiter controller
/// ///
/// project_stopping_point - returns vector to stopping point based on a horizontal position and velocity /// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav::project_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const
{ {
float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt. float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt.
float linear_velocity; // the velocity we swap between linear and sqrt. float linear_velocity; // the velocity we swap between linear and sqrt.
@ -132,7 +132,7 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& veloc
{ {
Vector3f target; Vector3f target;
calculate_loiter_leash_length(); calculate_loiter_leash_length();
project_stopping_point(position, velocity, target); get_stopping_point(position, velocity, target);
_target.x = target.x; _target.x = target.x;
_target.y = target.y; _target.y = target.y;
@ -264,7 +264,7 @@ void AC_WPNav::set_destination(const Vector3f& destination)
_origin = _destination; _origin = _destination;
}else{ }else{
// otherwise calculate origin from the current position and velocity // otherwise calculate origin from the current position and velocity
project_stopping_point(_inav->get_position(), _inav->get_velocity(), _origin); get_stopping_point(_inav->get_position(), _inav->get_velocity(), _origin);
} }
// set origin and destination // set origin and destination

6
libraries/AC_WPNav/AC_WPNav.h
View File

@ -71,6 +71,9 @@ public:
/// get_angle_limit - retrieve maximum angle in centi-degrees the copter will lean /// get_angle_limit - retrieve maximum angle in centi-degrees the copter will lean
int32_t get_angle_limit() const { return _lean_angle_max; } int32_t get_angle_limit() const { return _lean_angle_max; }
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const;
/// ///
/// waypoint controller /// waypoint controller
/// ///
@ -144,9 +147,6 @@ protected:
uint8_t fast_waypoint : 1; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint uint8_t fast_waypoint : 1; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
} _flags; } _flags;
/// project_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void project_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target);
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target /// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void translate_loiter_target_movements(float nav_dt); void translate_loiter_target_movements(float nav_dt);