mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
WPNav: add fast waypoints flag
Waypoint is considered "reached" when the intermediate point reaches the destination
This commit is contained in:
parent
4704b729c2
commit
9f735c8d03
libraries/AC_WPNav
@ -255,7 +255,7 @@ void AC_WPNav::calculate_loiter_leash_length()
|
|||||||
void AC_WPNav::set_destination(const Vector3f& destination)
|
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 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;
|
_origin = _destination;
|
||||||
}else{
|
}else{
|
||||||
// otherwise calculate origin from the current position and velocity
|
// 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
|
// initialise intermediate point to the origin
|
||||||
_track_desired = 0;
|
_track_desired = 0;
|
||||||
_target = origin;
|
_target = origin;
|
||||||
_reached_destination = false;
|
_flags.reached_destination = false;
|
||||||
|
|
||||||
// reset limited speed to zero to slow initial acceleration away from wp
|
// reset limited speed to zero to slow initial acceleration away from wp
|
||||||
_limited_speed_xy_cms = 0;
|
_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
|
/// 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;
|
_target.z = _origin.z + (_pos_delta_unit.z * _track_desired)/_vert_track_scale;
|
||||||
|
|
||||||
// check if we've reached the waypoint
|
// check if we've reached the waypoint
|
||||||
if( !_reached_destination ) {
|
if( !_flags.reached_destination ) {
|
||||||
if( _track_desired >= _track_length ) {
|
if( _track_desired >= _track_length ) {
|
||||||
|
// "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;
|
Vector3f dist_to_dest = curr_pos - _destination;
|
||||||
dist_to_dest.z *=_vert_track_scale;
|
dist_to_dest.z *=_vert_track_scale;
|
||||||
if( dist_to_dest.length() <= _wp_radius_cm ) {
|
if( dist_to_dest.length() <= _wp_radius_cm ) {
|
||||||
_reached_destination = true;
|
_flags.reached_destination = true;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -94,7 +94,10 @@ public:
|
|||||||
int32_t get_bearing_to_destination();
|
int32_t get_bearing_to_destination();
|
||||||
|
|
||||||
/// reached_destination - true when we have come within RADIUS cm of the waypoint
|
/// 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
|
/// update_wp - update waypoint controller
|
||||||
void update_wpnav();
|
void update_wpnav();
|
||||||
@ -135,6 +138,11 @@ public:
|
|||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
protected:
|
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
|
/// 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);
|
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_length; // distance in cm between origin and destination
|
||||||
float _track_desired; // our desired distance along the track in cm
|
float _track_desired; // our desired distance along the track in cm
|
||||||
float _distance_to_target; // distance to loiter target
|
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 _vert_track_scale; // vertical scaling to give altitude equal weighting to horizontal position
|
||||||
float _wp_leash_xy; // horizontal leash length in cm
|
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
|
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
|
||||||
|
Loading…
Reference in New Issue
Block a user