From f4df89b85b6aac52ea5c12f8a88bef317e254f6b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 28 Feb 2024 23:23:14 +1100 Subject: [PATCH] AP_Logger: move logging of PSC messages into AC_AttitudeControl --- libraries/AP_Logger/AP_Logger.h | 7 ---- libraries/AP_Logger/LogFile.cpp | 33 --------------- libraries/AP_Logger/LogStructure.h | 66 ++---------------------------- 3 files changed, 3 insertions(+), 103 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 77abfb4757..d3cb4b0d68 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -273,9 +273,6 @@ public: 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 @@ private: /* 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); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 2febb3577f..405f41ac2c 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/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 diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 54dd754e62..6d4542540a 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -143,6 +143,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include // structure used to define logging format // It is packed on ChibiOS to save flash space; however, this causes problems @@ -607,20 +608,6 @@ struct PACKED log_Winch { int8_t temp; }; -// 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; -}; - // thread stack usage struct PACKED log_STAK { LOG_PACKET_HEADER; @@ -685,10 +672,6 @@ struct PACKED log_VER { #define PID_UNITS "s-----------" #define PID_MULTS "F-----------" -#define PIDx_FMT "Qffffffff" -#define PIDx_UNITS "smmnnnooo" -#define PIDx_MULTS "F00000000" - // @LoggerMessage: ADSB // @Description: Automatic Dependent Serveillance - Broadcast detected vehicle information // @Field: TimeUS: Time since system startup @@ -1150,42 +1133,6 @@ struct PACKED log_VER { // @Field: Vcc: Voltage to Motor // @Field: Temp: Motor temperature -// @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 - // @LoggerMessage: STAK // @Description: Stack information // @Field: TimeUS: Time since system startup @@ -1344,12 +1291,7 @@ LOG_STRUCTURE_FROM_VISUALODOM \ "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" }, \ - { LOG_PSCN_MSG, sizeof(log_PSCx), \ - "PSCN", PIDx_FMT, "TimeUS,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", PIDx_UNITS, PIDx_MULTS }, \ - { LOG_PSCE_MSG, sizeof(log_PSCx), \ - "PSCE", PIDx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PIDx_UNITS, PIDx_MULTS }, \ - { LOG_PSCD_MSG, sizeof(log_PSCx), \ - "PSCD", PIDx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PIDx_UNITS, PIDx_MULTS }, \ + LOG_STRUCTURE_FROM_AC_ATTITUDECONTROL, \ { LOG_STAK_MSG, sizeof(log_STAK), \ "STAK", "QBBHHN", "TimeUS,Id,Pri,Total,Free,Name", "s#----", "F-----", true }, \ { LOG_FILE_MSG, sizeof(log_File), \ @@ -1434,9 +1376,7 @@ enum LogMessages : uint8_t { LOG_ADSB_MSG, LOG_ARM_DISARM_MSG, LOG_WINCH_MSG, - LOG_PSCN_MSG, - LOG_PSCE_MSG, - LOG_PSCD_MSG, + LOG_IDS_FROM_AC_ATTITUDECONTROL, LOG_IDS_FROM_PRECLAND, LOG_IDS_FROM_AIS, LOG_STAK_MSG,