Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: move code to generate terrain following kinematic path

This commit is contained in:
Leonard Hall 2021-06-24 00:10:46 +09:30 committed by Andrew Tridgell
parent a250a06080
commit 0269c49c14
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -421,6 +421,14 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
return false; 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) // 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}; 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(); 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 // convert final_target.z to altitude above the ekf origin
target_pos.z += _pos_terrain_offset; target_pos.z += _pos_terrain_offset;
target_vel.z += _vel_terrain_offset; target_vel.z += _vel_terrain_offset;