diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 94a52f6406..83e317794a 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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_rate_lat(pid_rate_lat), _pid_rate_lon(pid_rate_lon), + _speedz_cms(MAX_CLIMB_VELOCITY), _lean_angle_max(MAX_LEAN_ANGLE) { 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; _destination = destination; - _pos_delta = _destination - _origin; - _track_length = safe_sqrt(_pos_delta.x * _pos_delta.x + _pos_delta.y * _pos_delta.y); + _pos_delta_unit = _destination - _origin; + _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; } @@ -177,44 +182,27 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt) float cross_track_dist; float track_covered; float track_desired_max; - float line_a, line_b, line_c, line_m; + float alt_error; // get current location 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 - if( _pos_delta.x == 0 && _pos_delta.y == 0) { + if( _pos_delta_unit.x == 0 && _pos_delta_unit.y == 0 ) { _target = _destination; return; } - if( _pos_delta.x == 0 ) { - // x is zero - cross_track_dist = fabsf(curr.x - _destination.x); - 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); - } + 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; + 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); // 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 _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 - float track_length_pct = _track_desired/_track_length; - _target.x = _origin.x + _pos_delta.x * track_length_pct; - _target.y = _origin.y + _pos_delta.y * track_length_pct; - _target.z = _destination.z; + _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; } /// 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 // 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. - float speed_error_lon = 0; // The velocity in cm/s. - - float speed_lat = _inav->get_latitude_velocity(); - float speed_lon = _inav->get_longitude_velocity(); - - int32_t accel_lat; - int32_t accel_lon; - int32_t accel_total; - - int16_t lat_p,lat_i,lat_d; - int16_t lon_p,lon_i,lon_d; - - // 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); + Vector3f vel_curr = _inav->get_velocity(); // current velocity in cm/s + Vector3f vel_error; // The velocity error in cm/s. + Vector2f desired_accel; // the resulting desired acceleration + float accel_total; // total acceleration in cm/s/s + + // reset last velocity if this controller has just been engaged or dt is zero + if( dt == 0.0 ) { + desired_accel.x = 0; + desired_accel.y = 0; + } else { + // feed forward desired acceleration calculation + desired_accel.x = (vel_lat - _vel_last.x)/dt; + desired_accel.y = (vel_lon - _vel_last.y)/dt; + } - lon_p = _pid_rate_lon->get_p(speed_error_lon); - lon_i = _pid_rate_lon->get_i(speed_error_lon, dt); - lon_d = _pid_rate_lon->get_d(speed_error_lon, dt); + // store this iteration's velocities for the next iteration + _vel_last.x = vel_lat; + _vel_last.y = vel_lon; - accel_lat = (lat_p+lat_i+lat_d); - accel_lon = (lon_p+lon_i+lon_d); + // calculate velocity error + 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 ) { - accel_lat = MAX_LOITER_VEL_ACCEL * accel_lat/accel_total; - accel_lon = MAX_LOITER_VEL_ACCEL * accel_lon/accel_total; + desired_accel.x = MAX_LOITER_VEL_ACCEL * desired_accel.x/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 @@ -394,4 +381,7 @@ void AC_WPNav::reset_I() _pid_pos_lat->reset_I(); _pid_rate_lon->reset_I(); _pid_rate_lat->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 c9657b91fc..18034ee4ea 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -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 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 class AC_WPNav { @@ -112,6 +113,9 @@ public: _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[]; protected: @@ -125,7 +129,7 @@ protected: /// get_loiter_vel_lat_lon - loiter velocity controller /// 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 /// converts desired accelerations provided in lat/lon frame to roll/pitch angles @@ -148,6 +152,7 @@ protected: // parameters 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 float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame float _sin_yaw; @@ -163,12 +168,15 @@ protected: // internal variables Vector3f _target; // loiter's target location in cm from home 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 _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_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 // pilot inputs for loiter int16_t _pilot_vel_forward_cms;