zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_WPNav: Leonard's ff loiter and 3d wp nav

mission-4.1.18
Randy Mackay 12 years ago
parent
926c404994
commit
0ccdce1b22
2 changed files with 63 additions and 65 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 116
      libraries/AC_WPNav/AC_WPNav.cpp
  2. 12
      libraries/AC_WPNav/AC_WPNav.h

116
libraries/AC_WPNav/AC_WPNav.cpp
Unescape View File

@ -27,6 +27,7 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
_pid_pos_lon(pid_pos_lon), _pid_pos_lon(pid_pos_lon),
_pid_rate_lat(pid_rate_lat), _pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon), _pid_rate_lon(pid_rate_lon),
_speedz_cms(MAX_CLIMB_VELOCITY),
_lean_angle_max(MAX_LEAN_ANGLE) _lean_angle_max(MAX_LEAN_ANGLE)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
@ -166,8 +167,12 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
{ {
_origin = origin; _origin = origin;
_destination = destination; _destination = destination;
_pos_delta = _destination - _origin; _pos_delta_unit = _destination - _origin;
_track_length = safe_sqrt(_pos_delta.x * _pos_delta.x + _pos_delta.y * _pos_delta.y); _track_length = _pos_delta_unit.length();
_pos_delta_unit = _pos_delta_unit/_track_length;
_hoz_track_ratio = _track_length / sqrt(_pos_delta_unit.x*_pos_delta_unit.x + _pos_delta_unit.y*_pos_delta_unit.y);
_vert_track_ratio = _track_length / _pos_delta_unit.z;
_track_desired = 0; _track_desired = 0;
} }
@ -177,44 +182,27 @@ 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_covered;
float track_desired_max; float track_desired_max;
float line_a, line_b, line_c, line_m; float alt_error;
// get current location // get current location
Vector3f curr = _inav->get_position(); Vector3f curr = _inav->get_position();
// limit velocity to maximum possible
velocity_cms = min(velocity_cms, _speed_cms) * _hoz_track_ratio;
velocity_cms = min(velocity_cms, _speedz_cms * _vert_track_ratio);
// check for zero length segment // check for zero length segment
if( _pos_delta.x == 0 && _pos_delta.y == 0) { if( _pos_delta_unit.x == 0 && _pos_delta_unit.y == 0 ) {
_target = _destination; _target = _destination;
return; return;
} }
if( _pos_delta.x == 0 ) { track_covered = (curr.x-_origin.x) * _pos_delta_unit.x + (curr.y-_origin.y) * _pos_delta_unit.y + (curr.z-_origin.z) * _pos_delta_unit.z;
// x is zero cross_track_dist = -(curr.x-_origin.x) * _pos_delta_unit.y + (curr.y-_origin.y) * _pos_delta_unit.x;
cross_track_dist = fabsf(curr.x - _destination.x); alt_error = fabsf(_origin.z + _pos_delta_unit.z * track_covered - curr.z);
track_covered = fabsf(curr.y - _origin.y);
}else if(_pos_delta.y == 0) {
// y is zero
cross_track_dist = fabsf(curr.y - _destination.y);
track_covered = fabsf(curr.x - _origin.x);
}else{
// both x and y non zero
line_a = _pos_delta.y;
line_b = -_pos_delta.x;
line_c = _pos_delta.x * _origin.y - _pos_delta.y * _origin.x;
line_m = line_a / line_b;
cross_track_dist = fabsf(line_a * curr.x + line_b * curr.y + line_c ) / _track_length;
line_m = 1/line_m;
line_a = line_m;
line_b = -1;
line_c = curr.y - line_m * curr.x;
// calculate the distance to the closest point along the track and it's distance from the origin
track_covered = fabsf(line_a*_origin.x + line_b*_origin.y + line_c) / safe_sqrt(line_a*line_a+line_b*line_b);
}
// maximum distance along the track that we will allow (stops target point from getting too far from the current position) // maximum distance along the track that we will allow (stops target point from getting too far from the current position)
track_desired_max = track_covered + safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR-cross_track_dist*cross_track_dist); track_desired_max = track_covered + min( safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - cross_track_dist*cross_track_dist) * _hoz_track_ratio, (750-alt_error) * _vert_track_ratio);
// advance the current target // advance the current target
_track_desired += velocity_cms * dt; _track_desired += velocity_cms * dt;
@ -228,10 +216,9 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
} }
// recalculate the desired position // recalculate the desired position
float track_length_pct = _track_desired/_track_length; _target.x = _origin.x + _pos_delta_unit.x * _track_desired;
_target.x = _origin.x + _pos_delta.x * track_length_pct; _target.y = _origin.y + _pos_delta_unit.y * _track_desired;
_target.y = _origin.y + _pos_delta.y * track_length_pct; _target.z = _origin.z + _pos_delta_unit.z * _track_desired;
_target.z = _destination.z;
} }
/// get_distance_to_destination - get horizontal distance to destination in cm /// get_distance_to_destination - get horizontal distance to destination in cm
@ -317,44 +304,44 @@ void AC_WPNav::get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t targ
// get_loiter_vel_lat_lon - loiter velocity controller // get_loiter_vel_lat_lon - loiter velocity controller
// converts desired velocities in lat/lon frame to accelerations in lat/lon frame // converts desired velocities in lat/lon frame to accelerations in lat/lon frame
void AC_WPNav::get_loiter_vel_lat_lon(int16_t vel_lat, int16_t vel_lon, float dt) void AC_WPNav::get_loiter_vel_lat_lon(float vel_lat, float vel_lon, float dt)
{ {
float speed_error_lat = 0; // The velocity in cm/s. Vector3f vel_curr = _inav->get_velocity(); // current velocity in cm/s
float speed_error_lon = 0; // The velocity in cm/s. Vector3f vel_error; // The velocity error in cm/s.
Vector2f desired_accel; // the resulting desired acceleration
float speed_lat = _inav->get_latitude_velocity(); float accel_total; // total acceleration in cm/s/s
float speed_lon = _inav->get_longitude_velocity();
// reset last velocity if this controller has just been engaged or dt is zero
int32_t accel_lat; if( dt == 0.0 ) {
int32_t accel_lon; desired_accel.x = 0;
int32_t accel_total; desired_accel.y = 0;
} else {
int16_t lat_p,lat_i,lat_d; // feed forward desired acceleration calculation
int16_t lon_p,lon_i,lon_d; desired_accel.x = (vel_lat - _vel_last.x)/dt;
desired_accel.y = (vel_lon - _vel_last.y)/dt;
// calculate vel error }
speed_error_lat = vel_lat - speed_lat;
speed_error_lon = vel_lon - speed_lon;
lat_p = _pid_rate_lat->get_p(speed_error_lat);
lat_i = _pid_rate_lat->get_i(speed_error_lat, dt);
lat_d = _pid_rate_lat->get_d(speed_error_lat, dt);
lon_p = _pid_rate_lon->get_p(speed_error_lon); // store this iteration's velocities for the next iteration
lon_i = _pid_rate_lon->get_i(speed_error_lon, dt); _vel_last.x = vel_lat;
lon_d = _pid_rate_lon->get_d(speed_error_lon, dt); _vel_last.y = vel_lon;
accel_lat = (lat_p+lat_i+lat_d); // calculate velocity error
accel_lon = (lon_p+lon_i+lon_d); vel_error.x = vel_lat - vel_curr.x;
vel_error.y = vel_lon - vel_curr.y;
accel_total = safe_sqrt(accel_lat*accel_lat + accel_lon*accel_lon); // combine feed foward accel with PID outpu from velocity error
desired_accel.x += _pid_rate_lat->get_pid(vel_error.x, dt);
desired_accel.y += _pid_rate_lon->get_pid(vel_error.y, dt);
// scale desired acceleration if it's beyond acceptable limit
accel_total = safe_sqrt(desired_accel.x*desired_accel.x + desired_accel.y*desired_accel.y);
if( accel_total > MAX_LOITER_VEL_ACCEL ) { if( accel_total > MAX_LOITER_VEL_ACCEL ) {
accel_lat = MAX_LOITER_VEL_ACCEL * accel_lat/accel_total; desired_accel.x = MAX_LOITER_VEL_ACCEL * desired_accel.x/accel_total;
accel_lon = MAX_LOITER_VEL_ACCEL * accel_lon/accel_total; desired_accel.y = MAX_LOITER_VEL_ACCEL * desired_accel.y/accel_total;
} }
get_loiter_accel_lat_lon(accel_lat, accel_lon); // call accel based controller with desired acceleration
get_loiter_accel_lat_lon(desired_accel.x, desired_accel.y);
} }
// get_loiter_accel_lat_lon - loiter acceration controller // get_loiter_accel_lat_lon - loiter acceration controller
@ -394,4 +381,7 @@ void AC_WPNav::reset_I()
_pid_pos_lat->reset_I(); _pid_pos_lat->reset_I();
_pid_rate_lon->reset_I(); _pid_rate_lon->reset_I();
_pid_rate_lat->reset_I(); _pid_rate_lat->reset_I();
// set last velocity to current velocity
_vel_last = _inav->get_velocity();
} }

