AP_Logger: add structure for PSC message

libraries/AP_Logger/AP_Logger.h

@@ -289,6 +289,7 @@ public:
     void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest);
     void Write_SimpleAvoidance(uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, bool back_up);
     void Write_Winch(bool healthy, bool thread_end, bool moving, bool clutch, uint8_t mode, float desired_length, float length, float desired_rate, uint16_t tension, float voltage, int8_t temp);
+    void Write_PSC(const Vector3f &pos_target, const Vector3f &position, const Vector3f &vel_target, const Vector3f &velocity, const Vector3f &accel_target, const float &accel_x, const float &accel_y);
 
     void Write(const char *name, const char *labels, const char *fmt, ...);
     void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

libraries/AP_Logger/LogFile.cpp

@@ -1134,3 +1134,24 @@ void AP_Logger::Write_Winch(bool healthy, bool thread_end, bool moving, bool clu
     };
     WriteBlock(&pkt, sizeof(pkt));
 }
+
+void AP_Logger::Write_PSC(const Vector3f &pos_target, const Vector3f &position, const Vector3f &vel_target, const Vector3f &velocity, const Vector3f &accel_target, const float &accel_x, const float &accel_y)
+{
+    struct log_PSC pkt{
+        LOG_PACKET_HEADER_INIT(LOG_PSC_MSG),
+        time_us         : AP_HAL::micros64(),
+        pos_target_x    : pos_target.x * 0.01f,
+        pos_target_Y    : pos_target.y * 0.01f,
+        position_x      : position.x * 0.01f,
+        position_y      : position.y * 0.01f,
+        vel_target_x    : vel_target.x * 0.01f,
+        vel_target_y    : vel_target.y * 0.01f,
+        velocity_x      : velocity.x * 0.01f,
+        velocity_y      : velocity.y * 0.01f,
+        accel_target_x  : accel_target.x * 0.01f,
+        accel_target_y  : accel_target.y * 0.01f,
+        accel_x         : accel_x * 0.01f,
+        accel_y         : accel_y * 0.01f
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}

libraries/AP_Logger/LogStructure.h

@@ -1271,6 +1271,23 @@ struct PACKED log_Winch {
     int8_t temp;
 };
 
+struct PACKED log_PSC {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float pos_target_x;
+    float pos_target_Y;
+    float position_x;
+    float position_y;
+    float vel_target_x;
+    float vel_target_y;
+    float velocity_x;
+    float velocity_y;
+    float accel_target_x;
+    float accel_target_y;
+    float accel_x;
+    float accel_y;
+};
+
 // FMT messages define all message formats other than FMT
 // UNIT messages define units which can be referenced by FMTU messages
 // FMTU messages associate types (e.g. centimeters/second/second) to FMT message fields
@@ -2451,6 +2468,22 @@ struct PACKED log_Winch {
 // @Field: Vcc: Voltage to Motor
 // @Field: Temp: Motor temperature
 
+// @LoggerMessage: PSC
+// @Description: Position Control data
+// @Field: TimeUS: Time since system startup
+// @Field: TPX: Target position relative to origin, X-axis
+// @Field: TPY: Target position relative to origin, Y-axis
+// @Field: PX: Position relative to origin, X-axis
+// @Field: PY: Position relative to origin, Y-axis
+// @Field: TVX: Target velocity, X-axis
+// @Field: TVY: Target velocity, Y-axis
+// @Field: VX: Velocity, X-axis
+// @Field: VY: Velocity, Y-axis
+// @Field: TAX: Target acceleration, X-axis
+// @Field: TAY: Target acceleration, Y-axis
+// @Field: AX: Acceleration, X-axis
+// @Field: AY: Acceleration, Y-axis
+
 // messages for all boards
 #define LOG_BASE_STRUCTURES \
     { LOG_FORMAT_MSG, sizeof(log_Format), \
@@ -2668,7 +2701,9 @@ struct PACKED log_Winch {
     { LOG_ERROR_MSG, sizeof(log_Error), \
       "ERR",   "QBB",         "TimeUS,Subsys,ECode", "s--", "F--" }, \
     { LOG_WINCH_MSG, sizeof(log_Winch), \
-      "WINC", "QBBBBBfffHfb", "TimeUS,Heal,ThEnd,Mov,Clut,Mode,DLen,Len,DRate,Tens,Vcc,Temp", "s-----mmn?vO", "F-----000000" }
+      "WINC", "QBBBBBfffHfb", "TimeUS,Heal,ThEnd,Mov,Clut,Mode,DLen,Len,DRate,Tens,Vcc,Temp", "s-----mmn?vO", "F-----000000" }, \
+    { LOG_PSC_MSG, sizeof(log_PSC), \
+      "PSC", "Qffffffffffff", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY", "smmmmnnnnoooo", "F000000000000" }
 
 // @LoggerMessage: SBPH
 // @Description: Swift Health Data
@@ -2830,6 +2865,7 @@ enum LogMessages : uint8_t {
     LOG_VISUALVEL_MSG,
     LOG_SIMPLE_AVOID_MSG,
     LOG_WINCH_MSG,
+    LOG_PSC_MSG,
 
     _LOG_LAST_MSG_
 };