From d0bbc02995c4dccfcc2b095df54ed7825850e807 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Nov 2024 11:13:26 +0000 Subject: [PATCH] AP_Logger: remove `Write_ServoStatus` and `CSRV` definition --- libraries/AP_Logger/AP_Logger.h | 2 -- libraries/AP_Logger/LogFile.cpp | 25 -------------------- libraries/AP_Logger/LogStructure.h | 37 +++--------------------------- 3 files changed, 3 insertions(+), 61 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 9fe2c07f26..13dde72061 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -262,8 +262,6 @@ public: void Write_Radio(const mavlink_radio_t &packet); void Write_Message(const char *message); void Write_MessageF(const char *fmt, ...); - void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct, - float pos_cmd, float voltage, float current, float mot_temp, float pcb_temp, uint8_t error); void Write_Compass(); void Write_Mode(uint8_t mode, const ModeReason reason); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 4765af723c..fc100c0e94 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -452,31 +452,6 @@ bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason) return WriteCriticalBlock(&pkt, sizeof(pkt)); } -/* - write servo status from CAN servo - */ -void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct, - float pos_cmd, float voltage, float current, float mot_temp, float pcb_temp, uint8_t error) -{ - const struct log_CSRV pkt { - LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG), - time_us : time_us, - id : id, - position : position, - force : force, - speed : speed, - power_pct : power_pct, - pos_cmd : pos_cmd, - voltage : voltage, - current : current, - mot_temp : mot_temp, - pcb_temp : pcb_temp, - error : error, - }; - WriteBlock(&pkt, sizeof(pkt)); -} - - // Write a Yaw PID packet void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info) { diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 3b7c599bcd..418b8de1c8 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -148,6 +148,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 @@ -477,22 +478,6 @@ struct PACKED log_TERRAIN { float reference_offset; }; -struct PACKED log_CSRV { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t id; - float position; - float force; - float speed; - uint8_t power_pct; - float pos_cmd; - float voltage; - float current; - float mot_temp; - float pcb_temp; - uint8_t error; -}; - struct PACKED log_ARSP { LOG_PACKET_HEADER; uint64_t time_us; @@ -699,21 +684,6 @@ struct PACKED log_VER { // @Field: TR: innovation test ratio // @Field: Pri: True if sensor is the primary sensor -// @LoggerMessage: CSRV -// @Description: Servo feedback data -// @Field: TimeUS: Time since system startup -// @Field: Id: Servo number this data relates to -// @Field: Pos: Current servo position -// @Field: Force: Force being applied -// @Field: Speed: Current servo movement speed -// @Field: Pow: Amount of rated power being applied -// @Field: PosCmd: commanded servo position -// @Field: V: Voltage -// @Field: A: Current -// @Field: MotT: motor temperature -// @Field: PCBT: PCB temperature -// @Field: Err: error flags - // @LoggerMessage: DMS // @Description: DataFlash-Over-MAVLink statistics // @Field: TimeUS: Time since system startup @@ -1223,8 +1193,7 @@ LOG_STRUCTURE_FROM_AVOIDANCE \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ "TERR","QBLLHffHHf","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded,ROfs", "s-DU-mm--m", "F-GG-00--0", true }, \ LOG_STRUCTURE_FROM_ESC_TELEM \ - { LOG_CSRV_MSG, sizeof(log_CSRV), \ - "CSRV","QBfffBfffffB","TimeUS,Id,Pos,Force,Speed,Pow,PosCmd,V,A,MotT,PCBT,Err", "s#---%dvAOO-", "F-000000000-", false }, \ +LOG_STRUCTURE_FROM_SERVO_TELEM \ { LOG_PIDR_MSG, sizeof(log_PID), \ "PIDR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS, true }, \ { LOG_PIDP_MSG, sizeof(log_PID), \ @@ -1307,7 +1276,7 @@ enum LogMessages : uint8_t { LOG_IDS_FROM_CAMERA, LOG_IDS_FROM_MOUNT, LOG_TERRAIN_MSG, - LOG_CSRV_MSG, + LOG_IDS_FROM_SERVO_TELEM, LOG_IDS_FROM_ESC_TELEM, LOG_IDS_FROM_BATTMONITOR, LOG_IDS_FROM_HAL_CHIBIOS,