diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 97dfbc5511..65e111d5e4 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -299,9 +299,7 @@ public: void Write_Radio(const mavlink_radio_t &packet); void Write_Message(const char *message); void Write_MessageF(const char *fmt, ...); - void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t esc_temp, uint16_t current_tot, int16_t motor_temp, float error_rate = 0.0f); void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct); - void Write_ESCStatus(uint64_t time_us, uint8_t id, uint32_t error_count, float voltage, float current, float temperature, int32_t rpm, uint8_t power_pct); 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 1901e96f19..9f3ecb893f 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -361,30 +361,6 @@ bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason) return WriteCriticalBlock(&pkt, sizeof(pkt)); } -// Write ESC status messages -// id starts from 0 -// rpm is eRPM (rpm * 100) -// voltage is in centi-volts -// current is in centi-amps -// temperature is in centi-degrees Celsius -// current_tot is in centi-amp hours -void AP_Logger::Write_ESC(uint8_t instance, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t esc_temp, uint16_t current_tot, int16_t motor_temp, float error_rate) -{ - const struct log_Esc pkt{ - LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC_MSG)), - time_us : time_us, - instance : instance, - rpm : rpm, - voltage : voltage, - current : current, - esc_temp : esc_temp, - current_tot : current_tot, - motor_temp : motor_temp, - error_rate : error_rate - }; - WriteBlock(&pkt, sizeof(pkt)); -} - /* write servo status from CAN servo */ @@ -402,25 +378,6 @@ void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position, WriteBlock(&pkt, sizeof(pkt)); } -/* - write ESC status from CAN ESC - */ -void AP_Logger::Write_ESCStatus(uint64_t time_us, uint8_t id, uint32_t error_count, float voltage, float current, float temperature, int32_t rpm, uint8_t power_pct) -{ - const struct log_CESC pkt { - LOG_PACKET_HEADER_INIT(LOG_CESC_MSG), - time_us : time_us, - id : id, - error_count : error_count, - voltage : voltage, - current : current, - temperature : temperature, - rpm : rpm, - power_pct : power_pct - }; - WriteBlock(&pkt, sizeof(pkt)); -} - // Write a Yaw PID packet void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index ddfabbb46e..47b8da22ad 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -45,6 +45,7 @@ const struct UnitStructure log_Units[] = { { '-', "" }, // no units e.g. Pi, or a string { '?', "UNKNOWN" }, // Units which haven't been worked out yet.... { 'A', "A" }, // Ampere + { 'a', "Ah" }, // Ampere hours { 'd', "deg" }, // of the angular variety, -180 to 180 { 'b', "B" }, // bytes { 'k', "deg/s" }, // degrees per second. Degrees are NOT SI, but is some situations more user-friendly than radians @@ -130,6 +131,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include // structure used to define logging format struct LogStructure { @@ -442,19 +444,6 @@ struct PACKED log_TERRAIN { uint16_t loaded; }; -struct PACKED log_Esc { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t instance; - int32_t rpm; - uint16_t voltage; - uint16_t current; - int16_t esc_temp; - uint16_t current_tot; - int16_t motor_temp; - float error_rate; -}; - struct PACKED log_CSRV { LOG_PACKET_HEADER; uint64_t time_us; @@ -465,18 +454,6 @@ struct PACKED log_CSRV { uint8_t power_pct; }; -struct PACKED log_CESC { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t id; - uint32_t error_count; - float voltage; - float current; - float temperature; - int32_t rpm; - uint8_t power_pct; -}; - struct PACKED log_ARSP { LOG_PACKET_HEADER; uint64_t time_us; @@ -789,17 +766,6 @@ struct PACKED log_PSCZ { // @Field: PosY: Calculated beacon position, y-axis // @Field: PosZ: Calculated beacon position, z-axis -// @LoggerMessage: CESC -// @Description: CAN ESC data -// @Field: TimeUS: Time since system startup -// @Field: Id: ESC identifier -// @Field: ECnt: Error count -// @Field: Voltage: Battery voltage measurement -// @Field: Curr: Battery current measurement -// @Field: Temp: Temperature -// @Field: RPM: Measured RPM -// @Field: Pow: Rated power output - // @LoggerMessage: CMD // @Description: Executed mission command information // @Field: TimeUS: Time since system startup @@ -874,18 +840,6 @@ struct PACKED log_PSCZ { // @Field: Subsys: Subsystem in which the error occurred // @Field: ECode: Subsystem-specific error code -// @LoggerMessage: ESC -// @Description: Feedback received from ESCs -// @Field: TimeUS: microseconds since system startup -// @Field: Instance: ESC instance number -// @Field: RPM: reported motor rotation rate -// @Field: Volt: Perceived input voltage for the ESC -// @Field: Curr: Perceived current through the ESC -// @Field: Temp: ESC temperature -// @Field: CTot: current consumed total -// @Field: MotTemp: measured motor temperature -// @Field: Err: error rate - // @LoggerMessage: EV // @Description: Specifically coded event messages // @Field: TimeUS: Time since system startup @@ -1310,12 +1264,9 @@ LOG_STRUCTURE_FROM_AVOIDANCE \ "SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????" }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ "TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded", "s-DU-mm--", "F-GG-00--" }, \ - { LOG_ESC_MSG, sizeof(log_Esc), \ - "ESC", "QBeCCcHcf", "TimeUS,Instance,RPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qvAO-O%", "F-BBBB-B-" }, \ +LOG_STRUCTURE_FROM_ESC_TELEM \ { LOG_CSRV_MSG, sizeof(log_CSRV), \ "CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000" }, \ - { LOG_CESC_MSG, sizeof(log_CESC), \ - "CESC","QBIfffiB","TimeUS,Id,ECnt,Voltage,Curr,Temp,RPM,Pow", "s#-vAOq%", "F-000000" }, \ { LOG_PIDR_MSG, sizeof(log_PID), \ "PIDR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, \ { LOG_PIDP_MSG, sizeof(log_PID), \ @@ -1418,10 +1369,9 @@ enum LogMessages : uint8_t { LOG_ATRP_MSG, LOG_IDS_FROM_CAMERA, LOG_TERRAIN_MSG, - LOG_ESC_MSG, LOG_CSRV_MSG, - LOG_CESC_MSG, LOG_ARSP_MSG, + LOG_IDS_FROM_ESC_TELEM, LOG_IDS_FROM_BATTMONITOR, LOG_MAG_MSG,