From 3165ec694ce3c1990ce5fd57312ec8db4030fe7a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 28 Feb 2024 23:23:13 +1100 Subject: [PATCH] AC_AttitudeControl: move logging of PSC messages into AC_AttitudeControl --- .../AC_AttitudeControl/AC_PosControl.cpp | 6 +- libraries/AC_AttitudeControl/AC_PosControl.h | 12 ++++ .../AC_PosControl_Logging.cpp | 43 +++++++++++ libraries/AC_AttitudeControl/LogStructure.h | 71 +++++++++++++++++++ 4 files changed, 129 insertions(+), 3 deletions(-) create mode 100644 libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp create mode 100644 libraries/AC_AttitudeControl/LogStructure.h diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index e82241ab3c..0f98cd4a06 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/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()); } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 8863125ac4..68827e214d 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -13,6 +13,8 @@ #include // Inertial Navigation library #include "AC_AttitudeControl.h" // Attitude control library +#include + // 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 @@ -484,4 +486,14 @@ protected: // return true if on a real vehicle or SITL with lock-step scheduling bool has_good_timing(void) const; + + 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); + +private: + // convenience method for writing out the identical PSCE, PSCN, PSCD - 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); + }; diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp new file mode 100644 index 0000000000..d02760cdeb --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp @@ -0,0 +1,43 @@ +#include + +#if HAL_LOGGING_ENABLED + +#include "AC_PosControl.h" + +#include +#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 diff --git a/libraries/AC_AttitudeControl/LogStructure.h b/libraries/AC_AttitudeControl/LogStructure.h new file mode 100644 index 0000000000..9b5fe8df95 --- /dev/null +++ b/libraries/AC_AttitudeControl/LogStructure.h @@ -0,0 +1,71 @@ +#pragma once + +#include + +#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 }