AC_WPNav: fix terrain following

rmackay9 · Sep 18, 2024 · a98b3ba · a98b3ba
1 parent 5abe1b9
commit a98b3ba
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -531,6 +531,11 @@ 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);