Copter: partial fix for subtract-pos-offsets

rmackay9 · Sep 30, 2024 · a22b312 · a22b312
1 parent 2d5967f
commit a22b312
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
@@ -1502,9 +1502,10 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const
 // should be used when the location will be used as a target for the position controller
 void ModeAuto::subtract_pos_offsets(Location& target_loc) const
 {
-    // QUESTION: do we need to remove terrain altitude here too?
     // subtract position controller offsets from target location
-    target_loc.offset(-pos_control->get_pos_offset_cm() * 0.01);
+    const Vector3p& pos_ofs_neu_cm = pos_control->get_pos_offset_cm();
+    Vector3p pos_ofs_ned_m = Vector3p{pos_ofs_neu_cm.x * 0.01, pos_ofs_neu_cm.y * 0.01, -pos_ofs_neu_cm.z * 0.01};
+    target_loc.offset(-pos_ofs_ned_m);
 }
 
 /********************************************************************************/