diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 07341752a1..dfe9d1408e 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -258,7 +258,7 @@ public: void Write_CameraInfo(enum LogMessages msg, const Location ¤t_loc, uint64_t timestamp_us=0); void Write_Camera(const Location ¤t_loc, uint64_t timestamp_us=0); void Write_Trigger(const Location ¤t_loc); - 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); + 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_Attitude(const Vector3f &targets); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 11f977c018..e3769f6306 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -718,7 +718,7 @@ bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason) // 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) +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)), @@ -729,7 +729,8 @@ void AP_Logger::Write_ESC(uint8_t instance, uint64_t time_us, int32_t rpm, uint1 current : current, esc_temp : esc_temp, current_tot : current_tot, - motor_temp : motor_temp + motor_temp : motor_temp, + error_rate : error_rate }; WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index c10206b368..199c2a20da 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -712,6 +712,7 @@ struct PACKED log_Esc { int16_t esc_temp; uint16_t current_tot; int16_t motor_temp; + float error_rate; }; struct PACKED log_CSRV { @@ -1307,6 +1308,7 @@ struct PACKED log_PSC { // @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 @@ -1973,7 +1975,7 @@ struct PACKED log_PSC { { LOG_GPS_RAWS_MSG, sizeof(log_GPS_RAWS), \ "GRXS", "QddfBBBHBBBBB", "TimeUS,prMes,cpMes,doMes,gnss,sv,freq,lock,cno,prD,cpD,doD,trk", "s------------", "F------------" }, \ { LOG_ESC_MSG, sizeof(log_Esc), \ - "ESC", "QBeCCcHc", "TimeUS,Instance,RPM,Volt,Curr,Temp,CTot,MotTemp", "s#qvAO-O", "F-BBBB-B" }, \ + "ESC", "QBeCCcHcf", "TimeUS,Instance,RPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qvAO-O%", "F-BBBB-B-" }, \ { LOG_CSRV_MSG, sizeof(log_CSRV), \ "CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000" }, \ { LOG_CESC_MSG, sizeof(log_CESC), \