From ec53ea7f8e2d3c03abdcf46612d417bc2577f198 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 24 Jun 2021 00:10:46 +0930 Subject: [PATCH] AC_WPNav: move code to generate terrain following kinematic path --- libraries/AC_WPNav/AC_WPNav.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index bb93552a64..7126d4fe64 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -421,6 +421,14 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) return false; } + // input shape the terrain offset + shape_pos_vel_accel(terr_offset, 0.0f, 0.0f, + _pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, + 0.0f, _pos_control.get_max_speed_down_cms(), _pos_control.get_max_speed_up_cms(), + -_pos_control.get_max_accel_z_cmss(), _pos_control.get_max_accel_z_cmss(), _pos_control.get_shaping_tc_z_s(), dt); + + update_pos_vel_accel(_pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, dt, 0.0f); + // 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}; @@ -465,14 +473,6 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) s_finished = _spline_this_leg.reached_destination(); } - // input shape the terrain offset - shape_pos_vel_accel(terr_offset, 0.0f, 0.0f, - _pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, - 0.0f, _pos_control.get_max_speed_down_cms(), _pos_control.get_max_speed_up_cms(), - -_pos_control.get_max_accel_z_cmss(), _pos_control.get_max_accel_z_cmss(), _pos_control.get_shaping_tc_z_s(), dt); - - update_pos_vel_accel(_pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, dt, 0.0f); - // convert final_target.z to altitude above the ekf origin target_pos.z += _pos_terrain_offset; target_vel.z += _vel_terrain_offset;