From c6b68c7843d7e9a12d6249e5e139dcab55cbb2bb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 6 Apr 2013 22:25:58 +0900 Subject: [PATCH] AC_WPNav: Leonard's 3d leash calculator --- libraries/AC_WPNav/AC_WPNav.cpp | 94 ++++++++++++--------------------- libraries/AC_WPNav/AC_WPNav.h | 41 ++++++++++---- 2 files changed, 64 insertions(+), 71 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 4a11582c11..eae899ae30 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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(); -} \ No newline at end of file +} diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 03bdcd069a..3d5f63f6df 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -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;