From e715e1e14ec2d56ff6d6c6a7bde5662cf184d6f4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 9 Mar 2023 10:19:18 +1100 Subject: [PATCH] AP_Logger: factor Write_PSC[NED] methods to save bytes --- libraries/AP_Logger/AP_Logger.h | 4 +++ libraries/AP_Logger/LogFile.cpp | 58 +++++++++++------------------- libraries/AP_Logger/LogStructure.h | 48 +++++++------------------ 3 files changed, 36 insertions(+), 74 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 36f599e49e..f96a9d4916 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -582,6 +582,10 @@ 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 6441c08cc3..22bedfe546 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -518,53 +518,35 @@ void AP_Logger::Write_Winch(bool healthy, bool thread_end, bool moving, bool clu 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) +// 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_PSCN pkt{ - LOG_PACKET_HEADER_INIT(LOG_PSCN_MSG), - 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 + 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) { - const struct log_PSCE pkt{ - LOG_PACKET_HEADER_INIT(LOG_PSCE_MSG), - 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)); + 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) { - const struct log_PSCD pkt{ - LOG_PACKET_HEADER_INIT(LOG_PSCD_MSG), - 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)); + Write_PSCx(LOG_PSCD_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 481b4a8c51..61a6da48ca 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -613,36 +613,8 @@ struct PACKED log_Winch { int8_t temp; }; -// position controller North axis logging -struct PACKED log_PSCN { - 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; -}; - -// position controller East axis logging -struct PACKED log_PSCE { - 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; -}; - -// position controller Down axis logging -struct PACKED log_PSCD { +// position controller per-axis logging +struct PACKED log_PSCx { LOG_PACKET_HEADER; uint64_t time_us; float pos_target; @@ -718,6 +690,10 @@ 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 @@ -1342,12 +1318,12 @@ 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_PSCN), \ - "PSCN", "Qffffffff", "TimeUS,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", "smmnnnooo", "F00000000" }, \ - { LOG_PSCE_MSG, sizeof(log_PSCE), \ - "PSCE", "Qffffffff", "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", "smmnnnooo", "F00000000" }, \ - { LOG_PSCD_MSG, sizeof(log_PSCD), \ - "PSCD", "Qffffffff", "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", "smmnnnooo", "F00000000" }, \ + { 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_STAK_MSG, sizeof(log_STAK), \ "STAK", "QBBHHN", "TimeUS,Id,Pri,Total,Free,Name", "s#----", "F-----", true }, \ { LOG_FILE_MSG, sizeof(log_File), \