AC_PosControl: PSCx gets offsets

ArduPilot · Sep 16, 2024 · 790a1dd · 790a1dd
1 parent cae3a7d
commit 790a1dd
Show file tree

Hide file tree

Showing 4 changed files with 75 additions and 27 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -1322,18 +1322,19 @@ void AC_PosControl::write_log()
     if (is_active_xy()) {
         float accel_x, accel_y;
         lean_angles_to_accel_xy(accel_x, accel_y);
-        Write_PSCN(get_pos_target_cm().x, _inav.get_position_neu_cm().x,
-                                get_vel_desired_cms().x, get_vel_target_cms().x, _inav.get_velocity_neu_cms().x,
-                                _accel_desired.x, get_accel_target_cmss().x, accel_x);
-        Write_PSCE(get_pos_target_cm().y, _inav.get_position_neu_cm().y,
-                                get_vel_desired_cms().y, get_vel_target_cms().y, _inav.get_velocity_neu_cms().y,
-                                _accel_desired.y, get_accel_target_cmss().y, accel_y);
+
+        Write_PSCN(get_pos_target_cm().x, _inav.get_position_neu_cm().x, _pos_offset_target.x, _pos_offset.x,
+                   get_vel_desired_cms().x, get_vel_target_cms().x, _inav.get_velocity_neu_cms().x, _vel_offset_target.x, _vel_offset.x,
+                   _accel_desired.x, get_accel_target_cmss().x, accel_x, _accel_offset.x);
+        Write_PSCE(get_pos_target_cm().y, _inav.get_position_neu_cm().y, _pos_offset_target.y, _pos_offset.y,
+                   get_vel_desired_cms().y, get_vel_target_cms().y, _inav.get_velocity_neu_cms().y, _vel_offset_target.y, _vel_offset.y,
+                   _accel_desired.y, get_accel_target_cmss().y, accel_y, _accel_offset.y);
     }
 
     if (is_active_z()) {
-        Write_PSCD(-get_pos_target_cm().z, -_inav.get_position_z_up_cm(),
-                                -get_vel_desired_cms().z, -get_vel_target_cms().z, -_inav.get_velocity_z_up_cms(),
-                                -_accel_desired.z, -get_accel_target_cmss().z, -get_z_accel_cmss());
+        Write_PSCD(-get_pos_target_cm().z, -_inav.get_position_z_up_cm(), 0, 0,
+                   -get_vel_desired_cms().z, -get_vel_target_cms().z, -_inav.get_velocity_z_up_cms(), 0, 0,
+                   -_accel_desired.z, -get_accel_target_cmss().z, -get_z_accel_cmss(), 0);
     }
 }
 #endif  // HAL_LOGGING_ENABLED

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -417,9 +417,15 @@ class AC_PosControl
 
     static const struct AP_Param::GroupInfo var_info[];
 
-    static void Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
-    static void Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
-    static void Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
+    static void Write_PSCN(float pos_target, float pos, float pos_target_offset, float pos_offset,
+                           float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset,
+                           float accel_desired, float accel_target, float accel, float accel_offset);
+    static void Write_PSCE(float pos_target, float pos, float pos_target_offset, float pos_offset,
+                           float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset,
+                           float accel_desired, float accel_target, float accel, float accel_offset);
+    static void Write_PSCD(float pos_target, float pos, float pos_target_offset, float pos_offset,
+                           float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset,
+                           float accel_desired, float accel_target, float accel, float accel_offset);
 
 protected:
 
@@ -534,6 +540,8 @@ class AC_PosControl
 private:
     // convenience method for writing out the identical PSCE, PSCN, PSCD - and
     // to save bytes
-    static void Write_PSCx(LogMessages ID, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
+    static void Write_PSCx(LogMessages ID, float pos_target, float pos, float pos_target_offset, float pos_offset,
+                           float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset,
+                           float accel_desired, float accel_target, float accel, float accel_offset);
 
 };
diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp
@@ -8,36 +8,55 @@
 #include "LogStructure.h"
 
 // a convenience function for writing out the position controller PIDs
