diff --git a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionFaults.java b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionFaults.java index 052c72b4cdf..86f002ff24b 100644 --- a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionFaults.java +++ b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionFaults.java @@ -4,12 +4,20 @@ package edu.wpi.first.hal; +import java.nio.ByteBuffer; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; + /** * Faults for a PowerDistribution device. These faults are only active while the condition is * active. */ @SuppressWarnings("MemberName") -public class PowerDistributionFaults { +public class PowerDistributionFaults implements StructSerializable { + /** The faults bitfield the object was constructed with */ + public final int bitfield; + /** Breaker fault on channel 0. */ public final boolean Channel0BreakerFault; @@ -136,6 +144,7 @@ public final boolean getBreakerFault(int channel) { * @param faults faults */ public PowerDistributionFaults(int faults) { + bitfield = faults; Channel0BreakerFault = (faults & 0x1) != 0; Channel1BreakerFault = (faults & 0x2) != 0; Channel2BreakerFault = (faults & 0x4) != 0; @@ -164,4 +173,69 @@ public PowerDistributionFaults(int faults) { CanWarning = (faults & 0x2000000) != 0; HardwareFault = (faults & 0x4000000) != 0; } + + public static final PowerDistributionFaultsStruct struct = new PowerDistributionFaultsStruct(); + + public static final class PowerDistributionFaultsStruct implements Struct { + @Override + public Class getTypeClass() { + return PowerDistributionFaults.class; + } + + @Override + public int getSize() { + return kSizeInt32; + } + + @Override + public String getSchema() { + return "bool channel0BreakerFault:1; " + + "bool channel1BreakerFault:1; " + + "bool channel2BreakerFault:1; " + + "bool channel3BreakerFault:1; " + + "bool channel4BreakerFault:1; " + + "bool channel5BreakerFault:1; " + + "bool channel6BreakerFault:1; " + + "bool channel7BreakerFault:1; " + + "bool channel8BreakerFault:1; " + + "bool channel9BreakerFault:1; " + + "bool channel10BreakerFault:1; " + + "bool channel11BreakerFault:1; " + + "bool channel12BreakerFault:1; " + + "bool channel13BreakerFault:1; " + + "bool channel14BreakerFault:1; " + + "bool channel15BreakerFault:1; " + + "bool channel16BreakerFault:1; " + + "bool channel17BreakerFault:1; " + + "bool channel18BreakerFault:1; " + + "bool channel19BreakerFault:1; " + + "bool channel20BreakerFault:1; " + + "bool channel21BreakerFault:1; " + + "bool channel22BreakerFault:1; " + + "bool channel23BreakerFault:1; " + + "bool brownout:1; " + + "bool canWarning:1; " + + "bool hardwareFault:1;"; + } + + @Override + public String getTypeName() { + return "PowerDistributionFaults"; + } + + @Override + public void pack(ByteBuffer bb, PowerDistributionFaults value) { + bb.putInt(value.bitfield); + } + + public void pack(ByteBuffer bb, int value) { + bb.putInt(value); + } + + @Override + public PowerDistributionFaults unpack(ByteBuffer bb) { + int packed = bb.getInt(); + return new PowerDistributionFaults(packed); + } + } } diff --git a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionStickyFaults.java b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionStickyFaults.java index 4d7836c5e64..e1fc30c4039 100644 --- a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionStickyFaults.java +++ b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionStickyFaults.java @@ -4,12 +4,20 @@ package edu.wpi.first.hal; +import java.nio.ByteBuffer; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; + /** * Sticky faults for a PowerDistribution device. These faults will remain active until they are * reset by the user. */ @SuppressWarnings("MemberName") -public class PowerDistributionStickyFaults { +public class PowerDistributionStickyFaults implements StructSerializable { + /** The faults bitfield the object was constructed with */ + public final int bitfield; + /** Breaker fault on channel 0. */ public final boolean Channel0BreakerFault; @@ -145,6 +153,7 @@ public final boolean getBreakerFault(int channel) { * @param faults faults */ public PowerDistributionStickyFaults(int faults) { + bitfield = faults; Channel0BreakerFault = (faults & 0x1) != 0; Channel1BreakerFault = (faults & 0x2) != 0; Channel2BreakerFault = (faults & 0x4) != 0; @@ -176,4 +185,70 @@ public PowerDistributionStickyFaults(int faults) { FirmwareFault = (faults & 0x10000000) != 0; HasReset = (faults & 0x20000000) != 0; } + + public static final PowerDistributionStickyFaultsStruct struct = new PowerDistributionStickyFaultsStruct(); + + public static final class PowerDistributionStickyFaultsStruct implements Struct { + @Override + public Class getTypeClass() { + return PowerDistributionStickyFaults.class; + } + + @Override + public int getSize() { + return 4; //doing bitfields on a u32 + } + + @Override + public String getSchema() { + return "bool channel0BreakerFault:1; " + + "bool channel1BreakerFault:1; " + + "bool channel2BreakerFault:1; " + + "bool channel3BreakerFault:1; " + + "bool channel4BreakerFault:1; " + + "bool channel5BreakerFault:1; " + + "bool channel6BreakerFault:1; " + + "bool channel7BreakerFault:1; " + + "bool channel8BreakerFault:1; " + + "bool channel9BreakerFault:1; " + + "bool channel10BreakerFault:1; " + + "bool channel11BreakerFault:1; " + + "bool channel12BreakerFault:1; " + + "bool channel13BreakerFault:1; " + + "bool channel14BreakerFault:1; " + + "bool channel15BreakerFault:1; " + + "bool channel16BreakerFault:1; " + + "bool channel17BreakerFault:1; " + + "bool channel18BreakerFault:1; " + + "bool channel19BreakerFault:1; " + + "bool channel20BreakerFault:1; " + + "bool channel21BreakerFault:1; " + + "bool channel22BreakerFault:1; " + + "bool channel23BreakerFault:1; " + + "bool brownout:1; " + + "bool canWarning:1; " + + "bool canBusOff:1; " + + "bool hasReset:1;"; + } + + @Override + public String getTypeName() { + return "PowerDistributionStickyFaults"; + } + + @Override + public void pack(ByteBuffer bb, PowerDistributionStickyFaults value) { + bb.putInt(value.bitfield); + } + + public void pack(ByteBuffer bb, int value) { + bb.putInt(value); + } + + @Override + public PowerDistributionStickyFaults unpack(ByteBuffer bb) { + int packed = bb.getInt(); + return new PowerDistributionStickyFaults(packed); + } + } } diff --git a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionVersion.java b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionVersion.java index bfb5ff2d0e3..ac425f7800e 100644 --- a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionVersion.java +++ b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionVersion.java @@ -4,9 +4,14 @@ package edu.wpi.first.hal; +import java.nio.ByteBuffer; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; + /** Power distribution version. */ @SuppressWarnings("MemberName") -public class PowerDistributionVersion { +public class PowerDistributionVersion implements StructSerializable { /** Firmware major version number. */ public final int firmwareMajor; @@ -49,4 +54,56 @@ public PowerDistributionVersion( this.hardwareMajor = hardwareMajor; this.uniqueId = uniqueId; } + + public static final PowerDistributionVersionStruct struct = new PowerDistributionVersionStruct(); + + public static final class PowerDistributionVersionStruct implements Struct { + @Override + public Class getTypeClass() { + return PowerDistributionVersion.class; + } + + @Override + public int getSize() { + return kSizeInt32; + } + + @Override + public String getSchema() { + return "int8 firmwareMajor; " + + "int8 firmwareMinor; " + + "int8 firmwareFix; " + + "int8 hardwareMinor; " + + "int8 hardwareMajor; " + + "int32 uniqueId;"; + } + + @Override + public String getTypeName() { + return "PowerDistributionVersion"; + } + + @Override + public void pack(ByteBuffer bb, PowerDistributionVersion value) { + bb.put((byte) value.firmwareMajor); + bb.put((byte) value.firmwareMinor); + bb.put((byte) value.firmwareFix); + bb.put((byte) value.hardwareMinor); + bb.put((byte) value.hardwareMajor); + bb.putLong(value.uniqueId); + } + + @Override + public PowerDistributionVersion unpack(ByteBuffer bb) { + return new PowerDistributionVersion( + bb.get(), + bb.get(), + bb.get(), + bb.get(), + bb.get(), + bb.getInt() + ); + } + } + } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java index 2325fe43422..b8d011f94bb 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java @@ -241,6 +241,26 @@ public PowerDistributionStickyFaults getStickyFaults() { return PowerDistributionJNI.getStickyFaults(m_handle); } + /** + * Used internally to get the handle for the PDP. + * + * @return The handle for the PDP + */ + public int getHandle() { + return m_handle; + } + + /** + * Creates a new {@link PowerDistributionLogger} for this instance. + * + * @param logPath The path to log the data to + * @param datalogOnly If true, only log to the datalog + * @return The new {@link PowerDistributionLogger} + */ + public PowerDistributionLogger createLogger(String logPath, boolean datalogOnly) { + return new PowerDistributionLogger(this, logPath, datalogOnly); + } + @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("PowerDistribution"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionLogger.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionLogger.java new file mode 100644 index 00000000000..6851faeaaa8 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionLogger.java @@ -0,0 +1,322 @@ +package edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.PowerDistributionFaults; +import edu.wpi.first.hal.PowerDistributionJNI; +import edu.wpi.first.hal.PowerDistributionStickyFaults; +import edu.wpi.first.hal.PowerDistributionVersion; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.util.datalog.StructLogEntry; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.nio.ByteBuffer; +import java.util.function.Consumer; + +/** + * A class for logging PowerDistribution data to NetworkTables or DataLog. + * + *

This class is designed to be instantiated in the `Robot.java` and have the {@link #log()} + * method called in the `robotPeriodic` method. + * + *

The logger supports rate limiting the logging to prevent needless JNI calls when no new data + * is available. This is done via {@link #setLogWindup(int)}. + */ +public class PowerDistributionLogger { + private final int m_handle; + private final Consumer m_output; + private final PowerDistributionStats m_stats = new PowerDistributionStats(); + + /** + * This value determines how many calls to {@link #log()} are required before the data is logged. + */ + private int m_logWindup; + + /** This value tracks the number of times {@link #log()} has been called. */ + private int m_logCount; + + /** + * Constructs a PowerDistributionLogger, this provides a performant way to log PowerDistribution + * data to NetworkTables or only to a DataLog. + * + * @param pd The PowerDistribution object to log + * @param logPath The path to log the data to + * @param datalogOnly If true, only log to the datalog + */ + public PowerDistributionLogger(PowerDistribution pd, String logPath, boolean datalogOnly) { + m_handle = pd.getHandle(); + String statsLogPath = NetworkTable.normalizeKey(logPath + "/Stats", true); + String metaLogPath = NetworkTable.normalizeKey(logPath + "/Meta", true); + PowerDistributionMeta meta = new PowerDistributionMeta(m_handle); + if (datalogOnly) { + StructLogEntry entry = + StructLogEntry.create( + DataLogManager.getLog(), statsLogPath, new PowerDistributionStatsStruct()); + m_output = entry::append; + StructLogEntry.create(DataLogManager.getLog(), metaLogPath, new PowerDistributionMetaStruct()) + .append(meta); + } else { + StructPublisher entry = + NetworkTableInstance.getDefault() + .getStructTopic(statsLogPath, new PowerDistributionStatsStruct()) + .publish(); + m_output = entry; + NetworkTableInstance.getDefault() + .getStructTopic(metaLogPath, new PowerDistributionMetaStruct()) + .publish() + .set(meta); + } + } + + /** + * Logs the PowerDistribution data to NetworkTables or DataLog. + * + *

If {@link #setLogWindup(int)} has been called, this method will only log every N calls to + * {@link #log()}. + */ + public void log() { + m_logCount++; + if (m_logWindup == 0 || m_logCount % m_logWindup == 0) { + m_stats.update(m_handle); + m_output.accept(m_stats); + } + } + + /** + * Sets the log windup value, this value determines how many calls to {@link #log()} are required + * before the data is logged. This is available to prevent needless JNI calls when no new data is + * available. + * + *

For example, if this value is set to 5, the data will only be logged every 5 calls to {@link + * #log()}. + * + *

The {@link PowerDistribution} refreshes it's data every 80ms. + * + * @param windup The number of calls to {@link #log()} required before the data is logged + */ + public void setLogWindup(int windup) { + m_logWindup = windup; + } + + @SuppressWarnings("PMD.RedundantFieldInitializer") + static final class PowerDistributionStats implements StructSerializable { + + private int m_faults = 0; + private int m_stickyFaults = 0; + private double m_voltage = 0.0; + private double m_totalCurrent = 0.0; + private boolean m_switchableChannel = false; + private double m_temperature = 0.0; + private double[] m_currents = new double[24]; + + PowerDistributionStats() {} + + PowerDistributionStats( + int faults, + int stickyFaults, + double voltage, + double totalCurrent, + boolean switchableChannel, + double temperature, + double[] currents) { + m_faults = faults; + m_stickyFaults = stickyFaults; + m_voltage = voltage; + m_totalCurrent = totalCurrent; + m_switchableChannel = switchableChannel; + m_temperature = temperature; + m_currents = currents; + } + + private void update(int handle) { + m_faults = PowerDistributionJNI.getFaultsNative(handle); + m_stickyFaults = PowerDistributionJNI.getStickyFaultsNative(handle); + m_voltage = PowerDistributionJNI.getVoltage(handle); + m_totalCurrent = PowerDistributionJNI.getTotalCurrent(handle); + m_switchableChannel = PowerDistributionJNI.getSwitchableChannel(handle); + m_temperature = PowerDistributionJNI.getTemperature(handle); + PowerDistributionJNI.getAllCurrents(handle, m_currents); + } + + @Override + public boolean equals(Object obj) { + if (obj instanceof PowerDistributionStats) { + PowerDistributionStats other = (PowerDistributionStats) obj; + if (m_currents.length == other.m_currents.length) { + for (int i = 0; i < m_currents.length; i++) { + if (Math.abs(m_currents[i] - other.m_currents[i]) > 0.1) { + return false; + } + } + } else { + return false; + } + return m_faults == other.m_faults + && m_stickyFaults == other.m_stickyFaults + && Math.abs(m_voltage - other.m_voltage) < 0.1 + && Math.abs(m_totalCurrent - other.m_totalCurrent) < 0.1 + && Math.abs(m_temperature - other.m_temperature) < 0.1 + && m_switchableChannel == other.m_switchableChannel; + } + return false; + } + } + + static final class PowerDistributionStatsStruct + implements Struct { + private static final int kSize = 4 + 4 + 8 + 8 + 1 + 8 + (8 * 24); + + @Override + public Class getTypeClass() { + return PowerDistributionStats.class; + } + + @Override + public int getSize() { + return kSize; + } + + @Override + public String getSchema() { + return "PowerDistributionFaults faults; " + + "PowerDistributionStickyFaults stickyFaults; " + + "double voltage; " + + "double totalCurrent; " + + "bool switchableChannel; " + + "double temperature;" + + "double currents[24];"; + } + + @Override + public String getTypeName() { + return "PowerDistributionStats"; + } + + @Override + public void pack(ByteBuffer bb, PowerDistributionStats value) { + bb.putInt(value.m_faults); + bb.putInt(value.m_stickyFaults); + bb.putDouble(value.m_voltage); + bb.putDouble(value.m_totalCurrent); + bb.put((byte) (value.m_switchableChannel ? 1 : 0)); + bb.putDouble(value.m_temperature); + for (double current : value.m_currents) { + bb.putDouble(current); + } + } + + @Override + public PowerDistributionStats unpack(ByteBuffer bb) { + PowerDistributionStats ret = + new PowerDistributionStats( + bb.getInt(), + bb.getInt(), + bb.getDouble(), + bb.getDouble(), + bb.get() != 0, + bb.getDouble(), + new double[24]); + for (int i = 0; i < 24; i++) { + ret.m_currents[i] = bb.getDouble(); + } + return ret; + } + + @Override + public Struct[] getNested() { + return new Struct[] { + new PowerDistributionFaults.PowerDistributionFaultsStruct(), + new PowerDistributionStickyFaults.PowerDistributionStickyFaultsStruct() + }; + } + } + + static final class PowerDistributionMeta implements StructSerializable { + + private final int m_canId; + private final int m_type; + private final int m_channelCount; + private final PowerDistributionVersion m_version; + + private PowerDistributionMeta(int handle) { + m_canId = PowerDistributionJNI.getModuleNumber(handle); + m_type = PowerDistributionJNI.getType(handle); + m_channelCount = PowerDistributionJNI.getNumChannels(handle); + m_version = PowerDistributionJNI.getVersion(handle); + } + + PowerDistributionMeta( + int canId, int type, int channelCount, PowerDistributionVersion version) { + m_canId = canId; + m_type = type; + m_channelCount = channelCount; + m_version = version; + } + + @Override + public boolean equals(Object obj) { + if (obj instanceof PowerDistributionMeta) { + PowerDistributionMeta other = (PowerDistributionMeta) obj; + return m_canId == other.m_canId + && m_type == other.m_type + && m_channelCount == other.m_channelCount + && m_version.firmwareFix == other.m_version.firmwareFix + && m_version.firmwareMajor == other.m_version.firmwareMajor + && m_version.firmwareMinor == other.m_version.firmwareMinor + && m_version.hardwareMajor == other.m_version.hardwareMajor + && m_version.hardwareMinor == other.m_version.hardwareMinor + && m_version.uniqueId == other.m_version.uniqueId; + } + return false; + } + } + + private static final class PowerDistributionMetaStruct implements Struct { + private static final int kSize = 4 + 4 + 4 + PowerDistributionVersion.struct.getSize(); + + @Override + public Class getTypeClass() { + return PowerDistributionMeta.class; + } + + @Override + public int getSize() { + return kSize; + } + + @Override + public String getSchema() { + return "uint8 canId; " + + "enum{REV_PDH=1,CTRE_PDP=2} uint8 type; " + + "uint8 channelCount; " + + "PowerDistributionVersion version;"; + } + + @Override + public String getTypeName() { + return "PowerDistributionMeta"; + } + + @Override + public void pack(ByteBuffer bb, PowerDistributionMeta value) { + bb.put((byte) value.m_canId); + bb.put((byte) value.m_type); + bb.put((byte) value.m_channelCount); + PowerDistributionVersion.struct.pack(bb, value.m_version); + } + + @Override + public PowerDistributionMeta unpack(ByteBuffer bb) { + return new PowerDistributionMeta( + (int) bb.get(), + (int) bb.get(), + (int) bb.get(), + PowerDistributionVersion.struct.unpack(bb)); + } + + @Override + public Struct[] getNested() { + return new Struct[] {new PowerDistributionVersion.PowerDistributionVersionStruct()}; + } + } +}