Browse Source

AP_Logger: ESC logging includes motor temperature

zr-v5.1
Randy Mackay 5 years ago committed by Andrew Tridgell
parent
commit
94d641d3d5
  1. 2
      libraries/AP_Logger/AP_Logger.h
  2. 7
      libraries/AP_Logger/LogFile.cpp
  3. 11
      libraries/AP_Logger/LogStructure.h

2
libraries/AP_Logger/AP_Logger.h

@ -257,7 +257,7 @@ public: @@ -257,7 +257,7 @@ public:
void Write_CameraInfo(enum LogMessages msg, const Location &current_loc, uint64_t timestamp_us=0);
void Write_Camera(const Location &current_loc, uint64_t timestamp_us=0);
void Write_Trigger(const Location &current_loc);
void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t temperature, uint16_t current_tot);
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_Attitude(const Vector3f &targets);
void Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets);
void Write_Current();

7
libraries/AP_Logger/LogFile.cpp

@ -780,7 +780,7 @@ bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason) @@ -780,7 +780,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 id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t temperature, uint16_t current_tot)
void AP_Logger::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)
{
// sanity check id
if (id >= 8) {
@ -792,8 +792,9 @@ void AP_Logger::Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t vo @@ -792,8 +792,9 @@ void AP_Logger::Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t vo
rpm : rpm,
voltage : voltage,
current : current,
temperature : temperature,
current_tot : current_tot
esc_temp : esc_temp,
current_tot : current_tot,
motor_temp : motor_temp
};
WriteBlock(&pkt, sizeof(pkt));
}

11
libraries/AP_Logger/LogStructure.h

@ -937,8 +937,9 @@ struct PACKED log_Esc { @@ -937,8 +937,9 @@ struct PACKED log_Esc {
int32_t rpm;
uint16_t voltage;
uint16_t current;
int16_t temperature;
int16_t esc_temp;
uint16_t current_tot;
int16_t motor_temp;
};
struct PACKED log_AIRSPEED {
@ -1219,10 +1220,10 @@ struct PACKED log_Arm_Disarm { @@ -1219,10 +1220,10 @@ struct PACKED log_Arm_Disarm {
#define BARO_UNITS "smPOnsmO-"
#define BARO_MULTS "F00B0C?0-"
#define ESC_LABELS "TimeUS,RPM,Volt,Curr,Temp,CTot"
#define ESC_FMT "QeCCcH"
#define ESC_UNITS "sqvAO-"
#define ESC_MULTS "FBBBB-"
#define ESC_LABELS "TimeUS,RPM,Volt,Curr,Temp,CTot,MotTemp"
#define ESC_FMT "QeCCcHc"
#define ESC_UNITS "sqvAO-O"
#define ESC_MULTS "FBBBB-B"
#define GPA_LABELS "TimeUS,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta"
#define GPA_FMT "QCCCCfBIH"

Loading…
Cancel
Save