/// advance_spline_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);
// update velocity
float spline_dist_to_wp = (_destination - target_pos).length();
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms);
} else if(_spline_vel_scaler < _wp_speed_cms) {
// increase velocity using acceleration
// To-Do: replace 0.1f below with update frequency passed in from main program
_spline_vel_scaler += _wp_accel_cms* 0.1f;
}
// constrain target velocity
if (_spline_vel_scaler > _wp_speed_cms) {
_spline_vel_scaler = _wp_speed_cms;
}
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
float target_vel_length = target_vel.length();
if (target_vel_length != 0.0f) {
_spline_time_scale = _spline_vel_scaler/target_vel_length;
}
// update target position
_pos_control.set_pos_target(target_pos);
// update the yaw
_yaw = RadiansToCentiDegrees(fast_atan2(target_vel.y,target_vel.x));
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
// we will reach the next waypoint in the next step so set reached_destination flag
// To-Do: is this one step too early?
if (_spline_time >= 1.0f) {
_flags.reached_destination = true;
}
}
}
int32_t AP_L1_Control::bearing_error_cd(void)
{
return RadiansToCentiDegrees(_bearing_error);
}
int32_t AP_L1_Control::nav_bearing_cd(void)
{
return wrap_180_cd(RadiansToCentiDegrees(_nav_bearing));
}
/// advance_spline_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);
_pos_delta_unit = target_vel/target_vel.length();
calculate_wp_leash_length();
// get current location
Vector3f curr_pos = _inav.get_position();
// get terrain altitude offset for origin and current position (i.e. change in terrain altitude from a position vs ekf origin)
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
// calculate position error
Vector3f track_error = curr_pos - target_pos;
track_error.z -= terr_offset;
// calculate the horizontal error
float track_error_xy = pythagorous2(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get position control leash lengths
float leash_xy = _pos_control.get_leash_xy();
float leash_z;
if (track_error.z >= 0) {
leash_z = _pos_control.get_leash_up_z();
}else{
leash_z = _pos_control.get_leash_down_z();
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);
if (track_leash_slack < 0.0f) {
track_leash_slack = 0.0f;
}
// update velocity
float spline_dist_to_wp = (_destination - target_pos).length();
float vel_limit = _wp_speed_cms;
if (!is_zero(dt)) {
vel_limit = MIN(vel_limit, track_leash_slack/dt);
}
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms);
}else if(_spline_vel_scaler < vel_limit) {
// increase velocity using acceleration
_spline_vel_scaler += _wp_accel_cms * dt;
}
// constrain target velocity
_spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit);
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
float target_vel_length = target_vel.length();
if (!is_zero(target_vel_length)) {
_spline_time_scale = _spline_vel_scaler/target_vel_length;
}
// update target position
target_pos.z += terr_offset;
_pos_control.set_pos_target(target_pos);
// update the yaw
_yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x));
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
// we will reach the next waypoint in the next step so set reached_destination flag
// To-Do: is this one step too early?
if (_spline_time >= 1.0f) {
_flags.reached_destination = true;
}
}
return true;
}
/// advance_spline_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);
// if target velocity is zero the origin and destination must be the same
// so flag reached destination (and protect against divide by zero)
float target_vel_length = target_vel.length();
if (is_zero(target_vel_length)) {
_flags.reached_destination = true;
return true;
}
_pos_delta_unit = target_vel / target_vel_length;
calculate_wp_leash_length();
// get current location
Vector3f curr_pos = _inav.get_position();
// get terrain altitude offset for origin and current position (i.e. change in terrain altitude from a position vs ekf origin)
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
// calculate position error
Vector3f track_error = curr_pos - target_pos;
track_error.z -= terr_offset;
// calculate the horizontal error
_track_error_xy = norm(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get position control leash lengths
float leash_xy = _pos_control.get_leash_xy();
float leash_z;
if (track_error.z >= 0) {
leash_z = _pos_control.get_leash_up_z();
}else{
leash_z = _pos_control.get_leash_down_z();
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-_track_error_xy)/leash_xy);
if (track_leash_slack < 0.0f) {
track_leash_slack = 0.0f;
}
// update velocity
float spline_dist_to_wp = (_destination - target_pos).length();
float vel_limit = _wp_speed_cms;
if (!is_zero(dt)) {
vel_limit = MIN(vel_limit, track_leash_slack/dt);
}
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cmss);
}else if(_spline_vel_scaler < vel_limit) {
// increase velocity using acceleration
_spline_vel_scaler += _wp_accel_cmss * dt;
}
// constrain target velocity
_spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit);
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
_spline_time_scale = _spline_vel_scaler / target_vel_length;
// update target position
target_pos.z += terr_offset;
_pos_control.set_pos_target(target_pos);
// update the target yaw if origin and destination are at least 2m apart horizontally
if (_track_length_xy >= WPNAV_YAW_DIST_MIN) {
if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN) {
// if the leash is very short (i.e. flying at low speed) use the target point's velocity along the track
if (!is_zero(target_vel.x) && !is_zero(target_vel.y)) {
set_yaw_cd(RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x)));
}
} else {
// point vehicle along the leash (i.e. point vehicle towards target point on the segment from origin to destination)
float track_error_xy_length = safe_sqrt(sq(track_error.x)+sq(track_error.y));
if (track_error_xy_length > MIN(WPNAV_YAW_DIST_MIN, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN)) {
// To-Do: why is track_error sign reversed?
set_yaw_cd(RadiansToCentiDegrees(atan2f(-track_error.y,-track_error.x)));
}
}
}
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
// we will reach the next waypoint in the next step so set reached_destination flag
// To-Do: is this one step too early?
if (_spline_time >= 1.0f) {
_flags.reached_destination = true;
}
}
return true;
}
/// advance_wp_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_wp_target_along_track(float dt)
{
float track_covered; // distance (in cm) along the track that the vehicle has traveled. Measured by drawing a perpendicular line from the track to the vehicle.
Vector3f track_error; // distance error (in cm) from the track_covered position (i.e. closest point on the line to the vehicle) and the vehicle
float track_desired_max; // the farthest distance (in cm) along the track that the leash will allow
float track_leash_slack; // additional distance (in cm) along the track from our track_covered position that our leash will allow
bool reached_leash_limit = false; // true when track has reached leash limit and we need to slow down the target point
// get current location
Vector3f curr_pos = _inav.get_position();
// calculate terrain adjustments
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
// calculate 3d vector from segment's origin
Vector3f curr_delta = (curr_pos - Vector3f(0,0,terr_offset)) - _origin;
// calculate how far along the track we are
track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
// calculate the point closest to the vehicle on the segment from origin to destination
Vector3f track_covered_pos = _pos_delta_unit * track_covered;
// calculate the distance vector from the vehicle to the closest point on the segment from origin to destination
track_error = curr_delta - track_covered_pos;
// calculate the horizontal error
_track_error_xy = norm(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get up leash if we are moving up, down leash if we are moving down
float leash_z = track_error.z >= 0 ? _pos_control.get_leash_up_z() : _pos_control.get_leash_down_z();
// use pythagoras's theorem calculate how far along the track we could move the intermediate target before reaching the end of the leash
// track_desired_max is the distance from the vehicle to our target point along the track. It is the "hypotenuse" which we want to be no longer than our leash (aka _track_leash_length)
// track_error is the line from the vehicle to the closest point on the track. It is the "opposite" side
// track_leash_slack is the line from the closest point on the track to the target point. It is the "adjacent" side. We adjust this so the track_desired_max is no longer than the leash
float track_leash_length_abs = fabsf(_track_leash_length);
float track_error_max_abs = MAX(_track_leash_length*track_error_z/leash_z, _track_leash_length*_track_error_xy/_pos_control.get_leash_xy());
track_leash_slack = (track_leash_length_abs > track_error_max_abs) ? safe_sqrt(sq(_track_leash_length) - sq(track_error_max_abs)) : 0;
track_desired_max = track_covered + track_leash_slack;
// check if target is already beyond the leash
if (_track_desired > track_desired_max) {
reached_leash_limit = true;
}
// get current velocity
const Vector3f &curr_vel = _inav.get_velocity();
// get speed along track
float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
// calculate point at which velocity switches from linear to sqrt
float linear_velocity = _wp_speed_cms;
float kP = _pos_control.get_pos_xy_p().kP();
if (is_positive(kP)) { // avoid divide by zero
linear_velocity = _track_accel/kP;
}
// let the limited_speed_xy_cms be some range above or below current velocity along track
if (speed_along_track < -linear_velocity) {
// we are traveling fast in the opposite direction of travel to the waypoint so do not move the intermediate point
_limited_speed_xy_cms = 0;
}else{
// increase intermediate target point's velocity if not yet at the leash limit
if(dt > 0 && !reached_leash_limit) {
_limited_speed_xy_cms += 2.0f * _track_accel * dt;
}
// do not allow speed to be below zero or over top speed
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms, 0.0f, _track_speed);
// check if we should begin slowing down
if (!_flags.fast_waypoint) {
float dist_to_dest = _track_length - _track_desired;
if (!_flags.slowing_down && dist_to_dest <= _slow_down_dist) {
_flags.slowing_down = true;
}
// if target is slowing down, limit the speed
if (_flags.slowing_down) {
_limited_speed_xy_cms = MIN(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel));
}
}
// if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity
if (fabsf(speed_along_track) < linear_velocity) {
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity);
}
}
// advance the current target
if (!reached_leash_limit) {
_track_desired += _limited_speed_xy_cms * dt;
// reduce speed if we reach end of leash
if (_track_desired > track_desired_max) {
_track_desired = track_desired_max;
_limited_speed_xy_cms -= 2.0f * _track_accel * dt;
if (_limited_speed_xy_cms < 0.0f) {
_limited_speed_xy_cms = 0.0f;
}
}
}
// do not let desired point go past the end of the track unless it's a fast waypoint
if (!_flags.fast_waypoint) {
_track_desired = constrain_float(_track_desired, 0, _track_length);
} else {
_track_desired = constrain_float(_track_desired, 0, _track_length + WPNAV_WP_FAST_OVERSHOOT_MAX);
}
// recalculate the desired position
Vector3f final_target = _origin + _pos_delta_unit * _track_desired;
// convert final_target.z to altitude above the ekf origin
final_target.z += terr_offset;
_pos_control.set_pos_target(final_target);
// check if we've reached the waypoint
if( !_flags.reached_destination ) {
if( _track_desired >= _track_length ) {
// "fast" waypoints are complete once the intermediate point reaches the destination
if (_flags.fast_waypoint) {
_flags.reached_destination = true;
}else{
// regular waypoints also require the copter to be within the waypoint radius
Vector3f dist_to_dest = (curr_pos - Vector3f(0,0,terr_offset)) - _destination;
if( dist_to_dest.length() <= _wp_radius_cm ) {
_flags.reached_destination = true;
}
}
}
}
// update the target yaw if origin and destination are at least 2m apart horizontally
if (_track_length_xy >= WPNAV_YAW_DIST_MIN) {
if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN) {
// if the leash is short (i.e. moving slowly) and destination is at least 2m horizontally, point along the segment from origin to destination
set_yaw_cd(get_bearing_cd(_origin, _destination));
} else {
Vector3f horiz_leash_xy = final_target - curr_pos;
horiz_leash_xy.z = 0;
if (horiz_leash_xy.length() > MIN(WPNAV_YAW_DIST_MIN, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN)) {
set_yaw_cd(RadiansToCentiDegrees(atan2f(horiz_leash_xy.y,horiz_leash_xy.x)));
}
}
}
// successfully advanced along track
return true;
}