WP_Nav terrain init bug

lthall · Sep 28, 2024 · f1f9913 · f1f9913
1 parent 786aac3
commit f1f9913
Showing 1 changed file with 4 additions and 4 deletions.
diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -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);
         }
     }
 
@@ -782,11 +782,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);
         }
     }