Move PositionControl logging (PSCN, PSCE, PSCD) into AC_AttitudeControl library

Blimp/Loiter.cpp

@@ -1,5 +1,7 @@
 #include "Blimp.h"
 
+#include <AC_AttitudeControl/AC_PosControl.h>
+
 #define MA 0.99
 #define MO (1-MA)
 
@@ -123,9 +125,9 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
     }
 
 #if HAL_LOGGING_ENABLED
-    AP::logger().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);
-    AP::logger().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);
-    AP::logger().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, 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);
 #endif
 }
 
@@ -201,8 +203,8 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax
     }
 
 #if HAL_LOGGING_ENABLED
-    AP::logger().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);
-    AP::logger().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);
-    AP::logger().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, 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);
 #endif
 }

Blimp/wscript

@@ -18,6 +18,7 @@ def build(bld):
             'AP_KDECAN',
             'AP_Beacon',
             'AP_AdvancedFailsafe', # TODO for some reason compiling GCS_Common.cpp (in libraries) fails if this isn't included.
+            'AC_AttitudeControl',  # for PSCx logging
         ],
     )
 

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -1167,16 +1167,16 @@ void AC_PosControl::write_log()
     if (is_active_xy()) {
         float accel_x, accel_y;
         lean_angles_to_accel_xy(accel_x, accel_y);
-        AP::logger().Write_PSCN(get_pos_target_cm().x, _inav.get_position_neu_cm().x,
+        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);
-        AP::logger().Write_PSCE(get_pos_target_cm().y, _inav.get_position_neu_cm().y,
+        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);
     }
 
     if (is_active_z()) {
-        AP::logger().Write_PSCD(-get_pos_target_cm().z, -_inav.get_position_z_up_cm(),
+        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());
     }

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -13,6 +13,8 @@
 #include <AP_InertialNav/AP_InertialNav.h>  // Inertial Navigation library
 #include "AC_AttitudeControl.h"     // Attitude control library
 
+#include <AP_Logger/LogStructure.h>
+
 // position controller default definitions
 #define POSCONTROL_ACCEL_XY                     100.0f  // default horizontal acceleration in cm/s/s.  This is overwritten by waypoint and loiter controllers
 #define POSCONTROL_JERK_XY                      5.0f    // default horizontal jerk m/s/s/s
@@ -399,6 +401,10 @@ 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);
+
 protected:
 
     // get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain)
@@ -484,4 +490,10 @@ class AC_PosControl
 
     // return true if on a real vehicle or SITL with lock-step scheduling
     bool has_good_timing(void) const;
+
+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);
+
 };

libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp

