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 9873c48c30..6a06ff909b 100644 --- a/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java @@ -45,4 +45,8 @@ public class PortsJNI extends JNIWrapper { public static native int getNumREVPDHModules(); public static native int getNumREVPDHChannels(); + + public static native int getNumREVPHModules(); + + public static native int getNumREVPHChannels(); } diff --git a/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java b/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java new file mode 100644 index 0000000000..a16410610e --- /dev/null +++ b/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java @@ -0,0 +1,32 @@ +// 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. + +package edu.wpi.first.hal; + +@SuppressWarnings("AbbreviationAsWordInName") +public class REVPHJNI extends JNIWrapper { + public static native int initialize(int module); + + public static native void free(int handle); + + public static native boolean checkSolenoidChannel(int channel); + + public static native boolean getCompressor(int handle); + + public static native void setClosedLoopControl(int handle, boolean enabled); + + public static native boolean getClosedLoopControl(int handle); + + public static native boolean getPressureSwitch(int handle); + + public static native double getAnalogPressure(int handle, int channel); + + public static native double getCompressorCurrent(int handle); + + public static native int getSolenoids(int handle); + + public static native void setSolenoids(int handle, int mask, int values); + + public static native void fireOneShot(int handle, int index, int durMs); +} diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java new file mode 100644 index 0000000000..b12fdcbe7d --- /dev/null +++ b/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java @@ -0,0 +1,72 @@ +// 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. + +package edu.wpi.first.hal.simulation; + +import edu.wpi.first.hal.JNIWrapper; + +@SuppressWarnings("AbbreviationAsWordInName") +public class REVPHDataJNI extends JNIWrapper { + public static native int registerInitializedCallback( + int index, NotifyCallback callback, boolean initialNotify); + + public static native void cancelInitializedCallback(int index, int uid); + + public static native boolean getInitialized(int index); + + public static native void setInitialized(int index, boolean initialized); + + public static native int registerSolenoidOutputCallback( + int index, int channel, NotifyCallback callback, boolean initialNotify); + + public static native void cancelSolenoidOutputCallback(int index, int channel, int uid); + + public static native boolean getSolenoidOutput(int index, int channel); + + public static native void setSolenoidOutput(int index, int channel, boolean solenoidOutput); + + public static native int registerCompressorOnCallback( + int index, NotifyCallback callback, boolean initialNotify); + + public static native void cancelCompressorOnCallback(int index, int uid); + + public static native boolean getCompressorOn(int index); + + public static native void setCompressorOn(int index, boolean compressorOn); + + public static native int registerClosedLoopEnabledCallback( + int index, NotifyCallback callback, boolean initialNotify); + + public static native void cancelClosedLoopEnabledCallback(int index, int uid); + + public static native boolean getClosedLoopEnabled(int index); + + public static native void setClosedLoopEnabled(int index, boolean closeLoopEnabled); + + public static native int registerPressureSwitchCallback( + int index, NotifyCallback callback, boolean initialNotify); + + public static native void cancelPressureSwitchCallback(int index, int uid); + + public static native boolean getPressureSwitch(int index); + + public static native void setPressureSwitch(int index, boolean pressureSwitch); + + public static native int registerCompressorCurrentCallback( + int index, NotifyCallback callback, boolean initialNotify); + + public static native void cancelCompressorCurrentCallback(int index, int uid); + + public static native double getCompressorCurrent(int index); + + public static native void setCompressorCurrent(int index, double compressorCurrent); + + public static native void registerAllNonSolenoidCallbacks( + int index, NotifyCallback callback, boolean initialNotify); + + public static native void registerAllSolenoidCallbacks( + int index, int channel, NotifyCallback callback, boolean initialNotify); + + public static native void resetData(int index); +} diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index bb2dc32f09..f344b005f5 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -41,6 +41,7 @@ namespace hal { namespace init { void InitializeHAL() { InitializeCTREPCM(); + InitializeREVPH(); InitializeAddressableLED(); InitializeAccelerometer(); InitializeAnalogAccumulator(); diff --git a/hal/src/main/native/athena/HALInitializer.h b/hal/src/main/native/athena/HALInitializer.h index 6fc03dc72e..182ab5b973 100644 --- a/hal/src/main/native/athena/HALInitializer.h +++ b/hal/src/main/native/athena/HALInitializer.h @@ -17,6 +17,7 @@ inline void CheckInit() { } extern void InitializeCTREPCM(); +extern void InitializeREVPH(); extern void InitializeAccelerometer(); extern void InitializeAddressableLED(); extern void InitializeAnalogAccumulator(); diff --git a/hal/src/main/native/athena/Ports.cpp b/hal/src/main/native/athena/Ports.cpp index 89f167d5ba..d27ecbd75c 100644 --- a/hal/src/main/native/athena/Ports.cpp +++ b/hal/src/main/native/athena/Ports.cpp @@ -74,6 +74,12 @@ int32_t HAL_GetNumREVPDHModules(void) { int32_t HAL_GetNumREVPDHChannels(void) { return kNumREVPDHChannels; } +int32_t HAL_GetNumREVPHModules(void) { + return kNumREVPHModules; +} +int32_t HAL_GetNumREVPHChannels(void) { + return kNumREVPHChannels; +} 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 11278584d0..18ce569057 100644 --- a/hal/src/main/native/athena/PortsInternal.h +++ b/hal/src/main/native/athena/PortsInternal.h @@ -36,5 +36,7 @@ constexpr int32_t kNumREVPDHModules = 63; constexpr int32_t kNumREVPDHChannels = 24; constexpr int32_t kNumDutyCycles = tDutyCycle::kNumSystems; constexpr int32_t kNumAddressableLEDs = tLED::kNumSystems; +constexpr int32_t kNumREVPHModules = 63; +constexpr int32_t kNumREVPHChannels = 16; } // namespace hal diff --git a/hal/src/main/native/athena/REVPH.cpp b/hal/src/main/native/athena/REVPH.cpp new file mode 100644 index 0000000000..9932146cb5 --- /dev/null +++ b/hal/src/main/native/athena/REVPH.cpp @@ -0,0 +1,479 @@ +// 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 "hal/REVPH.h" + +#include + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/CANAPI.h" +#include "hal/Errors.h" +#include "hal/handles/IndexedHandleResource.h" +#include "rev/PHFrames.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_kPneumatics; + +static constexpr int32_t kDefaultControlPeriod = 20; +// static constexpr uint8_t kDefaultSensorMask = (1 << +// HAL_REV_PHSENSOR_DIGITAL); +static constexpr uint8_t kDefaultCompressorDuty = 255; +static constexpr uint8_t kDefaultPressureTarget = 120; +static constexpr uint8_t kDefaultPressureHysteresis = 60; + +#define HAL_REV_MAX_PULSE_TIME 65534 +#define HAL_REV_MAX_PRESSURE_TARGET 120 +#define HAL_REV_MAX_PRESSURE_HYSTERESIS HAL_REV_MAX_PRESSURE_TARGET + +static constexpr uint32_t APIFromExtId(uint32_t extId) { + return (extId >> 6) & 0x3FF; +} + +static constexpr uint32_t PH_SET_ALL_FRAME_API = + APIFromExtId(PH_SET_ALL_FRAME_ID); +static constexpr uint32_t PH_PULSE_ONCE_FRAME_API = + APIFromExtId(PH_PULSE_ONCE_FRAME_ID); +static constexpr uint32_t PH_STATUS0_FRAME_API = + APIFromExtId(PH_STATUS0_FRAME_ID); +static constexpr uint32_t PH_STATUS1_FRAME_API = + APIFromExtId(PH_STATUS1_FRAME_ID); + +static constexpr int32_t kPHFrameStatus0Timeout = 50; +static constexpr int32_t kPHFrameStatus1Timeout = 50; + +namespace { + +struct REV_PHObj { + int32_t controlPeriod; + PH_set_all_t desiredSolenoidsState; + wpi::mutex solenoidLock; + HAL_CANHandle hcan; + std::string previousAllocation; +}; + +} // namespace + +static IndexedHandleResource* REVPHHandles; + +namespace hal::init { +void InitializeREVPH() { + static IndexedHandleResource + rH; + REVPHHandles = &rH; +} +} // namespace hal::init + +static PH_status0_t HAL_REV_ReadPHStatus0(HAL_CANHandle hcan, int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PH_status0_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PH_STATUS0_FRAME_API, packedData, &length, + ×tamp, kPHFrameStatus0Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PH_status0_unpack(&result, packedData, PH_STATUS0_LENGTH); + + return result; +} + +static PH_status1_t HAL_REV_ReadPHStatus1(HAL_CANHandle hcan, int32_t* status) { + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PH_status1_t result = {}; + + HAL_ReadCANPacketTimeout(hcan, PH_STATUS1_FRAME_API, packedData, &length, + ×tamp, kPHFrameStatus1Timeout * 2, status); + + if (*status != 0) { + return result; + } + + PH_status1_unpack(&result, packedData, PH_STATUS1_LENGTH); + + return result; +} + +enum REV_SolenoidState { + kSolenoidDisabled = 0, + kSolenoidEnabled, + kSolenoidControlledViaPulse +}; + +static void HAL_REV_UpdateDesiredPHSolenoidState(REV_PHObj* hph, + int32_t solenoid, + REV_SolenoidState state) { + switch (solenoid) { + case 0: + hph->desiredSolenoidsState.channel_0 = state; + break; + case 1: + hph->desiredSolenoidsState.channel_1 = state; + break; + case 2: + hph->desiredSolenoidsState.channel_2 = state; + break; + case 3: + hph->desiredSolenoidsState.channel_3 = state; + break; + case 4: + hph->desiredSolenoidsState.channel_4 = state; + break; + case 5: + hph->desiredSolenoidsState.channel_5 = state; + break; + case 6: + hph->desiredSolenoidsState.channel_6 = state; + break; + case 7: + hph->desiredSolenoidsState.channel_7 = state; + break; + case 8: + hph->desiredSolenoidsState.channel_8 = state; + break; + case 9: + hph->desiredSolenoidsState.channel_9 = state; + break; + case 10: + hph->desiredSolenoidsState.channel_10 = state; + break; + case 11: + hph->desiredSolenoidsState.channel_11 = state; + break; + case 12: + hph->desiredSolenoidsState.channel_12 = state; + break; + case 13: + hph->desiredSolenoidsState.channel_13 = state; + break; + case 14: + hph->desiredSolenoidsState.channel_14 = state; + break; + case 15: + hph->desiredSolenoidsState.channel_15 = state; + break; + } +} + +static void HAL_REV_SendSolenoidsState(REV_PHObj* hph, int32_t* status) { + uint8_t packedData[PH_SET_ALL_LENGTH] = {0}; + PH_set_all_pack(packedData, &(hph->desiredSolenoidsState), PH_SET_ALL_LENGTH); + HAL_WriteCANPacketRepeating(hph->hcan, packedData, PH_SET_ALL_LENGTH, + PH_SET_ALL_FRAME_API, hph->controlPeriod, status); +} + +static HAL_Bool HAL_REV_CheckPHPulseTime(int32_t time) { + return ((time > 0) && (time <= HAL_REV_MAX_PULSE_TIME)) ? 1 : 0; +} + +HAL_REVPHHandle HAL_InitializeREVPH(int32_t module, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + if (!HAL_CheckREVPHModuleNumber(module)) { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1, + kNumREVPHModules, module); + return HAL_kInvalidHandle; + } + + HAL_REVPHHandle handle; + auto hph = REVPHHandles->Allocate(module, &handle, status); + if (*status != 0) { + if (hph) { + hal::SetLastErrorPreviouslyAllocated(status, "REV PH", module, + hph->previousAllocation); + } else { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1, + kNumREVPHModules, module); + } + return HAL_kInvalidHandle; // failed to allocate. Pass error back. + } + + HAL_CANHandle hcan = + HAL_InitializeCAN(manufacturer, module, deviceType, status); + + if (*status != 0) { + REVPHHandles->Free(handle); + return HAL_kInvalidHandle; + } + + hph->previousAllocation = allocationLocation ? allocationLocation : ""; + hph->hcan = hcan; + hph->controlPeriod = kDefaultControlPeriod; + + // TODO any other things + + return handle; +} + +void HAL_FreeREVPH(HAL_REVPHHandle handle) { + auto hph = REVPHHandles->Get(handle); + if (hph == nullptr) + return; + + HAL_CleanCAN(hph->hcan); + + REVPHHandles->Free(handle); +} + +HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module) { + return module >= 1 && module < kNumREVPDHModules; +} + +HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel) { + return channel >= 0 && channel < kNumREVPHChannels; +} + +HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + + if (*status != 0) { + return false; + } + + return status0.compressor_on; +} + +void HAL_SetREVPHClosedLoopControl(HAL_REVPHHandle handle, HAL_Bool enabled, + int32_t* status) { + // TODO +} + +HAL_Bool HAL_GetREVPHClosedLoopControl(HAL_REVPHHandle handle, + int32_t* status) { + return false; // TODO +} + +HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + + if (*status != 0) { + return false; + } + + return status0.digital_sensor; +} + +double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status1_t status1 = HAL_REV_ReadPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status1_compressor_current_decode(status1.compressor_current); +} + +double HAL_GetREVPHAnalogPressure(HAL_REVPHHandle handle, int32_t channel, + int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + if (channel < 0 || channel > 1) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastErrorIndexOutOfRange(status, "Invalid REV Analog Index", 0, 2, + channel); + return 0; + } + + PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + if (channel == 1) { + return PH_status0_analog_0_decode(status0.analog_0); + } + return PH_status0_analog_1_decode(status0.analog_1); +} + +int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + uint32_t result = status0.channel_0; + result |= status0.channel_1 << 1; + result |= status0.channel_2 << 2; + result |= status0.channel_3 << 3; + result |= status0.channel_4 << 4; + result |= status0.channel_5 << 5; + result |= status0.channel_6 << 6; + result |= status0.channel_7 << 7; + result |= status0.channel_8 << 8; + result |= status0.channel_9 << 9; + result |= status0.channel_10 << 10; + result |= status0.channel_11 << 11; + result |= status0.channel_12 << 12; + result |= status0.channel_13 << 13; + result |= status0.channel_14 << 14; + result |= status0.channel_15 << 15; + + return result; +} + +void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values, + int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + std::scoped_lock lock{ph->solenoidLock}; + for (int solenoid = 0; solenoid < kNumREVPHChannels; solenoid++) { + if (mask & (1 << solenoid)) { + // The mask bit for the solenoid is set, so we update the solenoid state + REV_SolenoidState desiredSolenoidState = + values & (1 << solenoid) ? kSolenoidEnabled : kSolenoidDisabled; + HAL_REV_UpdateDesiredPHSolenoidState(ph.get(), solenoid, + desiredSolenoidState); + } + } + HAL_REV_SendSolenoidsState(ph.get(), status); +} + +void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs, + int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + if (index >= kNumREVPHChannels || index < 0) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastError( + status, + fmt::format("Only [0-15] are valid index values. Requested {}", index)); + return; + } + + if (!HAL_REV_CheckPHPulseTime(durMs)) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastError( + status, + fmt::format("Time not within expected range [0-65534]. Requested {}", + durMs)); + return; + } + + { + std::scoped_lock lock{ph->solenoidLock}; + HAL_REV_UpdateDesiredPHSolenoidState(ph.get(), index, + kSolenoidControlledViaPulse); + HAL_REV_SendSolenoidsState(ph.get(), status); + } + + if (*status != 0) { + return; + } + + PH_pulse_once_t pulse = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + pulse.pulse_length_ms = durMs; + + // Specify which solenoid should be pulsed + // The protocol supports specifying any number of solenoids to be pulsed at + // the same time, should that functionality be exposed to users in the future. + switch (index) { + case 0: + pulse.channel_0 = true; + break; + case 1: + pulse.channel_1 = true; + break; + case 2: + pulse.channel_2 = true; + break; + case 3: + pulse.channel_3 = true; + break; + case 4: + pulse.channel_4 = true; + break; + case 5: + pulse.channel_5 = true; + break; + case 6: + pulse.channel_6 = true; + break; + case 7: + pulse.channel_7 = true; + break; + case 8: + pulse.channel_8 = true; + break; + case 9: + pulse.channel_9 = true; + break; + case 10: + pulse.channel_10 = true; + break; + case 11: + pulse.channel_11 = true; + break; + case 12: + pulse.channel_12 = true; + break; + case 13: + pulse.channel_13 = true; + break; + case 14: + pulse.channel_14 = true; + break; + case 15: + pulse.channel_15 = true; + break; + } + + // Send pulse command + uint8_t packedData[PH_PULSE_ONCE_LENGTH] = {0}; + PH_pulse_once_pack(packedData, &pulse, PH_PULSE_ONCE_LENGTH); + HAL_WriteCANPacket(ph->hcan, packedData, PH_PULSE_ONCE_LENGTH, + PH_PULSE_ONCE_FRAME_API, status); +} diff --git a/hal/src/main/native/athena/mockdata/REVPHData.cpp b/hal/src/main/native/athena/mockdata/REVPHData.cpp new file mode 100644 index 0000000000..d6176948dd --- /dev/null +++ b/hal/src/main/native/athena/mockdata/REVPHData.cpp @@ -0,0 +1,37 @@ +// 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 "hal/simulation/REVPHData.h" + +#include "hal/simulation/SimDataValue.h" + +extern "C" { +void HALSIM_ResetREVPHData(int32_t index) {} + +#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \ + HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, REVPH##CAPINAME, RETURN) + +HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, REVPHSolenoidOutput, false) +DEFINE_CAPI(HAL_Bool, Initialized, false) +DEFINE_CAPI(HAL_Bool, CompressorOn, false) +DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, false) +DEFINE_CAPI(HAL_Bool, PressureSwitch, false) +DEFINE_CAPI(double, CompressorCurrent, 0) + +void HALSIM_GetREVPHAllSolenoids(int32_t index, uint8_t* values) { + *values = 0; +} + +void HALSIM_SetREVPHAllSolenoids(int32_t index, uint8_t values) {} + +void HALSIM_RegisterREVPHAllNonSolenoidCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} + +void HALSIM_RegisterREVPHAllSolenoidCallbacks(int32_t index, int32_t channel, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) {} +} // extern "C" diff --git a/hal/src/main/native/athena/rev/PHFrames.cpp b/hal/src/main/native/athena/rev/PHFrames.cpp new file mode 100644 index 0000000000..e7f77e0054 --- /dev/null +++ b/hal/src/main/native/athena/rev/PHFrames.cpp @@ -0,0 +1,1720 @@ +/** + * 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 + */ + +#include + +#include "PHFrames.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_right_shift_u16( + uint16_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 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); +} + +int PH_set_all_pack( + uint8_t *dst_p, + const struct PH_set_all_t *src_p, + size_t size) +{ + if (size < 4u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 4); + + dst_p[0] |= pack_left_shift_u8(src_p->channel_0, 0u, 0x03u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_1, 2u, 0x0cu); + dst_p[0] |= pack_left_shift_u8(src_p->channel_2, 4u, 0x30u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_3, 6u, 0xc0u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_4, 0u, 0x03u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_5, 2u, 0x0cu); + dst_p[1] |= pack_left_shift_u8(src_p->channel_6, 4u, 0x30u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_7, 6u, 0xc0u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_8, 0u, 0x03u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_9, 2u, 0x0cu); + dst_p[2] |= pack_left_shift_u8(src_p->channel_10, 4u, 0x30u); + dst_p[2] |= pack_left_shift_u8(src_p->channel_11, 6u, 0xc0u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_12, 0u, 0x03u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_13, 2u, 0x0cu); + dst_p[3] |= pack_left_shift_u8(src_p->channel_14, 4u, 0x30u); + dst_p[3] |= pack_left_shift_u8(src_p->channel_15, 6u, 0xc0u); + + return (4); +} + +int PH_set_all_unpack( + struct PH_set_all_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 4u) { + return (-EINVAL); + } + + dst_p->channel_0 = unpack_right_shift_u8(src_p[0], 0u, 0x03u); + dst_p->channel_1 = unpack_right_shift_u8(src_p[0], 2u, 0x0cu); + dst_p->channel_2 = unpack_right_shift_u8(src_p[0], 4u, 0x30u); + dst_p->channel_3 = unpack_right_shift_u8(src_p[0], 6u, 0xc0u); + dst_p->channel_4 = unpack_right_shift_u8(src_p[1], 0u, 0x03u); + dst_p->channel_5 = unpack_right_shift_u8(src_p[1], 2u, 0x0cu); + dst_p->channel_6 = unpack_right_shift_u8(src_p[1], 4u, 0x30u); + dst_p->channel_7 = unpack_right_shift_u8(src_p[1], 6u, 0xc0u); + dst_p->channel_8 = unpack_right_shift_u8(src_p[2], 0u, 0x03u); + dst_p->channel_9 = unpack_right_shift_u8(src_p[2], 2u, 0x0cu); + dst_p->channel_10 = unpack_right_shift_u8(src_p[2], 4u, 0x30u); + dst_p->channel_11 = unpack_right_shift_u8(src_p[2], 6u, 0xc0u); + dst_p->channel_12 = unpack_right_shift_u8(src_p[3], 0u, 0x03u); + dst_p->channel_13 = unpack_right_shift_u8(src_p[3], 2u, 0x0cu); + dst_p->channel_14 = unpack_right_shift_u8(src_p[3], 4u, 0x30u); + dst_p->channel_15 = unpack_right_shift_u8(src_p[3], 6u, 0xc0u); + + return (0); +} + +uint8_t PH_set_all_channel_0_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_0_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_0_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_1_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_1_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_1_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_2_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_2_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_2_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_3_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_3_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_3_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_4_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_4_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_4_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_5_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_5_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_5_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_6_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_6_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_6_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_7_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_7_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_7_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_8_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_8_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_8_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_9_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_9_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_9_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_10_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_10_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_10_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_11_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_11_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_11_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_12_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_12_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_12_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_13_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_13_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_13_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_14_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_14_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_14_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +uint8_t PH_set_all_channel_15_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_set_all_channel_15_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_set_all_channel_15_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + +int PH_pulse_once_pack( + uint8_t *dst_p, + const struct PH_pulse_once_t *src_p, + size_t size) +{ + if (size < 4u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 4); + + dst_p[0] |= pack_left_shift_u8(src_p->channel_0, 0u, 0x01u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_1, 1u, 0x02u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_2, 2u, 0x04u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_3, 3u, 0x08u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_4, 4u, 0x10u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_5, 5u, 0x20u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_6, 6u, 0x40u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_7, 7u, 0x80u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_8, 0u, 0x01u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_9, 1u, 0x02u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_10, 2u, 0x04u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_11, 3u, 0x08u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_12, 4u, 0x10u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_13, 5u, 0x20u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_14, 6u, 0x40u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_15, 7u, 0x80u); + dst_p[2] |= pack_left_shift_u16(src_p->pulse_length_ms, 0u, 0xffu); + dst_p[3] |= pack_right_shift_u16(src_p->pulse_length_ms, 8u, 0xffu); + + return (4); +} + +int PH_pulse_once_unpack( + struct PH_pulse_once_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 4u) { + return (-EINVAL); + } + + dst_p->channel_0 = unpack_right_shift_u8(src_p[0], 0u, 0x01u); + dst_p->channel_1 = unpack_right_shift_u8(src_p[0], 1u, 0x02u); + dst_p->channel_2 = unpack_right_shift_u8(src_p[0], 2u, 0x04u); + dst_p->channel_3 = unpack_right_shift_u8(src_p[0], 3u, 0x08u); + dst_p->channel_4 = unpack_right_shift_u8(src_p[0], 4u, 0x10u); + dst_p->channel_5 = unpack_right_shift_u8(src_p[0], 5u, 0x20u); + dst_p->channel_6 = unpack_right_shift_u8(src_p[0], 6u, 0x40u); + dst_p->channel_7 = unpack_right_shift_u8(src_p[0], 7u, 0x80u); + dst_p->channel_8 = unpack_right_shift_u8(src_p[1], 0u, 0x01u); + dst_p->channel_9 = unpack_right_shift_u8(src_p[1], 1u, 0x02u); + dst_p->channel_10 = unpack_right_shift_u8(src_p[1], 2u, 0x04u); + dst_p->channel_11 = unpack_right_shift_u8(src_p[1], 3u, 0x08u); + dst_p->channel_12 = unpack_right_shift_u8(src_p[1], 4u, 0x10u); + dst_p->channel_13 = unpack_right_shift_u8(src_p[1], 5u, 0x20u); + dst_p->channel_14 = unpack_right_shift_u8(src_p[1], 6u, 0x40u); + dst_p->channel_15 = unpack_right_shift_u8(src_p[1], 7u, 0x80u); + dst_p->pulse_length_ms = unpack_right_shift_u16(src_p[2], 0u, 0xffu); + dst_p->pulse_length_ms |= unpack_left_shift_u16(src_p[3], 8u, 0xffu); + + return (0); +} + +uint8_t PH_pulse_once_channel_0_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_0_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_0_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_1_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_1_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_1_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_2_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_2_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_2_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_3_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_3_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_3_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_4_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_4_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_4_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_5_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_5_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_5_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_6_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_6_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_6_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_7_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_7_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_7_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_8_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_8_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_8_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_9_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_9_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_9_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_10_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_10_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_10_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_11_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_11_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_11_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_12_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_12_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_12_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_13_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_13_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_13_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_14_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_14_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_14_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_pulse_once_channel_15_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_pulse_once_channel_15_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_channel_15_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint16_t PH_pulse_once_pulse_length_ms_encode(double value) +{ + return (uint16_t)(value); +} + +double PH_pulse_once_pulse_length_ms_decode(uint16_t value) +{ + return ((double)value); +} + +bool PH_pulse_once_pulse_length_ms_is_in_range(uint16_t value) +{ + (void)value; + + return (true); +} + +int PH_status0_pack( + uint8_t *dst_p, + const struct PH_status0_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->channel_0, 0u, 0x01u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_1, 1u, 0x02u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_2, 2u, 0x04u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_3, 3u, 0x08u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_4, 4u, 0x10u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_5, 5u, 0x20u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_6, 6u, 0x40u); + dst_p[0] |= pack_left_shift_u8(src_p->channel_7, 7u, 0x80u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_8, 0u, 0x01u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_9, 1u, 0x02u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_10, 2u, 0x04u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_11, 3u, 0x08u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_12, 4u, 0x10u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_13, 5u, 0x20u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_14, 6u, 0x40u); + dst_p[1] |= pack_left_shift_u8(src_p->channel_15, 7u, 0x80u); + dst_p[2] |= pack_left_shift_u8(src_p->analog_0, 0u, 0xffu); + dst_p[3] |= pack_left_shift_u8(src_p->analog_1, 0u, 0xffu); + dst_p[4] |= pack_left_shift_u8(src_p->digital_sensor, 0u, 0x01u); + dst_p[4] |= pack_left_shift_u8(src_p->brownout, 1u, 0x02u); + dst_p[4] |= pack_left_shift_u8(src_p->compressor_oc, 2u, 0x04u); + dst_p[4] |= pack_left_shift_u8(src_p->compressor_open, 3u, 0x08u); + dst_p[4] |= pack_left_shift_u8(src_p->solenoid_oc, 4u, 0x10u); + dst_p[4] |= pack_left_shift_u8(src_p->can_warning, 5u, 0x20u); + dst_p[4] |= pack_left_shift_u8(src_p->hardware_fault, 6u, 0x40u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_0_fault, 0u, 0x01u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_1_fault, 1u, 0x02u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_2_fault, 2u, 0x04u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_3_fault, 3u, 0x08u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_4_fault, 4u, 0x10u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_5_fault, 5u, 0x20u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_6_fault, 6u, 0x40u); + dst_p[5] |= pack_left_shift_u8(src_p->channel_7_fault, 7u, 0x80u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_8_fault, 0u, 0x01u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_9_fault, 1u, 0x02u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_10_fault, 2u, 0x04u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_11_fault, 3u, 0x08u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_12_fault, 4u, 0x10u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_13_fault, 5u, 0x20u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_14_fault, 6u, 0x40u); + dst_p[6] |= pack_left_shift_u8(src_p->channel_15_fault, 7u, 0x80u); + dst_p[7] |= pack_left_shift_u8(src_p->compressor_on, 0u, 0x01u); + dst_p[7] |= pack_left_shift_u8(src_p->system_enabled, 1u, 0x02u); + + return (8); +} + +int PH_status0_unpack( + struct PH_status0_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->channel_0 = unpack_right_shift_u8(src_p[0], 0u, 0x01u); + dst_p->channel_1 = unpack_right_shift_u8(src_p[0], 1u, 0x02u); + dst_p->channel_2 = unpack_right_shift_u8(src_p[0], 2u, 0x04u); + dst_p->channel_3 = unpack_right_shift_u8(src_p[0], 3u, 0x08u); + dst_p->channel_4 = unpack_right_shift_u8(src_p[0], 4u, 0x10u); + dst_p->channel_5 = unpack_right_shift_u8(src_p[0], 5u, 0x20u); + dst_p->channel_6 = unpack_right_shift_u8(src_p[0], 6u, 0x40u); + dst_p->channel_7 = unpack_right_shift_u8(src_p[0], 7u, 0x80u); + dst_p->channel_8 = unpack_right_shift_u8(src_p[1], 0u, 0x01u); + dst_p->channel_9 = unpack_right_shift_u8(src_p[1], 1u, 0x02u); + dst_p->channel_10 = unpack_right_shift_u8(src_p[1], 2u, 0x04u); + dst_p->channel_11 = unpack_right_shift_u8(src_p[1], 3u, 0x08u); + dst_p->channel_12 = unpack_right_shift_u8(src_p[1], 4u, 0x10u); + dst_p->channel_13 = unpack_right_shift_u8(src_p[1], 5u, 0x20u); + dst_p->channel_14 = unpack_right_shift_u8(src_p[1], 6u, 0x40u); + dst_p->channel_15 = unpack_right_shift_u8(src_p[1], 7u, 0x80u); + dst_p->analog_0 = unpack_right_shift_u8(src_p[2], 0u, 0xffu); + dst_p->analog_1 = unpack_right_shift_u8(src_p[3], 0u, 0xffu); + dst_p->digital_sensor = unpack_right_shift_u8(src_p[4], 0u, 0x01u); + dst_p->brownout = unpack_right_shift_u8(src_p[4], 1u, 0x02u); + dst_p->compressor_oc = unpack_right_shift_u8(src_p[4], 2u, 0x04u); + dst_p->compressor_open = unpack_right_shift_u8(src_p[4], 3u, 0x08u); + dst_p->solenoid_oc = unpack_right_shift_u8(src_p[4], 4u, 0x10u); + dst_p->can_warning = unpack_right_shift_u8(src_p[4], 5u, 0x20u); + dst_p->hardware_fault = unpack_right_shift_u8(src_p[4], 6u, 0x40u); + dst_p->channel_0_fault = unpack_right_shift_u8(src_p[5], 0u, 0x01u); + dst_p->channel_1_fault = unpack_right_shift_u8(src_p[5], 1u, 0x02u); + dst_p->channel_2_fault = unpack_right_shift_u8(src_p[5], 2u, 0x04u); + dst_p->channel_3_fault = unpack_right_shift_u8(src_p[5], 3u, 0x08u); + dst_p->channel_4_fault = unpack_right_shift_u8(src_p[5], 4u, 0x10u); + dst_p->channel_5_fault = unpack_right_shift_u8(src_p[5], 5u, 0x20u); + dst_p->channel_6_fault = unpack_right_shift_u8(src_p[5], 6u, 0x40u); + dst_p->channel_7_fault = unpack_right_shift_u8(src_p[5], 7u, 0x80u); + dst_p->channel_8_fault = unpack_right_shift_u8(src_p[6], 0u, 0x01u); + dst_p->channel_9_fault = unpack_right_shift_u8(src_p[6], 1u, 0x02u); + dst_p->channel_10_fault = unpack_right_shift_u8(src_p[6], 2u, 0x04u); + dst_p->channel_11_fault = unpack_right_shift_u8(src_p[6], 3u, 0x08u); + dst_p->channel_12_fault = unpack_right_shift_u8(src_p[6], 4u, 0x10u); + dst_p->channel_13_fault = unpack_right_shift_u8(src_p[6], 5u, 0x20u); + dst_p->channel_14_fault = unpack_right_shift_u8(src_p[6], 6u, 0x40u); + dst_p->channel_15_fault = unpack_right_shift_u8(src_p[6], 7u, 0x80u); + dst_p->compressor_on = unpack_right_shift_u8(src_p[7], 0u, 0x01u); + dst_p->system_enabled = unpack_right_shift_u8(src_p[7], 1u, 0x02u); + + return (0); +} + +uint8_t PH_status0_channel_0_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_0_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_0_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_1_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_1_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_1_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_2_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_2_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_2_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_3_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_3_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_3_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_4_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_4_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_4_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_5_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_5_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_5_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_6_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_6_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_6_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_7_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_7_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_7_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_8_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_8_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_8_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_9_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_9_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_9_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_10_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_10_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_10_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_11_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_11_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_11_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_12_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_12_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_12_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_13_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_13_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_13_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_14_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_14_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_14_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_15_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_15_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_15_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_analog_0_encode(double value) +{ + return (uint8_t)(value / 0.01961); +} + +double PH_status0_analog_0_decode(uint8_t value) +{ + return ((double)value * 0.01961); +} + +bool PH_status0_analog_0_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_status0_analog_1_encode(double value) +{ + return (uint8_t)(value / 0.01961); +} + +double PH_status0_analog_1_decode(uint8_t value) +{ + return ((double)value * 0.01961); +} + +bool PH_status0_analog_1_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_status0_digital_sensor_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_digital_sensor_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_digital_sensor_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_compressor_oc_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_compressor_oc_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_compressor_oc_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_compressor_open_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_compressor_open_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_compressor_open_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_solenoid_oc_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_solenoid_oc_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_solenoid_oc_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_can_warning_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_can_warning_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_can_warning_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_hardware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_hardware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_hardware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_0_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_0_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_0_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_1_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_1_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_1_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_2_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_2_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_2_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_3_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_3_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_3_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_4_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_4_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_4_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_5_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_5_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_5_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_6_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_6_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_6_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_7_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_7_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_7_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_8_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_8_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_8_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_9_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_9_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_9_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_10_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_10_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_10_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_11_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_11_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_11_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_12_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_12_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_12_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_13_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_13_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_13_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_14_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_14_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_14_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_channel_15_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_channel_15_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_channel_15_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_compressor_on_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_compressor_on_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_compressor_on_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_system_enabled_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_system_enabled_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_system_enabled_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +int PH_status1_pack( + uint8_t *dst_p, + const struct PH_status1_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->v_bus, 0u, 0xffu); + dst_p[1] |= pack_left_shift_u16(src_p->solenoid_voltage, 0u, 0xffu); + dst_p[2] |= pack_right_shift_u16(src_p->solenoid_voltage, 8u, 0x0fu); + dst_p[4] |= pack_left_shift_u8(src_p->compressor_current, 0u, 0xffu); + dst_p[5] |= pack_left_shift_u8(src_p->solenoid_current, 0u, 0xffu); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_brownout, 0u, 0x01u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_compressor_over_current, 1u, 0x02u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_compressor_not_present, 2u, 0x04u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_solenoid_over_current, 3u, 0x08u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_can_warning, 4u, 0x10u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_can_bus_off, 5u, 0x20u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_hardware_fault, 6u, 0x40u); + dst_p[6] |= pack_left_shift_u8(src_p->sticky_firmware_fault, 7u, 0x80u); + dst_p[7] |= pack_left_shift_u8(src_p->sticky_has_reset, 0u, 0x01u); + + return (8); +} + +int PH_status1_unpack( + struct PH_status1_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->v_bus = unpack_right_shift_u8(src_p[0], 0u, 0xffu); + dst_p->solenoid_voltage = unpack_right_shift_u16(src_p[1], 0u, 0xffu); + dst_p->solenoid_voltage |= unpack_left_shift_u16(src_p[2], 8u, 0x0fu); + dst_p->compressor_current = unpack_right_shift_u8(src_p[4], 0u, 0xffu); + dst_p->solenoid_current = unpack_right_shift_u8(src_p[5], 0u, 0xffu); + dst_p->sticky_brownout = unpack_right_shift_u8(src_p[6], 0u, 0x01u); + dst_p->sticky_compressor_over_current = unpack_right_shift_u8(src_p[6], 1u, 0x02u); + dst_p->sticky_compressor_not_present = unpack_right_shift_u8(src_p[6], 2u, 0x04u); + dst_p->sticky_solenoid_over_current = unpack_right_shift_u8(src_p[6], 3u, 0x08u); + dst_p->sticky_can_warning = unpack_right_shift_u8(src_p[6], 4u, 0x10u); + dst_p->sticky_can_bus_off = unpack_right_shift_u8(src_p[6], 5u, 0x20u); + dst_p->sticky_hardware_fault = unpack_right_shift_u8(src_p[6], 6u, 0x40u); + dst_p->sticky_firmware_fault = unpack_right_shift_u8(src_p[6], 7u, 0x80u); + dst_p->sticky_has_reset = unpack_right_shift_u8(src_p[7], 0u, 0x01u); + + return (0); +} + +uint8_t PH_status1_v_bus_encode(double value) +{ + return (uint8_t)((value - 4.0) / 0.0625); +} + +double PH_status1_v_bus_decode(uint8_t value) +{ + return (((double)value * 0.0625) + 4.0); +} + +bool PH_status1_v_bus_is_in_range(uint8_t value) +{ + return (value <= 192u); +} + +uint16_t PH_status1_solenoid_voltage_encode(double value) +{ + return (uint16_t)(value / 0.0078125); +} + +double PH_status1_solenoid_voltage_decode(uint16_t value) +{ + return ((double)value * 0.0078125); +} + +bool PH_status1_solenoid_voltage_is_in_range(uint16_t value) +{ + return (value <= 4096u); +} + +uint8_t PH_status1_compressor_current_encode(double value) +{ + return (uint8_t)(value / 0.125); +} + +double PH_status1_compressor_current_decode(uint8_t value) +{ + return ((double)value * 0.125); +} + +bool PH_status1_compressor_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_status1_solenoid_current_encode(double value) +{ + return (uint8_t)(value / 0.125); +} + +double PH_status1_solenoid_current_decode(uint8_t value) +{ + return ((double)value * 0.125); +} + +bool PH_status1_solenoid_current_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_status1_sticky_brownout_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status1_sticky_brownout_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status1_sticky_brownout_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status1_sticky_compressor_over_current_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status1_sticky_compressor_over_current_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status1_sticky_compressor_over_current_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status1_sticky_compressor_not_present_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status1_sticky_compressor_not_present_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status1_sticky_compressor_not_present_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status1_sticky_solenoid_over_current_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status1_sticky_solenoid_over_current_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status1_sticky_solenoid_over_current_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status1_sticky_can_warning_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status1_sticky_can_warning_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status1_sticky_can_warning_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status1_sticky_can_bus_off_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status1_sticky_can_bus_off_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status1_sticky_can_bus_off_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status1_sticky_hardware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status1_sticky_hardware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status1_sticky_hardware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status1_sticky_firmware_fault_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status1_sticky_firmware_fault_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status1_sticky_firmware_fault_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status1_sticky_has_reset_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status1_sticky_has_reset_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status1_sticky_has_reset_is_in_range(uint8_t value) +{ + return (value <= 1u); +} diff --git a/hal/src/main/native/athena/rev/PHFrames.h b/hal/src/main/native/athena/rev/PHFrames.h new file mode 100644 index 0000000000..295be0a31f --- /dev/null +++ b/hal/src/main/native/athena/rev/PHFrames.h @@ -0,0 +1,3249 @@ +/** + * 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 + */ + +#ifndef PH_H +#define PH_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#ifndef EINVAL +# define EINVAL 22 +#endif + +/* Frame ids. */ +#define PH_SET_ALL_FRAME_ID (0x9050c00u) +#define PH_PULSE_ONCE_FRAME_ID (0x9050c40u) +#define PH_STATUS0_FRAME_ID (0x9051800u) +#define PH_STATUS1_FRAME_ID (0x9051840u) + +/* Frame lengths in bytes. */ +#define PH_SET_ALL_LENGTH (4u) +#define PH_PULSE_ONCE_LENGTH (4u) +#define PH_STATUS0_LENGTH (8u) +#define PH_STATUS1_LENGTH (8u) + +/* Extended or standard frame types. */ +#define PH_SET_ALL_IS_EXTENDED (1) +#define PH_PULSE_ONCE_IS_EXTENDED (1) +#define PH_STATUS0_IS_EXTENDED (1) +#define PH_STATUS1_IS_EXTENDED (1) + +/* Frame cycle times in milliseconds. */ + + +/* Signal choices. */ + + +/** + * Signals in message SetAll. + * + * Set state of all channels + * + * All signal values are as on the CAN bus. + */ +struct PH_set_all_t { + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_0 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14 : 2; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15 : 2; +}; + +/** + * Signals in message PulseOnce. + * + * Pulse selected channels once + * + * All signal values are as on the CAN bus. + */ +struct PH_pulse_once_t { + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_0 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15 : 1; + + /** + * Range: 0..65535 (0..65535 -) + * Scale: 1 + * Offset: 0 + */ + uint16_t pulse_length_ms : 16; +}; + +/** + * Signals in message Status0. + * + * Periodic status frame 0 + * + * All signal values are as on the CAN bus. + */ +struct PH_status0_t { + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_0 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14 : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15 : 1; + + /** + * Range: 0..255 (0..5.00055 V) + * Scale: 0.01961 + * Offset: 0 + */ + uint8_t analog_0 : 8; + + /** + * Range: 0..255 (0..5.00055 V) + * Scale: 0.01961 + * Offset: 0 + */ + uint8_t analog_1 : 8; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t digital_sensor : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t brownout : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t compressor_oc : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t compressor_open : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t solenoid_oc : 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 channel_0_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_1_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_2_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_3_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_4_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_5_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_6_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_7_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_8_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_9_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_10_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_11_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_12_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_13_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_14_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t channel_15_fault : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t compressor_on : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t system_enabled : 1; +}; + +/** + * Signals in message Status1. + * + * Periodic status frame 1 + * + * All signal values are as on the CAN bus. + */ +struct PH_status1_t { + /** + * Range: 0..192 (4..16 V) + * Scale: 0.0625 + * Offset: 4 + */ + uint8_t v_bus : 8; + + /** + * Range: 0..4096 (0..32 V) + * Scale: 0.0078125 + * Offset: 0 + */ + uint16_t solenoid_voltage : 12; + + /** + * Range: 0..256 (0..32 A) + * Scale: 0.125 + * Offset: 0 + */ + uint8_t compressor_current : 8; + + /** + * Range: 0..256 (0..32 A) + * Scale: 0.125 + * Offset: 0 + */ + uint8_t solenoid_current : 8; + + /** + * 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_compressor_over_current : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_compressor_not_present : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t sticky_solenoid_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_has_reset : 1; +}; + +/** + * Pack message SetAll. + * + * @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 PH_set_all_pack( + uint8_t *dst_p, + const struct PH_set_all_t *src_p, + size_t size); + +/** + * Unpack message SetAll. + * + * @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 PH_set_all_unpack( + struct PH_set_all_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 PH_set_all_channel_0_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_0_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 PH_set_all_channel_0_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 PH_set_all_channel_1_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_1_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 PH_set_all_channel_1_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 PH_set_all_channel_2_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_2_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 PH_set_all_channel_2_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 PH_set_all_channel_3_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_3_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 PH_set_all_channel_3_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 PH_set_all_channel_4_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_4_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 PH_set_all_channel_4_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 PH_set_all_channel_5_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_5_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 PH_set_all_channel_5_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 PH_set_all_channel_6_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_6_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 PH_set_all_channel_6_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 PH_set_all_channel_7_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_7_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 PH_set_all_channel_7_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 PH_set_all_channel_8_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_8_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 PH_set_all_channel_8_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 PH_set_all_channel_9_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_9_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 PH_set_all_channel_9_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 PH_set_all_channel_10_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_10_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 PH_set_all_channel_10_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 PH_set_all_channel_11_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_11_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 PH_set_all_channel_11_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 PH_set_all_channel_12_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_12_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 PH_set_all_channel_12_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 PH_set_all_channel_13_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_13_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 PH_set_all_channel_13_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 PH_set_all_channel_14_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_14_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 PH_set_all_channel_14_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 PH_set_all_channel_15_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_set_all_channel_15_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 PH_set_all_channel_15_is_in_range(uint8_t value); + +/** + * Pack message PulseOnce. + * + * @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 PH_pulse_once_pack( + uint8_t *dst_p, + const struct PH_pulse_once_t *src_p, + size_t size); + +/** + * Unpack message PulseOnce. + * + * @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 PH_pulse_once_unpack( + struct PH_pulse_once_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 PH_pulse_once_channel_0_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_0_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 PH_pulse_once_channel_0_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 PH_pulse_once_channel_1_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_1_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 PH_pulse_once_channel_1_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 PH_pulse_once_channel_2_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_2_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 PH_pulse_once_channel_2_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 PH_pulse_once_channel_3_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_3_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 PH_pulse_once_channel_3_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 PH_pulse_once_channel_4_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_4_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 PH_pulse_once_channel_4_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 PH_pulse_once_channel_5_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_5_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 PH_pulse_once_channel_5_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 PH_pulse_once_channel_6_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_6_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 PH_pulse_once_channel_6_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 PH_pulse_once_channel_7_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_7_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 PH_pulse_once_channel_7_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 PH_pulse_once_channel_8_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_8_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 PH_pulse_once_channel_8_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 PH_pulse_once_channel_9_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_9_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 PH_pulse_once_channel_9_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 PH_pulse_once_channel_10_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_10_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 PH_pulse_once_channel_10_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 PH_pulse_once_channel_11_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_11_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 PH_pulse_once_channel_11_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 PH_pulse_once_channel_12_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_12_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 PH_pulse_once_channel_12_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 PH_pulse_once_channel_13_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_13_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 PH_pulse_once_channel_13_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 PH_pulse_once_channel_14_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_14_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 PH_pulse_once_channel_14_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 PH_pulse_once_channel_15_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_channel_15_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 PH_pulse_once_channel_15_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 PH_pulse_once_pulse_length_ms_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_pulse_once_pulse_length_ms_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 PH_pulse_once_pulse_length_ms_is_in_range(uint16_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 PH_status0_pack( + uint8_t *dst_p, + const struct PH_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 PH_status0_unpack( + struct PH_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. + */ +uint8_t PH_status0_channel_0_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_0_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 PH_status0_channel_0_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 PH_status0_channel_1_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_1_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 PH_status0_channel_1_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 PH_status0_channel_2_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_2_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 PH_status0_channel_2_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 PH_status0_channel_3_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_3_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 PH_status0_channel_3_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 PH_status0_channel_4_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_4_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 PH_status0_channel_4_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 PH_status0_channel_5_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_5_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 PH_status0_channel_5_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 PH_status0_channel_6_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_6_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 PH_status0_channel_6_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 PH_status0_channel_7_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_7_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 PH_status0_channel_7_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 PH_status0_channel_8_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_8_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 PH_status0_channel_8_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 PH_status0_channel_9_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_9_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 PH_status0_channel_9_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 PH_status0_channel_10_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_10_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 PH_status0_channel_10_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 PH_status0_channel_11_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_11_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 PH_status0_channel_11_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 PH_status0_channel_12_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_12_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 PH_status0_channel_12_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 PH_status0_channel_13_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_13_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 PH_status0_channel_13_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 PH_status0_channel_14_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_14_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 PH_status0_channel_14_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 PH_status0_channel_15_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_15_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 PH_status0_channel_15_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 PH_status0_analog_0_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_analog_0_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 PH_status0_analog_0_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 PH_status0_analog_1_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_analog_1_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 PH_status0_analog_1_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 PH_status0_digital_sensor_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_digital_sensor_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 PH_status0_digital_sensor_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 PH_status0_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_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 PH_status0_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 PH_status0_compressor_oc_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_compressor_oc_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 PH_status0_compressor_oc_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 PH_status0_compressor_open_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_compressor_open_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 PH_status0_compressor_open_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 PH_status0_solenoid_oc_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_solenoid_oc_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 PH_status0_solenoid_oc_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 PH_status0_can_warning_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_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 PH_status0_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 PH_status0_hardware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_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 PH_status0_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 PH_status0_channel_0_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_0_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 PH_status0_channel_0_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 PH_status0_channel_1_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_1_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 PH_status0_channel_1_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 PH_status0_channel_2_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_2_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 PH_status0_channel_2_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 PH_status0_channel_3_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_3_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 PH_status0_channel_3_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 PH_status0_channel_4_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_4_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 PH_status0_channel_4_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 PH_status0_channel_5_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_5_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 PH_status0_channel_5_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 PH_status0_channel_6_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_6_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 PH_status0_channel_6_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 PH_status0_channel_7_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_7_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 PH_status0_channel_7_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 PH_status0_channel_8_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_8_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 PH_status0_channel_8_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 PH_status0_channel_9_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_9_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 PH_status0_channel_9_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 PH_status0_channel_10_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_10_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 PH_status0_channel_10_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 PH_status0_channel_11_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_11_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 PH_status0_channel_11_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 PH_status0_channel_12_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_12_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 PH_status0_channel_12_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 PH_status0_channel_13_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_13_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 PH_status0_channel_13_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 PH_status0_channel_14_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_14_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 PH_status0_channel_14_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 PH_status0_channel_15_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_channel_15_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 PH_status0_channel_15_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 PH_status0_compressor_on_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_compressor_on_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 PH_status0_compressor_on_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 PH_status0_system_enabled_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_system_enabled_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 PH_status0_system_enabled_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 PH_status1_pack( + uint8_t *dst_p, + const struct PH_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 PH_status1_unpack( + struct PH_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. + */ +uint8_t PH_status1_v_bus_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_v_bus_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 PH_status1_v_bus_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 PH_status1_solenoid_voltage_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_solenoid_voltage_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 PH_status1_solenoid_voltage_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 PH_status1_compressor_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_compressor_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 PH_status1_compressor_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 PH_status1_solenoid_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_solenoid_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 PH_status1_solenoid_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 PH_status1_sticky_brownout_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_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 PH_status1_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 PH_status1_sticky_compressor_over_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_sticky_compressor_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 PH_status1_sticky_compressor_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 PH_status1_sticky_compressor_not_present_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_sticky_compressor_not_present_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 PH_status1_sticky_compressor_not_present_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 PH_status1_sticky_solenoid_over_current_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_sticky_solenoid_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 PH_status1_sticky_solenoid_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 PH_status1_sticky_can_warning_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_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 PH_status1_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 PH_status1_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 PH_status1_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 PH_status1_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 PH_status1_sticky_hardware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_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 PH_status1_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 PH_status1_sticky_firmware_fault_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_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 PH_status1_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 PH_status1_sticky_has_reset_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status1_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 PH_status1_sticky_has_reset_is_in_range(uint8_t value); + + +#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 2ac5a395f2..9757f44fe0 100644 --- a/hal/src/main/native/cpp/jni/PortsJNI.cpp +++ b/hal/src/main/native/cpp/jni/PortsJNI.cpp @@ -272,4 +272,30 @@ Java_edu_wpi_first_hal_PortsJNI_getNumREVPDHChannels jint value = HAL_GetNumREVPDHChannels(); return value; } + +/* + * Class: edu_wpi_first_hal_PortsJNI + * Method: getNumREVPHModules + * Signature: ()I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_PortsJNI_getNumREVPHModules + (JNIEnv* env, jclass) +{ + jint value = HAL_GetNumREVPHModules(); + return value; +} + +/* + * Class: edu_wpi_first_hal_PortsJNI + * Method: getNumREVPHChannels + * Signature: ()I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_PortsJNI_getNumREVPHChannels + (JNIEnv* env, jclass) +{ + jint value = HAL_GetNumREVPHChannels(); + return value; +} } // extern "C" diff --git a/hal/src/main/native/cpp/jni/REVPHJNI.cpp b/hal/src/main/native/cpp/jni/REVPHJNI.cpp new file mode 100644 index 0000000000..3e9943020c --- /dev/null +++ b/hal/src/main/native/cpp/jni/REVPHJNI.cpp @@ -0,0 +1,191 @@ +// 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 "HALUtil.h" +#include "edu_wpi_first_hal_REVPHJNI.h" +#include "hal/Ports.h" +#include "hal/REVPH.h" +#include "hal/handles/HandlesInternal.h" + +using namespace hal; + +extern "C" { + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: initialize + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_REVPHJNI_initialize + (JNIEnv* env, jclass, jint module) +{ + int32_t status = 0; + auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first"); + auto handle = HAL_InitializeREVPH(module, stack.c_str(), &status); + CheckStatusForceThrow(env, status); + return handle; +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: free + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_REVPHJNI_free + (JNIEnv* env, jclass, jint handle) +{ + HAL_FreeREVPH(handle); +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: checkSolenoidChannel + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_REVPHJNI_checkSolenoidChannel + (JNIEnv*, jclass, jint channel) +{ + return HAL_CheckREVPHSolenoidChannel(channel); +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: getCompressor + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_REVPHJNI_getCompressor + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetREVPHCompressor(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: setClosedLoopControl + * Signature: (IZ)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_REVPHJNI_setClosedLoopControl + (JNIEnv* env, jclass, jint handle, jboolean enabled) +{ + int32_t status = 0; + HAL_SetREVPHClosedLoopControl(handle, enabled, &status); + CheckStatus(env, status, false); +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: getClosedLoopControl + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_REVPHJNI_getClosedLoopControl + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetREVPHClosedLoopControl(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: getPressureSwitch + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_REVPHJNI_getPressureSwitch + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetREVPHPressureSwitch(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: getAnalogPressure + * Signature: (II)D + */ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_hal_REVPHJNI_getAnalogPressure + (JNIEnv* env, jclass, jint handle, jint channel) +{ + int32_t status = 0; + auto result = HAL_GetREVPHAnalogPressure(handle, channel, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: getCompressorCurrent + * Signature: (I)D + */ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_hal_REVPHJNI_getCompressorCurrent + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetREVPHCompressorCurrent(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: getSolenoids + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_REVPHJNI_getSolenoids + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetREVPHSolenoids(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: setSolenoids + * Signature: (III)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_REVPHJNI_setSolenoids + (JNIEnv* env, jclass, jint handle, jint mask, jint value) +{ + int32_t status = 0; + HAL_SetREVPHSolenoids(handle, mask, value, &status); + CheckStatus(env, status, false); +} + +/* + * Class: edu_wpi_first_hal_REVPHJNI + * Method: fireOneShot + * Signature: (III)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_REVPHJNI_fireOneShot + (JNIEnv* env, jclass, jint handle, jint index, jint durMs) +{ + int32_t status = 0; + HAL_FireREVPHOneShot(handle, index, durMs, &status); + CheckStatus(env, status, false); +} + +} // extern "C" diff --git a/hal/src/main/native/cpp/jni/simulation/REVPHDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/REVPHDataJNI.cpp new file mode 100644 index 0000000000..da473de88d --- /dev/null +++ b/hal/src/main/native/cpp/jni/simulation/REVPHDataJNI.cpp @@ -0,0 +1,365 @@ +// 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 "CallbackStore.h" +#include "edu_wpi_first_hal_simulation_REVPHDataJNI.h" +#include "hal/simulation/REVPHData.h" + +using namespace hal; + +extern "C" { + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: registerInitializedCallback + * Signature: (ILjava/lang/Object;Z)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerInitializedCallback + (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify) +{ + return sim::AllocateCallback(env, index, callback, initialNotify, + &HALSIM_RegisterREVPHInitializedCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: cancelInitializedCallback + * Signature: (II)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelInitializedCallback + (JNIEnv* env, jclass, jint index, jint handle) +{ + return sim::FreeCallback(env, handle, index, + &HALSIM_CancelREVPHInitializedCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: getInitialized + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getInitialized + (JNIEnv*, jclass, jint index) +{ + return HALSIM_GetREVPHInitialized(index); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: setInitialized + * Signature: (IZ)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setInitialized + (JNIEnv*, jclass, jint index, jboolean value) +{ + HALSIM_SetREVPHInitialized(index, value); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: registerSolenoidOutputCallback + * Signature: (IILjava/lang/Object;Z)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerSolenoidOutputCallback + (JNIEnv* env, jclass, jint index, jint channel, jobject callback, + jboolean initialNotify) +{ + return sim::AllocateChannelCallback( + env, index, channel, callback, initialNotify, + &HALSIM_RegisterREVPHSolenoidOutputCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: cancelSolenoidOutputCallback + * Signature: (III)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelSolenoidOutputCallback + (JNIEnv* env, jclass, jint index, jint channel, jint handle) +{ + return sim::FreeChannelCallback(env, handle, index, channel, + &HALSIM_CancelREVPHSolenoidOutputCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: getSolenoidOutput + * Signature: (II)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getSolenoidOutput + (JNIEnv*, jclass, jint index, jint channel) +{ + return HALSIM_GetREVPHSolenoidOutput(index, channel); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: setSolenoidOutput + * Signature: (IIZ)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setSolenoidOutput + (JNIEnv*, jclass, jint index, jint channel, jboolean value) +{ + HALSIM_SetREVPHSolenoidOutput(index, channel, value); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: registerCompressorOnCallback + * Signature: (ILjava/lang/Object;Z)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerCompressorOnCallback + (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify) +{ + return sim::AllocateCallback(env, index, callback, initialNotify, + &HALSIM_RegisterREVPHCompressorOnCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: cancelCompressorOnCallback + * Signature: (II)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelCompressorOnCallback + (JNIEnv* env, jclass, jint index, jint handle) +{ + return sim::FreeCallback(env, handle, index, + &HALSIM_CancelREVPHCompressorOnCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: getCompressorOn + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getCompressorOn + (JNIEnv*, jclass, jint index) +{ + return HALSIM_GetREVPHCompressorOn(index); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: setCompressorOn + * Signature: (IZ)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setCompressorOn + (JNIEnv*, jclass, jint index, jboolean value) +{ + HALSIM_SetREVPHCompressorOn(index, value); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: registerClosedLoopEnabledCallback + * Signature: (ILjava/lang/Object;Z)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerClosedLoopEnabledCallback + (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify) +{ + return sim::AllocateCallback(env, index, callback, initialNotify, + &HALSIM_RegisterREVPHClosedLoopEnabledCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: cancelClosedLoopEnabledCallback + * Signature: (II)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelClosedLoopEnabledCallback + (JNIEnv* env, jclass, jint index, jint handle) +{ + return sim::FreeCallback(env, handle, index, + &HALSIM_CancelREVPHClosedLoopEnabledCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: getClosedLoopEnabled + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getClosedLoopEnabled + (JNIEnv*, jclass, jint index) +{ + return HALSIM_GetREVPHClosedLoopEnabled(index); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: setClosedLoopEnabled + * Signature: (IZ)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setClosedLoopEnabled + (JNIEnv*, jclass, jint index, jboolean value) +{ + HALSIM_SetREVPHClosedLoopEnabled(index, value); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: registerPressureSwitchCallback + * Signature: (ILjava/lang/Object;Z)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerPressureSwitchCallback + (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify) +{ + return sim::AllocateCallback(env, index, callback, initialNotify, + &HALSIM_RegisterREVPHPressureSwitchCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: cancelPressureSwitchCallback + * Signature: (II)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelPressureSwitchCallback + (JNIEnv* env, jclass, jint index, jint handle) +{ + return sim::FreeCallback(env, handle, index, + &HALSIM_CancelREVPHPressureSwitchCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: getPressureSwitch + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getPressureSwitch + (JNIEnv*, jclass, jint index) +{ + return HALSIM_GetREVPHPressureSwitch(index); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: setPressureSwitch + * Signature: (IZ)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setPressureSwitch + (JNIEnv*, jclass, jint index, jboolean value) +{ + HALSIM_SetREVPHPressureSwitch(index, value); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: registerCompressorCurrentCallback + * Signature: (ILjava/lang/Object;Z)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerCompressorCurrentCallback + (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify) +{ + return sim::AllocateCallback(env, index, callback, initialNotify, + &HALSIM_RegisterREVPHCompressorCurrentCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: cancelCompressorCurrentCallback + * Signature: (II)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelCompressorCurrentCallback + (JNIEnv* env, jclass, jint index, jint handle) +{ + return sim::FreeCallback(env, handle, index, + &HALSIM_CancelREVPHCompressorCurrentCallback); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: getCompressorCurrent + * Signature: (I)D + */ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getCompressorCurrent + (JNIEnv*, jclass, jint index) +{ + return HALSIM_GetREVPHCompressorCurrent(index); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: setCompressorCurrent + * Signature: (ID)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setCompressorCurrent + (JNIEnv*, jclass, jint index, jdouble value) +{ + HALSIM_SetREVPHCompressorCurrent(index, value); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: registerAllNonSolenoidCallbacks + * Signature: (ILjava/lang/Object;Z)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerAllNonSolenoidCallbacks + (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify) +{ + sim::AllocateCallback( + env, index, callback, initialNotify, + [](int32_t index, HAL_NotifyCallback cb, void* param, HAL_Bool in) { + HALSIM_RegisterREVPHAllNonSolenoidCallbacks(index, cb, param, in); + return 0; + }); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: registerAllSolenoidCallbacks + * Signature: (IILjava/lang/Object;Z)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerAllSolenoidCallbacks + (JNIEnv* env, jclass, jint index, jint channel, jobject callback, + jboolean initialNotify) +{ + sim::AllocateChannelCallback( + env, index, channel, callback, initialNotify, + [](int32_t index, int32_t channel, HAL_NotifyCallback cb, void* param, + HAL_Bool in) { + HALSIM_RegisterREVPHAllSolenoidCallbacks(index, channel, cb, param, in); + return 0; + }); +} + +/* + * Class: edu_wpi_first_hal_simulation_REVPHDataJNI + * Method: resetData + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_simulation_REVPHDataJNI_resetData + (JNIEnv*, jclass, jint index) +{ + HALSIM_ResetREVPHData(index); +} + +} // extern "C" diff --git a/hal/src/main/native/include/hal/Ports.h b/hal/src/main/native/include/hal/Ports.h index aac6f4a639..a21244644e 100644 --- a/hal/src/main/native/include/hal/Ports.h +++ b/hal/src/main/native/include/hal/Ports.h @@ -156,6 +156,20 @@ int32_t HAL_GetNumREVPDHModules(void); */ int32_t HAL_GetNumREVPDHChannels(void); +/** + * Gets the number of PH modules in the current system. + * + * @return the number of PH modules + */ +int32_t HAL_GetNumREVPHModules(void); + +/** + * Gets the number of PH channels in the current system. + * + * @return the number of PH channels + */ +int32_t HAL_GetNumREVPHChannels(void); + /** * Gets the number of duty cycle inputs in the current system. * diff --git a/hal/src/main/native/include/hal/REVPH.h b/hal/src/main/native/include/hal/REVPH.h new file mode 100644 index 0000000000..0cab15e090 --- /dev/null +++ b/hal/src/main/native/include/hal/REVPH.h @@ -0,0 +1,49 @@ +// 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_ph REV PH Functions + * @ingroup hal_capi + * @{ + */ + +#ifdef __cplusplus +extern "C" { +#endif + +HAL_REVPHHandle HAL_InitializeREVPH(int32_t module, + const char* allocationLocation, + int32_t* status); + +void HAL_FreeREVPH(HAL_REVPHHandle handle); + +HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel); +HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module); + +HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status); +void HAL_SetREVPHClosedLoopControl(HAL_REVPHHandle handle, HAL_Bool enabled, + int32_t* status); +HAL_Bool HAL_GetREVPHClosedLoopControl(HAL_REVPHHandle handle, int32_t* status); +HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status); +double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status); +double HAL_GetREVPHAnalogPressure(HAL_REVPHHandle handle, int32_t channel, + int32_t* status); + +int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status); +void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values, + int32_t* status); + +void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs, + 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 d5a341c54e..6b954470a1 100644 --- a/hal/src/main/native/include/hal/Types.h +++ b/hal/src/main/native/include/hal/Types.h @@ -68,6 +68,8 @@ typedef HAL_Handle HAL_CTREPCMHandle; typedef HAL_Handle HAL_REVPDHHandle; +typedef HAL_Handle HAL_REVPHHandle; + 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 2c9c44cfe7..3307a0792c 100644 --- a/hal/src/main/native/include/hal/handles/HandlesInternal.h +++ b/hal/src/main/native/include/hal/handles/HandlesInternal.h @@ -68,7 +68,8 @@ enum class HAL_HandleEnum { AddressableLED = 23, CTREPCM = 24, CTREPDP = 25, - REVPDH = 26 + REVPDH = 26, + REVPH = 27, }; /** diff --git a/hal/src/main/native/include/hal/simulation/REVPHData.h b/hal/src/main/native/include/hal/simulation/REVPHData.h new file mode 100644 index 0000000000..17e4f2c127 --- /dev/null +++ b/hal/src/main/native/include/hal/simulation/REVPHData.h @@ -0,0 +1,80 @@ +// 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 "hal/Types.h" +#include "hal/simulation/NotifyListener.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void HALSIM_ResetREVPHData(int32_t index); +int32_t HALSIM_RegisterREVPHInitializedCallback(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify); +void HALSIM_CancelREVPHInitializedCallback(int32_t index, int32_t uid); +HAL_Bool HALSIM_GetREVPHInitialized(int32_t index); +void HALSIM_SetREVPHInitialized(int32_t index, HAL_Bool solenoidInitialized); + +int32_t HALSIM_RegisterREVPHSolenoidOutputCallback(int32_t index, + int32_t channel, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify); +void HALSIM_CancelREVPHSolenoidOutputCallback(int32_t index, int32_t channel, + int32_t uid); +HAL_Bool HALSIM_GetREVPHSolenoidOutput(int32_t index, int32_t channel); +void HALSIM_SetREVPHSolenoidOutput(int32_t index, int32_t channel, + HAL_Bool solenoidOutput); + +int32_t HALSIM_RegisterREVPHCompressorOnCallback(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify); +void HALSIM_CancelREVPHCompressorOnCallback(int32_t index, int32_t uid); +HAL_Bool HALSIM_GetREVPHCompressorOn(int32_t index); +void HALSIM_SetREVPHCompressorOn(int32_t index, HAL_Bool compressorOn); + +int32_t HALSIM_RegisterREVPHClosedLoopEnabledCallback( + int32_t index, HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify); +void HALSIM_CancelREVPHClosedLoopEnabledCallback(int32_t index, int32_t uid); +HAL_Bool HALSIM_GetREVPHClosedLoopEnabled(int32_t index); +void HALSIM_SetREVPHClosedLoopEnabled(int32_t index, + HAL_Bool closedLoopEnabled); + +int32_t HALSIM_RegisterREVPHPressureSwitchCallback(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify); +void HALSIM_CancelREVPHPressureSwitchCallback(int32_t index, int32_t uid); +HAL_Bool HALSIM_GetREVPHPressureSwitch(int32_t index); +void HALSIM_SetREVPHPressureSwitch(int32_t index, HAL_Bool pressureSwitch); + +int32_t HALSIM_RegisterREVPHCompressorCurrentCallback( + int32_t index, HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify); +void HALSIM_CancelREVPHCompressorCurrentCallback(int32_t index, int32_t uid); +double HALSIM_GetREVPHCompressorCurrent(int32_t index); +void HALSIM_SetREVPHCompressorCurrent(int32_t index, double compressorCurrent); + +void HALSIM_GetREVPHAllSolenoids(int32_t index, uint8_t* values); +void HALSIM_SetREVPHAllSolenoids(int32_t index, uint8_t values); + +void HALSIM_RegisterREVPHAllNonSolenoidCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify); + +void HALSIM_RegisterREVPHAllSolenoidCallbacks(int32_t index, int32_t channel, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/hal/src/main/native/sim/CTREPCM.cpp b/hal/src/main/native/sim/CTREPCM.cpp index 9d3119cf83..93f91356b9 100644 --- a/hal/src/main/native/sim/CTREPCM.cpp +++ b/hal/src/main/native/sim/CTREPCM.cpp @@ -48,7 +48,7 @@ HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module, pcm->previousAllocation); } else { hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PCM", 0, - kNumAccumulators, module); + kNumCTREPCMModules, module); } return HAL_kInvalidHandle; // failed to allocate. Pass error back. } diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index 9b93d19687..7910ccf657 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -74,6 +74,7 @@ void InitializeHAL() { InitializeEncoderData(); InitializeI2CData(); InitializeCTREPCMData(); + InitializeREVPHData(); InitializePowerDistributionData(); InitializePWMData(); InitializeRelayData(); @@ -107,6 +108,7 @@ void InitializeHAL() { InitializePorts(); InitializePower(); InitializeCTREPCM(); + InitializeREVPH(); InitializePWM(); InitializeRelay(); InitializeSerialPort(); diff --git a/hal/src/main/native/sim/HALInitializer.h b/hal/src/main/native/sim/HALInitializer.h index 3d96d8a18a..308e2f47cf 100644 --- a/hal/src/main/native/sim/HALInitializer.h +++ b/hal/src/main/native/sim/HALInitializer.h @@ -32,6 +32,7 @@ extern void InitializeDriverStationData(); extern void InitializeEncoderData(); extern void InitializeI2CData(); extern void InitializeCTREPCMData(); +extern void InitializeREVPHData(); extern void InitializePowerDistributionData(); extern void InitializePWMData(); extern void InitializeRelayData(); @@ -65,6 +66,7 @@ extern void InitializePowerDistribution(); extern void InitializePorts(); extern void InitializePower(); extern void InitializeCTREPCM(); +extern void InitializeREVPH(); extern void InitializePWM(); extern void InitializeRelay(); extern void InitializeSerialPort(); diff --git a/hal/src/main/native/sim/Ports.cpp b/hal/src/main/native/sim/Ports.cpp index 77cb22d246..08ca975e71 100644 --- a/hal/src/main/native/sim/Ports.cpp +++ b/hal/src/main/native/sim/Ports.cpp @@ -73,6 +73,12 @@ int32_t HAL_GetNumREVPDHModules(void) { int32_t HAL_GetNumREVPDHChannels(void) { return kNumREVPDHChannels; } +int32_t HAL_GetNumREVPHModules(void) { + return kNumREVPHModules; +} +int32_t HAL_GetNumREVPHChannels(void) { + return kNumREVPHChannels; +} 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 ba219c2f4c..2d1a205e27 100644 --- a/hal/src/main/native/sim/PortsInternal.h +++ b/hal/src/main/native/sim/PortsInternal.h @@ -29,4 +29,6 @@ constexpr int32_t kNumREVPDHModules = 63; constexpr int32_t kNumREVPDHChannels = 24; constexpr int32_t kNumDutyCycles = 8; constexpr int32_t kNumAddressableLEDs = 1; +constexpr int32_t kNumREVPHModules = 63; +constexpr int32_t kNumREVPHChannels = 16; } // namespace hal diff --git a/hal/src/main/native/sim/REVPH.cpp b/hal/src/main/native/sim/REVPH.cpp new file mode 100644 index 0000000000..9ddb655848 --- /dev/null +++ b/hal/src/main/native/sim/REVPH.cpp @@ -0,0 +1,181 @@ +// 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 "hal/REVPH.h" + +#include "HALInitializer.h" +#include "HALInternal.h" +#include "PortsInternal.h" +#include "hal/CANAPI.h" +#include "hal/Errors.h" +#include "hal/handles/IndexedHandleResource.h" +#include "mockdata/REVPHDataInternal.h" + +using namespace hal; + +namespace { +struct PCM { + int32_t module; + wpi::mutex lock; + std::string previousAllocation; +}; +} // namespace + +static IndexedHandleResource* pcmHandles; + +namespace hal::init { +void InitializeREVPH() { + static IndexedHandleResource + pH; + pcmHandles = &pH; +} +} // namespace hal::init + +HAL_REVPHHandle HAL_InitializeREVPH(int32_t module, + const char* allocationLocation, + int32_t* status) { + hal::init::CheckInit(); + + if (module == 0) { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1, + kNumREVPHModules, module); + return HAL_kInvalidHandle; + } + + HAL_REVPHHandle handle; + auto pcm = pcmHandles->Allocate(module, &handle, status); + + if (*status != 0) { + if (pcm) { + hal::SetLastErrorPreviouslyAllocated(status, "REV PH", module, + pcm->previousAllocation); + } else { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1, + kNumREVPHModules, module); + } + return HAL_kInvalidHandle; // failed to allocate. Pass error back. + } + + pcm->previousAllocation = allocationLocation ? allocationLocation : ""; + pcm->module = module; + + SimREVPHData[module].initialized = true; + // Enable closed loop + SimREVPHData[module].closedLoopEnabled = true; + + return handle; +} + +void HAL_FreeREVPH(HAL_REVPHHandle handle) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + pcmHandles->Free(handle); + return; + } + SimREVPHData[pcm->module].initialized = false; + pcmHandles->Free(handle); +} + +HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module) { + return module >= 1 && module < kNumREVPDHModules; +} + +HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel) { + return channel < kNumREVPHChannels && channel >= 0; +} + +HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + return SimREVPHData[pcm->module].compressorOn; +} + +void HAL_SetREVPHClosedLoopControl(HAL_REVPHHandle handle, HAL_Bool enabled, + int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + SimREVPHData[pcm->module].closedLoopEnabled = enabled; +} + +HAL_Bool HAL_GetREVPHClosedLoopControl(HAL_REVPHHandle handle, + int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + return SimREVPHData[pcm->module].closedLoopEnabled; +} + +HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + return SimREVPHData[pcm->module].pressureSwitch; +} + +double HAL_GetREVPHAnalogPressure(HAL_REVPHHandle handle, int32_t channel, + int32_t* status) { + return 0; +} + +double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + return SimREVPHData[pcm->module].compressorCurrent; +} + +int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + std::scoped_lock lock{pcm->lock}; + auto& data = SimREVPHData[pcm->module].solenoidOutput; + uint8_t ret = 0; + for (int i = 0; i < kNumREVPHChannels; i++) { + ret |= (data[i] << i); + } + return ret; +} +void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values, + int32_t* status) { + auto pcm = pcmHandles->Get(handle); + if (pcm == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + auto& data = SimREVPHData[pcm->module].solenoidOutput; + std::scoped_lock lock{pcm->lock}; + for (int i = 0; i < kNumREVPHChannels; i++) { + auto indexMask = (1 << i); + if ((mask & indexMask) != 0) { + data[i] = (values & indexMask) != 0; + } + } +} + +void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs, + int32_t* status) {} diff --git a/hal/src/main/native/sim/mockdata/REVPHData.cpp b/hal/src/main/native/sim/mockdata/REVPHData.cpp new file mode 100644 index 0000000000..6c0ed5abd6 --- /dev/null +++ b/hal/src/main/native/sim/mockdata/REVPHData.cpp @@ -0,0 +1,83 @@ +// 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 "../PortsInternal.h" +#include "REVPHDataInternal.h" + +using namespace hal; + +namespace hal::init { +void InitializeREVPHData() { + static REVPHData spd[kNumREVPHModules]; + ::hal::SimREVPHData = spd; +} +} // namespace hal::init + +REVPHData* hal::SimREVPHData; +void REVPHData::ResetData() { + for (int i = 0; i < kNumREVPHChannels; i++) { + solenoidOutput[i].Reset(false); + } + initialized.Reset(false); + compressorOn.Reset(false); + closedLoopEnabled.Reset(true); + pressureSwitch.Reset(false); + compressorCurrent.Reset(0.0); +} + +extern "C" { +void HALSIM_ResetREVPHData(int32_t index) { + SimREVPHData[index].ResetData(); +} + +#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \ + HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, REVPH##CAPINAME, SimREVPHData, \ + LOWERNAME) + +HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(HAL_Bool, HALSIM, REVPHSolenoidOutput, + SimREVPHData, solenoidOutput) +DEFINE_CAPI(HAL_Bool, Initialized, initialized) +DEFINE_CAPI(HAL_Bool, CompressorOn, compressorOn) +DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, closedLoopEnabled) +DEFINE_CAPI(HAL_Bool, PressureSwitch, pressureSwitch) +DEFINE_CAPI(double, CompressorCurrent, compressorCurrent) + +void HALSIM_GetREVPHAllSolenoids(int32_t index, uint8_t* values) { + auto& data = SimREVPHData[index].solenoidOutput; + uint8_t ret = 0; + for (int i = 0; i < kNumCTRESolenoidChannels; i++) { + ret |= (data[i] << i); + } + *values = ret; +} + +void HALSIM_SetREVPHAllSolenoids(int32_t index, uint8_t values) { + auto& data = SimREVPHData[index].solenoidOutput; + for (int i = 0; i < kNumCTRESolenoidChannels; i++) { + data[i] = (values & 0x1) != 0; + values >>= 1; + } +} + +#define REGISTER(NAME) \ + SimREVPHData[index].NAME.RegisterCallback(callback, param, initialNotify) + +void HALSIM_RegisterREVPHAllNonSolenoidCallbacks(int32_t index, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) { + REGISTER(initialized); + REGISTER(compressorOn); + REGISTER(closedLoopEnabled); + REGISTER(pressureSwitch); + REGISTER(compressorCurrent); +} + +void HALSIM_RegisterREVPHAllSolenoidCallbacks(int32_t index, int32_t channel, + HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify) { + REGISTER(solenoidOutput[channel]); +} +} // extern "C" diff --git a/hal/src/main/native/sim/mockdata/REVPHDataInternal.h b/hal/src/main/native/sim/mockdata/REVPHDataInternal.h new file mode 100644 index 0000000000..ebf4964e9d --- /dev/null +++ b/hal/src/main/native/sim/mockdata/REVPHDataInternal.h @@ -0,0 +1,43 @@ +// 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 "../PortsInternal.h" +#include "hal/simulation/REVPHData.h" +#include "hal/simulation/SimDataValue.h" + +namespace hal { +class REVPHData { + HAL_SIMDATAVALUE_DEFINE_NAME(Initialized) + HAL_SIMDATAVALUE_DEFINE_NAME(SolenoidOutput) + HAL_SIMDATAVALUE_DEFINE_NAME(CompressorOn) + HAL_SIMDATAVALUE_DEFINE_NAME(ClosedLoopEnabled) + HAL_SIMDATAVALUE_DEFINE_NAME(PressureSwitch) + HAL_SIMDATAVALUE_DEFINE_NAME(CompressorCurrent) + + static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr HAL_Bool + GetSolenoidOutputDefault() { + return false; + } + + public: + SimDataValue initialized{ + false}; + SimDataValue + solenoidOutput[kNumREVPHChannels]; + SimDataValue compressorOn{ + false}; + SimDataValue + closedLoopEnabled{true}; + SimDataValue pressureSwitch{ + false}; + SimDataValue + compressorCurrent{0.0}; + + virtual void ResetData(); +}; +extern REVPHData* SimREVPHData; +} // namespace hal diff --git a/wpilibc/src/main/native/cpp/PneumaticsBase.cpp b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp index ebe9775ee8..2291c87931 100644 --- a/wpilibc/src/main/native/cpp/PneumaticsBase.cpp +++ b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp @@ -6,6 +6,7 @@ #include "frc/Errors.h" #include "frc/PneumaticsControlModule.h" +#include "frc/PneumaticsHub.h" #include "frc/SensorUtil.h" using namespace frc; @@ -14,6 +15,8 @@ std::shared_ptr PneumaticsBase::GetForType( int module, PneumaticsModuleType moduleType) { if (moduleType == PneumaticsModuleType::CTREPCM) { return PneumaticsControlModule::GetForModule(module); + } else if (moduleType == PneumaticsModuleType::REVPH) { + return PneumaticsHub::GetForModule(module); } throw FRC_MakeError(err::InvalidParameter, "{}", moduleType); } @@ -21,6 +24,8 @@ std::shared_ptr PneumaticsBase::GetForType( int PneumaticsBase::GetDefaultForType(PneumaticsModuleType moduleType) { if (moduleType == PneumaticsModuleType::CTREPCM) { return SensorUtil::GetDefaultCTREPCMModule(); + } else if (moduleType == PneumaticsModuleType::REVPH) { + return SensorUtil::GetDefaultREVPHModule(); } throw FRC_MakeError(err::InvalidParameter, "{}", moduleType); } diff --git a/wpilibc/src/main/native/cpp/PneumaticsHub.cpp b/wpilibc/src/main/native/cpp/PneumaticsHub.cpp new file mode 100644 index 0000000000..565c7a82d3 --- /dev/null +++ b/wpilibc/src/main/native/cpp/PneumaticsHub.cpp @@ -0,0 +1,210 @@ +// 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/PneumaticsHub.h" + +#include +#include +#include + +#include "frc/Compressor.h" +#include "frc/DoubleSolenoid.h" +#include "frc/Errors.h" +#include "frc/SensorUtil.h" +#include "frc/Solenoid.h" + +using namespace frc; + +wpi::mutex PneumaticsHub::m_handleLock; +std::unique_ptr>> + PneumaticsHub::m_handleMap = nullptr; + +// Always called under lock, so we can avoid the double lock from the magic +// static +std::weak_ptr& PneumaticsHub::GetDataStore( + int module) { + if (!m_handleMap) { + m_handleMap = std::make_unique< + wpi::DenseMap>>(); + } + return (*m_handleMap)[module]; +} + +class PneumaticsHub::DataStore { + public: + explicit DataStore(int module, const char* stackTrace) { + int32_t status = 0; + HAL_REVPHHandle handle = HAL_InitializeREVPH(module, stackTrace, &status); + FRC_CheckErrorStatus(status, "Module {}", module); + m_moduleObject = PneumaticsHub{handle, module}; + m_moduleObject.m_dataStore = + std::shared_ptr{this, wpi::NullDeleter()}; + } + + ~DataStore() noexcept { HAL_FreeREVPH(m_moduleObject.m_handle); } + + DataStore(DataStore&&) = delete; + DataStore& operator=(DataStore&&) = delete; + + private: + friend class PneumaticsHub; + uint32_t m_reservedMask{0}; + bool m_compressorReserved{false}; + wpi::mutex m_reservedLock; + PneumaticsHub m_moduleObject{HAL_kInvalidHandle, 0}; +}; + +PneumaticsHub::PneumaticsHub() + : PneumaticsHub{SensorUtil::GetDefaultREVPHModule()} {} + +PneumaticsHub::PneumaticsHub(int module) { + std::string stackTrace = wpi::GetStackTrace(1); + std::scoped_lock lock(m_handleLock); + auto& res = GetDataStore(module); + m_dataStore = res.lock(); + if (!m_dataStore) { + m_dataStore = std::make_shared(module, stackTrace.c_str()); + res = m_dataStore; + } + m_handle = m_dataStore->m_moduleObject.m_handle; + m_module = module; +} + +PneumaticsHub::PneumaticsHub(HAL_REVPHHandle handle, int module) + : m_handle{handle}, m_module{module} {} + +bool PneumaticsHub::GetCompressor() const { + int32_t status = 0; + auto result = HAL_GetREVPHCompressor(m_handle, &status); + FRC_CheckErrorStatus(status, "Module {}", m_module); + return result; +} + +void PneumaticsHub::SetClosedLoopControl(bool enabled) { + int32_t status = 0; + HAL_SetREVPHClosedLoopControl(m_handle, enabled, &status); + FRC_CheckErrorStatus(status, "Module {}", m_module); +} + +bool PneumaticsHub::GetClosedLoopControl() const { + int32_t status = 0; + auto result = HAL_GetREVPHClosedLoopControl(m_handle, &status); + FRC_CheckErrorStatus(status, "Module {}", m_module); + return result; +} + +bool PneumaticsHub::GetPressureSwitch() const { + int32_t status = 0; + auto result = HAL_GetREVPHPressureSwitch(m_handle, &status); + FRC_CheckErrorStatus(status, "Module {}", m_module); + return result; +} + +double PneumaticsHub::GetCompressorCurrent() const { + int32_t status = 0; + auto result = HAL_GetREVPHCompressorCurrent(m_handle, &status); + FRC_CheckErrorStatus(status, "Module {}", m_module); + return result; +} + +void PneumaticsHub::SetSolenoids(int mask, int values) { + int32_t status = 0; + HAL_SetREVPHSolenoids(m_handle, mask, values, &status); + FRC_CheckErrorStatus(status, "Module {}", m_module); +} + +int PneumaticsHub::GetSolenoids() const { + int32_t status = 0; + auto result = HAL_GetREVPHSolenoids(m_handle, &status); + FRC_CheckErrorStatus(status, "Module {}", m_module); + return result; +} + +int PneumaticsHub::GetModuleNumber() const { + return m_module; +} + +int PneumaticsHub::GetSolenoidDisabledList() const { + return 0; + // TODO Fix me + // int32_t status = 0; + // auto result = HAL_GetREVPHSolenoidDisabledList(m_handle, &status); + // FRC_CheckErrorStatus(status, "Module {}", m_module); + // return result; +} + +void PneumaticsHub::FireOneShot(int index) { + // TODO Fix me + // int32_t status = 0; + // HAL_FireREVPHOneShot(m_handle, index, &status); + // FRC_CheckErrorStatus(status, "Module {}", m_module); +} + +void PneumaticsHub::SetOneShotDuration(int index, units::second_t duration) { + // TODO Fix me + // int32_t status = 0; + // units::millisecond_t millis = duration; + // HAL_SetREVPHOneShotDuration(m_handle, index, millis.to(), + // &status); FRC_CheckErrorStatus(status, "Module {}", m_module); +} + +bool PneumaticsHub::CheckSolenoidChannel(int channel) const { + return HAL_CheckREVPHSolenoidChannel(channel); +} + +int PneumaticsHub::CheckAndReserveSolenoids(int mask) { + std::scoped_lock lock{m_dataStore->m_reservedLock}; + uint32_t uMask = static_cast(mask); + if ((m_dataStore->m_reservedMask & uMask) != 0) { + return m_dataStore->m_reservedMask & uMask; + } + m_dataStore->m_reservedMask |= uMask; + return 0; +} + +void PneumaticsHub::UnreserveSolenoids(int mask) { + std::scoped_lock lock{m_dataStore->m_reservedLock}; + m_dataStore->m_reservedMask &= ~(static_cast(mask)); +} + +bool PneumaticsHub::ReserveCompressor() { + std::scoped_lock lock{m_dataStore->m_reservedLock}; + if (m_dataStore->m_compressorReserved) { + return false; + } + m_dataStore->m_compressorReserved = true; + return true; +} + +void PneumaticsHub::UnreserveCompressor() { + std::scoped_lock lock{m_dataStore->m_reservedLock}; + m_dataStore->m_compressorReserved = false; +} + +Solenoid PneumaticsHub::MakeSolenoid(int channel) { + return Solenoid{m_module, PneumaticsModuleType::REVPH, channel}; +} + +DoubleSolenoid PneumaticsHub::MakeDoubleSolenoid(int forwardChannel, + int reverseChannel) { + return DoubleSolenoid{m_module, PneumaticsModuleType::REVPH, forwardChannel, + reverseChannel}; +} + +Compressor PneumaticsHub::MakeCompressor() { + return Compressor{m_module, PneumaticsModuleType::REVPH}; +} + +std::shared_ptr PneumaticsHub::GetForModule(int module) { + std::string stackTrace = wpi::GetStackTrace(1); + std::scoped_lock lock(m_handleLock); + auto& res = GetDataStore(module); + std::shared_ptr dataStore = res.lock(); + if (!dataStore) { + dataStore = std::make_shared(module, stackTrace.c_str()); + res = dataStore; + } + + return std::shared_ptr{dataStore, &dataStore->m_moduleObject}; +} diff --git a/wpilibc/src/main/native/cpp/SensorUtil.cpp b/wpilibc/src/main/native/cpp/SensorUtil.cpp index 429eb67b05..55aacce0e0 100644 --- a/wpilibc/src/main/native/cpp/SensorUtil.cpp +++ b/wpilibc/src/main/native/cpp/SensorUtil.cpp @@ -23,6 +23,10 @@ int SensorUtil::GetDefaultCTREPCMModule() { return 0; } +int SensorUtil::GetDefaultREVPHModule() { + return 1; +} + bool SensorUtil::CheckDigitalChannel(int channel) { return HAL_CheckDIOChannel(channel); } diff --git a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp new file mode 100644 index 0000000000..dd6dba829f --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp @@ -0,0 +1,139 @@ +// 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/simulation/REVPHSim.h" + +#include +#include + +#include + +#include "frc/SensorUtil.h" + +using namespace frc; +using namespace frc::sim; + +REVPHSim::REVPHSim() : m_index{SensorUtil::GetDefaultREVPHModule()} {} + +REVPHSim::REVPHSim(int module) : m_index{module} {} + +REVPHSim::REVPHSim(const PneumaticsBase& pneumatics) + : m_index{pneumatics.GetModuleNumber()} {} + +std::unique_ptr REVPHSim::RegisterInitializedCallback( + NotifyCallback callback, bool initialNotify) { + auto store = std::make_unique( + m_index, -1, callback, &HALSIM_CancelREVPHInitializedCallback); + store->SetUid(HALSIM_RegisterREVPHInitializedCallback( + m_index, &CallbackStoreThunk, store.get(), initialNotify)); + return store; +} + +bool REVPHSim::GetInitialized() const { + return HALSIM_GetREVPHInitialized(m_index); +} + +void REVPHSim::SetInitialized(bool solenoidInitialized) { + HALSIM_SetREVPHInitialized(m_index, solenoidInitialized); +} + +std::unique_ptr REVPHSim::RegisterSolenoidOutputCallback( + int channel, NotifyCallback callback, bool initialNotify) { + auto store = std::make_unique( + m_index, channel, -1, callback, + &HALSIM_CancelREVPHSolenoidOutputCallback); + store->SetUid(HALSIM_RegisterREVPHSolenoidOutputCallback( + m_index, channel, &CallbackStoreThunk, store.get(), initialNotify)); + return store; +} + +bool REVPHSim::GetSolenoidOutput(int channel) const { + return HALSIM_GetREVPHSolenoidOutput(m_index, channel); +} + +void REVPHSim::SetSolenoidOutput(int channel, bool solenoidOutput) { + HALSIM_SetREVPHSolenoidOutput(m_index, channel, solenoidOutput); +} + +std::unique_ptr REVPHSim::RegisterCompressorOnCallback( + NotifyCallback callback, bool initialNotify) { + auto store = std::make_unique( + m_index, -1, callback, &HALSIM_CancelREVPHCompressorOnCallback); + store->SetUid(HALSIM_RegisterREVPHCompressorOnCallback( + m_index, &CallbackStoreThunk, store.get(), initialNotify)); + return store; +} + +bool REVPHSim::GetCompressorOn() const { + return HALSIM_GetREVPHCompressorOn(m_index); +} + +void REVPHSim::SetCompressorOn(bool compressorOn) { + HALSIM_SetREVPHCompressorOn(m_index, compressorOn); +} + +std::unique_ptr REVPHSim::RegisterClosedLoopEnabledCallback( + NotifyCallback callback, bool initialNotify) { + auto store = std::make_unique( + m_index, -1, callback, &HALSIM_CancelREVPHClosedLoopEnabledCallback); + store->SetUid(HALSIM_RegisterREVPHClosedLoopEnabledCallback( + m_index, &CallbackStoreThunk, store.get(), initialNotify)); + return store; +} + +bool REVPHSim::GetClosedLoopEnabled() const { + return HALSIM_GetREVPHClosedLoopEnabled(m_index); +} + +void REVPHSim::SetClosedLoopEnabled(bool closedLoopEnabled) { + HALSIM_SetREVPHClosedLoopEnabled(m_index, closedLoopEnabled); +} + +std::unique_ptr REVPHSim::RegisterPressureSwitchCallback( + NotifyCallback callback, bool initialNotify) { + auto store = std::make_unique( + m_index, -1, callback, &HALSIM_CancelREVPHPressureSwitchCallback); + store->SetUid(HALSIM_RegisterREVPHPressureSwitchCallback( + m_index, &CallbackStoreThunk, store.get(), initialNotify)); + return store; +} + +bool REVPHSim::GetPressureSwitch() const { + return HALSIM_GetREVPHPressureSwitch(m_index); +} + +void REVPHSim::SetPressureSwitch(bool pressureSwitch) { + HALSIM_SetREVPHPressureSwitch(m_index, pressureSwitch); +} + +std::unique_ptr REVPHSim::RegisterCompressorCurrentCallback( + NotifyCallback callback, bool initialNotify) { + auto store = std::make_unique( + m_index, -1, callback, &HALSIM_CancelREVPHCompressorCurrentCallback); + store->SetUid(HALSIM_RegisterREVPHCompressorCurrentCallback( + m_index, &CallbackStoreThunk, store.get(), initialNotify)); + return store; +} + +double REVPHSim::GetCompressorCurrent() const { + return HALSIM_GetREVPHCompressorCurrent(m_index); +} + +void REVPHSim::SetCompressorCurrent(double compressorCurrent) { + HALSIM_SetREVPHCompressorCurrent(m_index, compressorCurrent); +} + +uint8_t REVPHSim::GetAllSolenoidOutputs() const { + uint8_t ret = 0; + HALSIM_GetREVPHAllSolenoids(m_index, &ret); + return ret; +} + +void REVPHSim::SetAllSolenoidOutputs(uint8_t outputs) { + HALSIM_SetREVPHAllSolenoids(m_index, outputs); +} + +void REVPHSim::ResetData() { + HALSIM_ResetREVPHData(m_index); +} diff --git a/wpilibc/src/main/native/include/frc/PneumaticsHub.h b/wpilibc/src/main/native/include/frc/PneumaticsHub.h new file mode 100644 index 0000000000..f6a4b59e0a --- /dev/null +++ b/wpilibc/src/main/native/include/frc/PneumaticsHub.h @@ -0,0 +1,77 @@ +// 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 +#include +#include + +#include "PneumaticsBase.h" + +namespace frc { +class PneumaticsHub : public PneumaticsBase { + public: + PneumaticsHub(); + explicit PneumaticsHub(int module); + + ~PneumaticsHub() override = default; + + bool GetCompressor() const override; + + void SetClosedLoopControl(bool enabled) override; + + bool GetClosedLoopControl() const override; + + bool GetPressureSwitch() const override; + + double GetCompressorCurrent() const override; + + void SetSolenoids(int mask, int values) override; + + int GetSolenoids() const override; + + int GetModuleNumber() const override; + + int GetSolenoidDisabledList() const override; + + void FireOneShot(int index) override; + + void SetOneShotDuration(int index, units::second_t duration) override; + + bool CheckSolenoidChannel(int channel) const override; + + int CheckAndReserveSolenoids(int mask) override; + + void UnreserveSolenoids(int mask) override; + + bool ReserveCompressor() override; + + void UnreserveCompressor() override; + + Solenoid MakeSolenoid(int channel) override; + DoubleSolenoid MakeDoubleSolenoid(int forwardChannel, + int reverseChannel) override; + Compressor MakeCompressor() override; + + private: + class DataStore; + friend class DataStore; + friend class PneumaticsBase; + PneumaticsHub(HAL_REVPHHandle handle, int module); + + static std::shared_ptr GetForModule(int module); + + std::shared_ptr m_dataStore; + HAL_REVPHHandle m_handle; + int m_module; + + static wpi::mutex m_handleLock; + static std::unique_ptr>> + m_handleMap; + static std::weak_ptr& GetDataStore(int module); +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/SensorUtil.h b/wpilibc/src/main/native/include/frc/SensorUtil.h index 9f8bf3a2ed..60536b13ea 100644 --- a/wpilibc/src/main/native/include/frc/SensorUtil.h +++ b/wpilibc/src/main/native/include/frc/SensorUtil.h @@ -21,6 +21,13 @@ class SensorUtil final { */ static int GetDefaultCTREPCMModule(); + /** + * Get the number of the default solenoid module. + * + * @return The number of the default solenoid module. + */ + static int GetDefaultREVPHModule(); + /** * Check that the digital channel number is valid. * diff --git a/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h b/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h new file mode 100644 index 0000000000..0097d6f2cb --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h @@ -0,0 +1,214 @@ +// 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 "frc/PneumaticsBase.h" +#include "frc/simulation/CallbackStore.h" + +namespace frc { + +class Compressor; + +namespace sim { + +/** + * Class to control a simulated Pneumatic Control Module (PCM). + */ +class REVPHSim { + public: + /** + * Constructs with the default PCM module number (CAN ID). + */ + REVPHSim(); + + /** + * Constructs from a PCM module number (CAN ID). + * + * @param module module number + */ + explicit REVPHSim(int module); + + explicit REVPHSim(const PneumaticsBase& pneumatics); + + /** + * Register a callback to be run when a solenoid is initialized on a channel. + * + * @param channel the channel to monitor + * @param callback the callback + * @param initialNotify should the callback be run with the initial state + * @return the CallbackStore object associated with this callback + */ + [[nodiscard]] std::unique_ptr RegisterInitializedCallback( + NotifyCallback callback, bool initialNotify); + + /** + * Check if a solenoid has been initialized on a specific channel. + * + * @return true if initialized + */ + bool GetInitialized() const; + + /** + * Define whether a solenoid has been initialized on a specific channel. + * + * @param channel the channel + * @param solenoidInitialized is there a solenoid initialized on that channel + */ + void SetInitialized(bool solenoidInitialized); + + /** + * Register a callback to be run when the solenoid output on a channel + * changes. + * + * @param channel the channel to monitor + * @param callback the callback + * @param initialNotify should the callback be run with the initial value + * @return the CallbackStore object associated with this callback + */ + [[nodiscard]] std::unique_ptr RegisterSolenoidOutputCallback( + int channel, NotifyCallback callback, bool initialNotify); + + /** + * Check the solenoid output on a specific channel. + * + * @param channel the channel to check + * @return the solenoid output + */ + bool GetSolenoidOutput(int channel) const; + + /** + * Change the solenoid output on a specific channel. + * + * @param channel the channel to check + * @param solenoidOutput the new solenoid output + */ + void SetSolenoidOutput(int channel, bool solenoidOutput); + + /** + * Register a callback to be run when the compressor activates. + * + * @param callback the callback + * @param initialNotify whether to run the callback with the initial state + * @return the CallbackStore object associated with this callback + */ + [[nodiscard]] std::unique_ptr RegisterCompressorOnCallback( + NotifyCallback callback, bool initialNotify); + + /** + * Check if the compressor is on. + * + * @return true if the compressor is active + */ + bool GetCompressorOn() const; + + /** + * Set whether the compressor is active. + * + * @param compressorOn the new value + */ + void SetCompressorOn(bool compressorOn); + + /** + * Register a callback to be run whenever the closed loop state changes. + * + * @param callback the callback + * @param initialNotify whether the callback should be called with the + * initial value + * @return the CallbackStore object associated with this callback + */ + [[nodiscard]] std::unique_ptr + RegisterClosedLoopEnabledCallback(NotifyCallback callback, + bool initialNotify); + + /** + * Check whether the closed loop compressor control is active. + * + * @return true if active + */ + bool GetClosedLoopEnabled() const; + + /** + * Turn on/off the closed loop control of the compressor. + * + * @param closedLoopEnabled whether the control loop is active + */ + void SetClosedLoopEnabled(bool closedLoopEnabled); + + /** + * Register a callback to be run whenever the pressure switch value changes. + * + * @param callback the callback + * @param initialNotify whether the callback should be called with the + * initial value + * @return the CallbackStore object associated with this callback + */ + [[nodiscard]] std::unique_ptr RegisterPressureSwitchCallback( + NotifyCallback callback, bool initialNotify); + + /** + * Check the value of the pressure switch. + * + * @return the pressure switch value + */ + bool GetPressureSwitch() const; + + /** + * Set the value of the pressure switch. + * + * @param pressureSwitch the new value + */ + void SetPressureSwitch(bool pressureSwitch); + + /** + * Register a callback to be run whenever the compressor current changes. + * + * @param callback the callback + * @param initialNotify whether to call the callback with the initial state + * @return the CallbackStore object associated with this callback + */ + [[nodiscard]] std::unique_ptr + RegisterCompressorCurrentCallback(NotifyCallback callback, + bool initialNotify); + + /** + * Read the compressor current. + * + * @return the current of the compressor connected to this module + */ + double GetCompressorCurrent() const; + + /** + * Set the compressor current. + * + * @param compressorCurrent the new compressor current + */ + void SetCompressorCurrent(double compressorCurrent); + + /** + * Get the current value of all solenoid outputs. + * + * @return the solenoid outputs (1 bit per output) + */ + uint8_t GetAllSolenoidOutputs() const; + + /** + * Change all of the solenoid outputs. + * + * @param outputs the new solenoid outputs (1 bit per output) + */ + void SetAllSolenoidOutputs(uint8_t outputs); + + /** + * Reset all simulation data for this object. + */ + void ResetData(); + + private: + int m_index; +}; +} // namespace sim +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/DoubleSolenoidTestCTRE.cpp b/wpilibc/src/test/native/cpp/DoubleSolenoidTestCTRE.cpp new file mode 100644 index 0000000000..2678395fa2 --- /dev/null +++ b/wpilibc/src/test/native/cpp/DoubleSolenoidTestCTRE.cpp @@ -0,0 +1,75 @@ +// 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 "frc/DoubleSolenoid.h" +#include "frc/PneumaticsControlModule.h" +#include "frc/Solenoid.h" +#include "gtest/gtest.h" + +namespace frc { + +TEST(DoubleSolenoidCTRETest, ValidInitialization) { + DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2, 3}; + solenoid.Set(DoubleSolenoid::kReverse); + EXPECT_EQ(DoubleSolenoid::kReverse, solenoid.Get()); + + solenoid.Set(DoubleSolenoid::kForward); + EXPECT_EQ(DoubleSolenoid::kForward, solenoid.Get()); + + solenoid.Set(DoubleSolenoid::kOff); + EXPECT_EQ(DoubleSolenoid::kOff, solenoid.Get()); +} + +TEST(DoubleSolenoidCTRETest, ThrowForwardPortAlreadyInitialized) { + // Single solenoid that is reused for forward port + Solenoid solenoid{5, frc::PneumaticsModuleType::CTREPCM, 2}; + EXPECT_THROW(DoubleSolenoid(5, frc::PneumaticsModuleType::CTREPCM, 2, 3), + std::runtime_error); +} + +TEST(DoubleSolenoidCTRETest, ThrowReversePortAlreadyInitialized) { + // Single solenoid that is reused for forward port + Solenoid solenoid{6, frc::PneumaticsModuleType::CTREPCM, 3}; + EXPECT_THROW(DoubleSolenoid(6, frc::PneumaticsModuleType::CTREPCM, 2, 3), + std::runtime_error); +} + +TEST(DoubleSolenoidCTRETest, ThrowBothPortsAlreadyInitialized) { + PneumaticsControlModule pcm{6}; + // Single solenoid that is reused for forward port + Solenoid solenoid0(6, frc::PneumaticsModuleType::CTREPCM, 2); + Solenoid solenoid1(6, frc::PneumaticsModuleType::CTREPCM, 3); + EXPECT_THROW(DoubleSolenoid(6, frc::PneumaticsModuleType::CTREPCM, 2, 3), + std::runtime_error); +} + +TEST(DoubleSolenoidCTRETest, Toggle) { + DoubleSolenoid solenoid{4, frc::PneumaticsModuleType::CTREPCM, 2, 3}; + // Bootstrap it into reverse + solenoid.Set(DoubleSolenoid::kReverse); + + solenoid.Toggle(); + EXPECT_EQ(DoubleSolenoid::kForward, solenoid.Get()); + + solenoid.Toggle(); + EXPECT_EQ(DoubleSolenoid::kReverse, solenoid.Get()); + + // Of shouldn't do anything on toggle + solenoid.Set(DoubleSolenoid::kOff); + solenoid.Toggle(); + EXPECT_EQ(DoubleSolenoid::kOff, solenoid.Get()); +} + +TEST(DoubleSolenoidCTRETest, InvalidForwardPort) { + EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 100, 1), + std::runtime_error); +} + +TEST(DoubleSolenoidCTRETest, InvalidReversePort) { + EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 0, 100), + std::runtime_error); +} +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/DoubleSolenoidTest.cpp b/wpilibc/src/test/native/cpp/DoubleSolenoidTestREV.cpp similarity index 85% rename from wpilibc/src/test/native/cpp/DoubleSolenoidTest.cpp rename to wpilibc/src/test/native/cpp/DoubleSolenoidTestREV.cpp index 9ed21d32e7..23893c47b7 100644 --- a/wpilibc/src/test/native/cpp/DoubleSolenoidTest.cpp +++ b/wpilibc/src/test/native/cpp/DoubleSolenoidTestREV.cpp @@ -11,7 +11,7 @@ namespace frc { -TEST(DoubleSolenoidTest, ValidInitialization) { +TEST(DoubleSolenoidREVTest, ValidInitialization) { DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2, 3}; solenoid.Set(DoubleSolenoid::kReverse); EXPECT_EQ(DoubleSolenoid::kReverse, solenoid.Get()); @@ -23,21 +23,21 @@ TEST(DoubleSolenoidTest, ValidInitialization) { EXPECT_EQ(DoubleSolenoid::kOff, solenoid.Get()); } -TEST(DoubleSolenoidTest, ThrowForwardPortAlreadyInitialized) { +TEST(DoubleSolenoidREVTest, ThrowForwardPortAlreadyInitialized) { // Single solenoid that is reused for forward port Solenoid solenoid{5, frc::PneumaticsModuleType::CTREPCM, 2}; EXPECT_THROW(DoubleSolenoid(5, frc::PneumaticsModuleType::CTREPCM, 2, 3), std::runtime_error); } -TEST(DoubleSolenoidTest, ThrowReversePortAlreadyInitialized) { +TEST(DoubleSolenoidREVTest, ThrowReversePortAlreadyInitialized) { // Single solenoid that is reused for forward port Solenoid solenoid{6, frc::PneumaticsModuleType::CTREPCM, 3}; EXPECT_THROW(DoubleSolenoid(6, frc::PneumaticsModuleType::CTREPCM, 2, 3), std::runtime_error); } -TEST(DoubleSolenoidTest, ThrowBothPortsAlreadyInitialized) { +TEST(DoubleSolenoidREVTest, ThrowBothPortsAlreadyInitialized) { PneumaticsControlModule pcm{6}; // Single solenoid that is reused for forward port Solenoid solenoid0(6, frc::PneumaticsModuleType::CTREPCM, 2); @@ -46,7 +46,7 @@ TEST(DoubleSolenoidTest, ThrowBothPortsAlreadyInitialized) { std::runtime_error); } -TEST(DoubleSolenoidTest, Toggle) { +TEST(DoubleSolenoidREVTest, Toggle) { DoubleSolenoid solenoid{4, frc::PneumaticsModuleType::CTREPCM, 2, 3}; // Bootstrap it into reverse solenoid.Set(DoubleSolenoid::kReverse); @@ -63,12 +63,12 @@ TEST(DoubleSolenoidTest, Toggle) { EXPECT_EQ(DoubleSolenoid::kOff, solenoid.Get()); } -TEST(DoubleSolenoidTest, InvalidForwardPort) { +TEST(DoubleSolenoidREVTest, InvalidForwardPort) { EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 100, 1), std::runtime_error); } -TEST(DoubleSolenoidTest, InvalidReversePort) { +TEST(DoubleSolenoidREVTest, InvalidReversePort) { EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 0, 100), std::runtime_error); } diff --git a/wpilibc/src/test/native/cpp/SolenoidTest.cpp b/wpilibc/src/test/native/cpp/SolenoidTestCTRE.cpp similarity index 84% rename from wpilibc/src/test/native/cpp/SolenoidTest.cpp rename to wpilibc/src/test/native/cpp/SolenoidTestCTRE.cpp index 86e5b72a97..61e3fb61cb 100644 --- a/wpilibc/src/test/native/cpp/SolenoidTest.cpp +++ b/wpilibc/src/test/native/cpp/SolenoidTestCTRE.cpp @@ -10,7 +10,7 @@ #include "gtest/gtest.h" namespace frc { -TEST(SolenoidTest, ValidInitialization) { +TEST(SolenoidCTRETest, ValidInitialization) { Solenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2}; EXPECT_EQ(2, solenoid.GetChannel()); @@ -21,24 +21,24 @@ TEST(SolenoidTest, ValidInitialization) { EXPECT_FALSE(solenoid.Get()); } -TEST(SolenoidTest, DoubleInitialization) { +TEST(SolenoidCTRETest, DoubleInitialization) { Solenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2}; EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::CTREPCM, 2), std::runtime_error); } -TEST(SolenoidTest, DoubleInitializationFromDoubleSolenoid) { +TEST(SolenoidCTRETest, DoubleInitializationFromDoubleSolenoid) { DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2, 3}; EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::CTREPCM, 2), std::runtime_error); } -TEST(SolenoidTest, InvalidChannel) { +TEST(SolenoidCTRETest, InvalidChannel) { EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::CTREPCM, 100), std::runtime_error); } -TEST(SolenoidTest, Toggle) { +TEST(SolenoidCTRETest, Toggle) { Solenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2}; solenoid.Set(true); EXPECT_TRUE(solenoid.Get()); diff --git a/wpilibc/src/test/native/cpp/SolenoidTestREV.cpp b/wpilibc/src/test/native/cpp/SolenoidTestREV.cpp new file mode 100644 index 0000000000..75ea261e68 --- /dev/null +++ b/wpilibc/src/test/native/cpp/SolenoidTestREV.cpp @@ -0,0 +1,52 @@ +// 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 "frc/DoubleSolenoid.h" +#include "frc/PneumaticsControlModule.h" +#include "frc/Solenoid.h" +#include "gtest/gtest.h" + +namespace frc { +TEST(SolenoidREVTest, ValidInitialization) { + Solenoid solenoid{3, frc::PneumaticsModuleType::REVPH, 2}; + EXPECT_EQ(2, solenoid.GetChannel()); + + solenoid.Set(true); + EXPECT_TRUE(solenoid.Get()); + + solenoid.Set(false); + EXPECT_FALSE(solenoid.Get()); +} + +TEST(SolenoidREVTest, DoubleInitialization) { + Solenoid solenoid{3, frc::PneumaticsModuleType::REVPH, 2}; + EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::REVPH, 2), + std::runtime_error); +} + +TEST(SolenoidREVTest, DoubleInitializationFromDoubleSolenoid) { + DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::REVPH, 2, 3}; + EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::REVPH, 2), + std::runtime_error); +} + +TEST(SolenoidREVTest, InvalidChannel) { + EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::REVPH, 100), + std::runtime_error); +} + +TEST(SolenoidREVTest, Toggle) { + Solenoid solenoid{3, frc::PneumaticsModuleType::REVPH, 2}; + solenoid.Set(true); + EXPECT_TRUE(solenoid.Get()); + + solenoid.Toggle(); + EXPECT_FALSE(solenoid.Get()); + + solenoid.Toggle(); + EXPECT_TRUE(solenoid.Get()); +} +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp new file mode 100644 index 0000000000..f622a73e9f --- /dev/null +++ b/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp @@ -0,0 +1,150 @@ +// 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/simulation/REVPHSim.h" // NOLINT(build/include_order) + +#include + +#include "callback_helpers/TestCallbackHelpers.h" +#include "frc/DoubleSolenoid.h" +#include "frc/PneumaticsHub.h" +#include "gtest/gtest.h" + +namespace frc::sim { + +TEST(REVPHSimTest, InitializedCallback) { + REVPHSim sim; + + sim.ResetData(); + EXPECT_FALSE(sim.GetInitialized()); + + BooleanCallback callback; + auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false); + + PneumaticsHub ph; + EXPECT_TRUE(sim.GetInitialized()); + EXPECT_TRUE(callback.WasTriggered()); + EXPECT_TRUE(callback.GetLastValue()); +} + +TEST(REVPHSimTest, SolenoidOutput) { + PneumaticsHub ph; + REVPHSim sim(ph); + sim.ResetData(); + + DoubleSolenoid doubleSolenoid{1, frc::PneumaticsModuleType::REVPH, 3, 4}; + + BooleanCallback callback3; + BooleanCallback callback4; + auto cb3 = + sim.RegisterSolenoidOutputCallback(3, callback3.GetCallback(), false); + auto cb4 = + sim.RegisterSolenoidOutputCallback(4, callback4.GetCallback(), false); + + callback3.Reset(); + callback4.Reset(); + doubleSolenoid.Set(DoubleSolenoid::kReverse); + EXPECT_FALSE(callback3.WasTriggered()); + EXPECT_FALSE(callback3.GetLastValue()); + EXPECT_TRUE(callback4.WasTriggered()); + EXPECT_TRUE(callback4.GetLastValue()); + EXPECT_FALSE(sim.GetSolenoidOutput(3)); + EXPECT_TRUE(sim.GetSolenoidOutput(4)); + EXPECT_EQ(0b00010000, ph.GetSolenoids()); + EXPECT_EQ(0b00010000, sim.GetAllSolenoidOutputs()); + + callback3.Reset(); + callback4.Reset(); + doubleSolenoid.Set(DoubleSolenoid::kForward); + EXPECT_TRUE(callback3.WasTriggered()); + EXPECT_TRUE(callback3.GetLastValue()); + EXPECT_TRUE(callback4.WasTriggered()); + EXPECT_FALSE(callback4.GetLastValue()); + EXPECT_TRUE(sim.GetSolenoidOutput(3)); + EXPECT_FALSE(sim.GetSolenoidOutput(4)); + EXPECT_EQ(0b00001000, ph.GetSolenoids()); + EXPECT_EQ(0b00001000, sim.GetAllSolenoidOutputs()); + + callback3.Reset(); + callback4.Reset(); + doubleSolenoid.Set(DoubleSolenoid::kOff); + EXPECT_TRUE(callback3.WasTriggered()); + EXPECT_FALSE(callback3.GetLastValue()); + EXPECT_FALSE(callback4.WasTriggered()); + EXPECT_FALSE(callback4.GetLastValue()); + EXPECT_FALSE(sim.GetSolenoidOutput(3)); + EXPECT_FALSE(sim.GetSolenoidOutput(4)); + EXPECT_EQ(0b00000000, ph.GetSolenoids()); + EXPECT_EQ(0b00000000, sim.GetAllSolenoidOutputs()); +} + +TEST(REVPHSimTest, SetCompressorOn) { + PneumaticsHub ph; + REVPHSim sim(ph); + sim.ResetData(); + + BooleanCallback callback; + auto cb = sim.RegisterCompressorOnCallback(callback.GetCallback(), false); + + EXPECT_FALSE(ph.GetCompressor()); + EXPECT_FALSE(ph.GetCompressor()); + sim.SetCompressorOn(true); + EXPECT_TRUE(sim.GetCompressorOn()); + EXPECT_TRUE(ph.GetCompressor()); + EXPECT_TRUE(callback.WasTriggered()); + EXPECT_TRUE(callback.GetLastValue()); +} + +TEST(REVPHSimTest, SetClosedLoopEnabled) { + PneumaticsHub ph; + REVPHSim sim(ph); + sim.ResetData(); + + BooleanCallback callback; + auto cb = + sim.RegisterClosedLoopEnabledCallback(callback.GetCallback(), false); + + ph.SetClosedLoopControl(false); + EXPECT_FALSE(ph.GetClosedLoopControl()); + + ph.SetClosedLoopControl(true); + EXPECT_TRUE(sim.GetClosedLoopEnabled()); + EXPECT_TRUE(ph.GetClosedLoopControl()); + EXPECT_TRUE(callback.WasTriggered()); + EXPECT_TRUE(callback.GetLastValue()); +} + +TEST(REVPHSimTest, SetPressureSwitchEnabled) { + PneumaticsHub ph; + REVPHSim sim(ph); + sim.ResetData(); + + BooleanCallback callback; + auto cb = sim.RegisterPressureSwitchCallback(callback.GetCallback(), false); + + EXPECT_FALSE(ph.GetPressureSwitch()); + + sim.SetPressureSwitch(true); + EXPECT_TRUE(sim.GetPressureSwitch()); + EXPECT_TRUE(ph.GetPressureSwitch()); + EXPECT_TRUE(callback.WasTriggered()); + EXPECT_TRUE(callback.GetLastValue()); +} + +TEST(REVPHSimTest, SetCompressorCurrent) { + PneumaticsHub ph; + REVPHSim sim(ph); + sim.ResetData(); + + DoubleCallback callback; + auto cb = + sim.RegisterCompressorCurrentCallback(callback.GetCallback(), false); + + sim.SetCompressorCurrent(35.04); + EXPECT_EQ(35.04, sim.GetCompressorCurrent()); + EXPECT_EQ(35.04, ph.GetCompressorCurrent()); + EXPECT_TRUE(callback.WasTriggered()); + EXPECT_EQ(35.04, callback.GetLastValue()); +} +} // namespace frc::sim diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java index c893d48557..d063227d63 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java @@ -12,9 +12,11 @@ public interface PneumaticsBase extends AutoCloseable { * @param type module type * @return module */ - static PneumaticsControlModule getForType(int module, PneumaticsModuleType type) { + static PneumaticsBase getForType(int module, PneumaticsModuleType type) { if (type == PneumaticsModuleType.CTREPCM) { - return new PneumaticsControlModule(); + return new PneumaticsControlModule(module); + } else if (type == PneumaticsModuleType.REVPH) { + return new PneumaticsHub(module); } throw new IllegalArgumentException("Unknown module type"); } @@ -28,6 +30,8 @@ public interface PneumaticsBase extends AutoCloseable { static int getDefaultForType(PneumaticsModuleType type) { if (type == PneumaticsModuleType.CTREPCM) { return SensorUtil.getDefaultCTREPCMModule(); + } else if (type == PneumaticsModuleType.REVPH) { + return SensorUtil.getDefaultREVPHModule(); } throw new IllegalArgumentException("Unknown module type"); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java index fcb0ec8262..79f245bfdb 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java @@ -147,7 +147,7 @@ public class PneumaticsControlModule implements PneumaticsBase { @Override public int getModuleNumber() { - return m_dataStore.m_handle; + return m_dataStore.m_module; } @Override diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsHub.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsHub.java new file mode 100644 index 0000000000..104973ab01 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsHub.java @@ -0,0 +1,203 @@ +// 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. + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.REVPHJNI; +import java.util.HashMap; +import java.util.Map; + +/** Module class for controlling a REV Robotics Pneumatics Hub. */ +public class PneumaticsHub implements PneumaticsBase { + private static class DataStore implements AutoCloseable { + public final int m_module; + public final int m_handle; + private int m_refCount; + private int m_reservedMask; + private boolean m_compressorReserved; + private final Object m_reserveLock = new Object(); + + DataStore(int module) { + m_handle = REVPHJNI.initialize(module); + m_module = module; + m_handleMap.put(module, this); + } + + @Override + public void close() { + REVPHJNI.free(m_handle); + m_handleMap.remove(m_module); + } + + public void addRef() { + m_refCount++; + } + + public void removeRef() { + m_refCount--; + if (m_refCount == 0) { + this.close(); + } + } + } + + private static final Map m_handleMap = new HashMap<>(); + private static final Object m_handleLock = new Object(); + + private static DataStore getForModule(int module) { + synchronized (m_handleLock) { + Integer moduleBoxed = module; + DataStore pcm = m_handleMap.get(moduleBoxed); + if (pcm == null) { + pcm = new DataStore(module); + } + pcm.addRef(); + return pcm; + } + } + + private static void freeModule(DataStore store) { + synchronized (m_handleLock) { + store.removeRef(); + } + } + + private final DataStore m_dataStore; + private final int m_handle; + + /** Constructs a PneumaticsHub with the default id (1). */ + public PneumaticsHub() { + this(SensorUtil.getDefaultREVPHModule()); + } + + /** + * Constructs a PneumaticsHub. + * + * @param module module number to construct + */ + public PneumaticsHub(int module) { + m_dataStore = getForModule(module); + m_handle = m_dataStore.m_handle; + } + + @Override + public void close() { + freeModule(m_dataStore); + } + + @Override + public boolean getCompressor() { + return REVPHJNI.getCompressor(m_handle); + } + + @Override + public void setClosedLoopControl(boolean enabled) { + REVPHJNI.setClosedLoopControl(m_handle, enabled); + } + + @Override + public boolean getClosedLoopControl() { + return REVPHJNI.getClosedLoopControl(m_handle); + } + + @Override + public boolean getPressureSwitch() { + return REVPHJNI.getPressureSwitch(m_handle); + } + + @Override + public double getCompressorCurrent() { + return REVPHJNI.getCompressorCurrent(m_handle); + } + + @Override + public void setSolenoids(int mask, int values) { + REVPHJNI.setSolenoids(m_handle, mask, values); + } + + @Override + public int getSolenoids() { + return REVPHJNI.getSolenoids(m_handle); + } + + @Override + public int getModuleNumber() { + return m_dataStore.m_module; + } + + @Override + public void fireOneShot(int index) { + // TODO Combine APIs + // REVPHJNI.fireOneShot(m_handle, index, durMs); + } + + @Override + public void setOneShotDuration(int index, int durMs) { + // TODO Combine APIs + // REVPHJNI.setOneShotDuration(m_handle, index, durMs); + } + + @Override + public boolean checkSolenoidChannel(int channel) { + return REVPHJNI.checkSolenoidChannel(channel); + } + + @Override + public int checkAndReserveSolenoids(int mask) { + synchronized (m_dataStore.m_reserveLock) { + if ((m_dataStore.m_reservedMask & mask) != 0) { + return m_dataStore.m_reservedMask & mask; + } + m_dataStore.m_reservedMask |= mask; + return 0; + } + } + + @Override + public void unreserveSolenoids(int mask) { + synchronized (m_dataStore.m_reserveLock) { + m_dataStore.m_reservedMask &= ~mask; + } + } + + @Override + public Solenoid makeSolenoid(int channel) { + return new Solenoid(m_dataStore.m_module, PneumaticsModuleType.REVPH, channel); + } + + @Override + public DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel) { + return new DoubleSolenoid( + m_dataStore.m_module, PneumaticsModuleType.REVPH, forwardChannel, reverseChannel); + } + + @Override + public Compressor makeCompressor() { + return new Compressor(m_dataStore.m_module, PneumaticsModuleType.REVPH); + } + + @Override + public boolean reserveCompressor() { + synchronized (m_dataStore.m_reserveLock) { + if (m_dataStore.m_compressorReserved) { + return false; + } + m_dataStore.m_compressorReserved = true; + return true; + } + } + + @Override + public void unreserveCompressor() { + synchronized (m_dataStore.m_reserveLock) { + m_dataStore.m_compressorReserved = false; + } + } + + @Override + public int getSolenoidDisabledList() { + // TODO Get this working + return 0; + } +} 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 07fd98d465..0ac24dee19 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java @@ -47,6 +47,10 @@ public final class SensorUtil { /** Number of PCM Modules. */ public static final int kCTREPCMModules = PortsJNI.getNumCTREPCMModules(); + public static final int kREVPHChannels = PortsJNI.getNumREVPHChannels(); + + public static final int kREVPHModules = PortsJNI.getNumREVPHModules(); + /** * Check that the digital channel number is valid. Verify that the channel number is one of the * legal channel numbers. Channel numbers are 0-based. @@ -142,5 +146,15 @@ public final class SensorUtil { return 0; } + /** + * Get the number of the default solenoid module. + * + * @return The number of the default solenoid module. + */ + @SuppressWarnings("AbbreviationAsWordInName") + public static int getDefaultREVPHModule() { + return 1; + } + private SensorUtil() {} } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/REVPHSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/REVPHSim.java new file mode 100644 index 0000000000..31166626a3 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/REVPHSim.java @@ -0,0 +1,239 @@ +// 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. + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.hal.simulation.NotifyCallback; +import edu.wpi.first.hal.simulation.REVPHDataJNI; +import edu.wpi.first.wpilibj.PneumaticsBase; +import edu.wpi.first.wpilibj.SensorUtil; + +/** Class to control a simulated PneumaticsHub (PH). */ +@SuppressWarnings("AbbreviationAsWordInName") +public class REVPHSim { + private final int m_index; + + /** Constructs for the default PH. */ + public REVPHSim() { + m_index = SensorUtil.getDefaultREVPHModule(); + } + + /** + * Constructs from a PH module number (CAN ID). + * + * @param module module number + */ + public REVPHSim(int module) { + m_index = module; + } + + /** + * Constructs from a Compressor object. + * + * @param module PCM module to simulate + */ + public REVPHSim(PneumaticsBase module) { + m_index = module.getModuleNumber(); + } + + /** + * Register a callback to be run when the solenoid output on a channel changes. + * + * @param channel the channel to monitor + * @param callback the callback + * @param initialNotify should the callback be run with the initial value + * @return the {@link CallbackStore} object associated with this callback. Save a reference to + * this object so GC doesn't cancel the callback. + */ + public CallbackStore registerSolenoidOutputCallback( + int channel, NotifyCallback callback, boolean initialNotify) { + int uid = + REVPHDataJNI.registerSolenoidOutputCallback(m_index, channel, callback, initialNotify); + return new CallbackStore(m_index, channel, uid, REVPHDataJNI::cancelSolenoidOutputCallback); + } + + /** + * Check the solenoid output on a specific channel. + * + * @param channel the channel to check + * @return the solenoid output + */ + public boolean getSolenoidOutput(int channel) { + return REVPHDataJNI.getSolenoidOutput(m_index, channel); + } + + /** + * Change the solenoid output on a specific channel. + * + * @param channel the channel to check + * @param solenoidOutput the new solenoid output + */ + public void setSolenoidOutput(int channel, boolean solenoidOutput) { + REVPHDataJNI.setSolenoidOutput(m_index, channel, solenoidOutput); + } + + /** + * Register a callback to be run when the compressor is initialized. + * + * @param callback the callback + * @param initialNotify whether to run the callback with the initial state + * @return the {@link CallbackStore} object associated with this callback. Save a reference to + * this object so GC doesn't cancel the callback. + */ + public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) { + int uid = REVPHDataJNI.registerInitializedCallback(m_index, callback, initialNotify); + return new CallbackStore(m_index, uid, REVPHDataJNI::cancelInitializedCallback); + } + + /** + * Check whether the compressor has been initialized. + * + * @return true if initialized + */ + public boolean getInitialized() { + return REVPHDataJNI.getInitialized(m_index); + } + + /** + * Define whether the compressor has been initialized. + * + * @param initialized whether the compressor is initialized + */ + public void setInitialized(boolean initialized) { + REVPHDataJNI.setInitialized(m_index, initialized); + } + + /** + * Register a callback to be run when the compressor activates. + * + * @param callback the callback + * @param initialNotify whether to run the callback with the initial state + * @return the {@link CallbackStore} object associated with this callback. Save a reference to + * this object so GC doesn't cancel the callback. + */ + public CallbackStore registerCompressorOnCallback( + NotifyCallback callback, boolean initialNotify) { + int uid = REVPHDataJNI.registerCompressorOnCallback(m_index, callback, initialNotify); + return new CallbackStore(m_index, uid, REVPHDataJNI::cancelCompressorOnCallback); + } + + /** + * Check if the compressor is on. + * + * @return true if the compressor is active + */ + public boolean getCompressorOn() { + return REVPHDataJNI.getCompressorOn(m_index); + } + + /** + * Set whether the compressor is active. + * + * @param compressorOn the new value + */ + public void setCompressorOn(boolean compressorOn) { + REVPHDataJNI.setCompressorOn(m_index, compressorOn); + } + + /** + * Register a callback to be run whenever the closed loop state changes. + * + * @param callback the callback + * @param initialNotify whether the callback should be called with the initial value + * @return the {@link CallbackStore} object associated with this callback. Save a reference to + * this object so GC doesn't cancel the callback. + */ + public CallbackStore registerClosedLoopEnabledCallback( + NotifyCallback callback, boolean initialNotify) { + int uid = REVPHDataJNI.registerClosedLoopEnabledCallback(m_index, callback, initialNotify); + return new CallbackStore(m_index, uid, REVPHDataJNI::cancelClosedLoopEnabledCallback); + } + + /** + * Check whether the closed loop compressor control is active. + * + * @return true if active + */ + public boolean getClosedLoopEnabled() { + return REVPHDataJNI.getClosedLoopEnabled(m_index); + } + + /** + * Turn on/off the closed loop control of the compressor. + * + * @param closedLoopEnabled whether the control loop is active + */ + public void setClosedLoopEnabled(boolean closedLoopEnabled) { + REVPHDataJNI.setClosedLoopEnabled(m_index, closedLoopEnabled); + } + + /** + * Register a callback to be run whenever the pressure switch value changes. + * + * @param callback the callback + * @param initialNotify whether the callback should be called with the initial value + * @return the {@link CallbackStore} object associated with this callback. Save a reference to + * this object so GC doesn't cancel the callback. + */ + public CallbackStore registerPressureSwitchCallback( + NotifyCallback callback, boolean initialNotify) { + int uid = REVPHDataJNI.registerPressureSwitchCallback(m_index, callback, initialNotify); + return new CallbackStore(m_index, uid, REVPHDataJNI::cancelPressureSwitchCallback); + } + + /** + * Check the value of the pressure switch. + * + * @return the pressure switch value + */ + public boolean getPressureSwitch() { + return REVPHDataJNI.getPressureSwitch(m_index); + } + + /** + * Set the value of the pressure switch. + * + * @param pressureSwitch the new value + */ + public void setPressureSwitch(boolean pressureSwitch) { + REVPHDataJNI.setPressureSwitch(m_index, pressureSwitch); + } + + /** + * Register a callback to be run whenever the compressor current changes. + * + * @param callback the callback + * @param initialNotify whether to call the callback with the initial state + * @return the {@link CallbackStore} object associated with this callback. Save a reference to + * this object so GC doesn't cancel the callback. + */ + public CallbackStore registerCompressorCurrentCallback( + NotifyCallback callback, boolean initialNotify) { + int uid = REVPHDataJNI.registerCompressorCurrentCallback(m_index, callback, initialNotify); + return new CallbackStore(m_index, uid, REVPHDataJNI::cancelCompressorCurrentCallback); + } + + /** + * Read the compressor current. + * + * @return the current of the compressor connected to this module + */ + public double getCompressorCurrent() { + return REVPHDataJNI.getCompressorCurrent(m_index); + } + + /** + * Set the compressor current. + * + * @param compressorCurrent the new compressor current + */ + public void setCompressorCurrent(double compressorCurrent) { + REVPHDataJNI.setCompressorCurrent(m_index, compressorCurrent); + } + + /** Reset all simulation data for this object. */ + public void resetData() { + REVPHDataJNI.resetData(m_index); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestCTRE.java similarity index 98% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTest.java rename to wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestCTRE.java index 249c9461d1..72a3e896fe 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestCTRE.java @@ -10,7 +10,7 @@ import static org.junit.jupiter.api.Assertions.assertThrows; import edu.wpi.first.hal.util.AllocationException; import org.junit.jupiter.api.Test; -public class DoubleSolenoidTest { +public class DoubleSolenoidTestCTRE { @Test void testValidInitialization() { try (DoubleSolenoid solenoid = new DoubleSolenoid(3, PneumaticsModuleType.CTREPCM, 2, 3)) { diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestREV.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestREV.java new file mode 100644 index 0000000000..888b67749a --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestREV.java @@ -0,0 +1,91 @@ +// 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. + +package edu.wpi.first.wpilibj; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; + +import edu.wpi.first.hal.util.AllocationException; +import org.junit.jupiter.api.Test; + +public class DoubleSolenoidTestREV { + @Test + void testValidInitialization() { + try (DoubleSolenoid solenoid = new DoubleSolenoid(3, PneumaticsModuleType.REVPH, 2, 3)) { + solenoid.set(DoubleSolenoid.Value.kReverse); + assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get()); + + solenoid.set(DoubleSolenoid.Value.kForward); + assertEquals(DoubleSolenoid.Value.kForward, solenoid.get()); + + solenoid.set(DoubleSolenoid.Value.kOff); + assertEquals(DoubleSolenoid.Value.kOff, solenoid.get()); + } + } + + @Test + void testThrowForwardPortAlreadyInitialized() { + try ( + // Single solenoid that is reused for forward port + Solenoid solenoid = new Solenoid(5, PneumaticsModuleType.REVPH, 2)) { + assertThrows( + AllocationException.class, () -> new DoubleSolenoid(5, PneumaticsModuleType.REVPH, 2, 3)); + } + } + + @Test + void testThrowReversePortAlreadyInitialized() { + try ( + // Single solenoid that is reused for forward port + Solenoid solenoid = new Solenoid(6, PneumaticsModuleType.REVPH, 3)) { + assertThrows( + AllocationException.class, () -> new DoubleSolenoid(6, PneumaticsModuleType.REVPH, 2, 3)); + } + } + + @Test + void testThrowBothPortsAlreadyInitialized() { + try ( + // Single solenoid that is reused for forward port + Solenoid solenoid0 = new Solenoid(6, PneumaticsModuleType.REVPH, 2); + Solenoid solenoid1 = new Solenoid(6, PneumaticsModuleType.REVPH, 3)) { + assertThrows( + AllocationException.class, () -> new DoubleSolenoid(6, PneumaticsModuleType.REVPH, 2, 3)); + } + } + + @Test + void testToggle() { + try (DoubleSolenoid solenoid = new DoubleSolenoid(4, PneumaticsModuleType.REVPH, 2, 3)) { + // Bootstrap it into reverse + solenoid.set(DoubleSolenoid.Value.kReverse); + + solenoid.toggle(); + assertEquals(DoubleSolenoid.Value.kForward, solenoid.get()); + + solenoid.toggle(); + assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get()); + + // Of shouldn't do anything on toggle + solenoid.set(DoubleSolenoid.Value.kOff); + solenoid.toggle(); + assertEquals(DoubleSolenoid.Value.kOff, solenoid.get()); + } + } + + @Test + void testInvalidForwardPort() { + assertThrows( + IllegalArgumentException.class, + () -> new DoubleSolenoid(1, PneumaticsModuleType.REVPH, 100, 1)); + } + + @Test + void testInvalidReversePort() { + assertThrows( + IllegalArgumentException.class, + () -> new DoubleSolenoid(1, PneumaticsModuleType.REVPH, 0, 100)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestCTRE.java similarity index 98% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTest.java rename to wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestCTRE.java index 687035dc3c..0077650444 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestCTRE.java @@ -12,7 +12,7 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.hal.util.AllocationException; import org.junit.jupiter.api.Test; -public class SolenoidTest { +public class SolenoidTestCTRE { @Test void testValidInitialization() { try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.CTREPCM, 2)) { diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestREV.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestREV.java new file mode 100644 index 0000000000..3f07406bda --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestREV.java @@ -0,0 +1,62 @@ +// 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. + +package edu.wpi.first.wpilibj; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.hal.util.AllocationException; +import org.junit.jupiter.api.Test; + +public class SolenoidTestREV { + @Test + void testValidInitialization() { + try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.REVPH, 2)) { + assertEquals(2, solenoid.getChannel()); + + solenoid.set(true); + assertTrue(solenoid.get()); + + solenoid.set(false); + assertFalse(solenoid.get()); + } + } + + @Test + void testDoubleInitialization() { + try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.REVPH, 2)) { + assertThrows(AllocationException.class, () -> new Solenoid(3, PneumaticsModuleType.REVPH, 2)); + } + } + + @Test + void testDoubleInitializationFromDoubleSolenoid() { + try (DoubleSolenoid solenoid = new DoubleSolenoid(3, PneumaticsModuleType.REVPH, 2, 3)) { + assertThrows(AllocationException.class, () -> new Solenoid(3, PneumaticsModuleType.REVPH, 2)); + } + } + + @Test + void testInvalidChannel() { + assertThrows( + IllegalArgumentException.class, () -> new Solenoid(3, PneumaticsModuleType.REVPH, 100)); + } + + @Test + void testToggle() { + try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.REVPH, 2)) { + solenoid.set(true); + assertTrue(solenoid.get()); + + solenoid.toggle(); + assertFalse(solenoid.get()); + + solenoid.toggle(); + assertTrue(solenoid.get()); + } + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/REVPHSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/REVPHSimTest.java new file mode 100644 index 0000000000..ac5841f494 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/REVPHSimTest.java @@ -0,0 +1,168 @@ +// 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. + +package edu.wpi.first.wpilibj.simulation; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNull; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsHub; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback; +import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback; +import org.junit.jupiter.api.Test; + +@SuppressWarnings("AbbreviationAsWordInName") +class REVPHSimTest { + @Test + void testInitialization() { + HAL.initialize(500, 0); + + REVPHSim sim = new REVPHSim(1); + sim.resetData(); + assertFalse(sim.getInitialized()); + + BooleanCallback callback = new BooleanCallback(); + + try (CallbackStore cb = sim.registerInitializedCallback(callback, false); + PneumaticsHub ph = new PneumaticsHub(1)) { + assertTrue(sim.getInitialized()); + } + assertFalse(sim.getInitialized()); + } + + @Test + void solenoidOutputTest() { + HAL.initialize(500, 0); + + try (PneumaticsHub ph = new PneumaticsHub(1); + DoubleSolenoid doubleSolenoid = new DoubleSolenoid(1, PneumaticsModuleType.REVPH, 3, 4)) { + REVPHSim sim = new REVPHSim(ph); + sim.resetData(); + + BooleanCallback callback3 = new BooleanCallback(); + BooleanCallback callback4 = new BooleanCallback(); + + try (CallbackStore cb3 = sim.registerSolenoidOutputCallback(3, callback3, false); + CallbackStore cb4 = sim.registerSolenoidOutputCallback(4, callback4, false)) { + // Reverse + callback3.reset(); + callback4.reset(); + doubleSolenoid.set(DoubleSolenoid.Value.kReverse); + assertFalse(callback3.wasTriggered()); + assertNull(callback3.getSetValue()); + assertTrue(callback4.wasTriggered()); + assertTrue(callback4.getSetValue()); + assertFalse(sim.getSolenoidOutput(3)); + assertTrue(sim.getSolenoidOutput(4)); + assertEquals(0x10, ph.getSolenoids()); + + // Forward + callback3.reset(); + callback4.reset(); + doubleSolenoid.set(DoubleSolenoid.Value.kForward); + assertTrue(callback3.wasTriggered()); + assertTrue(callback3.getSetValue()); + assertTrue(callback4.wasTriggered()); + assertFalse(callback4.getSetValue()); + assertTrue(sim.getSolenoidOutput(3)); + assertFalse(sim.getSolenoidOutput(4)); + assertEquals(0x8, ph.getSolenoids()); + + // Off + callback3.reset(); + callback4.reset(); + doubleSolenoid.set(DoubleSolenoid.Value.kOff); + assertTrue(callback3.wasTriggered()); + assertFalse(callback3.getSetValue()); + assertFalse(callback4.wasTriggered()); + assertNull(callback4.getSetValue()); + assertFalse(sim.getSolenoidOutput(3)); + assertFalse(sim.getSolenoidOutput(4)); + assertEquals(0x0, ph.getSolenoids()); + } + } + } + + @Test + void setCompressorOnTest() { + HAL.initialize(500, 0); + + REVPHSim sim = new REVPHSim(1); + BooleanCallback callback = new BooleanCallback(); + + try (PneumaticsHub ph = new PneumaticsHub(1); + CallbackStore cb = sim.registerCompressorOnCallback(callback, false)) { + assertFalse(ph.getCompressor()); + assertFalse(sim.getCompressorOn()); + sim.setCompressorOn(true); + assertTrue(ph.getCompressor()); + assertTrue(sim.getCompressorOn()); + assertTrue(callback.wasTriggered()); + assertTrue(callback.getSetValue()); + } + } + + @Test + void setClosedLoopEnabled() { + HAL.initialize(500, 0); + + REVPHSim sim = new REVPHSim(1); + BooleanCallback callback = new BooleanCallback(); + + try (PneumaticsHub ph = new PneumaticsHub(1); + CallbackStore cb = sim.registerClosedLoopEnabledCallback(callback, false)) { + ph.setClosedLoopControl(false); + assertFalse(ph.getClosedLoopControl()); + + ph.setClosedLoopControl(true); + assertTrue(sim.getClosedLoopEnabled()); + assertTrue(ph.getClosedLoopControl()); + assertTrue(callback.wasTriggered()); + assertTrue(callback.getSetValue()); + } + } + + @Test + void setPressureSwitchEnabledTest() { + HAL.initialize(500, 0); + + REVPHSim sim = new REVPHSim(1); + BooleanCallback callback = new BooleanCallback(); + + try (PneumaticsHub ph = new PneumaticsHub(1); + CallbackStore cb = sim.registerPressureSwitchCallback(callback, false)) { + assertFalse(ph.getPressureSwitch()); + + sim.setPressureSwitch(true); + assertTrue(sim.getPressureSwitch()); + assertTrue(ph.getPressureSwitch()); + assertTrue(callback.wasTriggered()); + assertTrue(callback.getSetValue()); + } + } + + @Test + void setCompressorCurrentTest() { + HAL.initialize(500, 0); + + REVPHSim sim = new REVPHSim(1); + DoubleCallback callback = new DoubleCallback(); + + try (PneumaticsHub ph = new PneumaticsHub(1); + CallbackStore cb = sim.registerCompressorCurrentCallback(callback, false)) { + assertFalse(ph.getPressureSwitch()); + + sim.setCompressorCurrent(35.04); + assertEquals(35.04, sim.getCompressorCurrent()); + assertEquals(35.04, ph.getCompressorCurrent()); + assertTrue(callback.wasTriggered()); + assertEquals(35.04, callback.getSetValue()); + } + } +}