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

AC_WPNav: support pos vel accel offsets 

Co-authored-by: Leonard Hall <leonardthall@gmail.com>
This commit is contained in:
Randy Mackay 2024-08-02 18:31:28 +09:00
parent c706d01d7f
commit 951ff473c3
1 changed files with 11 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -323,11 +323,11 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
if (terrain_alt) {
// new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
_origin.z -= origin_terr_offset;
_pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() + origin_terr_offset);
_pos_control.init_pos_terrain_cm(origin_terr_offset);
} else {
// new destination is alt-above-ekf-origin, previous destination was alt-above-terrain
_origin.z += origin_terr_offset;
_pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() - origin_terr_offset);
_pos_control.init_pos_terrain_cm(0.0);
}
}
@ -435,7 +435,7 @@ void AC_WPNav::shift_wp_origin_and_destination_to_stopping_point_xy()
_destination.xy() = stopping_point;
// move pos controller target horizontally
_pos_control.set_pos_target_xy_cm(stopping_point.x, stopping_point.y);
_pos_control.set_pos_desired_xy_cm(stopping_point);
}
/// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
@ -466,10 +466,14 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
const float offset_z_scaler = _pos_control.pos_offset_z_scaler(terr_offset, get_terrain_margin() * 100.0);
// input shape the terrain offset
_pos_control.update_pos_offset_z(terr_offset);
_pos_control.set_pos_terrain_target_cm(terr_offset);
// get position controller's position offset (post input shaping) so it can be used in position error calculation
const Vector3p& psc_pos_offset = _pos_control.get_pos_offset_cm();
// get current position and adjust altitude to origin and destination's frame (i.e. _frame)
const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{0, 0, terr_offset};
Vector3f curr_pos = _inav.get_position_neu_cm() - psc_pos_offset.tofloat();
curr_pos.z -= terr_offset;
Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();
@ -528,11 +532,6 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
target_accel *= sq(vel_scaler_dt);
target_accel += accel_offset;
// convert final_target.z to altitude above the ekf origin
target_pos.z += _pos_control.get_pos_offset_z_cm();
target_vel.z += _pos_control.get_vel_offset_z_cms();
target_accel.z += _pos_control.get_accel_offset_z_cmss();
// pass new target to the position controller
_pos_control.set_pos_vel_accel(target_pos.topostype(), target_vel, target_accel);
@ -778,11 +777,11 @@ bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_
if (terrain_alt) {
// new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
_origin.z -= origin_terr_offset;
_pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() + origin_terr_offset);
_pos_control.init_pos_terrain_cm(origin_terr_offset);
} else {
// new destination is alt-above-ekf-origin, previous destination was alt-above-terrain
_origin.z += origin_terr_offset;
_pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() - origin_terr_offset);
_pos_control.init_pos_terrain_cm(0.0);
}
}