/// set_wp_destination waypoint using position vector (distance from home in cm) /// terrain_alt should be true if destination.z is a desired altitude above terrain bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) { Vector3f origin; // if waypoint controller is active use the existing position target as the origin if ((AP_HAL::millis() - _wp_last_update) < 1000) { origin = _pos_control.get_pos_target(); } else { // if waypoint controller is not active, set origin to reasonable stopping point (using curr pos and velocity) _pos_control.get_stopping_point_xy(origin); _pos_control.get_stopping_point_z(origin); } // convert origin to alt-above-terrain if (terrain_alt) { float origin_terr_offset; if (!get_terrain_offset(origin_terr_offset)) { return false; } origin.z -= origin_terr_offset; } // set origin and destination return set_wp_origin_and_destination(origin, destination, terrain_alt); }
/// set_spline_destination waypoint using position vector (distance from home in cm) /// returns false if conversion from location to vector from ekf origin cannot be calculated /// terrain_alt should be true if destination.z is a desired altitudes above terrain (false if its desired altitudes above ekf origin) /// stopped_at_start should be set to true if vehicle is stopped at the origin /// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type /// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination) { Vector3f origin; // if waypoint controller is active and copter has reached the previous waypoint use current pos target as the origin if ((AP_HAL::millis() - _wp_last_update) < 1000) { origin = _pos_control.get_pos_target(); }else{ // otherwise calculate origin from the current position and velocity _pos_control.get_stopping_point_xy(origin); _pos_control.get_stopping_point_z(origin); } // convert origin to alt-above-terrain if (terrain_alt) { float terr_offset; if (!get_terrain_offset(terr_offset)) { return false; } origin.z -= terr_offset; } // set origin and destination return set_spline_origin_and_destination(origin, destination, terrain_alt, stopped_at_start, seg_end_type, next_destination); }
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) /// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin) /// returns false on failure (likely caused by missing terrain data) bool AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt) { // store origin and destination locations _origin = origin; _destination = destination; _terrain_alt = terrain_alt; Vector3f pos_delta = _destination - _origin; _track_length = pos_delta.length(); // get track length // calculate each axis' percentage of the total distance to the destination if (is_zero(_track_length)) { // avoid possible divide by zero _pos_delta_unit.x = 0; _pos_delta_unit.y = 0; _pos_delta_unit.z = 0; }else{ _pos_delta_unit = pos_delta/_track_length; } // calculate leash lengths calculate_wp_leash_length(); // initialise yaw heading if (_track_length >= WPNAV_YAW_DIST_MIN) { _yaw = get_bearing_cd(_origin, _destination); } else { // set target yaw to current heading target _yaw = _attitude_control.get_att_target_euler_cd().z; } // get origin's alt-above-terrain float origin_terr_offset = 0.0f; if (terrain_alt) { if (!get_terrain_offset(origin_terr_offset)) { return false; } } // initialise intermediate point to the origin _pos_control.set_pos_target(origin + Vector3f(0,0,origin_terr_offset)); _track_desired = 0; // target is at beginning of track _flags.reached_destination = false; _flags.fast_waypoint = false; // default waypoint back to slow _flags.slowing_down = false; // target is not slowing down yet _flags.segment_type = SEGMENT_STRAIGHT; _flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition // initialise the limited speed to current speed along the track const Vector3f &curr_vel = _inav.get_velocity(); // get speed along track (note: we convert vertical speed into horizontal speed equivalent) 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; _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms); return true; }
/// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) /// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if desired altitudes above ekf origin) /// seg_type should be calculated by calling function based on the mission bool AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination) { // mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint bool prev_segment_exists = (_flags.reached_destination && ((AP_HAL::millis() - _wp_last_update) < 1000)); float dt = _pos_control.get_dt_xy(); // check _wp_accel_cms is reasonable to avoid divide by zero if (_wp_accel_cms <= 0) { _wp_accel_cms.set_and_save(WPNAV_ACCELERATION); } // segment start types // stop - vehicle is not moving at origin // straight-fast - vehicle is moving, previous segment is straight. vehicle will fly straight through the waypoint before beginning it's spline path to the next wp // _flag.segment_type holds whether prev segment is straight vs spline but we don't know if it has a delay // spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination) // calculate spline velocity at origin if (stopped_at_start || !prev_segment_exists) { // if vehicle is stopped at the origin, set origin velocity to 0.02 * distance vector from origin to destination _spline_origin_vel = (destination - origin) * dt; _spline_time = 0.0f; _spline_vel_scaler = 0.0f; }else{ // look at previous segment to determine velocity at origin if (_flags.segment_type == SEGMENT_STRAIGHT) { // previous segment is straight, vehicle is moving so vehicle should fly straight through the origin // before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination _spline_origin_vel = (_destination - _origin); _spline_time = 0.0f; // To-Do: this should be set based on how much overrun there was from straight segment? _spline_vel_scaler = _pos_control.get_vel_target().length(); // start velocity target from current target velocity }else{ // previous segment is splined, vehicle will fly through origin // we can use the previous segment's destination velocity as this segment's origin velocity // Note: previous segment will leave destination velocity parallel to position difference vector // from previous segment's origin to this segment's destination) _spline_origin_vel = _spline_destination_vel; if (_spline_time > 1.0f && _spline_time < 1.1f) { // To-Do: remove hard coded 1.1f _spline_time -= 1.0f; }else{ _spline_time = 0.0f; } // Note: we leave _spline_vel_scaler as it was from end of previous segment } } // calculate spline velocity at destination switch (seg_end_type) { case SEGMENT_END_STOP: // if vehicle stops at the destination set destination velocity to 0.02 * distance vector from origin to destination _spline_destination_vel = (destination - origin) * dt; _flags.fast_waypoint = false; break; case SEGMENT_END_STRAIGHT: // if next segment is straight, vehicle's final velocity should face along the next segment's position _spline_destination_vel = (next_destination - destination); _flags.fast_waypoint = true; break; case SEGMENT_END_SPLINE: // if next segment is splined, vehicle's final velocity should face parallel to the line from the origin to the next destination _spline_destination_vel = (next_destination - origin); _flags.fast_waypoint = true; break; } // code below ensures we don't get too much overshoot when the next segment is short float vel_len = _spline_origin_vel.length() + _spline_destination_vel.length(); float pos_len = (destination - origin).length() * 4.0f; if (vel_len > pos_len) { // if total start+stop velocity is more than twice position difference // use a scaled down start and stop velocityscale the start and stop velocities down float vel_scaling = pos_len / vel_len; // update spline calculator update_spline_solution(origin, destination, _spline_origin_vel * vel_scaling, _spline_destination_vel * vel_scaling); }else{ // update spline calculator update_spline_solution(origin, destination, _spline_origin_vel, _spline_destination_vel); } // initialise yaw heading to current heading _yaw = _attitude_control.get_att_target_euler_cd().z; // store origin and destination locations _origin = origin; _destination = destination; _terrain_alt = terrain_alt; // calculate slow down distance calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms); // get alt-above-terrain float terr_offset = 0.0f; if (terrain_alt) { if (!get_terrain_offset(terr_offset)) { return false; } } // initialise intermediate point to the origin _pos_control.set_pos_target(origin + Vector3f(0,0,terr_offset)); _flags.reached_destination = false; _flags.segment_type = SEGMENT_SPLINE; _flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition 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; Vector3f track_covered_pos = _pos_delta_unit * track_covered; track_error = curr_delta - track_covered_pos; // 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 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) { track_desired_max = track_covered; }else{ 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_kP(); if (kP >= 0.0f) { // 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; } } } } // successfully advanced along track 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); _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; }