From 087a2b92c3a4be647b2eeb033b869bad1eef5e27 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 22 Sep 2021 12:11:15 +0930 Subject: [PATCH] AP_Logger: Update PSC logging to include desired --- libraries/AP_Logger/AP_Logger.h | 5 +- libraries/AP_Logger/LogFile.cpp | 68 ++++++++------- libraries/AP_Logger/LogStructure.h | 130 +++++++++++++++++------------ 3 files changed, 118 insertions(+), 85 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index b25c8da3e7..f63c6e3df0 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -334,8 +334,9 @@ public: #endif 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_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_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, ...); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 864c104d7c..7c0c5d78cb 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -570,41 +570,53 @@ void AP_Logger::Write_Winch(bool healthy, bool thread_end, bool moving, bool clu WriteBlock(&pkt, sizeof(pkt)); } -void AP_Logger::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 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) { - struct log_PSC pkt{ - LOG_PACKET_HEADER_INIT(LOG_PSC_MSG), + const struct log_PSCN pkt{ + LOG_PACKET_HEADER_INIT(LOG_PSCN_MSG), time_us : AP_HAL::micros64(), - pos_target_x : pos_target.x * 0.01f, - pos_target_Y : pos_target.y * 0.01f, - position_x : position.x * 0.01f, - position_y : position.y * 0.01f, - vel_target_x : vel_target.x * 0.01f, - vel_target_y : vel_target.y * 0.01f, - velocity_x : velocity.x * 0.01f, - velocity_y : velocity.y * 0.01f, - accel_target_x : accel_target.x * 0.01f, - accel_target_y : accel_target.y * 0.01f, - accel_x : accel_x * 0.01f, - accel_y : accel_y * 0.01f + 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_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 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_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 + 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)); +} + +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)); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index ba978adfd7..a0d6833838 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -687,36 +687,46 @@ struct PACKED log_Winch { int8_t temp; }; -struct PACKED log_PSC { +// position controller North axis logging +struct PACKED log_PSCN { LOG_PACKET_HEADER; uint64_t time_us; - float pos_target_x; - float pos_target_Y; - float position_x; - float position_y; - float vel_target_x; - float vel_target_y; - float velocity_x; - float velocity_y; - float accel_target_x; - float accel_target_y; - float accel_x; - float accel_y; + float pos_target; + float pos; + float vel_desired; + float vel_target; + float vel; + float accel_desired; + float accel_target; + float accel; }; -// position controller z-axis logging -struct PACKED log_PSCZ { +// position controller East axis logging +struct PACKED log_PSCE { 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; + 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 { + 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; }; // FMT messages define all message formats other than FMT @@ -1190,34 +1200,41 @@ struct PACKED log_PSCZ { // @Field: Vcc: Voltage to Motor // @Field: Temp: Motor temperature -// @LoggerMessage: PSC -// @Description: Position Control data +// @LoggerMessage: PSCN +// @Description: Position Control North // @Field: TimeUS: Time since system startup -// @Field: TPX: Target position relative to origin, North -// @Field: TPY: Target position relative to origin, East -// @Field: PX: Position relative to origin, North -// @Field: PY: Position relative to origin, East -// @Field: TVX: Target velocity, North -// @Field: TVY: Target velocity, East -// @Field: VX: Velocity, North -// @Field: VY: Velocity, East -// @Field: TAX: Target acceleration, X-axis -// @Field: TAY: Target acceleration, Y-axis -// @Field: AX: Acceleration, X-axis -// @Field: AY: Acceleration, Y-axis +// @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: PSCZ -// @Description: Position Control Z-axis +// @LoggerMessage: PSCE +// @Description: Position Control East // @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 +// @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 // messages for all boards #define LOG_BASE_STRUCTURES \ @@ -1326,10 +1343,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_PSC_MSG, sizeof(log_PSC), \ - "PSC", "Qffffffffffff", "TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY", "smmmmnnnnoooo", "F000000000000", true }, \ - { LOG_PSCZ_MSG, sizeof(log_PSCZ), \ - "PSCZ", "Qfffffffff", "TimeUS,TPZ,PZ,DVZ,TVZ,VZ,DAZ,TAZ,AZ,ThO", "smmnnnooo%", "F000000002", true }, \ + { 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_STRUCTURE_FROM_AIS \ // @LoggerMessage: SBPH @@ -1440,8 +1459,9 @@ enum LogMessages : uint8_t { LOG_ARM_DISARM_MSG, LOG_IDS_FROM_AVOIDANCE, LOG_WINCH_MSG, - LOG_PSC_MSG, - LOG_PSCZ_MSG, + LOG_PSCN_MSG, + LOG_PSCE_MSG, + LOG_PSCD_MSG, LOG_RAW_PROXIMITY_MSG, LOG_IDS_FROM_PRECLAND, LOG_IDS_FROM_AIS,