mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-03 06:28:32 -04:00
AP_Logger: improve unit names in comments.
added Ampere hours unit in LOG_ESC_MSG log ESC volts, amps and consumption as floats update ESC log file structures consumption in mAh Correct the current_tot unit, motor_temp unit and error_rate unit in comments (<amilcar.lucas@iav.de>) move ESC_Telem logging to the AP_ESC_Telem class (<amilcar.lucas@iav.de>) correct log structure (<amilcar.lucas@iav.de>)
This commit is contained in:
parent
71e7f7e476
commit
c323ee4f56
@ -299,9 +299,7 @@ public:
|
|||||||
void Write_Radio(const mavlink_radio_t &packet);
|
void Write_Radio(const mavlink_radio_t &packet);
|
||||||
void Write_Message(const char *message);
|
void Write_Message(const char *message);
|
||||||
void Write_MessageF(const char *fmt, ...);
|
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_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_Compass();
|
||||||
void Write_Mode(uint8_t mode, const ModeReason reason);
|
void Write_Mode(uint8_t mode, const ModeReason reason);
|
||||||
|
|
||||||
|
@ -361,30 +361,6 @@ bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason)
|
|||||||
return WriteCriticalBlock(&pkt, sizeof(pkt));
|
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
|
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));
|
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
|
// Write a Yaw PID packet
|
||||||
void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info)
|
void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info)
|
||||||
|
@ -45,6 +45,7 @@ const struct UnitStructure log_Units[] = {
|
|||||||
{ '-', "" }, // no units e.g. Pi, or a string
|
{ '-', "" }, // no units e.g. Pi, or a string
|
||||||
{ '?', "UNKNOWN" }, // Units which haven't been worked out yet....
|
{ '?', "UNKNOWN" }, // Units which haven't been worked out yet....
|
||||||
{ 'A', "A" }, // Ampere
|
{ 'A', "A" }, // Ampere
|
||||||
|
{ 'a', "Ah" }, // Ampere hours
|
||||||
{ 'd', "deg" }, // of the angular variety, -180 to 180
|
{ 'd', "deg" }, // of the angular variety, -180 to 180
|
||||||
{ 'b', "B" }, // bytes
|
{ 'b', "B" }, // bytes
|
||||||
{ 'k', "deg/s" }, // degrees per second. Degrees are NOT SI, but is some situations more user-friendly than radians
|
{ '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 <AP_VisualOdom/LogStructure.h>
|
#include <AP_VisualOdom/LogStructure.h>
|
||||||
#include <AC_PrecLand/LogStructure.h>
|
#include <AC_PrecLand/LogStructure.h>
|
||||||
#include <AC_Avoidance/LogStructure.h>
|
#include <AC_Avoidance/LogStructure.h>
|
||||||
|
#include <AP_ESC_Telem/LogStructure.h>
|
||||||
|
|
||||||
// structure used to define logging format
|
// structure used to define logging format
|
||||||
struct LogStructure {
|
struct LogStructure {
|
||||||
@ -442,19 +444,6 @@ struct PACKED log_TERRAIN {
|
|||||||
uint16_t loaded;
|
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 {
|
struct PACKED log_CSRV {
|
||||||
LOG_PACKET_HEADER;
|
LOG_PACKET_HEADER;
|
||||||
uint64_t time_us;
|
uint64_t time_us;
|
||||||
@ -465,18 +454,6 @@ struct PACKED log_CSRV {
|
|||||||
uint8_t power_pct;
|
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 {
|
struct PACKED log_ARSP {
|
||||||
LOG_PACKET_HEADER;
|
LOG_PACKET_HEADER;
|
||||||
uint64_t time_us;
|
uint64_t time_us;
|
||||||
@ -789,17 +766,6 @@ struct PACKED log_PSCZ {
|
|||||||
// @Field: PosY: Calculated beacon position, y-axis
|
// @Field: PosY: Calculated beacon position, y-axis
|
||||||
// @Field: PosZ: Calculated beacon position, z-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
|
// @LoggerMessage: CMD
|
||||||
// @Description: Executed mission command information
|
// @Description: Executed mission command information
|
||||||
// @Field: TimeUS: Time since system startup
|
// @Field: TimeUS: Time since system startup
|
||||||
@ -874,18 +840,6 @@ struct PACKED log_PSCZ {
|
|||||||
// @Field: Subsys: Subsystem in which the error occurred
|
// @Field: Subsys: Subsystem in which the error occurred
|
||||||
// @Field: ECode: Subsystem-specific error code
|
// @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
|
// @LoggerMessage: EV
|
||||||
// @Description: Specifically coded event messages
|
// @Description: Specifically coded event messages
|
||||||
// @Field: TimeUS: Time since system startup
|
// @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????" }, \
|
"SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????" }, \
|
||||||
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
|
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
|
||||||
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded", "s-DU-mm--", "F-GG-00--" }, \
|
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded", "s-DU-mm--", "F-GG-00--" }, \
|
||||||
{ LOG_ESC_MSG, sizeof(log_Esc), \
|
LOG_STRUCTURE_FROM_ESC_TELEM \
|
||||||
"ESC", "QBeCCcHcf", "TimeUS,Instance,RPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qvAO-O%", "F-BBBB-B-" }, \
|
|
||||||
{ LOG_CSRV_MSG, sizeof(log_CSRV), \
|
{ LOG_CSRV_MSG, sizeof(log_CSRV), \
|
||||||
"CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000" }, \
|
"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), \
|
{ LOG_PIDR_MSG, sizeof(log_PID), \
|
||||||
"PIDR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, \
|
"PIDR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, \
|
||||||
{ LOG_PIDP_MSG, sizeof(log_PID), \
|
{ LOG_PIDP_MSG, sizeof(log_PID), \
|
||||||
@ -1418,10 +1369,9 @@ enum LogMessages : uint8_t {
|
|||||||
LOG_ATRP_MSG,
|
LOG_ATRP_MSG,
|
||||||
LOG_IDS_FROM_CAMERA,
|
LOG_IDS_FROM_CAMERA,
|
||||||
LOG_TERRAIN_MSG,
|
LOG_TERRAIN_MSG,
|
||||||
LOG_ESC_MSG,
|
|
||||||
LOG_CSRV_MSG,
|
LOG_CSRV_MSG,
|
||||||
LOG_CESC_MSG,
|
|
||||||
LOG_ARSP_MSG,
|
LOG_ARSP_MSG,
|
||||||
|
LOG_IDS_FROM_ESC_TELEM,
|
||||||
LOG_IDS_FROM_BATTMONITOR,
|
LOG_IDS_FROM_BATTMONITOR,
|
||||||
LOG_MAG_MSG,
|
LOG_MAG_MSG,
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user