Browse Source

AP_Logger: log MCU temperature and voltage

gps-1.3.1
Andrew Tridgell 4 years ago
parent
commit
0f59e1ebb8
  1. 16
      libraries/AP_Logger/LogFile.cpp
  2. 10
      libraries/AP_Logger/LogStructure.h

16
libraries/AP_Logger/LogFile.cpp

@ -293,6 +293,16 @@ void AP_Logger::Write_Power(void)
// encode armed state in bit 3 // encode armed state in bit 3
safety_and_armed |= 1U<<2; safety_and_armed |= 1U<<2;
} }
float MCU_temp = 0;
float MCU_voltage = 0;
float MCU_vmin = 0;
float MCU_vmax = 0;
#if HAL_WITH_MCU_MONITORING
MCU_temp = hal.analogin->mcu_temperature();
MCU_voltage = hal.analogin->mcu_voltage();
MCU_vmin = hal.analogin->mcu_voltage_min();
MCU_vmax = hal.analogin->mcu_voltage_max();
#endif
const struct log_POWR pkt{ const struct log_POWR pkt{
LOG_PACKET_HEADER_INIT(LOG_POWR_MSG), LOG_PACKET_HEADER_INIT(LOG_POWR_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
@ -300,7 +310,11 @@ void AP_Logger::Write_Power(void)
Vservo : hal.analogin->servorail_voltage(), Vservo : hal.analogin->servorail_voltage(),
flags : hal.analogin->power_status_flags(), flags : hal.analogin->power_status_flags(),
accumulated_flags : hal.analogin->accumulated_power_status_flags(), accumulated_flags : hal.analogin->accumulated_power_status_flags(),
safety_and_arm : safety_and_armed safety_and_arm : safety_and_armed,
MCU_temp : MCU_temp,
MCU_voltage : MCU_voltage,
MCU_voltage_min : MCU_vmin,
MCU_voltage_max : MCU_vmax,
}; };
WriteBlock(&pkt, sizeof(pkt)); WriteBlock(&pkt, sizeof(pkt));
#endif #endif

10
libraries/AP_Logger/LogStructure.h

@ -309,6 +309,10 @@ struct PACKED log_POWR {
uint16_t flags; uint16_t flags;
uint16_t accumulated_flags; uint16_t accumulated_flags;
uint8_t safety_and_arm; uint8_t safety_and_arm;
float MCU_temp;
float MCU_voltage;
float MCU_voltage_min;
float MCU_voltage_max;
}; };
struct PACKED log_Cmd { struct PACKED log_Cmd {
@ -1003,6 +1007,10 @@ struct PACKED log_PSCZ {
// @Field: Flags: System power flags // @Field: Flags: System power flags
// @Field: AccFlags: Accumulated System power flags; all flags which have ever been set // @Field: AccFlags: Accumulated System power flags; all flags which have ever been set
// @Field: Safety: Hardware Safety Switch status // @Field: Safety: Hardware Safety Switch status
// @Field: MTemp: MCU Temperature
// @Field: MVolt: MCU Voltage
// @Field: MVmin: MCU Voltage min
// @Field: MVmax: MCU Voltage max
// @LoggerMessage: PRX // @LoggerMessage: PRX
// @Description: Proximity Filtered sensor data // @Description: Proximity Filtered sensor data
@ -1237,7 +1245,7 @@ LOG_STRUCTURE_FROM_GPS \
LOG_STRUCTURE_FROM_BARO \ LOG_STRUCTURE_FROM_BARO \
LOG_STRUCTURE_FROM_PRECLAND \ LOG_STRUCTURE_FROM_PRECLAND \
{ LOG_POWR_MSG, sizeof(log_POWR), \ { LOG_POWR_MSG, sizeof(log_POWR), \
"POWR","QffHHB","TimeUS,Vcc,VServo,Flags,AccFlags,Safety", "svv---", "F00---", true }, \ "POWR","QffHHBffff","TimeUS,Vcc,VServo,Flags,AccFlags,Safety,MTemp,MVolt,MVmin,MVmax", "svv---Ovvv", "F00---0000", true }, \
{ LOG_CMD_MSG, sizeof(log_Cmd), \ { LOG_CMD_MSG, sizeof(log_Cmd), \
"CMD", "QHHHffffLLfB","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,Frame", "s-------DUm-", "F-------GG0-" }, \ "CMD", "QHHHffffLLfB","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,Frame", "s-------DUm-", "F-------GG0-" }, \
{ LOG_MAVLINK_COMMAND_MSG, sizeof(log_MAVLink_Command), \ { LOG_MAVLINK_COMMAND_MSG, sizeof(log_MAVLink_Command), \

Loading…
Cancel
Save