@@ -0,0 +1,43 @@
+#include <AP_Logger/AP_Logger_config.h>
+
+#if HAL_LOGGING_ENABLED
+
+#include "AC_PosControl.h"
+
+#include <AP_Logger/AP_Logger.h>
+#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)
+{
+    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,
+            vel_desired   : vel_desired * 0.01f,
+            vel_target    : vel_target * 0.01f,
+            vel           : vel * 0.01f,
+            accel_desired : accel_desired * 0.01f,
+            accel_target  : accel_target * 0.01f,
+            accel         : accel * 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)
+{
+    Write_PSCx(LOG_PSCN_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
+}
+
+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)
+{
+    Write_PSCx(LOG_PSCE_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
+}
+
+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)
+{
+    Write_PSCx(LOG_PSCD_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
+}
+
+#endif  // HAL_LOGGING_ENABLED

libraries/AC_AttitudeControl/LogStructure.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <AP_Logger/LogStructure.h>
+
+#define LOG_IDS_FROM_AC_ATTITUDECONTROL \
+    LOG_PSCN_MSG, \
+    LOG_PSCE_MSG, \
+    LOG_PSCD_MSG
+
+// @LoggerMessage: PSCN
+// @Description: Position Control North
+// @Field: TimeUS: Time since system startup
+// @Field: TPN: Target position relative to EKF origin
+// @Field: PN: Position relative to EKF origin
+// @Field: DVN: Desired velocity North
+// @Field: TVN: Target velocity North
+// @Field: VN: Velocity North
+// @Field: DAN: Desired acceleration North
+// @Field: TAN: Target acceleration North
+// @Field: AN: Acceleration 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: DVE: Desired velocity East
+// @Field: TVE: Target velocity East
+// @Field: VE: Velocity East
+// @Field: DAE: Desired acceleration East
+// @Field: TAE: Target acceleration East
+// @Field: AE: Acceleration 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: DVD: Desired velocity Down
+// @Field: TVD: Target velocity Down
+// @Field: VD: Velocity Down
+// @Field: DAD: Desired acceleration Down
+// @Field: TAD: Target acceleration Down
+// @Field: AD: Acceleration Down
+
+
+// position controller per-axis logging
+struct PACKED log_PSCx {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float pos_target;
+    float pos;
+    float vel_desired;
+    float vel_target;
+    float vel;
+    float accel_desired;
+    float accel_target;
+    float accel;
+};
+
+#define PSCx_FMT "Qffffffff"
+#define PSCx_UNITS "smmnnnooo"
+#define PSCx_MULTS "F00000000"
+
+#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 }, \
+    { LOG_PSCE_MSG, sizeof(log_PSCx), \
+      "PSCE", PSCx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", 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 }

libraries/APM_Control/AR_PosControl.cpp

@@ -21,6 +21,7 @@
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AC_Avoidance/AC_Avoid.h>
+#include <AC_AttitudeControl/AC_PosControl.h>
 
 #define AR_POSCON_TIMEOUT_MS            100     // timeout after 0.1 sec
 #define AR_POSCON_POS_P                 0.2f    // default position P gain
@@ -385,15 +386,16 @@ void AR_PosControl::write_log()
     // convert position to required format
     Vector2f pos_target_2d_cm = get_pos_target().tofloat() * 100.0;
 
-    AP::logger().Write_PSCN(pos_target_2d_cm.x,     // position target
+    // reuse logging from AC_PosControl:
+    AC_PosControl::Write_PSCN(pos_target_2d_cm.x,     // position target
                             curr_pos_NED.x * 100.0, // position
                             _vel_desired.x * 100.0, // desired velocity
                             _vel_target.x * 100.0,  // target velocity
                             curr_vel_NED.x * 100.0, // velocity
                             _accel_desired.x * 100.0,   // desired accel
                             _accel_target.x * 100.0,    // target accel
                             curr_accel_NED.x);      // accel
-    AP::logger().Write_PSCE(pos_target_2d_cm.y,     // position target
+    AC_PosControl::Write_PSCE(pos_target_2d_cm.y,     // position target
                             curr_pos_NED.y * 100.0, // position
                             _vel_desired.y * 100.0, // desired velocity
                             _vel_target.y * 100.0,  // target velocity

libraries/AP_Logger/AP_Logger.h

@@ -273,9 +273,6 @@ class AP_Logger
                           const class RallyLocation &rally_point);
     void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);
     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_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
-    void Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
-    void Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
 
     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, ...);
@@ -598,10 +595,6 @@ class AP_Logger
 
     /* end support for retrieving logs via mavlink: */
 
-    // convenience method for writing out the identical NED PIDs - and
-    // to save bytes
-    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);
-
 #if HAL_LOGGER_FILE_CONTENTS_ENABLED
     void log_file_content(FileContent &file_content, const char *filename);
     void file_content_update(FileContent &file_content);

libraries/AP_Logger/LogFile.cpp

@@ -550,37 +550,4 @@ void AP_Logger::Write_Winch(bool healthy, bool thread_end, bool moving, bool clu
     WriteBlock(&pkt, sizeof(pkt));
 }
 
-// a convenience function for writing out the position controller PIDs
-void AP_Logger::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)
-{
-    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,
-            vel_desired   : vel_desired * 0.01f,
-            vel_target    : vel_target * 0.01f,
-            vel           : vel * 0.01f,
-            accel_desired : accel_desired * 0.01f,
-            accel_target  : accel_target * 0.01f,
-            accel         : accel * 0.01f
-    };
-    WriteBlock(&pkt, sizeof(pkt));
-}
-
-void AP_Logger::Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
-{
-    Write_PSCx(LOG_PSCN_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
-}
-
-void AP_Logger::Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
-{
-    Write_PSCx(LOG_PSCE_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
-}
-
-void AP_Logger::Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
-{
-    Write_PSCx(LOG_PSCD_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
-}
-
 #endif  // HAL_LOGGING_ENABLED