From 32c27b32aaf0259ac0ff5d70624c089d4dc5840f Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 4 Jan 2020 16:47:59 +1030 Subject: [PATCH] AC_WPNav: support for SCurve navigation set_wp_destination clear yaw target limit velocity and acceleration based on track slope add is_active() and remove unused reached_spline_destination init accepts desired speed set_kinematic_limits uses current speed limits instead of defaults add time compression to prevent target moving too fast for air implement alternative spline remove vel-target-length set_wp_destination always calculates this leg set_kinematic_limits moved to scurve fix origin speed after spline segment spline terrain following fix handle s-curves with mismatching alt types fix set_spline_destination_next add update_track_with_speed_accel_limits Change to next waypoint at corner apex use scurve advance along track remove unused definitions and out-of-date todo set_spline_destination_next sets fast_waypoint scurve origin speed set from spline target velocity fixup takeoff delay --- libraries/AC_WPNav/AC_WPNav.cpp | 1140 ++++++++++++---------------- libraries/AC_WPNav/AC_WPNav.h | 185 ++--- libraries/AC_WPNav/AC_WPNav_OA.cpp | 10 +- libraries/AC_WPNav/AC_WPNav_OA.h | 6 +- 4 files changed, 550 insertions(+), 791 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 78e5d86dc1..819f2a7f29 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -67,6 +67,14 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @User: Advanced AP_GROUPINFO("RFND_USE", 10, AC_WPNav, _rangefinder_use, 1), + // @Param: JERK + // @DisplayName: Waypoint Jerk + // @Description: Defines the horizontal jerk in m/s/s used during missions + // @Units: m/s/s + // @Range: 1 20 + // @User: Standard + AP_GROUPINFO("JERK", 11, AC_WPNav, _wp_jerk, 1.0f), + AP_GROUPEND }; @@ -85,9 +93,6 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC // init flags _flags.reached_destination = false; _flags.fast_waypoint = false; - _flags.slowing_down = false; - _flags.recalc_wp_leash = false; - _flags.new_wp_destination = false; _flags.segment_type = SEGMENT_STRAIGHT; // sanity check some parameters @@ -118,9 +123,10 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const /// /// wp_and_spline_init - initialise straight line and spline waypoint controllers +/// speed_cms should be a positive value or left at zero to use the default speed /// updates target roll, pitch targets and I terms based on vehicle lean angles /// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination -void AC_WPNav::wp_and_spline_init() +void AC_WPNav::wp_and_spline_init(float speed_cms) { // check _wp_accel_cmss is reasonable if (_wp_accel_cmss <= 0) { @@ -130,24 +136,48 @@ void AC_WPNav::wp_and_spline_init() // initialise position controller _pos_control.set_desired_accel_xy(0.0f,0.0f); _pos_control.init_xy_controller(); - _pos_control.clear_desired_velocity_ff_z(); // initialise feed forward velocity to zero _pos_control.set_desired_velocity_xy(0.0f, 0.0f); // initialize the desired wp speed if not already done - _wp_desired_speed_xy_cms = _wp_speed_cms; + _wp_desired_speed_xy_cms = is_positive(speed_cms) ? speed_cms : _wp_speed_cms; // initialise position controller speed and acceleration - _pos_control.set_max_speed_xy(_wp_speed_cms); + _pos_control.set_max_speed_xy(_wp_desired_speed_xy_cms); _pos_control.set_max_accel_xy(_wp_accel_cmss); _pos_control.set_max_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms); _pos_control.set_max_accel_z(_wp_accel_z_cmss); _pos_control.calc_leash_length_xy(); _pos_control.calc_leash_length_z(); - // initialise yaw heading to current heading target + // calculate scurve jerk and jerk time + if (!is_positive(_wp_jerk)) { + _wp_jerk = _wp_accel_cmss; + } + calc_scurve_jerk_and_jerk_time(); + + _scurve_prev_leg.init(); + _scurve_this_leg.init(); + _scurve_next_leg.init(); + _track_scalar_dt = 1.0f; + + // reset input shaped terrain offsets + _pos_terrain_offset = 0.0f; + _vel_terrain_offset = 0.0f; + _accel_terrain_offset = 0.0f; + + // set flag so get_yaw() returns current heading target _flags.wp_yaw_set = false; + _flags.reached_destination = false; + _flags.fast_waypoint = false; + + // initialise origin and destination to stopping point + Vector3f stopping_point; + get_wp_stopping_point(stopping_point); + _origin = _destination = stopping_point; + _terrain_alt = false; + _this_leg_is_spline = false; } /// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation @@ -156,6 +186,8 @@ void AC_WPNav::set_speed_xy(float speed_cms) // range check target speed if (speed_cms >= WPNAV_WP_SPEED_MIN) { _wp_desired_speed_xy_cms = speed_cms; + _pos_control.set_max_speed_xy(_wp_desired_speed_xy_cms); + update_track_with_speed_accel_limits(); } } @@ -163,21 +195,19 @@ void AC_WPNav::set_speed_xy(float speed_cms) void AC_WPNav::set_speed_up(float speed_up_cms) { _pos_control.set_max_speed_z(_pos_control.get_max_speed_down(), speed_up_cms); - // flag that wp leash must be recalculated - _flags.recalc_wp_leash = true; + update_track_with_speed_accel_limits(); } /// set current target descent rate during wp navigation void AC_WPNav::set_speed_down(float speed_down_cms) { _pos_control.set_max_speed_z(speed_down_cms, _pos_control.get_max_speed_up()); - // flag that wp leash must be recalculated - _flags.recalc_wp_leash = true; + update_track_with_speed_accel_limits(); } /// set_wp_destination waypoint using location class /// returns false if conversion from location to vector from ekf origin cannot be calculated -bool AC_WPNav::set_wp_destination(const Location& destination) +bool AC_WPNav::set_wp_destination_loc(const Location& destination) { bool terr_alt; Vector3f dest_neu; @@ -191,7 +221,23 @@ bool AC_WPNav::set_wp_destination(const Location& destination) return set_wp_destination(dest_neu, terr_alt); } -bool AC_WPNav::get_wp_destination(Location& destination) const +/// set next destination using location class +/// returns false if conversion from location to vector from ekf origin cannot be calculated +bool AC_WPNav::set_wp_destination_next_loc(const Location& destination) +{ + bool terr_alt; + Vector3f dest_neu; + + // convert destination location to vector + if (!get_vector_NEU(destination, dest_neu, terr_alt)) { + return false; + } + + // set target as vector from EKF origin + return set_wp_destination_next(dest_neu, terr_alt); +} + +bool AC_WPNav::get_wp_destination_loc(Location& destination) const { Vector3f dest = get_wp_destination(); if (!AP::ahrs().get_origin(destination)) { @@ -202,32 +248,103 @@ bool AC_WPNav::get_wp_destination(Location& destination) const return true; } -/// 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 +/// set_wp_destination - set destination waypoints using position vectors (distance from ekf origin in cm) +/// terrain_alt should be true if destination.z is an altitude above terrain (false if alt-above-ekf-origin) +/// returns false on failure (likely caused by missing terrain data) 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); + // re-initialise if previous destination has been interrupted + if (!is_active() || !_flags.reached_destination) { + wp_and_spline_init(_wp_desired_speed_xy_cms); } - // convert origin to alt-above-terrain - if (terrain_alt) { + _scurve_prev_leg.init(); + float origin_speed = 0.0f; + + // use previous destination as origin + _origin = _destination; + + if (terrain_alt == _terrain_alt) { + if (_this_leg_is_spline) { + // if previous leg was a spline we can use current target velocity vector for origin velocity vector + Vector3f target_velocity = _pos_control.get_desired_velocity(); + target_velocity.z -= _vel_terrain_offset; + origin_speed = target_velocity.length(); + } else { + // store previous leg + _scurve_prev_leg = _scurve_this_leg; + } + } else { + + // get current alt above terrain float origin_terr_offset; if (!get_terrain_offset(origin_terr_offset)) { return false; } - origin.z -= origin_terr_offset; + + // convert origin to alt-above-terrain if necessary + if (terrain_alt) { + // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin + _origin.z -= origin_terr_offset; + _pos_terrain_offset += origin_terr_offset; + } else { + // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain + _origin.z += origin_terr_offset; + _pos_terrain_offset -= origin_terr_offset; + } } - // set origin and destination - return set_wp_origin_and_destination(origin, destination, terrain_alt); + // update destination + _destination = destination; + _terrain_alt = terrain_alt; + + if (_flags.fast_waypoint && !_this_leg_is_spline && !_next_leg_is_spline && !_scurve_next_leg.finished()) { + _scurve_this_leg = _scurve_next_leg; + } else { + _scurve_this_leg.calculate_track(_origin, _destination, + _pos_control.get_max_speed_xy(), _pos_control.get_max_speed_up(), _pos_control.get_max_speed_down(), + _wp_accel_cmss, _wp_accel_z_cmss, + _scurve_jerk_time, _scurve_jerk * 100.0f); + if (!is_zero(origin_speed)) { + // rebuild start of scurve if we have a non-zero origin speed + _scurve_this_leg.set_origin_speed_max(origin_speed); + } + } + + _this_leg_is_spline = false; + _scurve_next_leg.init(); + _flags.fast_waypoint = false; // default waypoint back to slow + _flags.reached_destination = false; + _flags.wp_yaw_set = false; + + return true; +} + +/// set next destination using position vector (distance from ekf origin in cm) +/// terrain_alt should be true if destination.z is a desired altitude above terrain +/// provide next_destination +bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain_alt) +{ + // do not add next point if alt types don't match + if (terrain_alt != _terrain_alt) { + return true; + } + + _scurve_next_leg.calculate_track(_destination, destination, + _pos_control.get_max_speed_xy(), _pos_control.get_max_speed_up(), _pos_control.get_max_speed_down(), + _wp_accel_cmss, _wp_accel_z_cmss, + _scurve_jerk_time, _scurve_jerk * 100.0f); + if (_this_leg_is_spline) { + const float this_leg_dest_speed_max = _spline_this_leg.get_destination_speed_max(); + const float next_leg_origin_speed_max = _scurve_next_leg.set_origin_speed_max(this_leg_dest_speed_max); + _spline_this_leg.set_destination_speed_max(next_leg_origin_speed_max); + } + _next_leg_is_spline = false; + + // next destination provided so fast waypoint + _flags.fast_waypoint = true; + + return true; } /// set waypoint destination using NED position vector from ekf origin in meters @@ -237,58 +354,11 @@ bool AC_WPNav::set_wp_destination_NED(const Vector3f& destination_NED) return set_wp_destination(Vector3f(destination_NED.x * 100.0f, destination_NED.y * 100.0f, -destination_NED.z * 100.0f), false); } -/// 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) +/// set waypoint destination using NED position vector from ekf origin in meters +bool AC_WPNav::set_wp_destination_next_NED(const Vector3f& destination_NED) { - // 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 - _track_length_xy = safe_sqrt(sq(pos_delta.x)+sq(pos_delta.y)); // get horizontal track length (used to decide if we should update yaw) - - // 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(); - - // 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 - _flags.wp_yaw_set = false; - - // 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, _pos_control.get_max_speed_xy()); - - return true; + // convert NED to NEU and do not use terrain following + return set_wp_destination_next(Vector3f(destination_NED.x * 100.0f, destination_NED.y * 100.0f, -destination_NED.z * 100.0f), false); } /// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position @@ -314,7 +384,6 @@ void AC_WPNav::shift_wp_origin_to_current_pos() // move pos controller target and disable feed forward _pos_control.set_pos_target(curr_pos); - _pos_control.freeze_ff_z(); } /// shifts the origin and destination horizontally to the current position @@ -374,158 +443,134 @@ void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point) const /// 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 - const 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; + // get current position and adjust altitude to origin and destination's frame (i.e. _frame) + const Vector3f &curr_pos = _inav.get_position() - Vector3f{0, 0, terr_offset}; - // 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; + // target position, velocity and acceleration from straight line or spline calculators + Vector3f target_pos, target_vel, target_accel; + bool s_finished; - // calculate the point closest to the vehicle on the segment from origin to destination - Vector3f track_covered_pos = _pos_delta_unit * track_covered; + // Use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of aircraft + Vector3f target_velocity = _pos_control.get_desired_velocity(); + target_velocity.z -= _vel_terrain_offset; - // 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 = _pos_control.get_max_speed_xy(); - 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; + float track_error = 0.0f; + float track_velocity = 0.0f; + float track_scaler_dt = 1.0f; + // check target velocity is non-zero + if (is_positive(target_velocity.length())) { + Vector3f track_direction = target_velocity.normalized(); + track_error = _pos_control.get_pos_error().dot(track_direction); + track_velocity = _inav.get_velocity().dot(track_direction); + // set time scaler to be consistent with the achievable aircraft speed with a 5% buffer for short term variation. + track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / target_velocity.length(), 0.1f, 1.0f); + // set time scaler to not exceed the maximum vertical velocity during terrain following. + if (is_positive(target_velocity.z)) { + track_scaler_dt = MIN(track_scaler_dt, fabsf(_wp_speed_up_cms / target_velocity.z)); + } else if (is_negative(target_velocity.z)) { + track_scaler_dt = MIN(track_scaler_dt, fabsf(_wp_speed_down_cms / target_velocity.z)); } - // 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); + track_scaler_dt = 1.0f; + } + // change s-curve time speed with a time constant of maximum acceleration / maximum jerk + float track_scaler_tc = 0.01f * _wp_accel_cmss/_wp_jerk; + if (!is_zero(_wp_jerk)) { + track_scaler_tc = 0.01f * _wp_accel_cmss/_wp_jerk; + } else { + track_scaler_tc = 1.0f; + } + _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc); + + if (!_this_leg_is_spline) { + // update target position, velocity and acceleration + target_pos = _origin; + s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _flags.fast_waypoint, _track_scalar_dt * dt, target_pos, target_vel, target_accel); + } else { + // splinetarget_vel + target_vel = target_velocity; + _spline_this_leg.advance_target_along_track(_track_scalar_dt * dt, target_pos, target_vel); + s_finished = _spline_this_leg.reached_destination(); } - // recalculate the desired position - Vector3f final_target = _origin + _pos_delta_unit * _track_desired; + // input shape the terrain offset + shape_pos_vel(terr_offset, 0.0f, _pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, 0.0f, 0.0f, _pos_control.get_max_accel_z()*4.0f, 0.05f, _track_scalar_dt * dt); + update_pos_vel_accel(_pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, _track_scalar_dt * dt); + // convert final_target.z to altitude above the ekf origin - final_target.z += terr_offset; - _pos_control.set_pos_target(final_target); + target_pos.z += _pos_terrain_offset; + target_vel.z += _vel_terrain_offset; + target_accel.z += _accel_terrain_offset; + + // pass new target to the position controller + _pos_control.set_pos_vel_accel_target(target_pos, target_vel, target_accel); // check if we've reached the waypoint - if( !_flags.reached_destination ) { - if( _track_desired >= _track_length ) { + if (!_flags.reached_destination) { + if (s_finished) { // "fast" waypoints are complete once the intermediate point reaches the destination if (_flags.fast_waypoint) { _flags.reached_destination = true; - }else{ + } 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 ) { + const Vector3f dist_to_dest = curr_pos - _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))); - } + // Calculate the turn rate + float turn_rate = 0.0f; + const float target_vel_xy_len = Vector2f(target_vel.x, target_vel.y).length(); + if (is_positive(target_vel_xy_len)) { + const float accel_forward = (target_accel.x * target_vel.x + target_accel.y * target_vel.y + target_accel.z * target_vel.z)/target_vel.length(); + const Vector3f accel_turn = target_accel - target_vel * accel_forward / target_vel.length(); + const float accel_turn_xy_len = Vector2f(accel_turn.x, accel_turn.y).length(); + turn_rate = accel_turn_xy_len / target_vel_xy_len; + if ((accel_turn.y * target_vel.x - accel_turn.x * target_vel.y) < 0.0) { + turn_rate *= -1.0f; } } + // update the target yaw if origin and destination are at least 2m apart horizontally + const Vector2f target_vel_xy(target_vel.x, target_vel.y); + if (target_vel_xy.length() > WPNAV_YAW_VEL_MIN) { + set_yaw_cd(degrees(target_vel_xy.angle()) * 100.0f); + set_yaw_rate_cds(turn_rate*degrees(100.0f)); + } + // successfully advanced along track return true; } +/// recalculate path with update speed and/or acceleration limits +void AC_WPNav::update_track_with_speed_accel_limits() +{ + // update this leg + if (_this_leg_is_spline) { + _spline_this_leg.set_speed_accel(_pos_control.get_max_speed_xy(), _pos_control.get_max_speed_up(), _pos_control.get_max_speed_down(), + _wp_accel_cmss, _wp_accel_z_cmss); + } else { + _scurve_this_leg.set_speed_max(_pos_control.get_max_speed_xy(), _pos_control.get_max_speed_up(), _pos_control.get_max_speed_down()); + } + + // update next leg + if (_next_leg_is_spline) { + _spline_next_leg.set_speed_accel(_pos_control.get_max_speed_xy(), _pos_control.get_max_speed_up(), _pos_control.get_max_speed_down(), + _wp_accel_cmss, _wp_accel_z_cmss); + } else { + _scurve_next_leg.set_speed_max(_pos_control.get_max_speed_xy(), _pos_control.get_max_speed_up(), _pos_control.get_max_speed_down()); + } +} + /// get_wp_distance_to_destination - get horizontal distance to destination in cm float AC_WPNav::get_wp_distance_to_destination() const { @@ -553,80 +598,23 @@ bool AC_WPNav::update_wpnav() _pos_control.set_max_accel_xy(_wp_accel_cmss); _pos_control.set_max_accel_z(_wp_accel_z_cmss); - // wp_speed_update - update _pos_control.set_max_speed_xy if speed change has been requested - wp_speed_update(dt); - // advance the target if necessary if (!advance_wp_target_along_track(dt)) { // To-Do: handle inability to advance along track (probably because of missing terrain data) ret = false; } - // freeze feedforwards during known discontinuities - if (_flags.new_wp_destination) { - _flags.new_wp_destination = false; - _pos_control.freeze_ff_z(); - } - _pos_control.update_xy_controller(); - check_wp_leash_length(); _wp_last_update = AP_HAL::millis(); return ret; } -// check_wp_leash_length - check if waypoint leash lengths need to be recalculated -// should be called after _pos_control.update_xy_controller which may have changed the position controller leash lengths -void AC_WPNav::check_wp_leash_length() +// returns true if update_wpnav has been run very recently +bool AC_WPNav::is_active() const { - // exit immediately if recalc is not required - if (_flags.recalc_wp_leash) { - calculate_wp_leash_length(); - } -} - -/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller -void AC_WPNav::calculate_wp_leash_length() -{ - // length of the unit direction vector in the horizontal - float pos_delta_unit_xy = norm(_pos_delta_unit.x, _pos_delta_unit.y); - float pos_delta_unit_z = fabsf(_pos_delta_unit.z); - - float speed_z; - float leash_z; - if (_pos_delta_unit.z >= 0.0f) { - speed_z = _pos_control.get_max_speed_up(); - leash_z = _pos_control.get_leash_up_z(); - }else{ - speed_z = fabsf(_pos_control.get_max_speed_down()); - leash_z = _pos_control.get_leash_down_z(); - } - - // calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel - if(is_zero(pos_delta_unit_z) && is_zero(pos_delta_unit_xy)){ - _track_accel = 0; - _track_speed = 0; - _track_leash_length = WPNAV_LEASH_LENGTH_MIN; - }else if(is_zero(_pos_delta_unit.z)){ - _track_accel = _wp_accel_cmss/pos_delta_unit_xy; - _track_speed = _pos_control.get_max_speed_xy() / pos_delta_unit_xy; - _track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy; - }else if(is_zero(pos_delta_unit_xy)){ - _track_accel = _wp_accel_z_cmss/pos_delta_unit_z; - _track_speed = speed_z/pos_delta_unit_z; - _track_leash_length = leash_z/pos_delta_unit_z; - }else{ - _track_accel = MIN(_wp_accel_z_cmss/pos_delta_unit_z, _wp_accel_cmss/pos_delta_unit_xy); - _track_speed = MIN(speed_z/pos_delta_unit_z, _pos_control.get_max_speed_xy() / pos_delta_unit_xy); - _track_leash_length = MIN(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy); - } - - // calculate slow down distance (the distance from the destination when the target point should begin to slow down) - calc_slow_down_distance(_track_speed, _track_accel); - - // set recalc leash flag to false - _flags.recalc_wp_leash = false; + return (AP_HAL::millis() - _wp_last_update) < 200; } // returns target yaw in centi-degrees (used for wp and spline navigation) @@ -640,6 +628,17 @@ float AC_WPNav::get_yaw() const } } +// returns target yaw rate in centi-degrees / second (used for wp and spline navigation) +float AC_WPNav::get_yaw_rate() const +{ + if (_flags.wp_yaw_set) { + return _yaw_rate; + } + + // if yaw has not been set return zero turn rate + return 0.0f; +} + // set heading used for spline and waypoint navigation void AC_WPNav::set_yaw_cd(float heading_cd) { @@ -647,360 +646,10 @@ void AC_WPNav::set_yaw_cd(float heading_cd) _flags.wp_yaw_set = true; } -/// -/// spline methods -/// - -/// set_spline_destination waypoint using location class -/// returns false if conversion from location to vector from ekf origin cannot be calculated -/// 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 Location& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Location next_destination) +// set yaw rate used for spline and waypoint navigation +void AC_WPNav::set_yaw_rate_cds(float yaw_rate_cds) { - // convert destination location to vector - Vector3f dest_neu; - bool dest_terr_alt; - if (!get_vector_NEU(destination, dest_neu, dest_terr_alt)) { - return false; - } - - Vector3f next_dest_neu; // left uninitialised for valgrind - if (seg_end_type == SEGMENT_END_STRAIGHT || - seg_end_type == SEGMENT_END_SPLINE) { - // make altitude frames consistent - if (!next_destination.change_alt_frame(destination.get_alt_frame())) { - return false; - } - - // convert next destination to vector - bool next_dest_terr_alt; - if (!get_vector_NEU(next_destination, next_dest_neu, next_dest_terr_alt)) { - return false; - } - } - - // set target as vector from EKF origin - return set_spline_destination(dest_neu, dest_terr_alt, stopped_at_start, seg_end_type, next_dest_neu); -} - -/// 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_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)); - - // get dt from pos controller - float dt = _pos_control.get_dt(); - - // check _wp_accel_cmss is reasonable to avoid divide by zero - if (_wp_accel_cmss <= 0) { - _wp_accel_cmss.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); - } - - // store origin and destination locations - _origin = origin; - _destination = destination; - _terrain_alt = terrain_alt; - - // calculate slow down distance - calc_slow_down_distance(_pos_control.get_max_speed_xy(), _wp_accel_cmss); - - // 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 - _flags.wp_yaw_set = false; - - // initialise yaw related variables - _track_length_xy = safe_sqrt(sq(_destination.x - _origin.x)+sq(_destination.y - _origin.y)); // horizontal track length (used to decide if we should update yaw) - - return true; -} - -/// update_spline - update spline controller -bool AC_WPNav::update_spline() -{ - // exit immediately if this is not a spline segment - if (_flags.segment_type != SEGMENT_SPLINE) { - return false; - } - - bool ret = true; - - // get dt from pos controller - float dt = _pos_control.get_dt(); - - // wp_speed_update - update _pos_control.set_max_speed_xy if speed change has been requested - wp_speed_update(dt); - - // advance the target if necessary - if (!advance_spline_target_along_track(dt)) { - // To-Do: handle failure to advance along track (due to missing terrain data) - ret = false; - } - - // freeze feedforwards during known discontinuities - if (_flags.new_wp_destination) { - _flags.new_wp_destination = false; - _pos_control.freeze_ff_z(); - } - - // run horizontal position controller - _pos_control.update_xy_controller(); - - _wp_last_update = AP_HAL::millis(); - - return ret; -} - -/// update_spline_solution - recalculates hermite_spline_solution grid -/// relies on _spline_origin_vel, _spline_destination_vel and _origin and _destination -void AC_WPNav::update_spline_solution(const Vector3f& origin, const Vector3f& dest, const Vector3f& origin_vel, const Vector3f& dest_vel) -{ - _hermite_spline_solution[0] = origin; - _hermite_spline_solution[1] = origin_vel; - _hermite_spline_solution[2] = -origin*3.0f -origin_vel*2.0f + dest*3.0f - dest_vel; - _hermite_spline_solution[3] = origin*2.0f + origin_vel -dest*2.0f + dest_vel; - } - -/// 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 - const 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 = _pos_control.get_max_speed_xy(); - 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; -} - -// calc_spline_pos_vel_accel - calculates target position, velocity and acceleration for the given "spline_time" -/// relies on update_spline_solution being called when the segment's origin and destination were set -void AC_WPNav::calc_spline_pos_vel(float spline_time, Vector3f& position, Vector3f& velocity) -{ - float spline_time_sqrd = spline_time * spline_time; - float spline_time_cubed = spline_time_sqrd * spline_time; - - position = _hermite_spline_solution[0] + \ - _hermite_spline_solution[1] * spline_time + \ - _hermite_spline_solution[2] * spline_time_sqrd + \ - _hermite_spline_solution[3] * spline_time_cubed; - - velocity = _hermite_spline_solution[1] + \ - _hermite_spline_solution[2] * 2.0f * spline_time + \ - _hermite_spline_solution[3] * 3.0f * spline_time_sqrd; + _yaw_rate = yaw_rate_cds; } // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) @@ -1031,6 +680,191 @@ bool AC_WPNav::get_terrain_offset(float& offset_cm) return false; } +/// +/// spline methods +/// + +/// set_spline_destination waypoint using location class +/// returns false if conversion from location to vector from ekf origin cannot be calculated +/// next_destination should be the next segment's destination +/// next_is_spline should be true if path to next_destination should be a spline +bool AC_WPNav::set_spline_destination_loc(const Location& destination, const Location& next_destination, bool next_is_spline) +{ + // convert destination location to vector + Vector3f dest_neu; + bool dest_terr_alt; + if (!get_vector_NEU(destination, dest_neu, dest_terr_alt)) { + return false; + } + + // convert next destination to vector + Vector3f next_dest_neu; + bool next_dest_terr_alt; + if (!get_vector_NEU(next_destination, next_dest_neu, next_dest_terr_alt)) { + return false; + } + + // set target as vector from EKF origin + return set_spline_destination(dest_neu, dest_terr_alt, next_dest_neu, next_dest_terr_alt, next_is_spline); +} + +/// set next destination (e.g. the one after the current destination) as a spline segment specified as a location +/// returns false if conversion from location to vector from ekf origin cannot be calculated +/// next_next_destination should be the next segment's destination +bool AC_WPNav::set_spline_destination_next_loc(const Location& next_destination, const Location& next_next_destination, bool next_next_is_spline) +{ + // convert next_destination location to vector + Vector3f next_dest_neu; + bool next_dest_terr_alt; + if (!get_vector_NEU(next_destination, next_dest_neu, next_dest_terr_alt)) { + return false; + } + + // convert next_next_destination to vector + Vector3f next_next_dest_neu; + bool next_next_dest_terr_alt; + if (!get_vector_NEU(next_next_destination, next_next_dest_neu, next_next_dest_terr_alt)) { + return false; + } + + // set target as vector from EKF origin + return set_spline_destination_next(next_dest_neu, next_dest_terr_alt, next_next_dest_neu, next_next_dest_terr_alt, next_next_is_spline); +} + +/// set_spline_destination waypoint using position vector (distance from ekf origin in cm) +/// terrain_alt should be true if destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) +/// next_destination should be set to the next segment's destination +/// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) +/// next_destination.z must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination should be too) +bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_alt, const Vector3f& next_destination, bool next_terrain_alt, bool next_is_spline) +{ + // re-initialise if previous destination has been interrupted + if (!is_active() || !_flags.reached_destination) { + wp_and_spline_init(_wp_desired_speed_xy_cms); + } + + // update spline calculators speeds, accelerations and leash lengths + _spline_this_leg.set_speed_accel(_pos_control.get_max_speed_xy(), _pos_control.get_max_speed_up(), _pos_control.get_max_speed_down(), + _pos_control.get_max_accel_xy(), _pos_control.get_max_accel_z()); + + // calculate origin and origin velocity vector + Vector3f origin_vector; + if (terrain_alt == _terrain_alt && _flags.fast_waypoint) { + // calculate origin vector + if (_this_leg_is_spline) { + // if previous leg was a spline we can use destination velocity vector for origin velocity vector + origin_vector = _spline_this_leg.get_destination_vel(); + } else { + // use direction of the previous straight line segment + origin_vector = _destination - _origin; + } + + // use previous destination as origin + _origin = _destination; + } else { + + // use previous destination as origin + _origin = _destination; + + // get current alt above terrain + float origin_terr_offset; + if (!get_terrain_offset(origin_terr_offset)) { + return false; + } + + // convert origin to alt-above-terrain if necessary + if (terrain_alt) { + // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin + _origin.z -= origin_terr_offset; + _pos_terrain_offset += origin_terr_offset; + } else { + // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain + _origin.z += origin_terr_offset; + _pos_terrain_offset -= origin_terr_offset; + } + } + + // store destination location + _destination = destination; + _terrain_alt = terrain_alt; + + // calculate destination velocity vector + Vector3f destination_vector; + if (terrain_alt == next_terrain_alt) { + if (next_is_spline) { + // leave this segment moving parallel to vector from origin to next destination + destination_vector = next_destination - _origin; + } else { + // leave this segment moving parallel to next segment + destination_vector = next_destination - _destination; + } + } + _flags.fast_waypoint = !destination_vector.is_zero(); + + // setup spline leg + _spline_this_leg.set_origin_and_destination(_origin, _destination, origin_vector, destination_vector); + _this_leg_is_spline = true; + _flags.reached_destination = false; + _flags.wp_yaw_set = false; + + return true; +} + +/// set next destination (e.g. the one after the current destination) as an offset (in cm, NEU frame) from the EKF origin +/// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) +/// next_next_destination should be set to the next segment's destination +/// next_next_terrain_alt should be true if next_next_destination.z is a desired altitude above terrain (false if it is desired altitude above ekf origin) +/// next_next_destination.z must be in the same "frame" as destination.z (i.e. if next_destination is a alt-above-terrain, next_next_destination should be too) +bool AC_WPNav::set_spline_destination_next(const Vector3f& next_destination, bool next_terrain_alt, const Vector3f& next_next_destination, bool next_next_terrain_alt, bool next_next_is_spline) +{ + // do not add next point if alt types don't match + if (next_terrain_alt != _terrain_alt) { + return true; + } + + // calculate origin and origin velocity vector + Vector3f origin_vector; + if (_this_leg_is_spline) { + // if previous leg was a spline we can use destination velocity vector for origin velocity vector + origin_vector = _spline_this_leg.get_destination_vel(); + } else { + // use the direction of the previous straight line segment + origin_vector = _destination - _origin; + } + + // calculate destination velocity vector + Vector3f destination_vector; + if (next_terrain_alt == next_next_terrain_alt) { + if (next_next_is_spline) { + // leave this segment moving parallel to vector from this leg's origin (i.e. prev leg's destination) to next next destination + destination_vector = next_next_destination - _destination; + } else { + // leave this segment moving parallel to next segment + destination_vector = next_next_destination - next_destination; + } + } + + // update spline calculators speeds and accelerations + _spline_next_leg.set_speed_accel(_pos_control.get_max_speed_xy(), _pos_control.get_max_speed_up(), _pos_control.get_max_speed_down(), + _pos_control.get_max_accel_xy(), _pos_control.get_max_accel_z()); + + // setup next spline leg. Note this could be made local + _spline_next_leg.set_origin_and_destination(_destination, next_destination, origin_vector, destination_vector); + _next_leg_is_spline = true; + + // next destination provided so fast waypoint + _flags.fast_waypoint = true; + + // update this_leg's final velocity to match next spline leg + if (!_this_leg_is_spline) { + _scurve_this_leg.set_destination_speed_max(_spline_next_leg.get_origin_speed_max()); + } else { + _spline_this_leg.set_destination_speed_max(_spline_next_leg.get_origin_speed_max()); + } + + return true; +} + // convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain // returns false if conversion failed (likely because terrain data was not available) bool AC_WPNav::get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_alt) @@ -1066,68 +900,26 @@ bool AC_WPNav::get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_ return true; } -/// -/// shared methods -/// - -/// calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is travelling at full speed -void AC_WPNav::calc_slow_down_distance(float speed_cms, float accel_cmss) +// helper function to calculate scurve jerk and jerk_time values +// updates _scurve_jerk and _scurve_jerk_time +void AC_WPNav::calc_scurve_jerk_and_jerk_time() { - // protect against divide by zero - if (accel_cmss <= 0.0f) { - _slow_down_dist = 0.0f; - return; - } - // To-Do: should we use a combination of horizontal and vertical speeds? - // To-Do: update this automatically when speed or acceleration is changed - _slow_down_dist = speed_cms * speed_cms / (4.0f*accel_cmss); -} - -/// get_slow_down_speed - returns target speed of target point based on distance from the destination (in cm) -float AC_WPNav::get_slow_down_speed(float dist_from_dest_cm, float accel_cmss) -{ - // return immediately if distance is zero (or less) - if (dist_from_dest_cm <= 0) { - return WPNAV_WP_TRACK_SPEED_MIN; - } - - // calculate desired speed near destination - float target_speed = safe_sqrt(dist_from_dest_cm * 4.0f * accel_cmss); - - // ensure desired speed never becomes too low - if (target_speed < WPNAV_WP_TRACK_SPEED_MIN) { - return WPNAV_WP_TRACK_SPEED_MIN; + // calculate jerk + _scurve_jerk = MIN(_attitude_control.get_ang_vel_roll_max_radss() * GRAVITY_MSS, _attitude_control.get_ang_vel_pitch_max_radss() * GRAVITY_MSS); + if (is_zero(_scurve_jerk)) { + _scurve_jerk = _wp_jerk; } else { - return target_speed; - } -} - -/// wp_speed_update - calculates how to handle speed change requests -void AC_WPNav::wp_speed_update(float dt) -{ - // return if speed has not changed - float curr_max_speed_xy_cms = _pos_control.get_max_speed_xy(); - if (is_equal(_wp_desired_speed_xy_cms, curr_max_speed_xy_cms)) { - return; - } - // calculate speed change - if (_wp_desired_speed_xy_cms > curr_max_speed_xy_cms) { - // speed up is requested so increase speed within limit set by WPNAV_ACCEL - curr_max_speed_xy_cms += _wp_accel_cmss * dt; - if (curr_max_speed_xy_cms > _wp_desired_speed_xy_cms) { - curr_max_speed_xy_cms = _wp_desired_speed_xy_cms; - } - } else if (_wp_desired_speed_xy_cms < curr_max_speed_xy_cms) { - // slow down is requested so reduce speed within limit set by WPNAV_ACCEL - curr_max_speed_xy_cms -= _wp_accel_cmss * dt; - if (curr_max_speed_xy_cms < _wp_desired_speed_xy_cms) { - curr_max_speed_xy_cms = _wp_desired_speed_xy_cms; - } + _scurve_jerk = MIN(_scurve_jerk, _wp_jerk); } - // update position controller speed - _pos_control.set_max_speed_xy(curr_max_speed_xy_cms); - - // flag that wp leash must be recalculated - _flags.recalc_wp_leash = true; + // calculate jerk time + // jounce (the rate of change of jerk) uses the attitude control input time constant because multicopters + // lean to accelerate meaning the change in angle is equivalent to the change in acceleration + const float jounce = MIN(_attitude_control.get_accel_roll_max() * GRAVITY_MSS, _attitude_control.get_accel_pitch_max() * GRAVITY_MSS); + if (is_positive(jounce)) { + _scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.5f * _scurve_jerk * M_PI / jounce); + } else { + _scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.1f); + } + _scurve_jerk_time *= 2.0f; } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index be69e6c498..460f4c1f1a 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -3,6 +3,8 @@ #include #include #include +#include +#include #include #include // Inertial Navigation library #include // Position control library @@ -12,11 +14,8 @@ // maximum velocities and accelerations #define WPNAV_ACCELERATION 250.0f // maximum horizontal acceleration in cm/s/s that wp navigation will request -#define WPNAV_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter - #define WPNAV_WP_SPEED 1000.0f // default horizontal speed between waypoints in cm/s #define WPNAV_WP_SPEED_MIN 20.0f // minimum horizontal speed between waypoints in cm/s -#define WPNAV_WP_TRACK_SPEED_MIN 50.0f // minimum speed along track of the target point the vehicle is chasing in cm/s (used as target slows down before reaching destination) #define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm #define WPNAV_WP_RADIUS_MIN 5.0f // minimum waypoint radius in cm @@ -24,15 +23,7 @@ #define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity #define WPNAV_WP_ACCEL_Z_DEFAULT 100.0f // default vertical acceleration between waypoints in cm/s/s - -#define WPNAV_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm - -#define WPNAV_WP_FAST_OVERSHOOT_MAX 200.0f // 2m overshoot is allowed during fast waypoints to allow for smooth transitions to next waypoint - -#define WPNAV_YAW_DIST_MIN 200 // minimum track length which will lead to target yaw being updated to point at next waypoint. Under this distance the yaw target will be frozen at the current heading -#define WPNAV_YAW_LEASH_PCT_MIN 0.134f // target point must be at least this distance from the vehicle (expressed as a percentage of the maximum distance it can be from the vehicle - i.e. the leash length) - -#define WPNAV_RANGEFINDER_FILT_Z 0.25f // range finder distance filtered at 0.25hz +#define WPNAV_YAW_VEL_MIN 10 // target velocity must be at least 10cm/s for vehicle's yaw to change class AC_WPNav { @@ -71,9 +62,10 @@ public: /// /// wp_and_spline_init - initialise straight line and spline waypoint controllers + /// speed_cms is the desired max speed to travel between waypoints. should be a positive value or left at zero to use the default speed /// updates target roll, pitch targets and I terms based on vehicle lean angles /// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination - void wp_and_spline_init(); + void wp_and_spline_init(float speed_cms = 0.0f); /// set current target horizontal speed during wp navigation void set_speed_xy(float speed_cms); @@ -109,29 +101,29 @@ public: bool origin_and_destination_are_terrain_alt() const { return _terrain_alt; } /// set_wp_destination waypoint using location class + /// provide the next_destination if known /// returns false if conversion from location to vector from ekf origin cannot be calculated - bool set_wp_destination(const Location& destination); + bool set_wp_destination_loc(const Location& destination); + bool set_wp_destination_next_loc(const Location& destination); // returns wp location using location class. // returns false if unable to convert from target vector to global // coordinates - bool get_wp_destination(Location& destination) const; + bool get_wp_destination_loc(Location& destination) const; // returns object avoidance adjusted destination which is always the same as get_wp_destination // having this function unifies the AC_WPNav_OA and AC_WPNav interfaces making vehicle code simpler - virtual bool get_oa_wp_destination(Location& destination) const { return get_wp_destination(destination); } + virtual bool get_oa_wp_destination(Location& destination) const { return get_wp_destination_loc(destination); } /// set_wp_destination waypoint using position vector (distance from ekf origin in cm) /// terrain_alt should be true if destination.z is a desired altitude above terrain - bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false); + virtual bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false); + bool set_wp_destination_next(const Vector3f& destination, bool terrain_alt = false); /// set waypoint destination using NED position vector from ekf origin in meters + /// provide next_destination_NED if known bool set_wp_destination_NED(const Vector3f& destination_NED); - - /// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from ekf origin 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) - virtual bool set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt = false); + bool set_wp_destination_next_NED(const Vector3f& destination_NED); /// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position /// used to reset the position just before takeoff @@ -167,63 +159,47 @@ public: return get_wp_distance_to_destination() < _wp_radius_cm; } - /// set_fast_waypoint - set to true to ignore the waypoint radius and consider the waypoint 'reached' the moment the intermediate point reaches it - void set_fast_waypoint(bool fast) { _flags.fast_waypoint = fast; } - /// update_wpnav - run the wp controller - should be called at 100hz or higher virtual bool update_wpnav(); - // check_wp_leash_length - check recalc_wp_leash flag and calls calculate_wp_leash_length() if necessary - // should be called after _pos_control.update_xy_controller which may have changed the position controller leash lengths - void check_wp_leash_length(); - - /// calculate_wp_leash_length - calculates track speed, acceleration and leash lengths for waypoint controller - void calculate_wp_leash_length(); + // returns true if update_wpnav has been run very recently + bool is_active() const; /// /// spline methods /// - // 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) - - // segment end types - // stop - vehicle is not moving at destination - // straight-fast - next segment is straight, vehicle's destination velocity should be directly along track from this segment's destination to next segment's destination - // spline-fast - next segment is spline, vehicle's destination velocity should be parallel to position difference vector from previous segment's origin to this segment's destination - // get target yaw in centi-degrees (used for wp and spline navigation) float get_yaw() const; + float get_yaw_rate() const; /// set_spline_destination waypoint using location class /// 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 altitude above terrain (false if its desired altitude 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 set_spline_destination(const Location& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Location next_destination); + /// next_destination should be the next segment's destination + /// next_is_spline should be true if next_destination is a spline segment + bool set_spline_destination_loc(const Location& destination, const Location& next_destination, bool next_is_spline); + + /// set next destination (e.g. the one after the current destination) as a spline segment specified as a location + /// returns false if conversion from location to vector from ekf origin cannot be calculated + /// next_next_destination should be the next segment's destination + /// next_next_is_spline should be true if next_next_destination is a spline segment + bool set_spline_destination_next_loc(const Location& next_destination, const Location& next_next_destination, bool next_next_is_spline); /// set_spline_destination waypoint using position vector (distance from ekf origin 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 - /// next_destination.z must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination should be too) - bool set_spline_destination(const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination); + /// terrain_alt should be true if destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) + /// next_destination is the next segment's destination + /// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) + /// next_destination.z must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination must be too) + /// next_is_spline should be true if next_destination is a spline segment + bool set_spline_destination(const Vector3f& destination, bool terrain_alt, const Vector3f& next_destination, bool next_terrain_alt, bool next_is_spline); - /// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from ekf origin 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) - /// 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 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); - - /// update_spline - update spline controller - bool update_spline(); + /// set next destination (e.g. the one after the current destination) as an offset (in cm, NEU frame) from the EKF origin + /// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) + /// next_next_destination is the next segment's destination + /// next_next_terrain_alt should be true if next_next_destination.z is a desired altitude above terrain (false if it is desired altitude above ekf origin) + /// next_next_destination.z must be in the same "frame" as destination.z (i.e. if next_destination is a alt-above-terrain, next_next_destination must be too) + /// next_next_is_spline should be true if next_next_destination is a spline segment + bool set_spline_destination_next(const Vector3f& next_destination, bool next_terrain_alt, const Vector3f& next_next_destination, bool next_next_terrain_alt, bool next_next_is_spline); /// /// shared methods @@ -236,6 +212,9 @@ public: /// advance_wp_target_along_track - move target location along track from origin to destination bool advance_wp_target_along_track(float dt); + /// recalculate path with update speed and/or acceleration limits + void update_track_with_speed_accel_limits(); + /// return the crosstrack_error - horizontal error of the actual position vs the desired position float crosstrack_error() const { return _track_error_xy;} @@ -253,35 +232,10 @@ protected: struct wpnav_flags { uint8_t reached_destination : 1; // true if we have reached the destination uint8_t fast_waypoint : 1; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint - uint8_t slowing_down : 1; // true when target point is slowing down before reaching the destination - uint8_t recalc_wp_leash : 1; // true if we need to recalculate the leash lengths because of changes in speed or acceleration - uint8_t new_wp_destination : 1; // true if we have just received a new destination. allows us to freeze the position controller's xy feed forward SegmentType segment_type : 1; // active segment is either straight or spline uint8_t wp_yaw_set : 1; // true if yaw target has been set } _flags; - /// calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is traveling at full speed - void calc_slow_down_distance(float speed_cms, float accel_cmss); - - /// get_slow_down_speed - returns target speed of target point based on distance from the destination (in cm) - float get_slow_down_speed(float dist_from_dest_cm, float accel_cmss); - - /// wp_speed_update - calculates how to change speed when changes are requested - void wp_speed_update(float dt); - - /// spline protected functions - - /// update_spline_solution - recalculates hermite_spline_solution grid - void update_spline_solution(const Vector3f& origin, const Vector3f& dest, const Vector3f& origin_vel, const Vector3f& dest_vel); - - /// advance_spline_target_along_track - move target location along track from origin to destination - /// returns false if it is unable to advance (most likely because of missing terrain data) - bool advance_spline_target_along_track(float dt); - - /// calc_spline_pos_vel - update position and velocity from given spline time - /// relies on update_spline_solution being called since the previous - void calc_spline_pos_vel(float spline_time, Vector3f& position, Vector3f& velocity); - // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) bool get_terrain_offset(float& offset_cm); @@ -291,6 +245,11 @@ protected: // set heading used for spline and waypoint navigation void set_yaw_cd(float heading_cd); + void set_yaw_rate_cds(float yaw_rate_cds); + + // helper function to calculate scurve jerk and jerk_time values + // updates _scurve_jerk and _scurve_jerk_time + void calc_scurve_jerk_and_jerk_time(); // references and pointers to external libraries const AP_InertialNav& _inav; @@ -306,36 +265,44 @@ protected: AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached AP_Float _wp_accel_cmss; // horizontal acceleration in cm/s/s during missions AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions + AP_Float _wp_jerk; // maximum jerk used to generate s-curve trajectories in m/s/s/s + + // scurve + SCurve _scurve_prev_leg; // previous spline trajectory used to blend with current s-curve trajectory + SCurve _scurve_this_leg; // current spline trajectory + SCurve _scurve_next_leg; // next spline trajectory used to blend with current s-curve trajectory + float _scurve_jerk; // scurve jerk max in m/s/s/s + float _scurve_jerk_time; // scurve jerk time (time in seconds for jerk to increase from zero _scurve_jerk) + + // spline curves + SplineCurve _spline_this_leg; // spline curve for current segment + SplineCurve _spline_next_leg; // spline curve for next segment + + // the type of this leg + bool _this_leg_is_spline; // true if this leg is a spline + bool _next_leg_is_spline; // true if the next leg is a spline // waypoint controller internal variables uint32_t _wp_last_update; // time of last update_wpnav call float _wp_desired_speed_xy_cms; // desired wp speed in cm/sec Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin Vector3f _destination; // target destination in cm from ekf origin - Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination float _track_error_xy; // horizontal error of the actual position vs the desired position - float _track_length; // distance in cm between origin and destination - float _track_length_xy; // horizontal distance in cm between origin and destination float _track_desired; // our desired distance along the track in cm - float _limited_speed_xy_cms; // horizontal speed in cm/s used to advance the intermediate target towards the destination. used to limit extreme acceleration after passing a waypoint - float _track_accel; // acceleration along track - float _track_speed; // speed in cm/s along track - float _track_leash_length; // leash length along track - float _slow_down_dist; // vehicle should begin to slow down once it is within this distance from the destination - - // spline variables - float _spline_time; // current spline time between origin and destination - float _spline_time_scale; // current spline time between origin and destination - Vector3f _spline_origin_vel; // the target velocity vector at the origin of the spline segment - Vector3f _spline_destination_vel;// the target velocity vector at the destination point of the spline segment - Vector3f _hermite_spline_solution[4]; // array describing spline path between origin and destination - float _spline_vel_scaler; // - float _yaw; // heading according to yaw + float _track_scalar_dt; // time compression multiplier to slow the progress along the track + float _yaw; // current yaw heading in centi-degrees based on track direction + float _yaw_rate; // current yaw rate in centi-degrees/second based on track curvature // terrain following variables bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin - bool _rangefinder_available; - AP_Int8 _rangefinder_use; - bool _rangefinder_healthy; - float _rangefinder_alt_cm; + bool _rangefinder_available; // true if rangefinder is enabled (user switch can turn this true/false) + AP_Int8 _rangefinder_use; // parameter that specifies if the range finder should be used for terrain following commands + bool _rangefinder_healthy; // true if rangefinder distance is healthy (i.e. between min and maximum) + float _rangefinder_alt_cm; // latest distance from the rangefinder + + // position, velocity and acceleration targets passed to position controller + float _pos_terrain_offset; + float _vel_terrain_offset; + float _accel_terrain_offset; + }; diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index 97c65eb5f2..cd400e70e7 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -11,7 +11,7 @@ bool AC_WPNav_OA::get_oa_wp_destination(Location& destination) const { // if oa inactive return unadjusted location if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) { - return get_wp_destination(destination); + return get_wp_destination_loc(destination); } // return latest destination provided by oa path planner @@ -19,12 +19,12 @@ bool AC_WPNav_OA::get_oa_wp_destination(Location& destination) const return true; } -/// 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) +/// set_wp_destination waypoint using position vector (distance from ekf origin in cm) +/// terrain_alt should be true if destination.z is a desired altitude above terrain /// returns false on failure (likely caused by missing terrain data) -bool AC_WPNav_OA::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt) +bool AC_WPNav_OA::set_wp_destination(const Vector3f& destination, bool terrain_alt) { - const bool ret = AC_WPNav::set_wp_origin_and_destination(origin, destination, terrain_alt); + const bool ret = AC_WPNav::set_wp_destination(destination, terrain_alt); if (ret) { // reset object avoidance state diff --git a/libraries/AC_WPNav/AC_WPNav_OA.h b/libraries/AC_WPNav/AC_WPNav_OA.h index 004e9b141a..cf532de016 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.h +++ b/libraries/AC_WPNav/AC_WPNav_OA.h @@ -15,10 +15,10 @@ public: // returns false if unable to convert from target vector to global coordinates bool get_oa_wp_destination(Location& destination) const override; - /// set origin and destination waypoints using position vectors (distance from ekf origin 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) + /// set_wp_destination waypoint using position vector (distance from ekf origin in cm) + /// terrain_alt should be true if destination.z is a desired altitude above terrain /// returns false on failure (likely caused by missing terrain data) - bool set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt = false) override; + bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false) override; /// get horizontal distance to destination in cm /// always returns distance to final destination (i.e. does not use oa adjusted destination)