AC_PosControl: update set-offsets to support Z-axis

rmackay9 · Sep 30, 2024 · 04e2574 · 04e2574
1 parent f31be39
commit 04e2574
Showing 1 changed file with 7 additions and 3 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -1190,16 +1190,20 @@ void AC_PosControl::init_offsets_z()
 bool AC_PosControl::set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED)
 {
     set_posvelaccel_offset_target_xy_cm(pos_offset_NED.topostype().xy() * 100.0, vel_offset_NED.xy() * 100.0, accel_offset_NED.xy() * 100.0);
+    set_posvelaccel_offset_target_z_cm(-pos_offset_NED.topostype().z * 100.0, -vel_offset_NED.z * 100, -accel_offset_NED.z * 100.0);
     return true;
 }
 
 // get position and velocity offset to vehicle's target velocity and acceleration
 // units are m and m/s in NED frame
 bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED)
 {
-    pos_offset_NED = _pos_offset_target.tofloat() * 0.01;
-    vel_offset_NED = _vel_offset_target * 0.01;
-    accel_offset_NED = _accel_offset_target * 0.01;
+    pos_offset_NED.xy() = _pos_offset_target.xy().tofloat() * 0.01;
+    pos_offset_NED.z = -_pos_offset_target.z * 0.01;
+    vel_offset_NED.xy() = _vel_offset_target.xy() * 0.01;
+    vel_offset_NED.z = -_vel_offset_target.z * 0.01;
+    accel_offset_NED.xy() = _accel_offset_target.xy() * 0.01;
+    accel_offset_NED.z = -_accel_offset_target.z * 0.01;
     return true;
 }
 #endif