Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: terrain spelling fix

Browse Source
This commit is contained in:
Randy Mackay 2021-09-03 12:58:21 +09:00
parent 8f06a27480
commit b66b5cd21c
2 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -176,8 +176,8 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
_this_leg_is_spline = false; _this_leg_is_spline = false;
// initialise the terrain velocity to the current maximum velocity // initialise the terrain velocity to the current maximum velocity
_terain_vel = _wp_desired_speed_xy_cms; _terrain_vel = _wp_desired_speed_xy_cms;
_terain_accel = 0.0; _terrain_accel = 0.0;
} }
/// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation /// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
@ -186,7 +186,7 @@ void AC_WPNav::set_speed_xy(float speed_cms)
// range check target speed // range check target speed
if (speed_cms >= WPNAV_WP_SPEED_MIN) { if (speed_cms >= WPNAV_WP_SPEED_MIN) {
// update terrain following speed scalar // 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 // initialize the desired wp speed
_wp_desired_speed_xy_cms = speed_cms; _wp_desired_speed_xy_cms = speed_cms;
@ -462,11 +462,11 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
float vel_time_scalar = 1.0; float vel_time_scalar = 1.0;
if (is_positive(_wp_desired_speed_xy_cms)) { 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, 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); -_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 // change s-curve time speed with a time constant of maximum acceleration / maximum jerk

4
libraries/AC_WPNav/AC_WPNav.h
View File

@ -266,8 +266,8 @@ protected:
Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin
Vector3f _destination; // target destination 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 _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 _terrain_vel; // maximum horizontal velocity used to ensure the aircraft can maintain height above terrain
float _terain_accel; // acceleration value used to change _terain_vel float _terrain_accel; // acceleration value used to change _terain_vel
// terrain following variables // terrain following variables
bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin