Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

WPNav: add fast waypoints flag 

Waypoint is considered "reached" when the intermediate point reaches the
destination
This commit is contained in:
Randy Mackay 2013-05-09 00:18:02 +09:00
parent 4704b729c2
commit 9f735c8d03
2 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -255,7 +255,7 @@ void AC_WPNav::calculate_loiter_leash_length()
void AC_WPNav::set_destination(const Vector3f& destination)
{
// if waypoint controlls is active and copter has reached the previous waypoint use it for the origin
if( _reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) {
if( _flags.reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) {
_origin = _destination;
}else{
// otherwise calculate origin from the current position and velocity
@ -286,10 +286,13 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
// initialise intermediate point to the origin
_track_desired = 0;
_target = origin;
_reached_destination = false;
_flags.reached_destination = false;
// reset limited speed to zero to slow initial acceleration away from wp
_limited_speed_xy_cms = 0;
// default waypoint back to slow
_flags.fast_waypoint = false;
}
/// advance_target_along_track - move target location along track from origin to destination
@ -341,12 +344,18 @@ void AC_WPNav::advance_target_along_track(float dt)
_target.z = _origin.z + (_pos_delta_unit.z * _track_desired)/_vert_track_scale;
// check if we've reached the waypoint
if( !_reached_destination ) {
if( !_flags.reached_destination ) {
if( _track_desired >= _track_length ) {
Vector3f dist_to_dest = curr_pos - _destination;
dist_to_dest.z *=_vert_track_scale;
if( dist_to_dest.length() <= _wp_radius_cm ) {
_reached_destination = true;
// "fast" waypoints are complete once the intermediate point reaches the destination
if (_flags.fast_waypoint) {
_flags.reached_destination = true;
}else{
// regular waypoints also require the copter to be within the waypoint radius
Vector3f dist_to_dest = curr_pos - _destination;
dist_to_dest.z *=_vert_track_scale;
if( dist_to_dest.length() <= _wp_radius_cm ) {
_flags.reached_destination = true;
}
}
}
}

11
libraries/AC_WPNav/AC_WPNav.h
View File

@ -94,7 +94,10 @@ public:
int32_t get_bearing_to_destination();
/// reached_destination - true when we have come within RADIUS cm of the waypoint
bool reached_destination() const { return _reached_destination; }
bool reached_destination() const { return _flags.reached_destination; }
/// set_fast_waypoint - set to true to ignore the waypoint radius and consider the waypoint 'reached' the moment the intermediate point reaches it
void set_fast_waypoint(bool fast) { _flags.fast_waypoint = fast; }
/// update_wp - update waypoint controller
void update_wpnav();
@ -135,6 +138,11 @@ public:
static const struct AP_Param::GroupInfo var_info[];
protected:
// flags structure
struct wpnav_flags {
uint8_t reached_destination : 1; // true if we have reached the destination
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;
/// 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);
@ -208,7 +216,6 @@ protected:
float _track_length; // distance in cm between origin and destination
float _track_desired; // our desired distance along the track in cm
float _distance_to_target; // distance to loiter target
bool _reached_destination; // true if we have reached the destination
float _vert_track_scale; // vertical scaling to give altitude equal weighting to horizontal position
float _wp_leash_xy; // horizontal leash length in cm
float _limited_speed_xy_cms; // horizontal speed in cm/s used to advance the intermediate target towards the destination. used to limit extreme acceleration after passing a waypoint