12
libraries/AC_WPNav/AC_WPNav.h
Unescape View File

@ -21,6 +21,7 @@
#define WPINAV_MAX_POS_ERROR 2000.0f // maximum distance (in cm) that the desired track can stray from our current location. #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 WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s
#define MAX_LEAN_ANGLE 4500 // default maximum lean angle #define MAX_LEAN_ANGLE 4500 // default maximum lean angle
#define MAX_CLIMB_VELOCITY 125 // maximum climb velocity - ToDo: pull this in from main code
class AC_WPNav class AC_WPNav
{ {
@ -112,6 +113,9 @@ public:
_cos_roll = cos_roll; _cos_roll = cos_roll;
} }
/// set_climb_velocity - allows main code to pass max climb velocity to wp navigation
void set_climb_velocity(float velocity_cms) { _speedz_cms = velocity_cms; };
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
protected: protected:
@ -125,7 +129,7 @@ protected:
/// get_loiter_vel_lat_lon - loiter velocity controller /// get_loiter_vel_lat_lon - loiter velocity controller
/// converts desired velocities in lat/lon frame to accelerations in lat/lon frame /// converts desired velocities in lat/lon frame to accelerations in lat/lon frame
void get_loiter_vel_lat_lon(int16_t vel_lat, int16_t vel_lon, float dt); void get_loiter_vel_lat_lon(float vel_lat, float vel_lon, float dt);
/// get_loiter_accel_lat_lon - loiter acceration controller /// get_loiter_accel_lat_lon - loiter acceration controller
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles /// converts desired accelerations provided in lat/lon frame to roll/pitch angles
@ -148,6 +152,7 @@ protected:
// parameters // parameters
AP_Float _speed_cms; // default horizontal speed in cm/s AP_Float _speed_cms; // default horizontal speed in cm/s
float _speedz_cms; // max vertical climb rate in cm/s. To-Do: rename or pull this from main code
uint32_t _last_update; // time of last update call uint32_t _last_update; // time of last update call
float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
float _sin_yaw; float _sin_yaw;
@ -163,12 +168,15 @@ protected:
// internal variables // internal variables
Vector3f _target; // loiter's target location in cm from home Vector3f _target; // loiter's target location in cm from home
Vector3f _target_vel; // loiter Vector3f _target_vel; // loiter
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 _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 _destination; // target destination in cm from home (equivalent to next_WP)
Vector3f _pos_delta; // position difference between origin and destination Vector3f _pos_delta_unit; // position difference between origin and destination
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
float _hoz_track_ratio; // horizontal component of track to next waypoint
float _vert_track_ratio; // vertical component of track to next waypoint
// pilot inputs for loiter // pilot inputs for loiter
int16_t _pilot_vel_forward_cms; int16_t _pilot_vel_forward_cms;

Write Preview
Loading…
Cancel
Save