Blimp: PSCx logging includes offsets

offsets are always zero
rmackay9 · Sep 16, 2024 · 1f77b3b · 1f77b3b
1 parent b93c1ca
commit 1f77b3b
Showing 1 changed file with 18 additions and 6 deletions.
diff --git a/Blimp/Loiter.cpp b/Blimp/Loiter.cpp
@@ -125,9 +125,15 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
     }
 
 #if HAL_LOGGING_ENABLED
-    AC_PosControl::Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
-    AC_PosControl::Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
-    AC_PosControl::Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
+    AC_PosControl::Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, 0.0,
+                              0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0,
+                              0.0, 0.0, 0.0, 0.0);
+    AC_PosControl::Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, 0.0,
+                              0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0,
+                              0.0, 0.0, 0.0, 0.0);
+    AC_PosControl::Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, 0.0,
+                              0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0,
+                              0.0, 0.0, 0.0, 0.0);
 #endif
 }
 
@@ -203,8 +209,14 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax
     }
 
 #if HAL_LOGGING_ENABLED
-    AC_PosControl::Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
-    AC_PosControl::Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
-    AC_PosControl::Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
+    AC_PosControl::Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, 0.0,
+                              0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0,
+                              0.0, 0.0, 0.0, 0.0);
+    AC_PosControl::Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, 0.0,
+                              0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0,
+                              0.0, 0.0, 0.0, 0.0);
+    AC_PosControl::Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, 0.0,
+                              0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0,
+                              0.0, 0.0, 0.0, 0.0);
 #endif
 }