-void AC_PosControl::Write_PSCx(LogMessages id, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
+void AC_PosControl::Write_PSCx(LogMessages id, float pos_target, float pos, float pos_target_offset, float pos_offset,
+                               float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset,
+                               float accel_desired, float accel_target, float accel, float accel_offset)
 {
     const struct log_PSCx pkt{
         LOG_PACKET_HEADER_INIT(id),
             time_us         : AP_HAL::micros64(),
             pos_target    : pos_target * 0.01f,
             pos           : pos * 0.01f,
+            pos_target_offset: pos_target_offset * 0.01f,
+            pos_offset    : pos_offset * 0.01f,
             vel_desired   : vel_desired * 0.01f,
             vel_target    : vel_target * 0.01f,
             vel           : vel * 0.01f,
+            vel_target_offset: vel_target_offset * 0.01f,
+            vel_offset    : vel_offset * 0.01f,
             accel_desired : accel_desired * 0.01f,
             accel_target  : accel_target * 0.01f,
-            accel         : accel * 0.01f
+            accel         : accel * 0.01f,
+            accel_offset  : accel_offset * 0.01f
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }
 
-void AC_PosControl::Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
+void AC_PosControl::Write_PSCN(float pos_target, float pos, float pos_target_offset, float pos_offset,
+                               float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset,
+                               float accel_desired, float accel_target, float accel, float accel_offset)
 {
-    Write_PSCx(LOG_PSCN_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
+    Write_PSCx(LOG_PSCN_MSG, pos_target, pos, pos_target_offset, pos_offset,
+               vel_desired, vel_target, vel, vel_target_offset, vel_offset,
+               accel_desired, accel_target, accel, accel_offset);
 }
 
-void AC_PosControl::Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
+void AC_PosControl::Write_PSCE(float pos_target, float pos, float pos_target_offset, float pos_offset,
+                               float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset,
+                               float accel_desired, float accel_target, float accel, float accel_offset)
 {
-    Write_PSCx(LOG_PSCE_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
+    Write_PSCx(LOG_PSCE_MSG, pos_target, pos, pos_target_offset, pos_offset,
+               vel_desired, vel_target, vel, vel_target_offset, vel_offset,
+               accel_desired, accel_target, accel, accel_offset);
 }
 
-void AC_PosControl::Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
+void AC_PosControl::Write_PSCD(float pos_target, float pos, float pos_target_offset, float pos_offset,
+                               float vel_desired, float vel_target, float vel, float vel_target_offset, float vel_offset,
+                               float accel_desired, float accel_target, float accel, float accel_offset)
 {
-    Write_PSCx(LOG_PSCD_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
+    Write_PSCx(LOG_PSCD_MSG, pos_target, pos, pos_target_offset, pos_offset,
+               vel_desired, vel_target, vel, vel_target_offset, vel_offset,
+               accel_desired, accel_target, accel, accel_offset);
 }
 
 #endif  // HAL_LOGGING_ENABLED
diff --git a/libraries/AC_AttitudeControl/LogStructure.h b/libraries/AC_AttitudeControl/LogStructure.h
@@ -12,36 +12,51 @@
 // @Field: TimeUS: Time since system startup
 // @Field: TPN: Target position relative to EKF origin
 // @Field: PN: Position relative to EKF origin
+// @Field: TPON: Target position offset North
+// @Field: PON: Position offset North
 // @Field: DVN: Desired velocity North
 // @Field: TVN: Target velocity North
 // @Field: VN: Velocity North
+// @Field: TVON: Target velocity offset North
+// @Field: VON: Velocity offset North
 // @Field: DAN: Desired acceleration North
 // @Field: TAN: Target acceleration North
 // @Field: AN: Acceleration North
+// @Field: AON: Acceleration offset North
 
 // @LoggerMessage: PSCE
 // @Description: Position Control East
 // @Field: TimeUS: Time since system startup
 // @Field: TPE: Target position relative to EKF origin
 // @Field: PE: Position relative to EKF origin
+// @Field: TPOE: Target position offset East
+// @Field: POE: Position offset East
 // @Field: DVE: Desired velocity East
 // @Field: TVE: Target velocity East
 // @Field: VE: Velocity East
+// @Field: TVOE: Target velocity offset East
+// @Field: VOE: Velocity offset East
 // @Field: DAE: Desired acceleration East
 // @Field: TAE: Target acceleration East
 // @Field: AE: Acceleration East
+// @Field: AOE: Acceleration offset East
 
 // @LoggerMessage: PSCD
 // @Description: Position Control Down
 // @Field: TimeUS: Time since system startup
 // @Field: TPD: Target position relative to EKF origin
 // @Field: PD: Position relative to EKF origin
+// @Field: TPOD: Target position offset Down
+// @Field: POD: Position offset Down
 // @Field: DVD: Desired velocity Down
 // @Field: TVD: Target velocity Down
 // @Field: VD: Velocity Down
+// @Field: TVOD: Target velocity offset Down
+// @Field: VOD: Velocity offset Down
 // @Field: DAD: Desired acceleration Down
 // @Field: TAD: Target acceleration Down
 // @Field: AD: Acceleration Down
+// @Field: AOD: Acceleration offset Down
 
 
 // position controller per-axis logging
@@ -50,12 +65,17 @@ struct PACKED log_PSCx {
     uint64_t time_us;
     float pos_target;
     float pos;
+    float pos_target_offset;
+    float pos_offset;
     float vel_desired;
     float vel_target;
     float vel;
+    float vel_target_offset;
+    float vel_offset;
     float accel_desired;
     float accel_target;
     float accel;
+    float accel_offset;
 };
 
 // @LoggerMessage: RATE
@@ -92,16 +112,16 @@ struct PACKED log_Rate {
     float   throttle_slew;
 };
 
-#define PSCx_FMT "Qffffffff"
-#define PSCx_UNITS "smmnnnooo"
-#define PSCx_MULTS "F00000000"
+#define PSCx_FMT "Qfffffffffffff"
+#define PSCx_UNITS "smmmmnnnnnoooo"
+#define PSCx_MULTS "F0000000000000"
 
 #define LOG_STRUCTURE_FROM_AC_ATTITUDECONTROL        \
     { LOG_PSCN_MSG, sizeof(log_PSCx), \
-      "PSCN", PSCx_FMT, "TimeUS,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", PSCx_UNITS, PSCx_MULTS }, \
+      "PSCN", PSCx_FMT, "TimeUS,TPN,PN,TPON,PON,DVN,TVN,VN,TVON,VON,DAN,TAN,AN,AON", PSCx_UNITS, PSCx_MULTS }, \
     { LOG_PSCE_MSG, sizeof(log_PSCx), \
-      "PSCE", PSCx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PSCx_UNITS, PSCx_MULTS }, \
+      "PSCE", PSCx_FMT, "TimeUS,TPE,PE,TPOE,POE,DVE,TVE,VE,TVOE,VOE,DAE,TAE,AE,AON", PSCx_UNITS, PSCx_MULTS }, \
     { LOG_PSCD_MSG, sizeof(log_PSCx), \
-      "PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \
+      "PSCD", PSCx_FMT, "TimeUS,TPD,PD,TPOD,POD,DVD,TVD,VD,TVOD,VOD,DAD,TAD,AD,AOD", PSCx_UNITS, PSCx_MULTS }, \
     { LOG_RATE_MSG, sizeof(log_Rate), \
         "RATE", "Qfffffffffffff",  "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true }