From a9931223f00dfa0b8b4b8651349db7023031415f Mon Sep 17 00:00:00 2001
From: Jan-Felix Abellera <jfabellera@gmail.com>
Date: Mon, 22 Nov 2021 23:15:32 -0600
Subject: [PATCH] [hal] Add REV PH faults (#3729)

---
 hal/src/main/native/athena/REVPH.cpp    | 56 +++++++++++++++++++++++++
 hal/src/main/native/include/hal/REVPH.h | 46 ++++++++++++++++++++
 2 files changed, 102 insertions(+)

diff --git a/hal/src/main/native/athena/REVPH.cpp b/hal/src/main/native/athena/REVPH.cpp
index 0161cf5dc8..059a6609db 100644
--- a/hal/src/main/native/athena/REVPH.cpp
+++ b/hal/src/main/native/athena/REVPH.cpp
@@ -562,3 +562,59 @@ void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs,
   HAL_WriteCANPacket(ph->hcan, packedData, PH_PULSE_ONCE_LENGTH,
                      PH_PULSE_ONCE_FRAME_API, status);
 }
+
+HAL_REVPHFaults HAL_GetREVPHFaults(HAL_REVPHHandle handle, int32_t* status) {
+  HAL_REVPHFaults faults = {};
+  auto ph = REVPHHandles->Get(handle);
+  if (ph == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return faults;
+  }
+
+  PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status);
+  faults.channel0Fault = status0.channel_0_fault;
+  faults.channel1Fault = status0.channel_1_fault;
+  faults.channel2Fault = status0.channel_2_fault;
+  faults.channel3Fault = status0.channel_3_fault;
+  faults.channel4Fault = status0.channel_4_fault;
+  faults.channel5Fault = status0.channel_5_fault;
+  faults.channel6Fault = status0.channel_6_fault;
+  faults.channel7Fault = status0.channel_7_fault;
+  faults.channel8Fault = status0.channel_8_fault;
+  faults.channel9Fault = status0.channel_9_fault;
+  faults.channel10Fault = status0.channel_10_fault;
+  faults.channel11Fault = status0.channel_11_fault;
+  faults.channel12Fault = status0.channel_12_fault;
+  faults.channel13Fault = status0.channel_13_fault;
+  faults.channel14Fault = status0.channel_14_fault;
+  faults.channel15Fault = status0.channel_15_fault;
+  faults.compressorOverCurrent = status0.compressor_oc;
+  faults.compressorOpen = status0.compressor_open;
+  faults.solenoidOverCurrent = status0.solenoid_oc;
+  faults.brownout = status0.brownout;
+  faults.canWarning = status0.can_warning;
+  faults.hardwareFault = status0.hardware_fault;
+
+  return faults;
+}
+
+HAL_REVPHStickyFaults HAL_GetREVPHStickyFaults(HAL_REVPHHandle handle,
+                                               int32_t* status) {
+  HAL_REVPHStickyFaults stickyFaults = {};
+  auto ph = REVPHHandles->Get(handle);
+  if (ph == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return stickyFaults;
+  }
+
+  PH_status1_t status1 = HAL_REV_ReadPHStatus1(ph->hcan, status);
+  stickyFaults.compressorOverCurrent = status1.sticky_compressor_over_current;
+  stickyFaults.compressorOpen = status1.sticky_compressor_not_present;
+  stickyFaults.solenoidOverCurrent = status1.sticky_solenoid_over_current;
+  stickyFaults.brownout = status1.sticky_brownout;
+  stickyFaults.canWarning = status1.sticky_can_warning;
+  stickyFaults.canBusOff = status1.sticky_can_bus_off;
+  stickyFaults.hasReset = status1.sticky_has_reset;
+
+  return stickyFaults;
+}
diff --git a/hal/src/main/native/include/hal/REVPH.h b/hal/src/main/native/include/hal/REVPH.h
index 19984c88fb..d095318750 100644
--- a/hal/src/main/native/include/hal/REVPH.h
+++ b/hal/src/main/native/include/hal/REVPH.h
@@ -34,6 +34,47 @@ struct HAL_REVPHCompressorConfig {
   HAL_Bool useDigital;
 };
 
+/**
+ * Storage for REV PH Faults
+ */
+struct HAL_REVPHFaults {
+  uint32_t channel0Fault : 1;
+  uint32_t channel1Fault : 1;
+  uint32_t channel2Fault : 1;
+  uint32_t channel3Fault : 1;
+  uint32_t channel4Fault : 1;
+  uint32_t channel5Fault : 1;
+  uint32_t channel6Fault : 1;
+  uint32_t channel7Fault : 1;
+  uint32_t channel8Fault : 1;
+  uint32_t channel9Fault : 1;
+  uint32_t channel10Fault : 1;
+  uint32_t channel11Fault : 1;
+  uint32_t channel12Fault : 1;
+  uint32_t channel13Fault : 1;
+  uint32_t channel14Fault : 1;
+  uint32_t channel15Fault : 1;
+  uint32_t compressorOverCurrent : 1;
+  uint32_t compressorOpen : 1;
+  uint32_t solenoidOverCurrent : 1;
+  uint32_t brownout : 1;
+  uint32_t canWarning : 1;
+  uint32_t hardwareFault : 1;
+};
+
+/**
+ * Storage for REV PH Sticky Faults
+ */
+struct HAL_REVPHStickyFaults {
+  uint32_t compressorOverCurrent : 1;
+  uint32_t compressorOpen : 1;
+  uint32_t solenoidOverCurrent : 1;
+  uint32_t brownout : 1;
+  uint32_t canWarning : 1;
+  uint32_t canBusOff : 1;
+  uint32_t hasReset : 1;
+};
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -80,6 +121,11 @@ void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values,
 void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs,
                           int32_t* status);
 
+HAL_REVPHFaults HAL_GetREVPHFaults(HAL_REVPHHandle handle, int32_t* status);
+
+HAL_REVPHStickyFaults HAL_GetREVPHStickyFaults(HAL_REVPHHandle handle,
+                                               int32_t* status);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif