diff --git a/hal/.styleguide b/hal/.styleguide index 5f7d6e3d7a..77cd815c74 100644 --- a/hal/.styleguide +++ b/hal/.styleguide @@ -10,6 +10,7 @@ cppSrcFileInclude { generatedFileExclude { hal/src/main/native/athena/ctre/ + hal/src/main/native/athena/rev/ hal/src/main/native/athena/frccansae/ hal/src/main/native/athena/visa/ hal/src/main/native/include/ctre/ diff --git a/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java b/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java index 3ff6eba427..9873c48c30 100644 --- a/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java @@ -4,6 +4,7 @@ package edu.wpi.first.hal; +@SuppressWarnings("AbbreviationAsWordInName") public class PortsJNI extends JNIWrapper { public static native int getNumAccumulators(); @@ -33,11 +34,15 @@ public class PortsJNI extends JNIWrapper { public static native int getNumRelayHeaders(); - public static native int getNumPCMModules(); + public static native int getNumCTREPCMModules(); - public static native int getNumSolenoidChannels(); + public static native int getNumCTRESolenoidChannels(); - public static native int getNumPDPModules(); + public static native int getNumCTREPDPModules(); - public static native int getNumPDPChannels(); + public static native int getNumCTREPDPChannels(); + + public static native int getNumREVPDHModules(); + + public static native int getNumREVPDHChannels(); } diff --git a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java index 8696d0d1e6..b2ac61aa19 100644 --- a/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java @@ -6,19 +6,30 @@ package edu.wpi.first.hal; @SuppressWarnings("AbbreviationAsWordInName") public class PowerDistributionJNI extends JNIWrapper { + public static final int AUTOMATIC_TYPE = 0; + public static final int CTRE_TYPE = 1; + public static final int REV_TYPE = 2; + public static final int DEFAULT_MODULE = -1; + public static native int initialize(int module, int type); + public static native void free(int handle); + + public static native int getModuleNumber(int handle); + public static native boolean checkModule(int module, int type); public static native boolean checkChannel(int handle, int channel); public static native int getType(int handle); + public static native int getNumChannels(int handle); + public static native double getTemperature(int handle); public static native double getVoltage(int handle); - public static native double getChannelCurrent(byte channel, int handle); + public static native double getChannelCurrent(int handle, int channel); public static native void getAllCurrents(int handle, double[] currents); @@ -31,4 +42,8 @@ public class PowerDistributionJNI extends JNIWrapper { public static native void resetTotalEnergy(int handle); public static native void clearStickyFaults(int handle); + + public static native boolean getSwitchableChannel(int handle); + + public static native void setSwitchableChannel(int handle, boolean enabled); } diff --git a/hal/src/main/native/athena/CTREPDP.cpp b/hal/src/main/native/athena/CTREPDP.cpp index f61d6571ea..1a25a6427f 100644 --- a/hal/src/main/native/athena/CTREPDP.cpp +++ b/hal/src/main/native/athena/CTREPDP.cpp @@ -110,12 +110,12 @@ struct PDP { }; } // namespace -static IndexedHandleResource* pdpHandles; namespace hal::init { -void InitializePDP() { - static IndexedHandleResource pH; pdpHandles = &pH; @@ -142,7 +142,7 @@ HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation, pdp->previousAllocation); } else { hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PDP", 0, - kNumPDPModules, module); + kNumCTREPDPModules, module); } return HAL_kInvalidHandle; // failed to allocate. Pass error back. } @@ -166,12 +166,16 @@ void HAL_CleanPDP(HAL_PDPHandle handle) { pdpHandles->Free(handle); } +int32_t HAL_GetPDPModuleNumber(HAL_PDPHandle handle, int32_t* status) { + return hal::getHandleIndex(handle); +} + HAL_Bool HAL_CheckPDPModule(int32_t module) { - return module < kNumPDPModules && module >= 0; + return module < kNumCTREPDPModules && module >= 0; } HAL_Bool HAL_CheckPDPChannel(int32_t channel) { - return channel < kNumPDPChannels && channel >= 0; + return channel < kNumCTREPDPChannels && channel >= 0; } double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) { diff --git a/hal/src/main/native/athena/CTREPDP.h b/hal/src/main/native/athena/CTREPDP.h index c3a9a5766d..43ec031d35 100644 --- a/hal/src/main/native/athena/CTREPDP.h +++ b/hal/src/main/native/athena/CTREPDP.h @@ -35,6 +35,11 @@ HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation, */ void HAL_CleanPDP(HAL_PDPHandle handle); +/** + * Gets the module number for a pdp. + */ +int32_t HAL_GetPDPModuleNumber(HAL_PDPHandle handle, int32_t* status); + /** * Checks if a PDP channel is valid. * diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 03cefc52d6..2d37e908df 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -64,7 +64,8 @@ void InitializeHAL() { InitializeInterrupts(); InitializeMain(); InitializeNotifier(); - InitializePDP(); + InitializeCTREPDP(); + InitializeREVPDH(); InitializePorts(); InitializePower(); InitializePWM(); diff --git a/hal/src/main/native/athena/HALInitializer.h b/hal/src/main/native/athena/HALInitializer.h index e84b47d2bb..6fc03dc72e 100644 --- a/hal/src/main/native/athena/HALInitializer.h +++ b/hal/src/main/native/athena/HALInitializer.h @@ -41,7 +41,8 @@ extern void InitializeI2C(); extern void InitializeInterrupts(); extern void InitializeMain(); extern void InitializeNotifier(); -extern void InitializePDP(); +extern void InitializeCTREPDP(); +extern void InitializeREVPDH(); extern void InitializePorts(); extern void InitializePower(); extern void InitializePWM(); diff --git a/hal/src/main/native/athena/Ports.cpp b/hal/src/main/native/athena/Ports.cpp index 9e8c29eeda..89f167d5ba 100644 --- a/hal/src/main/native/athena/Ports.cpp +++ b/hal/src/main/native/athena/Ports.cpp @@ -59,14 +59,20 @@ int32_t HAL_GetNumRelayHeaders(void) { int32_t HAL_GetNumCTREPCMModules(void) { return kNumCTREPCMModules; } -int32_t HAL_GetNumSolenoidChannels(void) { +int32_t HAL_GetNumCTRESolenoidChannels(void) { return kNumCTRESolenoidChannels; } -int32_t HAL_GetNumPDPModules(void) { - return kNumPDPModules; +int32_t HAL_GetNumCTREPDPModules(void) { + return kNumCTREPDPModules; } -int32_t HAL_GetNumPDPChannels(void) { - return kNumPDPChannels; +int32_t HAL_GetNumCTREPDPChannels(void) { + return kNumCTREPDPChannels; +} +int32_t HAL_GetNumREVPDHModules(void) { + return kNumREVPDHModules; +} +int32_t HAL_GetNumREVPDHChannels(void) { + return kNumREVPDHChannels; } int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; diff --git a/hal/src/main/native/athena/PortsInternal.h b/hal/src/main/native/athena/PortsInternal.h index 27bbfb8c56..11278584d0 100644 --- a/hal/src/main/native/athena/PortsInternal.h +++ b/hal/src/main/native/athena/PortsInternal.h @@ -30,8 +30,10 @@ constexpr int32_t kNumRelayChannels = 8; constexpr int32_t kNumRelayHeaders = kNumRelayChannels / 2; constexpr int32_t kNumCTREPCMModules = 63; constexpr int32_t kNumCTRESolenoidChannels = 8; -constexpr int32_t kNumPDPModules = 63; -constexpr int32_t kNumPDPChannels = 16; +constexpr int32_t kNumCTREPDPModules = 63; +constexpr int32_t kNumCTREPDPChannels = 16; +constexpr int32_t kNumREVPDHModules = 63; +constexpr int32_t kNumREVPDHChannels = 24; constexpr int32_t kNumDutyCycles = tDutyCycle::kNumSystems; constexpr int32_t kNumAddressableLEDs = tLED::kNumSystems; diff --git a/hal/src/main/native/athena/PowerDistribution.cpp b/hal/src/main/native/athena/PowerDistribution.cpp index 5936539984..f7fe88faaa 100644 --- a/hal/src/main/native/athena/PowerDistribution.cpp +++ b/hal/src/main/native/athena/PowerDistribution.cpp @@ -7,6 +7,7 @@ #include "CTREPDP.h" #include "HALInternal.h" #include "PortsInternal.h" +#include "REVPDH.h" #include "hal/Errors.h" #include "hal/handles/HandlesInternal.h" @@ -15,18 +16,24 @@ using namespace hal; extern "C" { HAL_PowerDistributionHandle HAL_InitializePowerDistribution( - int32_t moduleNumber, HAL_PowerDistributionType type, int32_t* status) { + int32_t moduleNumber, HAL_PowerDistributionType type, + const char* allocationLocation, int32_t* status) { if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic) { type = HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE; } if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) { + if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) { + moduleNumber = 0; + } return static_cast( - HAL_InitializePDP(moduleNumber, nullptr, status)); // TODO + HAL_InitializePDP(moduleNumber, allocationLocation, status)); // TODO } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); - return HAL_kInvalidHandle; + if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) { + moduleNumber = 1; + } + return static_cast( + HAL_REV_InitializePDH(moduleNumber, allocationLocation, status)); } } @@ -36,7 +43,16 @@ void HAL_CleanPowerDistribution(HAL_PowerDistributionHandle handle) { if (IsCtre(handle)) { HAL_CleanPDP(handle); } else { - // TODO + HAL_REV_FreePDH(handle); + } +} + +int32_t HAL_GetPowerDistributionModuleNumber(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + return HAL_GetPDPModuleNumber(handle, status); + } else { + return HAL_REV_GetPDHModuleNumber(handle, status); } } @@ -45,8 +61,7 @@ HAL_Bool HAL_CheckPowerDistributionChannel(HAL_PowerDistributionHandle handle, if (IsCtre(handle)) { return HAL_CheckPDPChannel(channel); } else { - return false; - // TODO + return HAL_REV_CheckPDHChannelNumber(channel); } } @@ -55,8 +70,7 @@ HAL_Bool HAL_CheckPowerDistributionModule(int32_t module, if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) { return HAL_CheckPDPModule(module); } else { - return false; - // TODO + return HAL_REV_CheckPDHModuleNumber(module); } } @@ -67,14 +81,22 @@ HAL_PowerDistributionType HAL_GetPowerDistributionType( : HAL_PowerDistributionType::HAL_PowerDistributionType_kRev; } +int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle, + int32_t* status) { + if (IsCtre(handle)) { + return kNumCTREPDPChannels; + } else { + return kNumREVPDHChannels; + } +} + double HAL_GetPowerDistributionTemperature(HAL_PowerDistributionHandle handle, int32_t* status) { if (IsCtre(handle)) { return HAL_GetPDPTemperature(handle, status); } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); - return false; + // Not supported + return 0; } } @@ -83,9 +105,7 @@ double HAL_GetPowerDistributionVoltage(HAL_PowerDistributionHandle handle, if (IsCtre(handle)) { return HAL_GetPDPVoltage(handle, status); } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); - return false; + return HAL_REV_GetPDHSupplyVoltage(handle, status); } } @@ -94,9 +114,7 @@ double HAL_GetPowerDistributionChannelCurrent( if (IsCtre(handle)) { return HAL_GetPDPChannelCurrent(handle, channel, status); } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); - return 0; + return HAL_REV_GetPDHChannelCurrent(handle, channel, status); } } @@ -104,15 +122,19 @@ void HAL_GetPowerDistributionAllChannelCurrents( HAL_PowerDistributionHandle handle, double* currents, int32_t currentsLength, int32_t* status) { if (IsCtre(handle)) { - if (currentsLength < kNumPDPChannels) { + if (currentsLength < kNumCTREPDPChannels) { *status = PARAMETER_OUT_OF_RANGE; SetLastError(status, "Output array not large enough"); return; } return HAL_GetPDPAllChannelCurrents(handle, currents, status); } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); + if (currentsLength < kNumREVPDHChannels) { + *status = PARAMETER_OUT_OF_RANGE; + SetLastError(status, "Output array not large enough"); + return; + } + return HAL_REV_GetPDHAllChannelCurrents(handle, currents, status); } } @@ -121,9 +143,7 @@ double HAL_GetPowerDistributionTotalCurrent(HAL_PowerDistributionHandle handle, if (IsCtre(handle)) { return HAL_GetPDPTotalCurrent(handle, status); } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); - return 0; + return HAL_REV_GetPDHTotalCurrent(handle, status); } } @@ -132,8 +152,7 @@ double HAL_GetPowerDistributionTotalPower(HAL_PowerDistributionHandle handle, if (IsCtre(handle)) { return HAL_GetPDPTotalPower(handle, status); } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); + // Not currently supported return 0; } } @@ -143,8 +162,7 @@ double HAL_GetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle, if (IsCtre(handle)) { return HAL_GetPDPTotalEnergy(handle, status); } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); + // Not currently supported return 0; } } @@ -154,8 +172,7 @@ void HAL_ResetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle, if (IsCtre(handle)) { HAL_ResetPDPTotalEnergy(handle, status); } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); + // Not supported } } @@ -164,8 +181,28 @@ void HAL_ClearPowerDistributionStickyFaults(HAL_PowerDistributionHandle handle, if (IsCtre(handle)) { HAL_ClearPDPStickyFaults(handle, status); } else { - *status = PARAMETER_OUT_OF_RANGE; - SetLastError(status, "Rev Power not currently supported"); + HAL_REV_ClearPDHFaults(handle, status); } } + +void HAL_SetPowerDistributionSwitchableChannel( + HAL_PowerDistributionHandle handle, HAL_Bool enabled, int32_t* status) { + if (IsCtre(handle)) { + // No-op on CTRE + return; + } else { + HAL_REV_SetPDHSwitchableChannel(handle, enabled, status); + } +} + +HAL_Bool HAL_GetPowerDistributionSwitchableChannel( + HAL_PowerDistributionHandle handle, int32_t* status) { + if (IsCtre(handle)) { + // No-op on CTRE + return false; + } else { + return HAL_REV_GetPDHSwitchableChannelState(handle, status); + } +} + } // extern "C" diff --git a/hal/src/main/native/athena/REVPDH.cpp b/hal/src/main/native/athena/REVPDH.cpp new file mode 100644 index 0000000000..ac4ef48ce7 --- /dev/null +++ b/hal/src/main/native/athena/REVPDH.cpp @@ -0,0 +1,820 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "REVPDH.h" + +#include +#include +#include +#include +#include + +#include + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "rev/PDHFrames.h" + +using namespace hal; + +static constexpr HAL_CANManufacturer manufacturer = + HAL_CANManufacturer::HAL_CAN_Man_kREV; + +static constexpr HAL_CANDeviceType deviceType = + HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution; + +static constexpr int32_t kDefaultControlPeriod = 50; + +namespace { + +struct REV_PDHObj { + int32_t controlPeriod; + HAL_CANHandle hcan; + std::string previousAllocation; +}; + +} // namespace + +static constexpr uint32_t APIFromExtId(uint32_t extId) { + return (extId >> 6) & 0x3FF; +} + +static constexpr uint32_t PDH_SWITCH_CHANNEL_SET_FRAME_API = + APIFromExtId(PDH_SWITCH_CHANNEL_SET_FRAME_ID); + +static constexpr uint32_t PDH_STATUS0_FRAME_API = + APIFromExtId(PDH_STATUS0_FRAME_ID); +static constexpr uint32_t PDH_STATUS1_FRAME_API = + APIFromExtId(PDH_STATUS1_FRAME_ID); +static constexpr uint32_t PDH_STATUS2_FRAME_API = + APIFromExtId(PDH_STATUS2_FRAME_ID); +static constexpr uint32_t PDH_STATUS3_FRAME_API = + APIFromExtId(PDH_STATUS3_FRAME_ID); +static constexpr uint32_t PDH_STATUS4_FRAME_API = + APIFromExtId(PDH_STATUS4_FRAME_ID); + +static constexpr uint32_t PDH_CLEAR_FAULTS_FRAME_API = + APIFromExtId(PDH_CLEAR_FAULTS_FRAME_ID); + +static constexpr uint32_t PDH_IDENTIFY_FRAME_API = + APIFromExtId(PDH_IDENTIFY_FRAME_ID); + +static constexpr uint32_t PDH_VERSION_FRAME_API = + APIFromExtId(PDH_VERSION_FRAME_ID); + +static constexpr uint32_t PDH_CONFIGURE_HR_CHANNEL_FRAME_API = + APIFromExtId(PDH_CONFIGURE_HR_CHANNEL_FRAME_ID); + +static constexpr int32_t kPDHFrameStatus0Timeout = 20; +static constexpr int32_t kPDHFrameStatus1Timeout = 20; +static constexpr int32_t kPDHFrameStatus2Timeout = 20; +static constexpr int32_t kPDHFrameStatus3Timeout = 20; +static constexpr int32_t kPDHFrameStatus4Timeout = 20; + +static IndexedHandleResource* REVPDHHandles; + +namespace hal::init { +void InitializeREVPDH() { + static IndexedHandleResource + rH; + REVPDHHandles = &rH; +} +} // namespace hal::init + +extern "C" { + +static PDH_status0_t HAL_REV_ReadPDHStatus0(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status0_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS0_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus0Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status0_unpack(&result, packedData, PDH_STATUS0_LENGTH); + + return result; +} + +static PDH_status1_t HAL_REV_ReadPDHStatus1(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status1_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS1_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus1Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status1_unpack(&result, packedData, PDH_STATUS1_LENGTH); + + return result; +} + +static PDH_status2_t HAL_REV_ReadPDHStatus2(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status2_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS2_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus2Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status2_unpack(&result, packedData, PDH_STATUS2_LENGTH); + + return result; +} + +static PDH_status3_t HAL_REV_ReadPDHStatus3(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status3_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS3_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus3Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status3_unpack(&result, packedData, PDH_STATUS3_LENGTH); + + return result; +} + +static PDH_status4_t HAL_REV_ReadPDHStatus4(HAL_CANHandle hcan, + int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_status4_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PDH_STATUS4_FRAME_API, packedData, &length, + ×tamp, kPDHFrameStatus4Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PDH_status4_unpack(&result, packedData, PDH_STATUS4_LENGTH); + + return result; +} + +/** + * Helper function for the individual getter functions for status 4 + */ +PDH_status4_t HAL_REV_GetPDHStatus4(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status4_t statusFrame = {}; + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return statusFrame; + } + + statusFrame = HAL_REV_ReadPDHStatus4(hpdh->hcan, status); + return statusFrame; +} + +HAL_REVPDHHandle HAL_REV_InitializePDH(int32_t module, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + if (!HAL_REV_CheckPDHModuleNumber(module)) { + *status = RESOURCE_OUT_OF_RANGE; + return HAL_kInvalidHandle; + } + + HAL_REVPDHHandle handle; + auto hpdh = REVPDHHandles->Allocate(module, &handle, status); + if (*status != 0) { + if (hpdh) { + hal::SetLastErrorPreviouslyAllocated(status, "REV PDH", module, + hpdh->previousAllocation); + } else { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PDH", 0, + kNumREVPDHModules, module); + } + return HAL_kInvalidHandle; // failed to allocate. Pass error back. + } + + HAL_CANHandle hcan = + HAL_InitializeCAN(manufacturer, module, deviceType, status); + + if (*status != 0) { + REVPDHHandles->Free(handle); + return HAL_kInvalidHandle; + } + + hpdh->previousAllocation = allocationLocation ? allocationLocation : ""; + hpdh->hcan = hcan; + hpdh->controlPeriod = kDefaultControlPeriod; + + return handle; +} + +void HAL_REV_FreePDH(HAL_REVPDHHandle handle) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + return; + } + + HAL_CleanCAN(hpdh->hcan); + + REVPDHHandles->Free(handle); +} + +int32_t HAL_REV_GetPDHModuleNumber(HAL_REVPDHHandle handle, int32_t* status) { + return hal::getHandleIndex(handle); +} + +HAL_Bool HAL_REV_CheckPDHModuleNumber(int32_t module) { + return ((module >= 1) && (module < kNumREVPDHModules)) ? 1 : 0; +} + +HAL_Bool HAL_REV_CheckPDHChannelNumber(int32_t channel) { + return ((channel >= 0) && (channel < kNumREVPDHChannels)) ? 1 : 0; +} + +double HAL_REV_GetPDHChannelCurrent(HAL_REVPDHHandle handle, int32_t channel, + int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + if (!HAL_REV_CheckPDHChannelNumber(channel)) { + *status = RESOURCE_OUT_OF_RANGE; + return 0; + } + + // Determine what periodic status the channel is in + if (channel < 6) { + // Periodic status 0 + PDH_status0_t statusFrame = HAL_REV_ReadPDHStatus0(hpdh->hcan, status); + switch (channel) { + case 0: + return PDH_status0_channel_0_current_decode( + statusFrame.channel_0_current); + case 1: + return PDH_status0_channel_1_current_decode( + statusFrame.channel_1_current); + case 2: + return PDH_status0_channel_2_current_decode( + statusFrame.channel_2_current); + case 3: + return PDH_status0_channel_3_current_decode( + statusFrame.channel_3_current); + case 4: + return PDH_status0_channel_4_current_decode( + statusFrame.channel_4_current); + case 5: + return PDH_status0_channel_5_current_decode( + statusFrame.channel_5_current); + } + } else if (channel < 12) { + // Periodic status 1 + PDH_status1_t statusFrame = HAL_REV_ReadPDHStatus1(hpdh->hcan, status); + switch (channel) { + case 6: + return PDH_status1_channel_6_current_decode( + statusFrame.channel_6_current); + case 7: + return PDH_status1_channel_7_current_decode( + statusFrame.channel_7_current); + case 8: + return PDH_status1_channel_8_current_decode( + statusFrame.channel_8_current); + case 9: + return PDH_status1_channel_9_current_decode( + statusFrame.channel_9_current); + case 10: + return PDH_status1_channel_10_current_decode( + statusFrame.channel_10_current); + case 11: + return PDH_status1_channel_11_current_decode( + statusFrame.channel_11_current); + } + } else if (channel < 18) { + // Periodic status 2 + PDH_status2_t statusFrame = HAL_REV_ReadPDHStatus2(hpdh->hcan, status); + switch (channel) { + case 12: + return PDH_status2_channel_12_current_decode( + statusFrame.channel_12_current); + case 13: + return PDH_status2_channel_13_current_decode( + statusFrame.channel_13_current); + case 14: + return PDH_status2_channel_14_current_decode( + statusFrame.channel_14_current); + case 15: + return PDH_status2_channel_15_current_decode( + statusFrame.channel_15_current); + case 16: + return PDH_status2_channel_16_current_decode( + statusFrame.channel_16_current); + case 17: + return PDH_status2_channel_17_current_decode( + statusFrame.channel_17_current); + } + } else if (channel < 24) { + // Periodic status 3 + PDH_status3_t statusFrame = HAL_REV_ReadPDHStatus3(hpdh->hcan, status); + switch (channel) { + case 18: + return PDH_status3_channel_18_current_decode( + statusFrame.channel_18_current); + case 19: + return PDH_status3_channel_19_current_decode( + statusFrame.channel_19_current); + case 20: + return PDH_status3_channel_20_current_decode( + statusFrame.channel_20_current); + case 21: + return PDH_status3_channel_21_current_decode( + statusFrame.channel_21_current); + case 22: + return PDH_status3_channel_22_current_decode( + statusFrame.channel_22_current); + case 23: + return PDH_status3_channel_23_current_decode( + statusFrame.channel_23_current); + } + } + return 0; +} + +void HAL_REV_GetPDHAllChannelCurrents(HAL_REVPDHHandle handle, double* currents, + int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + PDH_status0_t statusFrame0 = HAL_REV_ReadPDHStatus0(hpdh->hcan, status); + PDH_status1_t statusFrame1 = HAL_REV_ReadPDHStatus1(hpdh->hcan, status); + PDH_status2_t statusFrame2 = HAL_REV_ReadPDHStatus2(hpdh->hcan, status); + PDH_status3_t statusFrame3 = HAL_REV_ReadPDHStatus3(hpdh->hcan, status); + + currents[0] = + PDH_status0_channel_0_current_decode(statusFrame0.channel_0_current); + currents[1] = + PDH_status0_channel_1_current_decode(statusFrame0.channel_1_current); + currents[2] = + PDH_status0_channel_2_current_decode(statusFrame0.channel_2_current); + currents[3] = + PDH_status0_channel_3_current_decode(statusFrame0.channel_3_current); + currents[4] = + PDH_status0_channel_4_current_decode(statusFrame0.channel_4_current); + currents[5] = + PDH_status0_channel_5_current_decode(statusFrame0.channel_5_current); + currents[6] = + PDH_status1_channel_6_current_decode(statusFrame1.channel_6_current); + currents[7] = + PDH_status1_channel_7_current_decode(statusFrame1.channel_7_current); + currents[8] = + PDH_status1_channel_8_current_decode(statusFrame1.channel_8_current); + currents[9] = + PDH_status1_channel_9_current_decode(statusFrame1.channel_9_current); + currents[10] = + PDH_status1_channel_10_current_decode(statusFrame1.channel_10_current); + currents[11] = + PDH_status1_channel_11_current_decode(statusFrame1.channel_11_current); + currents[12] = + PDH_status2_channel_12_current_decode(statusFrame2.channel_12_current); + currents[13] = + PDH_status2_channel_13_current_decode(statusFrame2.channel_13_current); + currents[14] = + PDH_status2_channel_14_current_decode(statusFrame2.channel_14_current); + currents[15] = + PDH_status2_channel_15_current_decode(statusFrame2.channel_15_current); + currents[16] = + PDH_status2_channel_16_current_decode(statusFrame2.channel_16_current); + currents[17] = + PDH_status2_channel_17_current_decode(statusFrame2.channel_17_current); + currents[18] = + PDH_status3_channel_18_current_decode(statusFrame3.channel_18_current); + currents[19] = + PDH_status3_channel_19_current_decode(statusFrame3.channel_19_current); + currents[20] = + PDH_status3_channel_20_current_decode(statusFrame3.channel_20_current); + currents[21] = + PDH_status3_channel_21_current_decode(statusFrame3.channel_21_current); + currents[22] = + PDH_status3_channel_22_current_decode(statusFrame3.channel_22_current); + currents[23] = + PDH_status3_channel_23_current_decode(statusFrame3.channel_23_current); +} + +uint16_t HAL_REV_GetPDHTotalCurrent(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0; + } + + return PDH_status4_total_current_decode(statusFrame.total_current); +} + +void HAL_REV_SetPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled, + int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t packedData[8] = {0}; + PDH_switch_channel_set_t frame; + frame.output_set_value = enabled; + frame.use_system_enable = false; + PDH_switch_channel_set_pack(packedData, &frame, 1); + + HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_SWITCH_CHANNEL_SET_LENGTH, + PDH_SWITCH_CHANNEL_SET_FRAME_API, status); +} + +HAL_Bool HAL_REV_GetPDHSwitchableChannelState(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_sw_state_decode(statusFrame.sw_state); +} + +HAL_Bool HAL_REV_CheckPDHChannelBrownout(HAL_REVPDHHandle handle, + int32_t channel, int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + if (!HAL_REV_CheckPDHChannelNumber(channel)) { + *status = RESOURCE_OUT_OF_RANGE; + return 0; + } + + // Determine what periodic status the channel is in + if (channel < 4) { + // Periodic status 0 + PDH_status0_t statusFrame = HAL_REV_ReadPDHStatus0(hpdh->hcan, status); + switch (channel) { + case 0: + return PDH_status0_channel_0_brownout_decode( + statusFrame.channel_0_brownout); + case 1: + return PDH_status0_channel_1_brownout_decode( + statusFrame.channel_1_brownout); + case 2: + return PDH_status0_channel_2_brownout_decode( + statusFrame.channel_2_brownout); + case 3: + return PDH_status0_channel_3_brownout_decode( + statusFrame.channel_3_brownout); + } + } else if (channel < 8) { + // Periodic status 1 + PDH_status1_t statusFrame = HAL_REV_ReadPDHStatus1(hpdh->hcan, status); + switch (channel) { + case 4: + return PDH_status1_channel_4_brownout_decode( + statusFrame.channel_4_brownout); + case 5: + return PDH_status1_channel_5_brownout_decode( + statusFrame.channel_5_brownout); + case 6: + return PDH_status1_channel_6_brownout_decode( + statusFrame.channel_6_brownout); + case 7: + return PDH_status1_channel_7_brownout_decode( + statusFrame.channel_7_brownout); + } + } else if (channel < 12) { + // Periodic status 2 + PDH_status2_t statusFrame = HAL_REV_ReadPDHStatus2(hpdh->hcan, status); + switch (channel) { + case 8: + return PDH_status2_channel_8_brownout_decode( + statusFrame.channel_8_brownout); + case 9: + return PDH_status2_channel_9_brownout_decode( + statusFrame.channel_9_brownout); + case 10: + return PDH_status2_channel_10_brownout_decode( + statusFrame.channel_10_brownout); + case 11: + return PDH_status2_channel_11_brownout_decode( + statusFrame.channel_11_brownout); + } + } else if (channel < 24) { + // Periodic status 3 + PDH_status3_t statusFrame = HAL_REV_ReadPDHStatus3(hpdh->hcan, status); + switch (channel) { + case 12: + return PDH_status3_channel_12_brownout_decode( + statusFrame.channel_12_brownout); + case 13: + return PDH_status3_channel_13_brownout_decode( + statusFrame.channel_13_brownout); + case 14: + return PDH_status3_channel_14_brownout_decode( + statusFrame.channel_14_brownout); + case 15: + return PDH_status3_channel_15_brownout_decode( + statusFrame.channel_15_brownout); + case 16: + return PDH_status3_channel_16_brownout_decode( + statusFrame.channel_16_brownout); + case 17: + return PDH_status3_channel_17_brownout_decode( + statusFrame.channel_17_brownout); + case 18: + return PDH_status3_channel_18_brownout_decode( + statusFrame.channel_18_brownout); + case 19: + return PDH_status3_channel_19_brownout_decode( + statusFrame.channel_19_brownout); + case 20: + return PDH_status3_channel_20_brownout_decode( + statusFrame.channel_20_brownout); + case 21: + return PDH_status3_channel_21_brownout_decode( + statusFrame.channel_21_brownout); + case 22: + return PDH_status3_channel_22_brownout_decode( + statusFrame.channel_22_brownout); + case 23: + return PDH_status3_channel_23_brownout_decode( + statusFrame.channel_23_brownout); + } + } + return 0; +} + +double HAL_REV_GetPDHSupplyVoltage(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_v_bus_decode(statusFrame.v_bus); +} + +HAL_Bool HAL_REV_IsPDHEnabled(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return false; + } + + return PDH_status4_system_enable_decode(statusFrame.system_enable); +} + +HAL_Bool HAL_REV_CheckPDHBrownout(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return false; + } + + return PDH_status4_brownout_decode(statusFrame.brownout); +} + +HAL_Bool HAL_REV_CheckPDHOverCurrent(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return false; + } + + return PDH_status4_over_current_decode(statusFrame.over_current); +} + +HAL_Bool HAL_REV_CheckPDHCANWarning(HAL_REVPDHHandle handle, int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_can_warning_decode(statusFrame.can_warning); +} + +HAL_Bool HAL_REV_CheckPDHHardwareFault(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_hardware_fault_decode(statusFrame.hardware_fault); +} + +HAL_Bool HAL_REV_CheckPDHStickyBrownout(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_sticky_brownout_decode(statusFrame.sticky_brownout); +} + +HAL_Bool HAL_REV_CheckPDHStickyOverCurrent(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_sticky_over_current_decode( + statusFrame.sticky_over_current); +} + +HAL_Bool HAL_REV_CheckPDHStickyCANWarning(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_sticky_can_warning_decode(statusFrame.sticky_can_warning); +} + +HAL_Bool HAL_REV_CheckPDHStickyCANBusOff(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_sticky_can_bus_off_decode(statusFrame.sticky_can_bus_off); +} + +HAL_Bool HAL_REV_CheckPDHStickyHardwareFault(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_sticky_hardware_fault_decode( + statusFrame.sticky_hardware_fault); +} + +HAL_Bool HAL_REV_CheckPDHStickyFirmwareFault(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_sticky_firmware_fault_decode( + statusFrame.sticky_firmware_fault); +} + +HAL_Bool HAL_REV_CheckPDHStickyChannelBrownout(HAL_REVPDHHandle handle, + int32_t channel, + int32_t* status) { + if (channel < 20 || channel > 23) { + *status = RESOURCE_OUT_OF_RANGE; + return 0.0; + } + + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + switch (channel) { + case 20: + return PDH_status4_sticky_ch20_brownout_decode( + statusFrame.sticky_ch20_brownout); + case 21: + return PDH_status4_sticky_ch21_brownout_decode( + statusFrame.sticky_ch21_brownout); + case 22: + return PDH_status4_sticky_ch22_brownout_decode( + statusFrame.sticky_ch22_brownout); + case 23: + return PDH_status4_sticky_ch23_brownout_decode( + statusFrame.sticky_ch23_brownout); + } + return 0; +} + +HAL_Bool HAL_REV_CheckPDHStickyHasReset(HAL_REVPDHHandle handle, + int32_t* status) { + PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status); + + if (*status != 0) { + return 0.0; + } + + return PDH_status4_sticky_has_reset_decode(statusFrame.sticky_has_reset); +} + +REV_PDH_Version HAL_REV_GetPDHVersion(HAL_REVPDHHandle handle, + int32_t* status) { + REV_PDH_Version version; + std::memset(&version, 0, sizeof(version)); + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PDH_version_t result = {}; + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return version; + } + + HAL_WriteCANRTRFrame(hpdh->hcan, PDH_VERSION_LENGTH, PDH_VERSION_FRAME_API, + status); + + if (*status != 0) { + return version; + } + + HAL_ReadCANPacketTimeout(hpdh->hcan, PDH_VERSION_FRAME_API, packedData, + &length, ×tamp, kDefaultControlPeriod * 2, + status); + + if (*status != 0) { + return version; + } + + PDH_version_unpack(&result, packedData, PDH_VERSION_LENGTH); + + version.firmwareMajor = result.firmware_year; + version.firmwareMinor = result.firmware_minor; + version.firmwareFix = result.firmware_fix; + version.hardwareRev = result.hardware_code; + version.uniqueId = result.unique_id; + + return version; +} + +void HAL_REV_ClearPDHFaults(HAL_REVPDHHandle handle, int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t packedData[8] = {0}; + HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_CLEAR_FAULTS_LENGTH, + PDH_CLEAR_FAULTS_FRAME_API, status); +} + +void HAL_REV_IdentifyPDH(HAL_REVPDHHandle handle, int32_t* status) { + auto hpdh = REVPDHHandles->Get(handle); + if (hpdh == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t packedData[8] = {0}; + HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_IDENTIFY_LENGTH, + PDH_IDENTIFY_FRAME_API, status); +} + +} // extern "C" diff --git a/hal/src/main/native/athena/REVPDH.h b/hal/src/main/native/athena/REVPDH.h new file mode 100644 index 0000000000..7b501bdb5c --- /dev/null +++ b/hal/src/main/native/athena/REVPDH.h @@ -0,0 +1,339 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "hal/Types.h" + +/** + * @defgroup hal_rev_pdh REV Power Distribution Hub API Functions + * @ingroup hal_capi + * @{ + */ + +struct REV_PDH_Version { + uint32_t firmwareMajor; + uint32_t firmwareMinor; + uint32_t firmwareFix; + uint32_t hardwareRev; + uint32_t uniqueId; +}; + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Initializes a REV Power Distribution Hub (PDH) device. + * + * @param module the device CAN ID (1 .. 63) + * @return the created PDH handle + */ +HAL_REVPDHHandle HAL_REV_InitializePDH(int32_t module, + const char* allocationLocation, + int32_t* status); + +/** + * Frees a PDH device handle. + * + * @param handle the previously created PDH handle + */ +void HAL_REV_FreePDH(HAL_REVPDHHandle handle); + +/** + * Gets the module number for a pdh. + */ +int32_t HAL_REV_GetPDHModuleNumber(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Checks if a PDH module number is valid. + * + * Does not check if a PDH device with this module has been initialized. + * + * @param module module number (1 .. 63) + * @return 1 if the module number is valid; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHModuleNumber(int32_t module); + +/** + * Checks if a PDH channel number is valid. + * + * @param module channel number (0 .. HAL_REV_PDH_NUM_CHANNELS) + * @return 1 if the channel number is valid; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHChannelNumber(int32_t channel); + +/** + * Gets the current of a PDH channel in Amps. + * + * @param handle PDH handle + * @param channel the channel to retrieve the current of (0 .. + * HAL_REV_PDH_NUM_CHANNELS) + * + * @return the current of the PDH channel in Amps + */ +double HAL_REV_GetPDHChannelCurrent(HAL_REVPDHHandle handle, int32_t channel, + int32_t* status); + +/** + * @param handle PDH handle + * @param currents array of currents + */ +void HAL_REV_GetPDHAllChannelCurrents(HAL_REVPDHHandle handle, double* currents, + int32_t* status); + +/** + * Gets the total current of the PDH in Amps, measured to the nearest even + * integer. + * + * @param handle PDH handle + * + * @return the total current of the PDH in Amps + */ +uint16_t HAL_REV_GetPDHTotalCurrent(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Sets the state of the switchable channel on a PDH device. + * + * @param handle PDH handle + * @param enabled 1 if the switchable channel should be enabled; 0 + * otherwise + */ +void HAL_REV_SetPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled, + int32_t* status); + +/** + * Gets the current state of the switchable channel on a PDH device. + * + * This call relies on a periodic status sent by the PDH device and will be as + * fresh as the last packet received. + * + * @param handle PDH handle + * @return 1 if the switchable channel is enabled; 0 otherwise + */ +HAL_Bool HAL_REV_GetPDHSwitchableChannelState(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Checks if a PDH channel is currently experiencing a brownout condition. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * @param channel the channel to retrieve the brownout status of + * + * @return 1 if the channel is experiencing a brownout; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHChannelBrownout(HAL_REVPDHHandle handle, + int32_t channel, int32_t* status); + +/** + * Gets the voltage being supplied to a PDH device. + * + * @param handle PDH handle + * + * @return the voltage at the input of the PDH in Volts + */ +double HAL_REV_GetPDHSupplyVoltage(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Checks if a PDH device is currently enabled. + * + * @param handle PDH handle + * + * @return 1 if the PDH is enabled; 0 otherwise + */ +HAL_Bool HAL_REV_IsPDHEnabled(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Checks if the input voltage on a PDH device is currently below the minimum + * voltage. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the PDH is experiencing a brownout; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHBrownout(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Checks if the system current on a PDH device is currently exceeding the max + * system current. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the PDH is over max current; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHOverCurrent(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Checks if the CAN RX or TX error levels on a PDH device have exceeded the + * warning threshold. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the device has exceeded the warning threshold; 0 + * otherwise + */ +HAL_Bool HAL_REV_CheckPDHCANWarning(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Checks if a PDH device is currently malfunctioning. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the device is in a hardware fault state; 0 + * otherwise + */ +HAL_Bool HAL_REV_CheckPDHHardwareFault(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Checks if the input voltage on a PDH device has gone below the specified + * minimum voltage. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the device has had a brownout; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHStickyBrownout(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Checks if the system current on a PDH device has exceeded the maximum system + * current. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the device has been over current; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHStickyOverCurrent(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Checks if the CAN RX or TX error levels on a PDH device have exceeded the + * warning threshold. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the device has exceeded the CAN warning threshold; + * 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHStickyCANWarning(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Checks if the CAN bus on a PDH device has previously experienced a 'Bus Off' + * event. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the device has experienced a 'Bus Off' event; 0 + * otherwise + */ +HAL_Bool HAL_REV_CheckPDHStickyCANBusOff(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Checks if a PDH device has malfunctioned. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the device has had a malfunction; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHStickyHardwareFault(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Checks if the firmware on a PDH device has malfunctioned and reset during + * operation. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the device has had a malfunction and reset; 0 + * otherwise + */ +HAL_Bool HAL_REV_CheckPDHStickyFirmwareFault(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Checks if a brownout has happened on channels 20-23 of a PDH device while it + * was enabled. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * @param channel PDH channel to retrieve sticky brownout status (20 .. + * 23) + * + * + * @return 1 if the channel has had a brownout; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHStickyChannelBrownout(HAL_REVPDHHandle handle, + int32_t channel, + int32_t* status); + +/** + * Checks if a PDH device has reset. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + * + * @return 1 if the device has reset; 0 otherwise + */ +HAL_Bool HAL_REV_CheckPDHStickyHasReset(HAL_REVPDHHandle handle, + int32_t* status); + +/** + * Gets the firmware and hardware versions of a PDH device. + * + * @param handle PDH handle + * + * @return version information + */ +REV_PDH_Version HAL_REV_GetPDHVersion(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Clears the sticky faults on a PDH device. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + */ +void HAL_REV_ClearPDHFaults(HAL_REVPDHHandle handle, int32_t* status); + +/** + * Identifies a PDH device by blinking its LED. + * + * NOTE: Not implemented in firmware as of 2021-04-23. + * + * @param handle PDH handle + */ +void HAL_REV_IdentifyPDH(HAL_REVPDHHandle handle, int32_t* status); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/hal/src/main/native/athena/mockdata/PowerDistributionData.cpp b/hal/src/main/native/athena/mockdata/PowerDistributionData.cpp index 9d9b3b4902..a44e222214 100644 --- a/hal/src/main/native/athena/mockdata/PowerDistributionData.cpp +++ b/hal/src/main/native/athena/mockdata/PowerDistributionData.cpp @@ -18,14 +18,16 @@ DEFINE_CAPI(double, Temperature, 0) DEFINE_CAPI(double, Voltage, 0) HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(double, HALSIM, PowerDistributionCurrent, 0) -void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents) { - for (int i = 0; i < hal::kNumPDPChannels; i++) { +void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents, + int length) { + for (int i = 0; i < length; i++) { currents[i] = 0; } } void HALSIM_SetPowerDistributionAllCurrents(int32_t index, - const double* currents) {} + const double* currents, + int length) {} void HALSIM_RegisterPowerDistributionAllNonCurrentCallbacks( int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param, diff --git a/hal/src/main/native/athena/rev/PDHFrames.cpp b/hal/src/main/native/athena/rev/PDHFrames.cpp new file mode 100644 index 0000000000..a87abba49c --- /dev/null +++ b/hal/src/main/native/athena/rev/PDHFrames.cpp @@ -0,0 +1,1790 @@ +/** + * The MIT License (MIT) + * + * Copyright (c) 2018-2019 Erik Moqvist + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, + * modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/** + * This file was generated by cantools version 36.2.0 Fri Jul 9 14:22:19 2021. + */ + +#include + +#include "PDHFrames.h" + +static inline uint8_t pack_left_shift_u8( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + +static inline uint8_t pack_left_shift_u16( + uint16_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + +static inline uint8_t pack_left_shift_u32( + uint32_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + +static inline uint8_t pack_right_shift_u16( + uint16_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value >> shift) & mask); +} + +static inline uint8_t pack_right_shift_u32( + uint32_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value >> shift) & mask); +} + +static inline uint16_t unpack_left_shift_u16( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint16_t)((uint16_t)(value & mask) << shift); +} + +static inline uint32_t unpack_left_shift_u32( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint32_t)((uint32_t)(value & mask) << shift); +} + +static inline uint8_t unpack_right_shift_u8( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value & mask) >> shift); +} + +static inline uint16_t unpack_right_shift_u16( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint16_t)((uint16_t)(value & mask) >> shift); +} + +static inline uint32_t unpack_right_shift_u32( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint32_t)((uint32_t)(value & mask) >> shift); +} + +int PDH_switch_channel_set_pack( + uint8_t *dst_p, + const struct PDH_switch_channel_set_t *src_p, + size_t size) +{ + if (size < 1u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 1); + + dst_p[0] |= pack_left_shift_u8(src_p->output_set_value, 0u, 0x01u); + dst_p[0] |= pack_left_shift_u8(src_p->use_system_enable, 1u, 0x02u); + + return (1); +} + +int PDH_switch_channel_set_unpack( + struct PDH_switch_channel_set_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 1u) { + return (-EINVAL); + } + + dst_p->output_set_value = unpack_right_shift_u8(src_p[0], 0u, 0x01u); + dst_p->use_system_enable = unpack_right_shift_u8(src_p[0], 1u, 0x02u); + + return (0); +} + +uint8_t PDH_switch_channel_set_output_set_value_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_switch_channel_set_output_set_value_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_switch_channel_set_output_set_value_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_switch_channel_set_use_system_enable_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_switch_channel_set_use_system_enable_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_switch_channel_set_use_system_enable_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status0_pack( + uint8_t *dst_p, + const struct PDH_status0_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->channel_0_current, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->channel_0_current, 8u, 0x03u); + dst_p[1] |= pack_left_shift_u16(src_p->channel_1_current, 2u, 0xfcu); + dst_p[2] |= pack_right_shift_u16(src_p->channel_1_current, 6u, 0x0fu); + dst_p[2] |= pack_left_shift_u16(src_p->channel_2_current, 4u, 0xf0u); + dst_p[3] |= pack_right_shift_u16(src_p->channel_2_current, 4u, 0x3fu); + dst_p[3] |= pack_left_shift_u8(src_p->channel_0_brownout, 6u, 0x40u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_1_brownout, 7u, 0x80u); + dst_p[4] |= pack_left_shift_u16(src_p->channel_3_current, 0u, 0xffu); + dst_p[5] |= pack_right_shift_u16(src_p->channel_3_current, 8u, 0x03u); + dst_p[5] |= pack_left_shift_u16(src_p->channel_4_current, 2u, 0xfcu); + dst_p[6] |= pack_right_shift_u16(src_p->channel_4_current, 6u, 0x0fu); + dst_p[6] |= pack_left_shift_u16(src_p->channel_5_current, 4u, 0xf0u); + dst_p[7] |= pack_right_shift_u16(src_p->channel_5_current, 4u, 0x3fu); + dst_p[7] |= pack_left_shift_u8(src_p->channel_2_brownout, 6u, 0x40u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_3_brownout, 7u, 0x80u); + + return (8); +} + +int PDH_status0_unpack( + struct PDH_status0_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_0_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->channel_0_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u); + dst_p->channel_1_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu); + dst_p->channel_1_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu); + dst_p->channel_2_current = unpack_right_shift_u16(src_p[2], 4u, 0xf0u); + dst_p->channel_2_current |= unpack_left_shift_u16(src_p[3], 4u, 0x3fu); + dst_p->channel_0_brownout = unpack_right_shift_u8(src_p[3], 6u, 0x40u); + dst_p->channel_1_brownout = unpack_right_shift_u8(src_p[3], 7u, 0x80u); + dst_p->channel_3_current = unpack_right_shift_u16(src_p[4], 0u, 0xffu); + dst_p->channel_3_current |= unpack_left_shift_u16(src_p[5], 8u, 0x03u); + dst_p->channel_4_current = unpack_right_shift_u16(src_p[5], 2u, 0xfcu); + dst_p->channel_4_current |= unpack_left_shift_u16(src_p[6], 6u, 0x0fu); + dst_p->channel_5_current = unpack_right_shift_u16(src_p[6], 4u, 0xf0u); + dst_p->channel_5_current |= unpack_left_shift_u16(src_p[7], 4u, 0x3fu); + dst_p->channel_2_brownout = unpack_right_shift_u8(src_p[7], 6u, 0x40u); + dst_p->channel_3_brownout = unpack_right_shift_u8(src_p[7], 7u, 0x80u); + + return (0); +} + +uint16_t PDH_status0_channel_0_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status0_channel_0_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status0_channel_0_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status0_channel_1_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status0_channel_1_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status0_channel_1_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status0_channel_2_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status0_channel_2_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status0_channel_2_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status0_channel_0_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status0_channel_0_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status0_channel_0_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status0_channel_1_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status0_channel_1_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status0_channel_1_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint16_t PDH_status0_channel_3_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status0_channel_3_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status0_channel_3_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status0_channel_4_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status0_channel_4_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status0_channel_4_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status0_channel_5_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status0_channel_5_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status0_channel_5_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status0_channel_2_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status0_channel_2_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status0_channel_2_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status0_channel_3_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status0_channel_3_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status0_channel_3_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status1_pack( + uint8_t *dst_p, + const struct PDH_status1_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->channel_6_current, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->channel_6_current, 8u, 0x03u); + dst_p[1] |= pack_left_shift_u16(src_p->channel_7_current, 2u, 0xfcu); + dst_p[2] |= pack_right_shift_u16(src_p->channel_7_current, 6u, 0x0fu); + dst_p[2] |= pack_left_shift_u16(src_p->channel_8_current, 4u, 0xf0u); + dst_p[3] |= pack_right_shift_u16(src_p->channel_8_current, 4u, 0x3fu); + dst_p[3] |= pack_left_shift_u8(src_p->channel_4_brownout, 6u, 0x40u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_5_brownout, 7u, 0x80u); + dst_p[4] |= pack_left_shift_u16(src_p->channel_9_current, 0u, 0xffu); + dst_p[5] |= pack_right_shift_u16(src_p->channel_9_current, 8u, 0x03u); + dst_p[5] |= pack_left_shift_u16(src_p->channel_10_current, 2u, 0xfcu); + dst_p[6] |= pack_right_shift_u16(src_p->channel_10_current, 6u, 0x0fu); + dst_p[6] |= pack_left_shift_u16(src_p->channel_11_current, 4u, 0xf0u); + dst_p[7] |= pack_right_shift_u16(src_p->channel_11_current, 4u, 0x3fu); + dst_p[7] |= pack_left_shift_u8(src_p->channel_6_brownout, 6u, 0x40u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_7_brownout, 7u, 0x80u); + + return (8); +} + +int PDH_status1_unpack( + struct PDH_status1_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_6_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->channel_6_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u); + dst_p->channel_7_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu); + dst_p->channel_7_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu); + dst_p->channel_8_current = unpack_right_shift_u16(src_p[2], 4u, 0xf0u); + dst_p->channel_8_current |= unpack_left_shift_u16(src_p[3], 4u, 0x3fu); + dst_p->channel_4_brownout = unpack_right_shift_u8(src_p[3], 6u, 0x40u); + dst_p->channel_5_brownout = unpack_right_shift_u8(src_p[3], 7u, 0x80u); + dst_p->channel_9_current = unpack_right_shift_u16(src_p[4], 0u, 0xffu); + dst_p->channel_9_current |= unpack_left_shift_u16(src_p[5], 8u, 0x03u); + dst_p->channel_10_current = unpack_right_shift_u16(src_p[5], 2u, 0xfcu); + dst_p->channel_10_current |= unpack_left_shift_u16(src_p[6], 6u, 0x0fu); + dst_p->channel_11_current = unpack_right_shift_u16(src_p[6], 4u, 0xf0u); + dst_p->channel_11_current |= unpack_left_shift_u16(src_p[7], 4u, 0x3fu); + dst_p->channel_6_brownout = unpack_right_shift_u8(src_p[7], 6u, 0x40u); + dst_p->channel_7_brownout = unpack_right_shift_u8(src_p[7], 7u, 0x80u); + + return (0); +} + +uint16_t PDH_status1_channel_6_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status1_channel_6_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status1_channel_6_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status1_channel_7_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status1_channel_7_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status1_channel_7_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status1_channel_8_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status1_channel_8_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status1_channel_8_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status1_channel_4_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status1_channel_4_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status1_channel_4_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status1_channel_5_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status1_channel_5_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status1_channel_5_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint16_t PDH_status1_channel_9_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status1_channel_9_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status1_channel_9_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status1_channel_10_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status1_channel_10_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status1_channel_10_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status1_channel_11_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status1_channel_11_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status1_channel_11_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status1_channel_6_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status1_channel_6_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status1_channel_6_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status1_channel_7_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status1_channel_7_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status1_channel_7_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status2_pack( + uint8_t *dst_p, + const struct PDH_status2_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->channel_12_current, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->channel_12_current, 8u, 0x03u); + dst_p[1] |= pack_left_shift_u16(src_p->channel_13_current, 2u, 0xfcu); + dst_p[2] |= pack_right_shift_u16(src_p->channel_13_current, 6u, 0x0fu); + dst_p[2] |= pack_left_shift_u16(src_p->channel_14_current, 4u, 0xf0u); + dst_p[3] |= pack_right_shift_u16(src_p->channel_14_current, 4u, 0x3fu); + dst_p[3] |= pack_left_shift_u8(src_p->channel_8_brownout, 6u, 0x40u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_9_brownout, 7u, 0x80u); + dst_p[4] |= pack_left_shift_u16(src_p->channel_15_current, 0u, 0xffu); + dst_p[5] |= pack_right_shift_u16(src_p->channel_15_current, 8u, 0x03u); + dst_p[5] |= pack_left_shift_u16(src_p->channel_16_current, 2u, 0xfcu); + dst_p[6] |= pack_right_shift_u16(src_p->channel_16_current, 6u, 0x0fu); + dst_p[6] |= pack_left_shift_u16(src_p->channel_17_current, 4u, 0xf0u); + dst_p[7] |= pack_right_shift_u16(src_p->channel_17_current, 4u, 0x3fu); + dst_p[7] |= pack_left_shift_u8(src_p->channel_10_brownout, 6u, 0x40u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_11_brownout, 7u, 0x80u); + + return (8); +} + +int PDH_status2_unpack( + struct PDH_status2_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_12_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->channel_12_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u); + dst_p->channel_13_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu); + dst_p->channel_13_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu); + dst_p->channel_14_current = unpack_right_shift_u16(src_p[2], 4u, 0xf0u); + dst_p->channel_14_current |= unpack_left_shift_u16(src_p[3], 4u, 0x3fu); + dst_p->channel_8_brownout = unpack_right_shift_u8(src_p[3], 6u, 0x40u); + dst_p->channel_9_brownout = unpack_right_shift_u8(src_p[3], 7u, 0x80u); + dst_p->channel_15_current = unpack_right_shift_u16(src_p[4], 0u, 0xffu); + dst_p->channel_15_current |= unpack_left_shift_u16(src_p[5], 8u, 0x03u); + dst_p->channel_16_current = unpack_right_shift_u16(src_p[5], 2u, 0xfcu); + dst_p->channel_16_current |= unpack_left_shift_u16(src_p[6], 6u, 0x0fu); + dst_p->channel_17_current = unpack_right_shift_u16(src_p[6], 4u, 0xf0u); + dst_p->channel_17_current |= unpack_left_shift_u16(src_p[7], 4u, 0x3fu); + dst_p->channel_10_brownout = unpack_right_shift_u8(src_p[7], 6u, 0x40u); + dst_p->channel_11_brownout = unpack_right_shift_u8(src_p[7], 7u, 0x80u); + + return (0); +} + +uint16_t PDH_status2_channel_12_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status2_channel_12_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status2_channel_12_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status2_channel_13_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status2_channel_13_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status2_channel_13_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status2_channel_14_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status2_channel_14_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status2_channel_14_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status2_channel_8_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status2_channel_8_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status2_channel_8_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status2_channel_9_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status2_channel_9_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status2_channel_9_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint16_t PDH_status2_channel_15_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status2_channel_15_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status2_channel_15_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status2_channel_16_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status2_channel_16_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status2_channel_16_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status2_channel_17_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status2_channel_17_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status2_channel_17_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status2_channel_10_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status2_channel_10_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status2_channel_10_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status2_channel_11_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status2_channel_11_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status2_channel_11_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status3_pack( + uint8_t *dst_p, + const struct PDH_status3_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->channel_18_current, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->channel_18_current, 8u, 0x03u); + dst_p[1] |= pack_left_shift_u16(src_p->channel_19_current, 2u, 0xfcu); + dst_p[2] |= pack_right_shift_u16(src_p->channel_19_current, 6u, 0x0fu); + dst_p[2] |= pack_left_shift_u8(src_p->channel_12_brownout, 4u, 0x10u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_13_brownout, 5u, 0x20u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_14_brownout, 6u, 0x40u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_15_brownout, 7u, 0x80u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_20_current, 0u, 0xffu); + dst_p[4] |= pack_left_shift_u8(src_p->channel_21_current, 0u, 0xffu); + dst_p[5] |= pack_left_shift_u8(src_p->channel_22_current, 0u, 0xffu); + dst_p[6] |= pack_left_shift_u8(src_p->channel_23_current, 0u, 0xffu); + dst_p[7] |= pack_left_shift_u8(src_p->channel_16_brownout, 0u, 0x01u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_17_brownout, 1u, 0x02u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_18_brownout, 2u, 0x04u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_19_brownout, 3u, 0x08u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_20_brownout, 4u, 0x10u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_21_brownout, 5u, 0x20u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_22_brownout, 6u, 0x40u); + dst_p[7] |= pack_left_shift_u8(src_p->channel_23_brownout, 7u, 0x80u); + + return (8); +} + +int PDH_status3_unpack( + struct PDH_status3_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_18_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->channel_18_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u); + dst_p->channel_19_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu); + dst_p->channel_19_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu); + dst_p->channel_12_brownout = unpack_right_shift_u8(src_p[2], 4u, 0x10u); + dst_p->channel_13_brownout = unpack_right_shift_u8(src_p[2], 5u, 0x20u); + dst_p->channel_14_brownout = unpack_right_shift_u8(src_p[2], 6u, 0x40u); + dst_p->channel_15_brownout = unpack_right_shift_u8(src_p[2], 7u, 0x80u); + dst_p->channel_20_current = unpack_right_shift_u8(src_p[3], 0u, 0xffu); + dst_p->channel_21_current = unpack_right_shift_u8(src_p[4], 0u, 0xffu); + dst_p->channel_22_current = unpack_right_shift_u8(src_p[5], 0u, 0xffu); + dst_p->channel_23_current = unpack_right_shift_u8(src_p[6], 0u, 0xffu); + dst_p->channel_16_brownout = unpack_right_shift_u8(src_p[7], 0u, 0x01u); + dst_p->channel_17_brownout = unpack_right_shift_u8(src_p[7], 1u, 0x02u); + dst_p->channel_18_brownout = unpack_right_shift_u8(src_p[7], 2u, 0x04u); + dst_p->channel_19_brownout = unpack_right_shift_u8(src_p[7], 3u, 0x08u); + dst_p->channel_20_brownout = unpack_right_shift_u8(src_p[7], 4u, 0x10u); + dst_p->channel_21_brownout = unpack_right_shift_u8(src_p[7], 5u, 0x20u); + dst_p->channel_22_brownout = unpack_right_shift_u8(src_p[7], 6u, 0x40u); + dst_p->channel_23_brownout = unpack_right_shift_u8(src_p[7], 7u, 0x80u); + + return (0); +} + +uint16_t PDH_status3_channel_18_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status3_channel_18_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status3_channel_18_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint16_t PDH_status3_channel_19_current_encode(double value) +{ + return (uint16_t)(value / 0.125); +} + +double PDH_status3_channel_19_current_decode(uint16_t value) +{ + return ((double)value * 0.125); +} + +bool PDH_status3_channel_19_current_is_in_range(uint16_t value) +{ + return (value <= 1023u); +} + +uint8_t PDH_status3_channel_12_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_12_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_12_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_13_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_13_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_13_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_14_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_14_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_14_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_15_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_15_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_15_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_20_current_encode(double value) +{ + return (uint8_t)(value / 0.0625); +} + +double PDH_status3_channel_20_current_decode(uint8_t value) +{ + return ((double)value * 0.0625); +} + +bool PDH_status3_channel_20_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_status3_channel_21_current_encode(double value) +{ + return (uint8_t)(value / 0.0625); +} + +double PDH_status3_channel_21_current_decode(uint8_t value) +{ + return ((double)value * 0.0625); +} + +bool PDH_status3_channel_21_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_status3_channel_22_current_encode(double value) +{ + return (uint8_t)(value / 0.0625); +} + +double PDH_status3_channel_22_current_decode(uint8_t value) +{ + return ((double)value * 0.0625); +} + +bool PDH_status3_channel_22_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_status3_channel_23_current_encode(double value) +{ + return (uint8_t)(value / 0.0625); +} + +double PDH_status3_channel_23_current_decode(uint8_t value) +{ + return ((double)value * 0.0625); +} + +bool PDH_status3_channel_23_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_status3_channel_16_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_16_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_16_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_17_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_17_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_17_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_18_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_18_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_18_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_19_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_19_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_19_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_20_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_20_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_20_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_21_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_21_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_21_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_22_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_22_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_22_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status3_channel_23_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status3_channel_23_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status3_channel_23_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PDH_status4_pack( + uint8_t *dst_p, + const struct PDH_status4_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u16(src_p->v_bus, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->v_bus, 8u, 0x0fu); + dst_p[1] |= pack_left_shift_u8(src_p->system_enable, 4u, 0x10u); + dst_p[1] |= pack_left_shift_u8(src_p->rsvd, 5u, 0xe0u); + dst_p[2] |= pack_left_shift_u8(src_p->brownout, 0u, 0x01u); + dst_p[2] |= pack_left_shift_u8(src_p->over_current, 1u, 0x02u); + dst_p[2] |= pack_left_shift_u8(src_p->can_warning, 2u, 0x04u); + dst_p[2] |= pack_left_shift_u8(src_p->hardware_fault, 3u, 0x08u); + dst_p[2] |= pack_left_shift_u8(src_p->sw_state, 4u, 0x10u); + dst_p[2] |= pack_left_shift_u8(src_p->sticky_brownout, 5u, 0x20u); + dst_p[2] |= pack_left_shift_u8(src_p->sticky_over_current, 6u, 0x40u); + dst_p[2] |= pack_left_shift_u8(src_p->sticky_can_warning, 7u, 0x80u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_can_bus_off, 0u, 0x01u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_hardware_fault, 1u, 0x02u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_firmware_fault, 2u, 0x04u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch20_brownout, 3u, 0x08u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch21_brownout, 4u, 0x10u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch22_brownout, 5u, 0x20u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch23_brownout, 6u, 0x40u); + dst_p[3] |= pack_left_shift_u8(src_p->sticky_has_reset, 7u, 0x80u); + dst_p[4] |= pack_left_shift_u8(src_p->total_current, 0u, 0xffu); + + return (8); +} + +int PDH_status4_unpack( + struct PDH_status4_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->v_bus = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->v_bus |= unpack_left_shift_u16(src_p[1], 8u, 0x0fu); + dst_p->system_enable = unpack_right_shift_u8(src_p[1], 4u, 0x10u); + dst_p->rsvd = unpack_right_shift_u8(src_p[1], 5u, 0xe0u); + dst_p->brownout = unpack_right_shift_u8(src_p[2], 0u, 0x01u); + dst_p->over_current = unpack_right_shift_u8(src_p[2], 1u, 0x02u); + dst_p->can_warning = unpack_right_shift_u8(src_p[2], 2u, 0x04u); + dst_p->hardware_fault = unpack_right_shift_u8(src_p[2], 3u, 0x08u); + dst_p->sw_state = unpack_right_shift_u8(src_p[2], 4u, 0x10u); + dst_p->sticky_brownout = unpack_right_shift_u8(src_p[2], 5u, 0x20u); + dst_p->sticky_over_current = unpack_right_shift_u8(src_p[2], 6u, 0x40u); + dst_p->sticky_can_warning = unpack_right_shift_u8(src_p[2], 7u, 0x80u); + dst_p->sticky_can_bus_off = unpack_right_shift_u8(src_p[3], 0u, 0x01u); + dst_p->sticky_hardware_fault = unpack_right_shift_u8(src_p[3], 1u, 0x02u); + dst_p->sticky_firmware_fault = unpack_right_shift_u8(src_p[3], 2u, 0x04u); + dst_p->sticky_ch20_brownout = unpack_right_shift_u8(src_p[3], 3u, 0x08u); + dst_p->sticky_ch21_brownout = unpack_right_shift_u8(src_p[3], 4u, 0x10u); + dst_p->sticky_ch22_brownout = unpack_right_shift_u8(src_p[3], 5u, 0x20u); + dst_p->sticky_ch23_brownout = unpack_right_shift_u8(src_p[3], 6u, 0x40u); + dst_p->sticky_has_reset = unpack_right_shift_u8(src_p[3], 7u, 0x80u); + dst_p->total_current = unpack_right_shift_u8(src_p[4], 0u, 0xffu); + + return (0); +} + +uint16_t PDH_status4_v_bus_encode(double value) +{ + return (uint16_t)(value / 0.0078125); +} + +double PDH_status4_v_bus_decode(uint16_t value) +{ + return ((double)value * 0.0078125); +} + +bool PDH_status4_v_bus_is_in_range(uint16_t value) +{ + return (value <= 4095u); +} + +uint8_t PDH_status4_system_enable_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_system_enable_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_system_enable_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_rsvd_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_rsvd_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_rsvd_is_in_range(uint8_t value) +{ + return (value <= 7u); +} + +uint8_t PDH_status4_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_over_current_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_over_current_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_over_current_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_can_warning_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_can_warning_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_can_warning_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_hardware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_hardware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_hardware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sw_state_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sw_state_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sw_state_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_over_current_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_over_current_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_over_current_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_can_warning_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_can_warning_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_can_warning_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_can_bus_off_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_can_bus_off_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_can_bus_off_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_hardware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_hardware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_hardware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_firmware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_firmware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_firmware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_ch20_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_ch20_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_ch20_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_ch21_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_ch21_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_ch21_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_ch22_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_ch22_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_ch22_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_ch23_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_ch23_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_ch23_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_sticky_has_reset_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_status4_sticky_has_reset_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_status4_sticky_has_reset_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PDH_status4_total_current_encode(double value) +{ + return (uint8_t)(value / 2.0); +} + +double PDH_status4_total_current_decode(uint8_t value) +{ + return ((double)value * 2.0); +} + +bool PDH_status4_total_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +int PDH_clear_faults_pack( + uint8_t *dst_p, + const struct PDH_clear_faults_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PDH_clear_faults_unpack( + struct PDH_clear_faults_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PDH_identify_pack( + uint8_t *dst_p, + const struct PDH_identify_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PDH_identify_unpack( + struct PDH_identify_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PDH_version_pack( + uint8_t *dst_p, + const struct PDH_version_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u8(src_p->firmware_fix, 0u, 0xffu); + dst_p[1] |= pack_left_shift_u8(src_p->firmware_minor, 0u, 0xffu); + dst_p[2] |= pack_left_shift_u8(src_p->firmware_year, 0u, 0xffu); + dst_p[3] |= pack_left_shift_u8(src_p->hardware_code, 0u, 0xffu); + dst_p[4] |= pack_left_shift_u32(src_p->unique_id, 0u, 0xffu); + dst_p[5] |= pack_right_shift_u32(src_p->unique_id, 8u, 0xffu); + dst_p[6] |= pack_right_shift_u32(src_p->unique_id, 16u, 0xffu); + dst_p[7] |= pack_right_shift_u32(src_p->unique_id, 24u, 0xffu); + + return (8); +} + +int PDH_version_unpack( + struct PDH_version_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->firmware_fix = unpack_right_shift_u8(src_p[0], 0u, 0xffu); + dst_p->firmware_minor = unpack_right_shift_u8(src_p[1], 0u, 0xffu); + dst_p->firmware_year = unpack_right_shift_u8(src_p[2], 0u, 0xffu); + dst_p->hardware_code = unpack_right_shift_u8(src_p[3], 0u, 0xffu); + dst_p->unique_id = unpack_right_shift_u32(src_p[4], 0u, 0xffu); + dst_p->unique_id |= unpack_left_shift_u32(src_p[5], 8u, 0xffu); + dst_p->unique_id |= unpack_left_shift_u32(src_p[6], 16u, 0xffu); + dst_p->unique_id |= unpack_left_shift_u32(src_p[7], 24u, 0xffu); + + return (0); +} + +uint8_t PDH_version_firmware_fix_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_version_firmware_fix_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_version_firmware_fix_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_version_firmware_minor_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_version_firmware_minor_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_version_firmware_minor_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_version_firmware_year_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_version_firmware_year_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_version_firmware_year_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PDH_version_hardware_code_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_version_hardware_code_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_version_hardware_code_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint32_t PDH_version_unique_id_encode(double value) +{ + return (uint32_t)(value); +} + +double PDH_version_unique_id_decode(uint32_t value) +{ + return ((double)value); +} + +bool PDH_version_unique_id_is_in_range(uint32_t value) +{ + (void)value; + + return (true); +} + +int PDH_configure_hr_channel_pack( + uint8_t *dst_p, + const struct PDH_configure_hr_channel_t *src_p, + size_t size) +{ + if (size < 3u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 3); + + dst_p[0] |= pack_left_shift_u8(src_p->channel, 0u, 0xffu); + dst_p[1] |= pack_left_shift_u16(src_p->period, 0u, 0xffu); + dst_p[2] |= pack_right_shift_u16(src_p->period, 8u, 0xffu); + + return (3); +} + +int PDH_configure_hr_channel_unpack( + struct PDH_configure_hr_channel_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 3u) { + return (-EINVAL); + } + + dst_p->channel = unpack_right_shift_u8(src_p[0], 0u, 0xffu); + dst_p->period = unpack_right_shift_u16(src_p[1], 0u, 0xffu); + dst_p->period |= unpack_left_shift_u16(src_p[2], 8u, 0xffu); + + return (0); +} + +uint8_t PDH_configure_hr_channel_channel_encode(double value) +{ + return (uint8_t)(value); +} + +double PDH_configure_hr_channel_channel_decode(uint8_t value) +{ + return ((double)value); +} + +bool PDH_configure_hr_channel_channel_is_in_range(uint8_t value) +{ + return (value <= 23u); +} + +uint16_t PDH_configure_hr_channel_period_encode(double value) +{ + return (uint16_t)(value); +} + +double PDH_configure_hr_channel_period_decode(uint16_t value) +{ + return ((double)value); +} + +bool PDH_configure_hr_channel_period_is_in_range(uint16_t value) +{ + (void)value; + + return (true); +} + +int PDH_enter_bootloader_pack( + uint8_t *dst_p, + const struct PDH_enter_bootloader_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PDH_enter_bootloader_unpack( + struct PDH_enter_bootloader_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} diff --git a/hal/src/main/native/athena/rev/PDHFrames.h b/hal/src/main/native/athena/rev/PDHFrames.h new file mode 100644 index 0000000000..86da2ed0bb --- /dev/null +++ b/hal/src/main/native/athena/rev/PDHFrames.h @@ -0,0 +1,3136 @@ +/** + * The MIT License (MIT) + * + * Copyright (c) 2018-2019 Erik Moqvist + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, + * modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/** + * This file was generated by cantools version 36.2.0 Fri Jul 9 14:22:19 2021. + */ + +#ifndef PDH_H +#define PDH_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#ifndef EINVAL +# define EINVAL 22 +#endif + +/* Frame ids. */ +#define PDH_SWITCH_CHANNEL_SET_FRAME_ID (0x8050840u) +#define PDH_STATUS0_FRAME_ID (0x8051800u) +#define PDH_STATUS1_FRAME_ID (0x8051840u) +#define PDH_STATUS2_FRAME_ID (0x8051880u) +#define PDH_STATUS3_FRAME_ID (0x80518c0u) +#define PDH_STATUS4_FRAME_ID (0x8051900u) +#define PDH_CLEAR_FAULTS_FRAME_ID (0x8051b80u) +#define PDH_IDENTIFY_FRAME_ID (0x8051d80u) +#define PDH_VERSION_FRAME_ID (0x8052600u) +#define PDH_CONFIGURE_HR_CHANNEL_FRAME_ID (0x80538c0u) +#define PDH_ENTER_BOOTLOADER_FRAME_ID (0x8057fc0u) + +/* Frame lengths in bytes. */ +#define PDH_SWITCH_CHANNEL_SET_LENGTH (1u) +#define PDH_STATUS0_LENGTH (8u) +#define PDH_STATUS1_LENGTH (8u) +#define PDH_STATUS2_LENGTH (8u) +#define PDH_STATUS3_LENGTH (8u) +#define PDH_STATUS4_LENGTH (8u) +#define PDH_CLEAR_FAULTS_LENGTH (0u) +#define PDH_IDENTIFY_LENGTH (0u) +#define PDH_VERSION_LENGTH (8u) +#define PDH_CONFIGURE_HR_CHANNEL_LENGTH (3u) +#define PDH_ENTER_BOOTLOADER_LENGTH (0u) + +/* Extended or standard frame types. */ +#define PDH_SWITCH_CHANNEL_SET_IS_EXTENDED (1) +#define PDH_STATUS0_IS_EXTENDED (1) +#define PDH_STATUS1_IS_EXTENDED (1) +#define PDH_STATUS2_IS_EXTENDED (1) +#define PDH_STATUS3_IS_EXTENDED (1) +#define PDH_STATUS4_IS_EXTENDED (1) +#define PDH_CLEAR_FAULTS_IS_EXTENDED (1) +#define PDH_IDENTIFY_IS_EXTENDED (1) +#define PDH_VERSION_IS_EXTENDED (1) +#define PDH_CONFIGURE_HR_CHANNEL_IS_EXTENDED (1) +#define PDH_ENTER_BOOTLOADER_IS_EXTENDED (1) + +/* Frame cycle times in milliseconds. */ + + +/* Signal choices. */ + + +/** + * Signals in message SwitchChannelSet. + * + * Set the switch channel output + * + * All signal values are as on the CAN bus. + */ +struct PDH_switch_channel_set_t { + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t output_set_value : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t use_system_enable : 1; +}; + +/** + * Signals in message Status0. + * + * Periodic status frame 0 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status0_t { + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_0_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_1_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_2_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_0_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1_brownout : 1; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_3_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_4_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_5_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3_brownout : 1; +}; + +/** + * Signals in message Status1. + * + * Periodic status frame 1 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status1_t { + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_6_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_7_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_8_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5_brownout : 1; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_9_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_10_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_11_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7_brownout : 1; +}; + +/** + * Signals in message Status2. + * + * Periodic status frame 2 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status2_t { + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_12_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_13_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_14_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9_brownout : 1; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_15_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_16_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_17_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11_brownout : 1; +}; + +/** + * Signals in message Status3. + * + * Periodic status frame 3 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status3_t { + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_18_current : 10; + + /** + * Range: 0..1023 (0..127.875 A) + * Scale: 0.125 + * Offset: 0 + */ + uint16_t channel_19_current : 10; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15_brownout : 1; + + /** + * Range: 0..255 (0..15.9375 A) + * Scale: 0.0625 + * Offset: 0 + */ + uint8_t channel_20_current : 8; + + /** + * Range: 0..255 (0..15.9375 A) + * Scale: 0.0625 + * Offset: 0 + */ + uint8_t channel_21_current : 8; + + /** + * Range: 0..511 (0..31.9375 A) + * Scale: 0.0625 + * Offset: 0 + */ + uint8_t channel_22_current : 8; + + /** + * Range: 0..511 (0..31.9375 A) + * Scale: 0.0625 + * Offset: 0 + */ + uint8_t channel_23_current : 8; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_16_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_17_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_18_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_19_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_20_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_21_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_22_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_23_brownout : 1; +}; + +/** + * Signals in message Status4. + * + * Periodic status frame 4 + * + * All signal values are as on the CAN bus. + */ +struct PDH_status4_t { + /** + * Range: 0..4095 (0..31.9921875 V) + * Scale: 0.0078125 + * Offset: 0 + */ + uint16_t v_bus : 12; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t system_enable : 1; + + /** + * Range: 0..7 (0..7 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t rsvd : 3; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t over_current : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t can_warning : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sw_state : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_over_current : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_can_warning : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_can_bus_off : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_hardware_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_firmware_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch20_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch21_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch22_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_ch23_brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_has_reset : 1; + + /** + * Range: 0..255 (0..510 A) + * Scale: 2 + * Offset: 0 + */ + uint8_t total_current : 8; +}; + +/** + * Signals in message ClearFaults. + * + * Clear sticky faults on the device + * + * All signal values are as on the CAN bus. + */ +struct PDH_clear_faults_t { + /** + * Dummy signal in empty message. + */ + uint8_t dummy; +}; + +/** + * Signals in message Identify. + * + * Flash the LED on the device to identify this device + * + * All signal values are as on the CAN bus. + */ +struct PDH_identify_t { + /** + * Dummy signal in empty message. + */ + uint8_t dummy; +}; + +/** + * Signals in message Version. + * + * Get the version of the PDH device + * + * All signal values are as on the CAN bus. + */ +struct PDH_version_t { + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_fix : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_minor : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_year : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_code : 8; + + /** + * Range: 0..4294967295 (0..4294967295 -) + * Scale: 1 + * Offset: 0 + */ + uint32_t unique_id : 32; +}; + +/** + * Signals in message ConfigureHRChannel. + * + * Configure a periodic high-resolution channel frame to send back data for a particular channel. This can be useful for more detailed diagnostics, or even for current based control or monitoring. + * + * All signal values are as on the CAN bus. + */ +struct PDH_configure_hr_channel_t { + /** + * Range: 0..23 (0..23 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel : 8; + + /** + * Range: 0..65535 (0..65535 -) + * Scale: 1 + * Offset: 0 + */ + uint16_t period : 16; +}; + +/** + * Signals in message Enter_Bootloader. + * + * Enter the REV bootloader from user application + * + * All signal values are as on the CAN bus. + */ +struct PDH_enter_bootloader_t { + /** + * Dummy signal in empty message. + */ + uint8_t dummy; +}; + +/** + * Pack message SwitchChannelSet. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_switch_channel_set_pack( + uint8_t *dst_p, + const struct PDH_switch_channel_set_t *src_p, + size_t size); + +/** + * Unpack message SwitchChannelSet. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_switch_channel_set_unpack( + struct PDH_switch_channel_set_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_switch_channel_set_output_set_value_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_switch_channel_set_output_set_value_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_switch_channel_set_output_set_value_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_switch_channel_set_use_system_enable_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_switch_channel_set_use_system_enable_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_switch_channel_set_use_system_enable_is_in_range(uint8_t value); + +/** + * Pack message Status0. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status0_pack( + uint8_t *dst_p, + const struct PDH_status0_t *src_p, + size_t size); + +/** + * Unpack message Status0. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status0_unpack( + struct PDH_status0_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status0_channel_0_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_0_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_0_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status0_channel_1_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_1_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_1_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status0_channel_2_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_2_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_2_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status0_channel_0_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_0_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_0_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status0_channel_1_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_1_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_1_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status0_channel_3_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_3_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_3_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status0_channel_4_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_4_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_4_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status0_channel_5_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_5_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_5_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status0_channel_2_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_2_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_2_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status0_channel_3_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status0_channel_3_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status0_channel_3_brownout_is_in_range(uint8_t value); + +/** + * Pack message Status1. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status1_pack( + uint8_t *dst_p, + const struct PDH_status1_t *src_p, + size_t size); + +/** + * Unpack message Status1. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status1_unpack( + struct PDH_status1_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status1_channel_6_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_6_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_6_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status1_channel_7_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_7_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_7_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status1_channel_8_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_8_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_8_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status1_channel_4_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_4_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_4_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status1_channel_5_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_5_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_5_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status1_channel_9_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_9_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_9_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status1_channel_10_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_10_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_10_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status1_channel_11_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_11_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_11_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status1_channel_6_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_6_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_6_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status1_channel_7_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status1_channel_7_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status1_channel_7_brownout_is_in_range(uint8_t value); + +/** + * Pack message Status2. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status2_pack( + uint8_t *dst_p, + const struct PDH_status2_t *src_p, + size_t size); + +/** + * Unpack message Status2. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status2_unpack( + struct PDH_status2_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status2_channel_12_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_12_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_12_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status2_channel_13_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_13_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_13_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status2_channel_14_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_14_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_14_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status2_channel_8_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_8_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_8_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status2_channel_9_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_9_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_9_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status2_channel_15_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_15_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_15_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status2_channel_16_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_16_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_16_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status2_channel_17_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_17_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_17_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status2_channel_10_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_10_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_10_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status2_channel_11_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status2_channel_11_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status2_channel_11_brownout_is_in_range(uint8_t value); + +/** + * Pack message Status3. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status3_pack( + uint8_t *dst_p, + const struct PDH_status3_t *src_p, + size_t size); + +/** + * Unpack message Status3. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status3_unpack( + struct PDH_status3_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status3_channel_18_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_18_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_18_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status3_channel_19_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_19_current_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_19_current_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_12_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_12_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_12_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_13_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_13_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_13_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_14_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_14_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_14_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_15_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_15_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_15_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_20_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_20_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_20_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_21_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_21_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_21_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_22_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_22_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_22_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_23_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_23_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_23_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_16_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_16_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_16_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_17_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_17_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_17_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_18_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_18_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_18_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_19_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_19_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_19_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_20_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_20_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_20_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_21_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_21_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_21_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_22_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_22_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_22_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status3_channel_23_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status3_channel_23_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status3_channel_23_brownout_is_in_range(uint8_t value); + +/** + * Pack message Status4. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_status4_pack( + uint8_t *dst_p, + const struct PDH_status4_t *src_p, + size_t size); + +/** + * Unpack message Status4. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_status4_unpack( + struct PDH_status4_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_status4_v_bus_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_v_bus_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_v_bus_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_system_enable_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_system_enable_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_system_enable_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_rsvd_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_rsvd_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_rsvd_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_over_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_over_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_over_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_can_warning_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_can_warning_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_can_warning_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_hardware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_hardware_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_hardware_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sw_state_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sw_state_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sw_state_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_over_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_over_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_over_current_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_can_warning_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_can_warning_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_can_warning_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_can_bus_off_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_can_bus_off_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_can_bus_off_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_hardware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_hardware_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_hardware_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_firmware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_firmware_fault_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_firmware_fault_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_ch20_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_ch20_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_ch20_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_ch21_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_ch21_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_ch21_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_ch22_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_ch22_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_ch22_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_ch23_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_ch23_brownout_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_ch23_brownout_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_sticky_has_reset_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_sticky_has_reset_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_sticky_has_reset_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_status4_total_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_status4_total_current_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_status4_total_current_is_in_range(uint8_t value); + +/** + * Pack message ClearFaults. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_clear_faults_pack( + uint8_t *dst_p, + const struct PDH_clear_faults_t *src_p, + size_t size); + +/** + * Unpack message ClearFaults. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_clear_faults_unpack( + struct PDH_clear_faults_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Pack message Identify. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_identify_pack( + uint8_t *dst_p, + const struct PDH_identify_t *src_p, + size_t size); + +/** + * Unpack message Identify. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_identify_unpack( + struct PDH_identify_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Pack message Version. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_version_pack( + uint8_t *dst_p, + const struct PDH_version_t *src_p, + size_t size); + +/** + * Unpack message Version. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_version_unpack( + struct PDH_version_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_version_firmware_fix_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_firmware_fix_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_firmware_fix_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_version_firmware_minor_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_firmware_minor_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_firmware_minor_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_version_firmware_year_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_firmware_year_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_firmware_year_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_version_hardware_code_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_hardware_code_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_hardware_code_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint32_t PDH_version_unique_id_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_version_unique_id_decode(uint32_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_version_unique_id_is_in_range(uint32_t value); + +/** + * Pack message ConfigureHRChannel. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_configure_hr_channel_pack( + uint8_t *dst_p, + const struct PDH_configure_hr_channel_t *src_p, + size_t size); + +/** + * Unpack message ConfigureHRChannel. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_configure_hr_channel_unpack( + struct PDH_configure_hr_channel_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PDH_configure_hr_channel_channel_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_configure_hr_channel_channel_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_configure_hr_channel_channel_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PDH_configure_hr_channel_period_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PDH_configure_hr_channel_period_decode(uint16_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PDH_configure_hr_channel_period_is_in_range(uint16_t value); + +/** + * Pack message Enter_Bootloader. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PDH_enter_bootloader_pack( + uint8_t *dst_p, + const struct PDH_enter_bootloader_t *src_p, + size_t size); + +/** + * Unpack message Enter_Bootloader. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PDH_enter_bootloader_unpack( + struct PDH_enter_bootloader_t *dst_p, + const uint8_t *src_p, + size_t size); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/hal/src/main/native/cpp/jni/PortsJNI.cpp b/hal/src/main/native/cpp/jni/PortsJNI.cpp index 71447a1729..2ac5a395f2 100644 --- a/hal/src/main/native/cpp/jni/PortsJNI.cpp +++ b/hal/src/main/native/cpp/jni/PortsJNI.cpp @@ -197,11 +197,11 @@ Java_edu_wpi_first_hal_PortsJNI_getNumRelayHeaders /* * Class: edu_wpi_first_hal_PortsJNI - * Method: getNumPCMModules + * Method: getNumCTREPCMModules * Signature: ()I */ JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_PortsJNI_getNumPCMModules +Java_edu_wpi_first_hal_PortsJNI_getNumCTREPCMModules (JNIEnv* env, jclass) { jint value = HAL_GetNumCTREPCMModules(); @@ -210,40 +210,66 @@ Java_edu_wpi_first_hal_PortsJNI_getNumPCMModules /* * Class: edu_wpi_first_hal_PortsJNI - * Method: getNumSolenoidChannels + * Method: getNumCTRESolenoidChannels * Signature: ()I */ JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_PortsJNI_getNumSolenoidChannels +Java_edu_wpi_first_hal_PortsJNI_getNumCTRESolenoidChannels (JNIEnv* env, jclass) { - jint value = HAL_GetNumSolenoidChannels(); + jint value = HAL_GetNumCTRESolenoidChannels(); return value; } /* * Class: edu_wpi_first_hal_PortsJNI - * Method: getNumPDPModules + * Method: getNumCTREPDPModules * Signature: ()I */ JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_PortsJNI_getNumPDPModules +Java_edu_wpi_first_hal_PortsJNI_getNumCTREPDPModules (JNIEnv* env, jclass) { - jint value = HAL_GetNumPDPModules(); + jint value = HAL_GetNumCTREPDPModules(); return value; } /* * Class: edu_wpi_first_hal_PortsJNI - * Method: getNumPDPChannels + * Method: getNumCTREPDPChannels * Signature: ()I */ JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_PortsJNI_getNumPDPChannels +Java_edu_wpi_first_hal_PortsJNI_getNumCTREPDPChannels (JNIEnv* env, jclass) { - jint value = HAL_GetNumPDPChannels(); + jint value = HAL_GetNumCTREPDPChannels(); + return value; +} + +/* + * Class: edu_wpi_first_hal_PortsJNI + * Method: getNumREVPDHModules + * Signature: ()I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_PortsJNI_getNumREVPDHModules + (JNIEnv* env, jclass) +{ + jint value = HAL_GetNumREVPDHModules(); + return value; +} + +/* + * Class: edu_wpi_first_hal_PortsJNI + * Method: getNumREVPDHChannels + * Signature: ()I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_PortsJNI_getNumREVPDHChannels + (JNIEnv* env, jclass) +{ + jint value = HAL_GetNumREVPDHChannels(); return value; } } // extern "C" diff --git a/hal/src/main/native/cpp/jni/PowerDistributionJNI.cpp b/hal/src/main/native/cpp/jni/PowerDistributionJNI.cpp index b9527330fd..b6084589a8 100644 --- a/hal/src/main/native/cpp/jni/PowerDistributionJNI.cpp +++ b/hal/src/main/native/cpp/jni/PowerDistributionJNI.cpp @@ -2,6 +2,10 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + +#include + #include "HALUtil.h" #include "edu_wpi_first_hal_PowerDistributionJNI.h" #include "hal/Ports.h" @@ -9,6 +13,15 @@ using namespace hal; +static_assert(edu_wpi_first_hal_PowerDistributionJNI_AUTOMATIC_TYPE == + HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic); +static_assert(edu_wpi_first_hal_PowerDistributionJNI_CTRE_TYPE == + HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE); +static_assert(edu_wpi_first_hal_PowerDistributionJNI_REV_TYPE == + HAL_PowerDistributionType::HAL_PowerDistributionType_kRev); +static_assert(edu_wpi_first_hal_PowerDistributionJNI_DEFAULT_MODULE == + HAL_DEFAULT_POWER_DISTRIBUTION_MODULE); + extern "C" { /* @@ -21,12 +34,41 @@ Java_edu_wpi_first_hal_PowerDistributionJNI_initialize (JNIEnv* env, jclass, jint module, jint type) { int32_t status = 0; + auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first"); auto handle = HAL_InitializePowerDistribution( - module, static_cast(type), &status); + module, static_cast(type), stack.c_str(), + &status); CheckStatusForceThrow(env, status); return static_cast(handle); } +/* + * Class: edu_wpi_first_hal_PowerDistributionJNI + * Method: free + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_PowerDistributionJNI_free + (JNIEnv*, jclass, jint handle) +{ + HAL_CleanPowerDistribution(handle); +} + +/* + * Class: edu_wpi_first_hal_PowerDistributionJNI + * Method: getModuleNumber + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_PowerDistributionJNI_getModuleNumber + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetPowerDistributionModuleNumber(handle, &status); + CheckStatus(env, status, false); + return result; +} + /* * Class: edu_wpi_first_hal_PowerDistributionJNI * Method: checkChannel @@ -67,6 +109,21 @@ Java_edu_wpi_first_hal_PowerDistributionJNI_getType return result; } +/* + * Class: edu_wpi_first_hal_PowerDistributionJNI + * Method: getNumChannels + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_PowerDistributionJNI_getNumChannels + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetPowerDistributionNumChannels(handle, &status); + CheckStatus(env, status); + return result; +} + /* * Class: edu_wpi_first_hal_PowerDistributionJNI * Method: getTemperature @@ -100,11 +157,11 @@ Java_edu_wpi_first_hal_PowerDistributionJNI_getVoltage /* * Class: edu_wpi_first_hal_PowerDistributionJNI * Method: getChannelCurrent - * Signature: (BI)D + * Signature: (II)D */ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_hal_PowerDistributionJNI_getChannelCurrent - (JNIEnv* env, jclass, jbyte channel, jint handle) + (JNIEnv* env, jclass, jint handle, jint channel) { int32_t status = 0; double current = @@ -206,4 +263,33 @@ Java_edu_wpi_first_hal_PowerDistributionJNI_clearStickyFaults CheckStatus(env, status, false); } +/* + * Class: edu_wpi_first_hal_PowerDistributionJNI + * Method: setSwitchableChannel + * Signature: (IZ)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_PowerDistributionJNI_setSwitchableChannel + (JNIEnv* env, jclass, jint handle, jboolean enabled) +{ + int32_t status = 0; + HAL_SetPowerDistributionSwitchableChannel(handle, enabled, &status); + CheckStatus(env, status, false); +} + +/* + * Class: edu_wpi_first_hal_PowerDistributionJNI + * Method: getSwitchableChannel + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_PowerDistributionJNI_getSwitchableChannel + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto state = HAL_GetPowerDistributionSwitchableChannel(handle, &status); + CheckStatus(env, status, false); + return state; +} + } // extern "C" diff --git a/hal/src/main/native/include/hal/Ports.h b/hal/src/main/native/include/hal/Ports.h index edf2d167ab..aac6f4a639 100644 --- a/hal/src/main/native/include/hal/Ports.h +++ b/hal/src/main/native/include/hal/Ports.h @@ -126,21 +126,35 @@ int32_t HAL_GetNumCTREPCMModules(void); * * @return the number of solenoid channels */ -int32_t HAL_GetNumSolenoidChannels(void); +int32_t HAL_GetNumCTRESolenoidChannels(void); /** * Gets the number of PDP modules in the current system. * * @return the number of PDP modules */ -int32_t HAL_GetNumPDPModules(void); +int32_t HAL_GetNumCTREPDPModules(void); /** * Gets the number of PDP channels in the current system. * * @return the number of PDP channels */ -int32_t HAL_GetNumPDPChannels(void); +int32_t HAL_GetNumCTREPDPChannels(void); + +/** + * Gets the number of PDH modules in the current system. + * + * @return the number of PDH modules + */ +int32_t HAL_GetNumREVPDHModules(void); + +/** + * Gets the number of PDH channels in the current system. + * + * @return the number of PDH channels + */ +int32_t HAL_GetNumREVPDHChannels(void); /** * Gets the number of duty cycle inputs in the current system. diff --git a/hal/src/main/native/include/hal/PowerDistribution.h b/hal/src/main/native/include/hal/PowerDistribution.h index e9ecf5e6e5..99032af412 100644 --- a/hal/src/main/native/include/hal/PowerDistribution.h +++ b/hal/src/main/native/include/hal/PowerDistribution.h @@ -26,6 +26,8 @@ HAL_ENUM(HAL_PowerDistributionType) { }; // clang-format on +#define HAL_DEFAULT_POWER_DISTRIBUTION_MODULE -1 + #ifdef __cplusplus extern "C" { #endif @@ -38,7 +40,17 @@ extern "C" { * @return the created PowerDistribution */ HAL_PowerDistributionHandle HAL_InitializePowerDistribution( - int32_t moduleNumber, HAL_PowerDistributionType type, int32_t* status); + int32_t moduleNumber, HAL_PowerDistributionType type, + const char* allocationLocation, int32_t* status); + +/** + * Gets the module number for a specific handle. + * + * @param handle the module handle + * @return the module number + */ +int32_t HAL_GetPowerDistributionModuleNumber(HAL_PowerDistributionHandle handle, + int32_t* status); /** * Cleans a PowerDistribution module. @@ -74,6 +86,15 @@ HAL_Bool HAL_CheckPowerDistributionModule(int32_t module, HAL_PowerDistributionType HAL_GetPowerDistributionType( HAL_PowerDistributionHandle handle, int32_t* status); +/** + * Gets the number of channels for this handle. + * + * @param handle the handle + * @return number of channels + */ +int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle, + int32_t* status); + /** * Gets the temperature of the PowerDistribution. * @@ -103,7 +124,7 @@ double HAL_GetPowerDistributionChannelCurrent( HAL_PowerDistributionHandle handle, int32_t channel, int32_t* status); /** - * Gets the current of all 16 channels on the PowerDistribution. + * Gets the current of all 24 channels on the PowerDistribution. * * The array must be large enough to hold all channels. * @@ -157,6 +178,14 @@ void HAL_ResetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle, */ void HAL_ClearPowerDistributionStickyFaults(HAL_PowerDistributionHandle handle, int32_t* status); + +// REV PDH Specific functions. This functions will no-op on CTRE PDP +void HAL_SetPowerDistributionSwitchableChannel( + HAL_PowerDistributionHandle handle, HAL_Bool enabled, int32_t* status); + +HAL_Bool HAL_GetPowerDistributionSwitchableChannel( + HAL_PowerDistributionHandle handle, int32_t* status); + #ifdef __cplusplus } // extern "C" #endif diff --git a/hal/src/main/native/include/hal/Types.h b/hal/src/main/native/include/hal/Types.h index ec9f739af2..d5a341c54e 100644 --- a/hal/src/main/native/include/hal/Types.h +++ b/hal/src/main/native/include/hal/Types.h @@ -66,6 +66,8 @@ typedef HAL_Handle HAL_PowerDistributionHandle; typedef HAL_Handle HAL_CTREPCMHandle; +typedef HAL_Handle HAL_REVPDHHandle; + typedef int32_t HAL_Bool; #ifdef __cplusplus diff --git a/hal/src/main/native/include/hal/handles/HandlesInternal.h b/hal/src/main/native/include/hal/handles/HandlesInternal.h index c37a1ab229..2c9c44cfe7 100644 --- a/hal/src/main/native/include/hal/handles/HandlesInternal.h +++ b/hal/src/main/native/include/hal/handles/HandlesInternal.h @@ -67,7 +67,8 @@ enum class HAL_HandleEnum { DMA = 22, AddressableLED = 23, CTREPCM = 24, - CTREPDP = 25 + CTREPDP = 25, + REVPDH = 26 }; /** diff --git a/hal/src/main/native/include/hal/simulation/PowerDistributionData.h b/hal/src/main/native/include/hal/simulation/PowerDistributionData.h index c355bfd042..0552c49038 100644 --- a/hal/src/main/native/include/hal/simulation/PowerDistributionData.h +++ b/hal/src/main/native/include/hal/simulation/PowerDistributionData.h @@ -46,9 +46,10 @@ double HALSIM_GetPowerDistributionCurrent(int32_t index, int32_t channel); void HALSIM_SetPowerDistributionCurrent(int32_t index, int32_t channel, double current); -void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents); +void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents, + int length); void HALSIM_SetPowerDistributionAllCurrents(int32_t index, - const double* currents); + const double* currents, int length); void HALSIM_RegisterPowerDistributionAllNonCurrentCallbacks( int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param, diff --git a/hal/src/main/native/sim/Ports.cpp b/hal/src/main/native/sim/Ports.cpp index 4b39812306..77cb22d246 100644 --- a/hal/src/main/native/sim/Ports.cpp +++ b/hal/src/main/native/sim/Ports.cpp @@ -58,14 +58,20 @@ int32_t HAL_GetNumRelayHeaders(void) { int32_t HAL_GetNumCTREPCMModules(void) { return kNumCTREPCMModules; } -int32_t HAL_GetNumSolenoidChannels(void) { +int32_t HAL_GetNumCTRESolenoidChannels(void) { return kNumCTRESolenoidChannels; } -int32_t HAL_GetNumPDPModules(void) { - return kNumPDPModules; +int32_t HAL_GetNumCTREPDPModules(void) { + return kNumCTREPDPModules; } -int32_t HAL_GetNumPDPChannels(void) { - return kNumPDPChannels; +int32_t HAL_GetNumCTREPDPChannels(void) { + return kNumCTREPDPChannels; +} +int32_t HAL_GetNumREVPDHModules(void) { + return kNumREVPDHModules; +} +int32_t HAL_GetNumREVPDHChannels(void) { + return kNumREVPDHChannels; } int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; diff --git a/hal/src/main/native/sim/PortsInternal.h b/hal/src/main/native/sim/PortsInternal.h index c0464032d2..ba219c2f4c 100644 --- a/hal/src/main/native/sim/PortsInternal.h +++ b/hal/src/main/native/sim/PortsInternal.h @@ -23,8 +23,10 @@ constexpr int32_t kNumRelayChannels = 8; constexpr int32_t kNumRelayHeaders = kNumRelayChannels / 2; constexpr int32_t kNumCTREPCMModules = 63; constexpr int32_t kNumCTRESolenoidChannels = 8; -constexpr int32_t kNumPDPModules = 63; -constexpr int32_t kNumPDPChannels = 16; +constexpr int32_t kNumCTREPDPModules = 63; +constexpr int32_t kNumCTREPDPChannels = 16; +constexpr int32_t kNumREVPDHModules = 63; +constexpr int32_t kNumREVPDHChannels = 24; constexpr int32_t kNumDutyCycles = 8; constexpr int32_t kNumAddressableLEDs = 1; } // namespace hal diff --git a/hal/src/main/native/sim/PowerDistribution.cpp b/hal/src/main/native/sim/PowerDistribution.cpp index 3a37cc7d0b..85646cfab9 100644 --- a/hal/src/main/native/sim/PowerDistribution.cpp +++ b/hal/src/main/native/sim/PowerDistribution.cpp @@ -28,7 +28,8 @@ void InitializePowerDistribution() {} extern "C" { HAL_PowerDistributionHandle HAL_InitializePowerDistribution( - int32_t module, HAL_PowerDistributionType type, int32_t* status) { + int32_t module, HAL_PowerDistributionType type, + const char* allocationLocation, int32_t* status) { if (!HAL_CheckPowerDistributionModule(module, type)) { *status = PARAMETER_OUT_OF_RANGE; hal::SetLastError(status, fmt::format("Invalid pdp module {}", module)); @@ -46,14 +47,32 @@ HAL_PowerDistributionHandle HAL_InitializePowerDistribution( return handle; } +int32_t HAL_GetPowerDistributionModuleNumber(HAL_PowerDistributionHandle handle, + int32_t* status) { + auto module = hal::can::GetCANModuleFromHandle(handle, status); + if (*status != 0) { + return 0; + } + return module; +} + HAL_Bool HAL_CheckPowerDistributionModule(int32_t module, HAL_PowerDistributionType type) { - return module < kNumPDPModules && module >= 0; + if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) { + return module < kNumCTREPDPModules && module >= 0; + } else { + return module < kNumREVPDHModules && module >= 1; + } } HAL_Bool HAL_CheckPowerDistributionChannel(HAL_PowerDistributionHandle handle, int32_t channel) { - return channel < kNumPDPChannels && channel >= 0; + // TODO make this grab from the handle directly + if (false) { + return channel < kNumCTREPDPChannels && channel >= 0; + } else { + return channel < kNumREVPDHChannels && channel >= 0; + } } HAL_PowerDistributionType HAL_GetPowerDistributionType( @@ -61,6 +80,16 @@ HAL_PowerDistributionType HAL_GetPowerDistributionType( return HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE; } +int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle, + int32_t* status) { + // TODO make this grab from the handle directly + if (false) { + return kNumCTREPDPChannels; + } else { + return kNumREVPDHChannels; + } +} + void HAL_CleanPowerDistribution(HAL_PowerDistributionHandle handle) { HAL_CleanCAN(handle); } @@ -98,7 +127,8 @@ void HAL_GetPowerDistributionAllChannelCurrents( } auto& data = SimPowerDistributionData[module]; - for (int i = 0; i < kNumPDPChannels; i++) { + int toCopy = (std::min)(currentsLength, kNumPDSimChannels); + for (int i = 0; i < toCopy; i++) { currents[i] = data.current[i]; } } @@ -118,4 +148,11 @@ void HAL_ResetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle, int32_t* status) {} void HAL_ClearPowerDistributionStickyFaults(HAL_PowerDistributionHandle handle, int32_t* status) {} +void HAL_SetPowerDistributionSwitchableChannel( + HAL_PowerDistributionHandle handle, HAL_Bool state, int32_t* status) {} + +HAL_Bool HAL_GetPowerDistributionSwitchableChannel( + HAL_PowerDistributionHandle handle, int32_t* status) { + return false; +} } // extern "C" diff --git a/hal/src/main/native/sim/mockdata/PowerDistributionData.cpp b/hal/src/main/native/sim/mockdata/PowerDistributionData.cpp index 4234e49e87..9c5b82644c 100644 --- a/hal/src/main/native/sim/mockdata/PowerDistributionData.cpp +++ b/hal/src/main/native/sim/mockdata/PowerDistributionData.cpp @@ -9,7 +9,7 @@ using namespace hal; namespace hal::init { void InitializePowerDistributionData() { - static PowerDistributionData spd[kNumPDPChannels]; + static PowerDistributionData spd[kNumPDSimModules]; ::hal::SimPowerDistributionData = spd; } } // namespace hal::init @@ -19,7 +19,7 @@ void PowerDistributionData::ResetData() { initialized.Reset(false); temperature.Reset(0.0); voltage.Reset(12.0); - for (int i = 0; i < kNumPDPChannels; i++) { + for (int i = 0; i < kNumPDSimChannels; i++) { current[i].Reset(0.0); } } @@ -39,17 +39,21 @@ DEFINE_CAPI(double, Voltage, voltage) HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(double, HALSIM, PowerDistributionCurrent, SimPowerDistributionData, current) -void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents) { +void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents, + int length) { auto& data = SimPowerDistributionData[index].current; - for (int i = 0; i < kNumPDPChannels; i++) { + int toCopy = (std::min)(length, kNumPDSimChannels); + for (int i = 0; i < toCopy; i++) { currents[i] = data[i]; } } void HALSIM_SetPowerDistributionAllCurrents(int32_t index, - const double* currents) { + const double* currents, + int length) { auto& data = SimPowerDistributionData[index].current; - for (int i = 0; i < kNumPDPChannels; i++) { + int toCopy = (std::min)(length, kNumPDSimChannels); + for (int i = 0; i < toCopy; i++) { data[i] = currents[i]; } } diff --git a/hal/src/main/native/sim/mockdata/PowerDistributionDataInternal.h b/hal/src/main/native/sim/mockdata/PowerDistributionDataInternal.h index 49a1f87cc8..4876dbdfda 100644 --- a/hal/src/main/native/sim/mockdata/PowerDistributionDataInternal.h +++ b/hal/src/main/native/sim/mockdata/PowerDistributionDataInternal.h @@ -9,6 +9,9 @@ #include "hal/simulation/SimDataValue.h" namespace hal { +constexpr int32_t kNumPDSimModules = hal::kNumREVPDHModules; +constexpr int32_t kNumPDSimChannels = hal::kNumREVPDHChannels; + class PowerDistributionData { HAL_SIMDATAVALUE_DEFINE_NAME(Initialized) HAL_SIMDATAVALUE_DEFINE_NAME(Temperature) @@ -25,7 +28,7 @@ class PowerDistributionData { SimDataValue temperature{0.0}; SimDataValue voltage{12.0}; SimDataValue - current[kNumPDPChannels]; + current[kNumPDSimChannels]; virtual void ResetData(); }; diff --git a/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp b/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp index 6ac1e7f04c..faa58e9839 100644 --- a/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp +++ b/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp @@ -31,8 +31,8 @@ TEST(PdpSimTests, TestPdpInitialization) { // Use out of range index gTestPdpCallbackName = "Unset"; - HAL_InitializePowerDistribution(INDEX_TO_TEST, - HAL_PowerDistributionType_kCTRE, &status); + HAL_InitializePowerDistribution( + INDEX_TO_TEST, HAL_PowerDistributionType_kCTRE, nullptr, &status); EXPECT_EQ(0, status); EXPECT_STREQ("Initialized", gTestPdpCallbackName.c_str()); } diff --git a/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.cpp b/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.cpp index 735ec6e97a..bcc510a41a 100644 --- a/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.cpp @@ -95,7 +95,7 @@ class PCMSimModel : public glass::PCMModel { explicit PCMSimModel(int32_t index) : m_index{index}, m_compressor{index}, - m_solenoids(HAL_GetNumSolenoidChannels()) {} + m_solenoids(HAL_GetNumCTRESolenoidChannels()) {} void Update() override; diff --git a/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.cpp b/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.cpp index 660dd4a24c..c511345cf3 100644 --- a/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.cpp @@ -31,7 +31,8 @@ class PowerDistributionSimModel : public glass::PowerDistributionModel { public: explicit PowerDistributionSimModel(int32_t index) : m_index{index}, m_temp{index}, m_voltage{index} { - const int numChannels = HAL_GetNumPDPChannels(); + // TODO make this better + const int numChannels = HAL_GetNumREVPDHChannels(); m_currents.reserve(numChannels); for (int i = 0; i < numChannels; ++i) { m_currents.emplace_back( @@ -72,7 +73,7 @@ class PowerDistributionSimModel : public glass::PowerDistributionModel { class PowerDistributionsSimModel : public glass::PowerDistributionsModel { public: - PowerDistributionsSimModel() : m_models(HAL_GetNumPDPModules()) {} + PowerDistributionsSimModel() : m_models(HAL_GetNumREVPDHModules()) {} void Update() override; @@ -113,7 +114,7 @@ void PowerDistributionsSimModel::ForEachPowerDistribution( } static bool PowerDistributionsAnyInitialized() { - static const int32_t num = HAL_GetNumPDPModules(); + static const int32_t num = HAL_GetNumREVPDHModules(); for (int32_t i = 0; i < num; ++i) { if (HALSIM_GetPowerDistributionInitialized(i)) { return true; diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Solenoid.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Solenoid.cpp index c376d3a59d..a26a4e59ab 100644 --- a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Solenoid.cpp +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Solenoid.cpp @@ -22,7 +22,7 @@ void HALSimWSProviderSolenoid::Initialize(WSRegisterFunc webRegisterFunc) { for (int32_t CTREPCMIndex = 0; CTREPCMIndex < HAL_GetNumCTREPCMModules(); ++CTREPCMIndex) { for (int32_t solenoidIndex = 0; - solenoidIndex < HAL_GetNumSolenoidChannels(); ++solenoidIndex) { + solenoidIndex < HAL_GetNumCTRESolenoidChannels(); ++solenoidIndex) { auto key = fmt::format("Solenoid/{},{}", CTREPCMIndex, solenoidIndex); auto ptr = std::make_unique( CTREPCMIndex, solenoidIndex, key, "Solenoid"); diff --git a/wpilibc/src/main/native/cpp/PowerDistribution.cpp b/wpilibc/src/main/native/cpp/PowerDistribution.cpp new file mode 100644 index 0000000000..4c2acfcf5b --- /dev/null +++ b/wpilibc/src/main/native/cpp/PowerDistribution.cpp @@ -0,0 +1,146 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/PowerDistribution.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "frc/Errors.h" +#include "frc/SensorUtil.h" + +static_assert(static_cast( + frc::PowerDistribution::ModuleType::kAutomatic) == + HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic); +static_assert(static_cast( + frc::PowerDistribution::ModuleType::kCTRE) == + HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE); +static_assert(static_cast( + frc::PowerDistribution::ModuleType::kRev) == + HAL_PowerDistributionType::HAL_PowerDistributionType_kRev); +static_assert(frc::PowerDistribution::kDefaultModule == + HAL_DEFAULT_POWER_DISTRIBUTION_MODULE); + +using namespace frc; + +PowerDistribution::PowerDistribution() + : PowerDistribution(-1, ModuleType::kAutomatic) {} + +PowerDistribution::PowerDistribution(int module, ModuleType moduleType) { + auto stack = wpi::GetStackTrace(1); + + int32_t status = 0; + m_handle = HAL_InitializePowerDistribution( + module, static_cast(moduleType), stack.c_str(), + &status); + FRC_CheckErrorStatus(status, "Module {}", module); + m_module = HAL_GetPowerDistributionModuleNumber(m_handle, &status); + FRC_ReportError(status, "Module {}", module); + + HAL_Report(HALUsageReporting::kResourceType_PDP, m_module + 1); + wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module); +} + +PowerDistribution::~PowerDistribution() { + if (m_handle != HAL_kInvalidHandle) { + HAL_CleanPowerDistribution(m_handle); + m_handle = HAL_kInvalidHandle; + } +} + +double PowerDistribution::GetVoltage() const { + int32_t status = 0; + double voltage = HAL_GetPowerDistributionVoltage(m_handle, &status); + FRC_ReportError(status, "Module {}", m_module); + return voltage; +} + +double PowerDistribution::GetTemperature() const { + int32_t status = 0; + double temperature = HAL_GetPowerDistributionTemperature(m_handle, &status); + FRC_ReportError(status, "Module {}", m_module); + return temperature; +} + +double PowerDistribution::GetCurrent(int channel) const { + int32_t status = 0; + double current = + HAL_GetPowerDistributionChannelCurrent(m_handle, channel, &status); + FRC_ReportError(status, "Module {} Channel {}", m_module, channel); + + return current; +} + +double PowerDistribution::GetTotalCurrent() const { + int32_t status = 0; + double current = HAL_GetPowerDistributionTotalCurrent(m_handle, &status); + FRC_ReportError(status, "Module {}", m_module); + return current; +} + +double PowerDistribution::GetTotalPower() const { + int32_t status = 0; + double power = HAL_GetPowerDistributionTotalPower(m_handle, &status); + FRC_ReportError(status, "Module {}", m_module); + return power; +} + +double PowerDistribution::GetTotalEnergy() const { + int32_t status = 0; + double energy = HAL_GetPowerDistributionTotalEnergy(m_handle, &status); + FRC_ReportError(status, "Module {}", m_module); + return energy; +} + +void PowerDistribution::ResetTotalEnergy() { + int32_t status = 0; + HAL_ResetPowerDistributionTotalEnergy(m_handle, &status); + FRC_ReportError(status, "Module {}", m_module); +} + +void PowerDistribution::ClearStickyFaults() { + int32_t status = 0; + HAL_ClearPowerDistributionStickyFaults(m_handle, &status); + FRC_ReportError(status, "Module {}", m_module); +} + +int PowerDistribution::GetModule() const { + return m_module; +} + +bool PowerDistribution::GetSwitchableChannel() const { + int32_t status = 0; + bool state = HAL_GetPowerDistributionSwitchableChannel(m_handle, &status); + FRC_ReportError(status, "Module {}", m_module); + return state; +} + +void PowerDistribution::SetSwitchableChannel(bool enabled) { + int32_t status = 0; + HAL_SetPowerDistributionSwitchableChannel(m_handle, enabled, &status); + FRC_ReportError(status, "Module {}", m_module); +} + +void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) { + builder.SetSmartDashboardType("PowerDistribution"); + int32_t status = 0; + int numChannels = HAL_GetPowerDistributionNumChannels(m_handle, &status); + FRC_ReportError(status, "Module {}", m_module); + for (int i = 0; i < numChannels; ++i) { + builder.AddDoubleProperty( + fmt::format("Chan{}", i), [=] { return GetCurrent(i); }, nullptr); + } + builder.AddDoubleProperty( + "Voltage", [=] { return GetVoltage(); }, nullptr); + builder.AddDoubleProperty( + "TotalCurrent", [=] { return GetTotalCurrent(); }, nullptr); + builder.AddBooleanProperty( + "SwitchableChannel", [=] { return GetSwitchableChannel(); }, + [=](bool value) { SetSwitchableChannel(value); }); +} diff --git a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp deleted file mode 100644 index cca7916bb5..0000000000 --- a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include -#include -#include -#include -#include -#include - -#include "frc/Errors.h" -#include "frc/PowerDistribution.h" -#include "frc/SensorUtil.h" - -using namespace frc; - -PowerDistribution::PowerDistribution() : PowerDistribution(0) {} - -PowerDistribution::PowerDistribution(int module) : m_module(module) { - int32_t status = 0; - m_handle = HAL_InitializePowerDistribution( - module, HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic, - &status); - FRC_CheckErrorStatus(status, "Module {}", module); - - HAL_Report(HALUsageReporting::kResourceType_PDP, module + 1); - wpi::SendableRegistry::AddLW(this, "PowerDistribution", module); -} - -double PowerDistribution::GetVoltage() const { - int32_t status = 0; - double voltage = HAL_GetPowerDistributionVoltage(m_handle, &status); - FRC_CheckErrorStatus(status, "Module {}", m_module); - return voltage; -} - -double PowerDistribution::GetTemperature() const { - int32_t status = 0; - double temperature = HAL_GetPowerDistributionTemperature(m_handle, &status); - FRC_CheckErrorStatus(status, "Module {}", m_module); - return temperature; -} - -double PowerDistribution::GetCurrent(int channel) const { - int32_t status = 0; - - if (!HAL_CheckPowerDistributionChannel(m_handle, channel)) { - FRC_ReportError(err::ChannelIndexOutOfRange, "Module {} Channel {}", - m_module, channel); - return 0; - } - - double current = - HAL_GetPowerDistributionChannelCurrent(m_handle, channel, &status); - FRC_CheckErrorStatus(status, "Module {} Channel {}", m_module, channel); - - return current; -} - -double PowerDistribution::GetTotalCurrent() const { - int32_t status = 0; - double current = HAL_GetPowerDistributionTotalCurrent(m_handle, &status); - FRC_CheckErrorStatus(status, "Module {}", m_module); - return current; -} - -double PowerDistribution::GetTotalPower() const { - int32_t status = 0; - double power = HAL_GetPowerDistributionTotalPower(m_handle, &status); - FRC_CheckErrorStatus(status, "Module {}", m_module); - return power; -} - -double PowerDistribution::GetTotalEnergy() const { - int32_t status = 0; - double energy = HAL_GetPowerDistributionTotalEnergy(m_handle, &status); - FRC_CheckErrorStatus(status, "Module {}", m_module); - return energy; -} - -void PowerDistribution::ResetTotalEnergy() { - int32_t status = 0; - HAL_ResetPowerDistributionTotalEnergy(m_handle, &status); - FRC_CheckErrorStatus(status, "Module {}", m_module); -} - -void PowerDistribution::ClearStickyFaults() { - int32_t status = 0; - HAL_ClearPowerDistributionStickyFaults(m_handle, &status); - FRC_CheckErrorStatus(status, "Module {}", m_module); -} - -int PowerDistribution::GetModule() const { - return m_module; -} - -void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) { - builder.SetSmartDashboardType("PowerDistribution"); - for (int i = 0; i < SensorUtil::kPDPChannels; ++i) { - builder.AddDoubleProperty( - fmt::format("Chan{}", i), [=] { return GetCurrent(i); }, nullptr); - } - builder.AddDoubleProperty( - "Voltage", [=] { return GetVoltage(); }, nullptr); - builder.AddDoubleProperty( - "TotalCurrent", [=] { return GetTotalCurrent(); }, nullptr); -} diff --git a/wpilibc/src/main/native/cpp/SensorUtil.cpp b/wpilibc/src/main/native/cpp/SensorUtil.cpp index c25ecfa36e..429eb67b05 100644 --- a/wpilibc/src/main/native/cpp/SensorUtil.cpp +++ b/wpilibc/src/main/native/cpp/SensorUtil.cpp @@ -16,11 +16,8 @@ using namespace frc; const int SensorUtil::kDigitalChannels = HAL_GetNumDigitalChannels(); const int SensorUtil::kAnalogInputs = HAL_GetNumAnalogInputs(); const int SensorUtil::kAnalogOutputs = HAL_GetNumAnalogOutputs(); -const int SensorUtil::kSolenoidChannels = HAL_GetNumSolenoidChannels(); -const int SensorUtil::kSolenoidModules = HAL_GetNumCTREPCMModules(); const int SensorUtil::kPwmChannels = HAL_GetNumPWMChannels(); const int SensorUtil::kRelayChannels = HAL_GetNumRelayHeaders(); -const int SensorUtil::kPDPChannels = HAL_GetNumPDPChannels(); int SensorUtil::GetDefaultCTREPCMModule() { return 0; diff --git a/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp b/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp index 23b9bc2f2e..844a1b677f 100644 --- a/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp @@ -92,12 +92,12 @@ void PowerDistributionSim::SetCurrent(int channel, double current) { HALSIM_SetPowerDistributionCurrent(m_index, channel, current); } -void PowerDistributionSim::GetAllCurrents(double* currents) const { - HALSIM_GetPowerDistributionAllCurrents(m_index, currents); +void PowerDistributionSim::GetAllCurrents(double* currents, int length) const { + HALSIM_GetPowerDistributionAllCurrents(m_index, currents, length); } -void PowerDistributionSim::SetAllCurrents(const double* currents) { - HALSIM_SetPowerDistributionAllCurrents(m_index, currents); +void PowerDistributionSim::SetAllCurrents(const double* currents, int length) { + HALSIM_SetPowerDistributionAllCurrents(m_index, currents, length); } void PowerDistributionSim::ResetData() { diff --git a/wpilibc/src/main/native/include/frc/PowerDistribution.h b/wpilibc/src/main/native/include/frc/PowerDistribution.h index d1e3883258..2863daf08b 100644 --- a/wpilibc/src/main/native/include/frc/PowerDistribution.h +++ b/wpilibc/src/main/native/include/frc/PowerDistribution.h @@ -17,10 +17,13 @@ namespace frc { class PowerDistribution : public wpi::Sendable, public wpi::SendableHelper { public: + static constexpr int kDefaultModule = -1; + enum class ModuleType { kAutomatic = 0, kCTRE = 1, kRev = 2 }; + /** * Constructs a PowerDistribution. * - * Uses the default CAN ID (0). + * Uses the default CAN ID. */ PowerDistribution(); @@ -28,9 +31,11 @@ class PowerDistribution : public wpi::Sendable, * Constructs a PowerDistribution. * * @param module The CAN ID of the PDP + * @param moduleType The type of module */ - explicit PowerDistribution(int module); + PowerDistribution(int module, ModuleType moduleType); + ~PowerDistribution() override; PowerDistribution(PowerDistribution&&) = default; PowerDistribution& operator=(PowerDistribution&&) = default; @@ -93,10 +98,14 @@ class PowerDistribution : public wpi::Sendable, */ int GetModule() const; + bool GetSwitchableChannel() const; + + void SetSwitchableChannel(bool enabled); + void InitSendable(wpi::SendableBuilder& builder) override; private: - hal::Handle m_handle; + hal::Handle m_handle; int m_module; }; diff --git a/wpilibc/src/main/native/include/frc/SensorUtil.h b/wpilibc/src/main/native/include/frc/SensorUtil.h index a7f41263a6..9f8bf3a2ed 100644 --- a/wpilibc/src/main/native/include/frc/SensorUtil.h +++ b/wpilibc/src/main/native/include/frc/SensorUtil.h @@ -71,29 +71,11 @@ class SensorUtil final { */ static bool CheckAnalogOutputChannel(int channel); - /** - * Verify that the power distribution channel number is within limits. - * - * @return PDP channel is valid - */ - static bool CheckPDPChannel(int channel); - - /** - * Verify that the PDP module number is within limits. module numbers are - * 0-based - * - * @return PDP module is valid - */ - static bool CheckPDPModule(int module); - static const int kDigitalChannels; static const int kAnalogInputs; static const int kAnalogOutputs; - static const int kSolenoidChannels; - static const int kSolenoidModules; static const int kPwmChannels; static const int kRelayChannels; - static const int kPDPChannels; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/simulation/PowerDistributionSim.h b/wpilibc/src/main/native/include/frc/simulation/PowerDistributionSim.h index 5b0e90bbbe..34891c5f8d 100644 --- a/wpilibc/src/main/native/include/frc/simulation/PowerDistributionSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/PowerDistributionSim.h @@ -142,7 +142,7 @@ class PowerDistributionSim { * array must be big enough to hold all the PowerDistribution * channels */ - void GetAllCurrents(double* currents) const; + void GetAllCurrents(double* currents, int length) const; /** * Change the current in all of the PowerDistribution channels. @@ -151,7 +151,7 @@ class PowerDistributionSim { * array must be big enough to hold all the PowerDistribution * channels */ - void SetAllCurrents(const double* currents); + void SetAllCurrents(const double* currents, int length); /** * Reset all PowerDistribution simulation data. diff --git a/wpilibc/src/test/native/cpp/simulation/PDPSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/PDPSimTest.cpp index e5d28d2283..78711b1caf 100644 --- a/wpilibc/src/test/native/cpp/simulation/PDPSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/PDPSimTest.cpp @@ -20,7 +20,7 @@ TEST(PowerDistributionSimTest, Initialize) { BooleanCallback callback; auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false); - PowerDistribution pdp(2); + PowerDistribution pdp(2, frc::PowerDistribution::ModuleType::kCTRE); EXPECT_TRUE(sim.GetInitialized()); EXPECT_TRUE(callback.WasTriggered()); EXPECT_TRUE(callback.GetLastValue()); @@ -33,7 +33,7 @@ TEST(PowerDistributionSimTest, Initialize) { TEST(PowerDistributionSimTest, SetTemperature) { HAL_Initialize(500, 0); - PowerDistribution pdp{2}; + PowerDistribution pdp{2, frc::PowerDistribution::ModuleType::kCTRE}; PowerDistributionSim sim(pdp); DoubleCallback callback; @@ -48,7 +48,7 @@ TEST(PowerDistributionSimTest, SetTemperature) { TEST(PowerDistributionSimTest, SetVoltage) { HAL_Initialize(500, 0); - PowerDistribution pdp{2}; + PowerDistribution pdp{2, frc::PowerDistribution::ModuleType::kCTRE}; PowerDistributionSim sim(pdp); DoubleCallback callback; @@ -63,10 +63,10 @@ TEST(PowerDistributionSimTest, SetVoltage) { TEST(PowerDistributionSimTest, SetCurrent) { HAL_Initialize(500, 0); - PowerDistribution pdp{2}; + PowerDistribution pdp{2, frc::PowerDistribution::ModuleType::kCTRE}; PowerDistributionSim sim(pdp); - for (int channel = 0; channel < HAL_GetNumPDPChannels(); ++channel) { + for (int channel = 0; channel < HAL_GetNumCTREPDPChannels(); ++channel) { DoubleCallback callback; auto cb = sim.RegisterCurrentCallback(channel, callback.GetCallback(), false); diff --git a/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp index 6bc6d053a9..4697553919 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp @@ -21,7 +21,7 @@ class PowerDistributionTest : public testing::Test { }; TEST_F(PowerDistributionTest, CheckRepeatedCalls) { - auto numChannels = HAL_GetNumPDPChannels(); + auto numChannels = HAL_GetNumCTREPDPChannels(); // 1 second for (int i = 0; i < 50; i++) { for (int j = 0; j < numChannels; j++) { 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 af3f8e72c5..1c53353166 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java @@ -19,26 +19,40 @@ public class PowerDistribution implements Sendable, AutoCloseable { private final int m_handle; private final int m_module; + public static final int kDefaultModule = PowerDistributionJNI.DEFAULT_MODULE; + + public enum ModuleType { + kAutomatic(PowerDistributionJNI.AUTOMATIC_TYPE), + kCTRE(PowerDistributionJNI.CTRE_TYPE), + kRev(PowerDistributionJNI.REV_TYPE); + + public final int value; + + ModuleType(int value) { + this.value = value; + } + } + /** * Constructs a PowerDistribution. * * @param module The CAN ID of the PDP */ - public PowerDistribution(int module) { - m_handle = PowerDistributionJNI.initialize(module, 0); - m_module = module; + public PowerDistribution(int module, ModuleType moduleType) { + m_handle = PowerDistributionJNI.initialize(module, moduleType.value); + m_module = PowerDistributionJNI.getModuleNumber(m_handle); - HAL.report(tResourceType.kResourceType_PDP, module + 1); - SendableRegistry.addLW(this, "PowerDistribution", module); + HAL.report(tResourceType.kResourceType_PDP, m_module + 1); + SendableRegistry.addLW(this, "PowerDistribution", m_module); } /** * Constructs a PowerDistribution. * - *

Uses the default CAN ID (0). + *

Uses the default CAN ID. */ public PowerDistribution() { - this(0); + this(kDefaultModule, ModuleType.kAutomatic); } @Override @@ -46,6 +60,15 @@ public class PowerDistribution implements Sendable, AutoCloseable { SendableRegistry.remove(this); } + /** + * Gets the number of channel for this power distribution. + * + * @return Number of output channels. + */ + public int getNumChannels() { + return PowerDistributionJNI.getNumChannels(m_handle); + } + /** * Query the input voltage of the PDP. * @@ -71,7 +94,7 @@ public class PowerDistribution implements Sendable, AutoCloseable { * @return The current of one of the PDP channels (channels 0-15) in Amperes */ public double getCurrent(int channel) { - double current = PowerDistributionJNI.getChannelCurrent((byte) channel, m_handle); + double current = PowerDistributionJNI.getChannelCurrent(m_handle, channel); return current; } @@ -122,14 +145,25 @@ public class PowerDistribution implements Sendable, AutoCloseable { return m_module; } + public boolean getSwitchableChannel() { + return PowerDistributionJNI.getSwitchableChannel(m_handle); + } + + public void setSwitchableChannel(boolean enabled) { + PowerDistributionJNI.setSwitchableChannel(m_handle, enabled); + } + @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("PowerDistribution"); - for (int i = 0; i < SensorUtil.kPDPChannels; ++i) { + int numChannels = getNumChannels(); + for (int i = 0; i < numChannels; ++i) { final int chan = i; builder.addDoubleProperty("Chan" + i, () -> getCurrent(chan), null); } builder.addDoubleProperty("Voltage", this::getVoltage, null); builder.addDoubleProperty("TotalCurrent", this::getTotalCurrent, null); + builder.addBooleanProperty( + "SwitchableChannel", this::getSwitchableChannel, this::setSwitchableChannel); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java index b7d73f5825..07fd98d465 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java @@ -30,7 +30,7 @@ public final class SensorUtil { public static final int kAnalogOutputChannels = PortsJNI.getNumAnalogOutputs(); /** Number of solenoid channels per module. */ - public static final int kSolenoidChannels = PortsJNI.getNumSolenoidChannels(); + public static final int kCTRESolenoidChannels = PortsJNI.getNumCTRESolenoidChannels(); /** Number of PWM channels per roboRIO. */ public static final int kPwmChannels = PortsJNI.getNumPWMChannels(); @@ -39,13 +39,13 @@ public final class SensorUtil { public static final int kRelayChannels = PortsJNI.getNumRelayHeaders(); /** Number of power distribution channels per PDP. */ - public static final int kPDPChannels = PortsJNI.getNumPDPChannels(); + public static final int kCTREPDPChannels = PortsJNI.getNumCTREPDPChannels(); /** Number of power distribution modules per PDP. */ - public static final int kPDPModules = PortsJNI.getNumPDPModules(); + public static final int kCTREPDPModules = PortsJNI.getNumCTREPDPModules(); /** Number of PCM Modules. */ - public static final int kPCMModules = PortsJNI.getNumPCMModules(); + public static final int kCTREPCMModules = PortsJNI.getNumCTREPCMModules(); /** * Check that the digital channel number is valid. Verify that the channel number is one of the diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java index 26ff212e37..1d5e24f66e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java @@ -13,9 +13,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; * via CAN. The information will be displayed under variables through the SmartDashboard. */ public class Robot extends TimedRobot { - private static final int kPDPId = 0; - - private final PowerDistribution m_pdp = new PowerDistribution(kPDPId); + private final PowerDistribution m_pdp = new PowerDistribution(); @Override public void robotPeriodic() { diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java index 3f18086c1b..63ea22a7f2 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java @@ -37,12 +37,6 @@ public class ConstantsPortsTest extends AbstractComsSetup { assertEquals(2, SensorUtil.kAnalogOutputChannels); } - /** kSolenoidChannels. */ - @Test - public void testSolenoidChannels() { - assertEquals(8, SensorUtil.kSolenoidChannels); - } - /** kPwmChannels. */ @Test public void testPwmChannels() { @@ -54,22 +48,4 @@ public class ConstantsPortsTest extends AbstractComsSetup { public void testRelayChannels() { assertEquals(4, SensorUtil.kRelayChannels); } - - /** kPDPChannels. */ - @Test - public void testPDPChannels() { - assertEquals(16, SensorUtil.kPDPChannels); - } - - /** kPDPModules. */ - @Test - public void testPDPModules() { - assertEquals(63, SensorUtil.kPDPModules); - } - - /** kPCMModules. */ - @Test - public void testPCMModules() { - assertEquals(63, SensorUtil.kPCMModules); - } }