From 02c225ee93f2daa0e2dceb49c20f4891acb6eeaa Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 9 Sep 2020 08:16:39 +0100 Subject: [PATCH] AP_Logger: add structure for PSC message --- libraries/AP_Logger/AP_Logger.h | 1 + libraries/AP_Logger/LogFile.cpp | 21 +++++++++++++++++ libraries/AP_Logger/LogStructure.h | 38 +++++++++++++++++++++++++++++- 3 files changed, 59 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 7452613477..f1d59a3ac2 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -289,6 +289,7 @@ public: void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest); 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(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 e985e3cb32..3deed38b80 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -1134,3 +1134,24 @@ 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) +{ + struct log_PSC pkt{ + LOG_PACKET_HEADER_INIT(LOG_PSC_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 + }; + WriteBlock(&pkt, sizeof(pkt)); +} diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index c4fd585963..7e9a44d6ad 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1271,6 +1271,23 @@ struct PACKED log_Winch { int8_t temp; }; +struct PACKED log_PSC { + 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; +}; + // 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 @@ -2451,6 +2468,22 @@ struct PACKED log_Winch { // @Field: Vcc: Voltage to Motor // @Field: Temp: Motor temperature +// @LoggerMessage: PSC +// @Description: Position Control data +// @Field: TimeUS: Time since system startup +// @Field: TPX: Target position relative to origin, X-axis +// @Field: TPY: Target position relative to origin, Y-axis +// @Field: PX: Position relative to origin, X-axis +// @Field: PY: Position relative to origin, Y-axis +// @Field: TVX: Target velocity, X-axis +// @Field: TVY: Target velocity, Y-axis +// @Field: VX: Velocity, X-axis +// @Field: VY: Velocity, Y-axis +// @Field: TAX: Target acceleration, X-axis +// @Field: TAY: Target acceleration, Y-axis +// @Field: AX: Acceleration, X-axis +// @Field: AY: Acceleration, Y-axis + // messages for all boards #define LOG_BASE_STRUCTURES \ { LOG_FORMAT_MSG, sizeof(log_Format), \ @@ -2668,7 +2701,9 @@ struct PACKED log_Winch { { LOG_ERROR_MSG, sizeof(log_Error), \ "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" } + "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" } // @LoggerMessage: SBPH // @Description: Swift Health Data @@ -2830,6 +2865,7 @@ enum LogMessages : uint8_t { LOG_VISUALVEL_MSG, LOG_SIMPLE_AVOID_MSG, LOG_WINCH_MSG, + LOG_PSC_MSG, _LOG_LAST_MSG_ };