From 3ae8af6b217540cca450a5712ff6f8e3c166d8d0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 29 Jan 2021 13:42:26 +0900 Subject: [PATCH] AP_Logger: add PSCZ logging Co-authored-by: Peter Barker includes const log_PSCZ declaration also changed PSZ ThO to percentage --- libraries/AP_Logger/AP_Logger.h | 1 + libraries/AP_Logger/LogFile.cpp | 18 ++++++++++++++++ libraries/AP_Logger/LogStructure.h | 33 +++++++++++++++++++++++++++++- 3 files changed, 51 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 414d770cbe..608e85badf 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -335,6 +335,7 @@ public: 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_PSCZ(float pos_target_z, float pos_z, float vel_desired_z, float vel_target_z, float vel_z, float accel_desired_z, float accel_target_z, float accel_z, float throttle_out); 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, ...); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 7b3fad5388..77b0080120 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -747,3 +747,21 @@ void AP_Logger::Write_PSC(const Vector3f &pos_target, const Vector3f &position, }; WriteBlock(&pkt, sizeof(pkt)); } + +void AP_Logger::Write_PSCZ(float pos_target_z, float pos_z, float vel_desired_z, float vel_target_z, float vel_z, float accel_desired_z, float accel_target_z, float accel_z, float throttle_out) +{ + const struct log_PSCZ pkt{ + LOG_PACKET_HEADER_INIT(LOG_PSCZ_MSG), + time_us : AP_HAL::micros64(), + pos_target_z : pos_target_z * 0.01f, + pos_z : pos_z * 0.01f, + vel_desired_z : vel_desired_z * 0.01f, + vel_target_z : vel_target_z * 0.01f, + vel_z : vel_z * 0.01f, + accel_desired_z : accel_desired_z * 0.01f, + accel_target_z : accel_target_z * 0.01f, + accel_z : accel_z * 0.01f, + throttle_out : throttle_out + }; + WriteBlock(&pkt, sizeof(pkt)); +} diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index bdb9462b64..abe3fae164 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -898,6 +898,21 @@ struct PACKED log_PSC { float accel_y; }; +// position controller z-axis logging +struct PACKED log_PSCZ { + LOG_PACKET_HEADER; + uint64_t time_us; + float pos_target_z; + float pos_z; + float vel_desired_z; + float vel_target_z; + float vel_z; + float accel_desired_z; + float accel_target_z; + float accel_z; + float throttle_out; +}; + // 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 @@ -1571,6 +1586,19 @@ struct PACKED log_PSC { // @Field: AX: Acceleration, X-axis // @Field: AY: Acceleration, Y-axis +// @LoggerMessage: PSCZ +// @Description: Position Control Z-axis +// @Field: TimeUS: Time since system startup +// @Field: TPZ: Target position above EKF origin +// @Field: PZ: Position above EKF origin +// @Field: DVZ: Desired velocity Z-axis +// @Field: TVZ: Target velocity Z-axis +// @Field: VZ: Velocity Z-axis +// @Field: DAZ: Desired acceleration Z-axis +// @Field: TAZ: Target acceleration Z-axis +// @Field: AZ: Acceleration Z-axis +// @Field: ThO: Throttle output + // messages for all boards #define LOG_BASE_STRUCTURES \ { LOG_FORMAT_MSG, sizeof(log_Format), \ @@ -1703,7 +1731,9 @@ LOG_STRUCTURE_FROM_VISUALODOM \ { 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_PSC_MSG, sizeof(log_PSC), \ - "PSC", "Qffffffffffff", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY", "smmmmnnnnoooo", "F000000000000" } + "PSC", "Qffffffffffff", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY", "smmmmnnnnoooo", "F000000000000" }, \ + { LOG_PSCZ_MSG, sizeof(log_PSCZ), \ + "PSCZ", "Qfffffffff", "TimeUS,TPZ,PZ,DVZ,TVZ,VZ,DAZ,TAZ,AZ,ThO", "smmnnnooo%", "F000000002" } // @LoggerMessage: SBPH // @Description: Swift Health Data @@ -1825,6 +1855,7 @@ enum LogMessages : uint8_t { LOG_SIMPLE_AVOID_MSG, LOG_WINCH_MSG, LOG_PSC_MSG, + LOG_PSCZ_MSG, _LOG_LAST_MSG_ };