Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: Leonard's 3d leash calculator

Browse Source
This commit is contained in:
Randy Mackay 2013-04-06 22:25:58 +09:00
parent 74e1c2e660
commit c6b68c7843
2 changed files with 64 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -109,9 +109,9 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
// constrain the velocity vector and scale if necessary
vel_total = safe_sqrt(_target_vel.x*_target_vel.x + _target_vel.y*_target_vel.y);
if( vel_total > MAX_LOITER_POS_VEL_VELOCITY ) {
_target_vel.x = MAX_LOITER_POS_VEL_VELOCITY * _target_vel.x/vel_total;
_target_vel.y = MAX_LOITER_POS_VEL_VELOCITY * _target_vel.y/vel_total;
if( vel_total > MAX_LOITER_POS_VELOCITY ) {
_target_vel.x = MAX_LOITER_POS_VELOCITY * _target_vel.x/vel_total;
_target_vel.y = MAX_LOITER_POS_VELOCITY * _target_vel.y/vel_total;
}
// update target position
@ -157,7 +157,7 @@ void AC_WPNav::update_loiter()
// translate any adjustments from pilot to loiter target
translate_loiter_target_movements(dt);
// run loiter position controller
get_loiter_pos_lat_lon(_target.x, _target.y, dt);
}
@ -175,25 +175,14 @@ void AC_WPNav::set_destination(const Vector3f& destination)
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
{
Vector3f pos_delta = destination - origin;
_origin = origin;
_destination = destination;
_vert_track_scale = WPINAV_MAX_POS_ERROR / WPINAV_MAX_ALT_ERROR;
Vector3f pos_delta = _destination - _origin;
pos_delta.z = pos_delta.z * _vert_track_scale;
_track_length = pos_delta.length();
_pos_delta_pct = pos_delta/_track_length;
// calculate proportion of track that is horizontal
if( pos_delta.x == 0 && pos_delta.y == 0 ) {
_hoz_track_ratio = 0;
}else{
_hoz_track_ratio = _track_length / safe_sqrt(pos_delta.x*pos_delta.x + pos_delta.y*pos_delta.y);
}
// calculate proportion of track that is horizontal
if( pos_delta.z == 0 ) {
_vert_track_ratio = 0;
}else{
_vert_track_ratio = _track_length / fabsf(pos_delta.z);
}
_pos_delta_unit = pos_delta/_track_length;
_track_desired = 0;
}
@ -201,50 +190,32 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
/// advance_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
{
float cross_track_dist;
//float cross_track_dist;
float track_covered;
float track_error;
float track_desired_max;
float alt_error;
float hor_track_allowance, vert_track_allowance;
float track_extra_max;
float curr_delta_length;
//float alt_error;
// get current location
Vector3f curr = _inav->get_position();
Vector3f curr_delta = _inav->get_position() - _origin;
curr_delta.z = curr_delta.z * _vert_track_scale;
curr_delta_length = curr_delta.length();
// limit velocity to maximum possible
// to-do: assuming waypoint speed are not changed often we could move this to the set_origin_and_destination function
// no need to limit by horizontal speed if there is no horizontal component
if( _hoz_track_ratio >= 0 ) {
velocity_cms = min(velocity_cms, _speed_cms) * _hoz_track_ratio;
}
// no need to limit by vertical speed if there is no vertical component
if( _vert_track_ratio >= 0 ) {
velocity_cms = min(velocity_cms, _speedz_cms * _vert_track_ratio);
}
track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
track_error = safe_sqrt(curr_delta_length*curr_delta_length - track_covered*track_covered);
track_covered = (curr.x-_origin.x) * _pos_delta_pct.x + (curr.y-_origin.y) * _pos_delta_pct.y + (curr.z-_origin.z) * _pos_delta_pct.z;
cross_track_dist = -(curr.x-_origin.x) * _pos_delta_pct.y + (curr.y-_origin.y) * _pos_delta_pct.x;
alt_error = fabsf(_origin.z + _pos_delta_pct.z * track_covered - curr.z);
// we don't need the following but we may want to log them to see how we are going.
// cross_track_dist = -(curr.x-_origin.x) * _pos_delta_unit.y + (curr.y-_origin.y) * _pos_delta_unit.x;
// alt_error = fabsf(_origin.z + _pos_delta_unit.z * track_covered - curr.z);
// calculate maximum horizontal distance along the track that we will allow (stops target point from getting too far from the current position)
if( _hoz_track_ratio > 0 ) {
hor_track_allowance = safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - cross_track_dist*cross_track_dist) * _hoz_track_ratio;
hor_track_allowance = max(hor_track_allowance, 0);
}
// calculate maximum vertical distance along the track that we will allow
if( _vert_track_ratio > 0 ) {
vert_track_allowance = (750-alt_error) * _vert_track_ratio;
vert_track_allowance = max(vert_track_allowance, 0);
}
if( _hoz_track_ratio > 0 && _vert_track_ratio ) {
// both vertical and horizontal components so pick minimum track allowed of the two
track_desired_max = track_covered + min(hor_track_allowance, vert_track_allowance);
}else if( _hoz_track_ratio > 0 ) {
// only horizontal component
track_desired_max = track_covered + hor_track_allowance;
}else{
// only vertical component
track_desired_max = track_covered + vert_track_allowance;
}
track_extra_max = safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - track_error*track_error);
// we could save a sqrt by doing the following and not assigning track_error
// track_extra_max = safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - (curr_delta_length*curr_delta_length - track_covered*track_covered));
track_desired_max = track_covered + track_extra_max;
// advance the current target
_track_desired += velocity_cms * dt;
@ -257,9 +228,10 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
_track_desired = constrain(_track_desired, 0, _track_length);
// recalculate the desired position
_target.x = _origin.x + _pos_delta_pct.x * _track_desired;
_target.y = _origin.y + _pos_delta_pct.y * _track_desired;
_target.z = _origin.z + _pos_delta_pct.z * _track_desired;
_target.x = _origin.x + _pos_delta_unit.x * _track_desired;
_target.y = _origin.y + _pos_delta_unit.y * _track_desired;
_target.z = _origin.z + (_pos_delta_unit.z * _track_desired)/_vert_track_scale;
// we could save a divide by having a variable for 1/_vert_track_scale
}
/// get_distance_to_destination - get horizontal distance to destination in cm
@ -421,4 +393,4 @@ void AC_WPNav::reset_I()
// set last velocity to current velocity
_vel_last = _inav->get_velocity();
}
}

