From ba50f68390bfbcd41a083feaec12fdbd53a1b22b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 3 Sep 2021 12:58:21 +0900 Subject: [PATCH] AC_WPNav: terrain spelling fix --- libraries/AC_WPNav/AC_WPNav.cpp | 12 ++++++------ libraries/AC_WPNav/AC_WPNav.h | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 302a5c0df4c0a..5b9c9e15d0c01 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -177,8 +177,8 @@ void AC_WPNav::wp_and_spline_init(float speed_cms) _this_leg_is_spline = false; // initialise the terrain velocity to the current maximum velocity - _terain_vel = _wp_desired_speed_xy_cms; - _terain_accel = 0.0; + _terrain_vel = _wp_desired_speed_xy_cms; + _terrain_accel = 0.0; } /// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation @@ -187,7 +187,7 @@ void AC_WPNav::set_speed_xy(float speed_cms) // range check target speed if (speed_cms >= WPNAV_WP_SPEED_MIN) { // update terrain following speed scalar - _terain_vel = speed_cms * _terain_vel / _wp_desired_speed_xy_cms; + _terrain_vel = speed_cms * _terrain_vel / _wp_desired_speed_xy_cms; // initialize the desired wp speed _wp_desired_speed_xy_cms = speed_cms; @@ -463,11 +463,11 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) float vel_time_scalar = 1.0; if (is_positive(_wp_desired_speed_xy_cms)) { - update_vel_accel(_terain_vel, _terain_accel, dt, false); + update_vel_accel(_terrain_vel, _terrain_accel, dt, false); shape_vel_accel( _wp_desired_speed_xy_cms * offset_z_scaler, 0.0, - _terain_vel, _terain_accel, + _terrain_vel, _terrain_accel, -_wp_accel_cmss, _wp_accel_cmss, _pos_control.get_shaping_jerk_xy_cmsss(), dt, true); - vel_time_scalar = _terain_vel / _wp_desired_speed_xy_cms; + vel_time_scalar = _terrain_vel / _wp_desired_speed_xy_cms; } // change s-curve time speed with a time constant of maximum acceleration / maximum jerk diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index dddf3f6ed5b3a..e56122730ca4a 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -262,8 +262,8 @@ class AC_WPNav Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin Vector3f _destination; // target destination in cm from ekf origin float _track_scalar_dt; // time compression multiplier to slow the progress along the track - float _terain_vel; // maximum horizontal velocity used to ensure the aircraft can maintain height above terain - float _terain_accel; // acceleration value used to change _terain_vel + float _terrain_vel; // maximum horizontal velocity used to ensure the aircraft can maintain height above terrain + float _terrain_accel; // acceleration value used to change _terain_vel // terrain following variables bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin