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

AP_L1_Control: update_waypoint gets dist_min argument 

L1_dist can become extremely short for slow moving vehicles leading to unnecessarily jerk turns after passing waypoints.  This reduces the minimum length along the track that the vehicle will point.
This commit is contained in:
Randy Mackay 2018-03-15 11:25:42 +09:00
parent 41bd79a387
commit 4f0b26bd48
3 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -195,7 +195,7 @@ void AP_L1_Control::_prevent_indecision(float &Nu)
}
// update L1 control for waypoint navigation
void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP)
void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min)
{
struct Location _current_loc;
@ -238,7 +238,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
// Calculate time varying control parameters
// Calculate the L1 length required for specified period
// 0.3183099 = 1/1/pipi
_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
_L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * groundSpeed, dist_min);
// Calculate the NE position of WP B relative to WP A
Vector2f AB = location_diff(prev_WP, next_WP);

2
libraries/AP_L1_Control/AP_L1_Control.h
View File

@ -50,7 +50,7 @@ public:
float turn_distance(float wp_radius) const;
float turn_distance(float wp_radius, float turn_angle) const;
float loiter_radius (const float loiter_radius) const;
void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP);
void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min = 0.0f);
void update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction);
void update_heading_hold(int32_t navigation_heading_cd);
void update_level_flight(void);

2
libraries/AP_Navigation/AP_Navigation.h
View File

@ -67,7 +67,7 @@ public:
// main flight code will call an output function (such as
// nav_roll_cd()) after this function to ask for the new required
// navigation attitude/steering.
virtual void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP) = 0;
virtual void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min = 0.0f) = 0;
// update the internal state of the navigation controller for when
// the vehicle has been commanded to circle about a point. This