41
libraries/AC_WPNav/AC_WPNav.h
View File

@ -18,13 +18,35 @@
#define MAX_LOITER_VEL_ACCEL 400 // max acceleration in cm/s that the loiter velocity controller will ask from the lower accel controller.
// should be 1.5 times larger than MAX_LOITER_POS_ACCEL.
// max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
#define MAX_LOITER_POS_VEL_VELOCITY 1000
#define MAX_LOITER_OVERSHOOT 1000 // maximum distance (in cm) that we will allow the target loiter point to be from the current location when switching into loiter
#define WPINAV_MAX_POS_ERROR 2000.0f // maximum distance (in cm) that the desired track can stray from our current location.
#define WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s
#define MAX_LEAN_ANGLE 4500 // default maximum lean angle
#define MAX_CLIMB_VELOCITY 125 // maximum climb velocity - ToDo: pull this in from main code
#define MAX_LEAN_ANGLE 4500 // default maximum lean angle
#define MAX_LOITER_OVERSHOOT 531 // maximum distance (in cm) that we will allow the target loiter point to be from the current location when switching into loiter
// D0 = MAX_LOITER_POS_ACCEL/(2*Pid_P^2);
// if MAX_LOITER_POS_VELOCITY > 2*D0*Pid_P
// MAX_LOITER_OVERSHOOT = D0 + MAX_LOITER_POS_VELOCITY.^2 ./ (2*MAX_LOITER_POS_ACCEL);
// else
// MAX_LOITER_OVERSHOOT = min(200, MAX_LOITER_POS_VELOCITY/Pid_P); // to stop it being over sensitive to error
// end
#define WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s
#define WPINAV_MAX_POS_ERROR 531.25f // maximum distance (in cm) that the desired track can stray from our current location.
// D0 = MAX_LOITER_POS_ACCEL/(2*Pid_P^2);
// if WP_SPEED > 2*D0*Pid_P
// WPINAV_MAX_POS_ERROR = D0 + WP_SPEED.^2 ./ (2*MAX_LOITER_POS_ACCEL);
// else
// WPINAV_MAX_POS_ERROR = min(200, WP_SPEED/Pid_P); // to stop it being over sensitive to error
// end
// This should use the current waypoint max speed though rather than the default
#define MAX_CLIMB_VELOCITY 125 // maximum climb velocity - ToDo: pull this in from main code
#define WPINAV_MAX_ALT_ERROR 62.50f // maximum distance (in cm) that the desired track can stray from our current location.
// D0 = ALT_HOLD_ACCEL_MAX/(2*Pid_P^2);
// if g.pilot_velocity_z_max > 2*D0*Pid_P
// WPINAV_MAX_ALT_ERROR = D0 + MAX_CLIMB_VELOCITY.^2 ./ (2*ALT_HOLD_ACCEL_MAX);
// else
// WPINAV_MAX_ALT_ERROR = min(100, MAX_CLIMB_VELOCITY/Pid_P); // to stop it being over sensitive to error
// end
class AC_WPNav
{
public:
@ -65,7 +87,7 @@ public:
/// clear_angle_limit - reset angle limits back to defaults
void clear_angle_limit() { _lean_angle_max = MAX_LEAN_ANGLE; }
/// get_angle_limit - retrieve maximum angle in centi-degrees the copter will lean
int32_t get_angle_limit() { return _lean_angle_max; }
@ -176,12 +198,11 @@ protected:
Vector3f _vel_last; // previous iterations velocity in cm/s
Vector3f _origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP)
Vector3f _destination; // target destination in cm from home (equivalent to next_WP)
Vector3f _pos_delta_pct; // each axis's percentage of the total track from origin to destination
Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination
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
float _hoz_track_ratio; // horizontal component of track to next waypoint
float _vert_track_ratio; // vertical component of track to next waypoint
float _vert_track_scale; // vertical scaling to give altitude equal weighting to position
// pilot inputs for loiter
int16_t _pilot_vel_forward_cms;