From 109363daa4efedf4178924a2aa6d3f28dd2677da Mon Sep 17 00:00:00 2001 From: Jan-Felix Abellera Date: Thu, 9 Dec 2021 14:29:09 -0600 Subject: [PATCH] [hal] Add remaining driver functions for REVPH (#3776) Add the remaining HAL functions needed to fully support the Pneumatic Hub and its latest firmware. - Clear sticky faults - Get device voltage - Get 5v supply voltage (used for analog to PSI calculation) - Get solenoid voltage - Get solenoid current - Get device firmware and hardware version Some minor refactoring was done for naming of some internal functions for consistency purposes. --- hal/src/main/native/athena/REVPH.cpp | 191 +++++++++++--- hal/src/main/native/athena/rev/PHFrames.cpp | 193 ++++++++++++++ hal/src/main/native/athena/rev/PHFrames.h | 278 ++++++++++++++++++++ hal/src/main/native/include/hal/REVPH.h | 19 ++ 4 files changed, 644 insertions(+), 37 deletions(-) diff --git a/hal/src/main/native/athena/REVPH.cpp b/hal/src/main/native/athena/REVPH.cpp index 1a96abce64..8dc5854cf6 100644 --- a/hal/src/main/native/athena/REVPH.cpp +++ b/hal/src/main/native/athena/REVPH.cpp @@ -23,31 +23,35 @@ 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 +#define HAL_REVPH_MAX_PULSE_TIME 65534 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_COMPRESSOR_CONFIG_API = - APIFromExtId(PH_COMPRESSOR_CONFIG_FRAME_ID); static constexpr uint32_t PH_STATUS_0_FRAME_API = APIFromExtId(PH_STATUS_0_FRAME_ID); static constexpr uint32_t PH_STATUS_1_FRAME_API = APIFromExtId(PH_STATUS_1_FRAME_ID); +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_COMPRESSOR_CONFIG_API = + APIFromExtId(PH_COMPRESSOR_CONFIG_FRAME_ID); + +static constexpr uint32_t PH_CLEAR_FAULTS_FRAME_API = + APIFromExtId(PH_CLEAR_FAULTS_FRAME_ID); + +static constexpr uint32_t PH_VERSION_FRAME_API = + APIFromExtId(PH_VERSION_FRAME_ID); + static constexpr int32_t kPHFrameStatus0Timeout = 50; static constexpr int32_t kPHFrameStatus1Timeout = 50; @@ -75,8 +79,7 @@ void InitializeREVPH() { } } // namespace hal::init -static PH_status_0_t HAL_REV_ReadPHStatus0(HAL_CANHandle hcan, - int32_t* status) { +static PH_status_0_t HAL_ReadREVPHStatus0(HAL_CANHandle hcan, int32_t* status) { uint8_t packedData[8] = {0}; int32_t length = 0; uint64_t timestamp = 0; @@ -94,8 +97,7 @@ static PH_status_0_t HAL_REV_ReadPHStatus0(HAL_CANHandle hcan, return result; } -static PH_status_1_t HAL_REV_ReadPHStatus1(HAL_CANHandle hcan, - int32_t* status) { +static PH_status_1_t HAL_ReadREVPHStatus1(HAL_CANHandle hcan, int32_t* status) { uint8_t packedData[8] = {0}; int32_t length = 0; uint64_t timestamp = 0; @@ -119,9 +121,9 @@ enum REV_SolenoidState { kSolenoidControlledViaPulse }; -static void HAL_REV_UpdateDesiredPHSolenoidState(REV_PHObj* hph, - int32_t solenoid, - REV_SolenoidState state) { +static void HAL_UpdateDesiredREVPHSolenoidState(REV_PHObj* hph, + int32_t solenoid, + REV_SolenoidState state) { switch (solenoid) { case 0: hph->desiredSolenoidsState.channel_0 = state; @@ -174,15 +176,15 @@ static void HAL_REV_UpdateDesiredPHSolenoidState(REV_PHObj* hph, } } -static void HAL_REV_SendSolenoidsState(REV_PHObj* hph, int32_t* status) { +static void HAL_SendREVPHSolenoidsState(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; +static HAL_Bool HAL_CheckREVPHPulseTime(int32_t time) { + return ((time > 0) && (time <= HAL_REVPH_MAX_PULSE_TIME)) ? 1 : 0; } HAL_REVPHHandle HAL_InitializeREVPH(int32_t module, @@ -221,7 +223,7 @@ HAL_REVPHHandle HAL_InitializeREVPH(int32_t module, hph->controlPeriod = kDefaultControlPeriod; // Start closed-loop compressor control by starting solenoid state updates - HAL_REV_SendSolenoidsState(hph.get(), status); + HAL_SendREVPHSolenoidsState(hph.get(), status); return handle; } @@ -251,7 +253,7 @@ HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status) { return false; } - PH_status_0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); if (*status != 0) { return false; @@ -333,7 +335,7 @@ HAL_REVPHCompressorConfigType HAL_GetREVPHCompressorConfig( return HAL_REVPHCompressorConfigType_kDisabled; } - PH_status_0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); if (*status != 0) { return HAL_REVPHCompressorConfigType_kDisabled; @@ -349,7 +351,7 @@ HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) { return false; } - PH_status_0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); if (*status != 0) { return false; @@ -365,7 +367,7 @@ double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status) { return 0; } - PH_status_1_t status1 = HAL_REV_ReadPHStatus1(ph->hcan, status); + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); if (*status != 0) { return 0; @@ -389,7 +391,7 @@ double HAL_GetREVPHAnalogPressure(HAL_REVPHHandle handle, int32_t channel, return 0; } - PH_status_0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); if (*status != 0) { return 0; @@ -401,6 +403,109 @@ double HAL_GetREVPHAnalogPressure(HAL_REVPHHandle handle, int32_t channel, return PH_status_0_analog_1_decode(status0.analog_1); } +double HAL_GetREVPHVoltage(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status_1_v_bus_decode(status1.v_bus); +} + +double HAL_GetREVPH5VVoltage(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status_1_supply_voltage_5_v_decode(status1.supply_voltage_5_v); +} + +double HAL_GetREVPHSolenoidCurrent(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status_1_solenoid_current_decode(status1.solenoid_current); +} + +double HAL_GetREVPHSolenoidVoltage(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return 0; + } + + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); + + if (*status != 0) { + return 0; + } + + return PH_status_1_solenoid_voltage_decode(status1.solenoid_voltage); +} + +HAL_REVPHVersion HAL_GetREVPHVersion(HAL_REVPHHandle handle, int32_t* status) { + HAL_REVPHVersion version; + std::memset(&version, 0, sizeof(version)); + uint8_t packedData[8] = {0}; + int32_t length = 0; + uint64_t timestamp = 0; + PH_version_t result = {}; + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return version; + } + + HAL_WriteCANRTRFrame(ph->hcan, PH_VERSION_LENGTH, PH_VERSION_FRAME_API, + status); + + if (*status != 0) { + return version; + } + + HAL_ReadCANPacketTimeout(ph->hcan, PH_VERSION_FRAME_API, packedData, &length, + ×tamp, kDefaultControlPeriod * 2, status); + + if (*status != 0) { + return version; + } + + PH_version_unpack(&result, packedData, PH_VERSION_LENGTH); + + version.firmwareMajor = result.firmware_year; + version.firmwareMinor = result.firmware_minor; + version.firmwareFix = result.firmware_fix; + version.hardwareMinor = result.hardware_minor; + version.hardwareMajor = result.hardware_major; + version.uniqueId = result.unique_id; + + return version; +} + int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) { auto ph = REVPHHandles->Get(handle); if (ph == nullptr) { @@ -408,7 +513,7 @@ int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) { return 0; } - PH_status_0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); if (*status != 0) { return 0; @@ -448,11 +553,11 @@ void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values, // 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_UpdateDesiredREVPHSolenoidState(ph.get(), solenoid, + desiredSolenoidState); } } - HAL_REV_SendSolenoidsState(ph.get(), status); + HAL_SendREVPHSolenoidsState(ph.get(), status); } void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs, @@ -471,7 +576,7 @@ void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs, return; } - if (!HAL_REV_CheckPHPulseTime(durMs)) { + if (!HAL_CheckREVPHPulseTime(durMs)) { *status = PARAMETER_OUT_OF_RANGE; hal::SetLastError( status, @@ -482,9 +587,9 @@ void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs, { std::scoped_lock lock{ph->solenoidLock}; - HAL_REV_UpdateDesiredPHSolenoidState(ph.get(), index, - kSolenoidControlledViaPulse); - HAL_REV_SendSolenoidsState(ph.get(), status); + HAL_UpdateDesiredREVPHSolenoidState(ph.get(), index, + kSolenoidControlledViaPulse); + HAL_SendREVPHSolenoidsState(ph.get(), status); } if (*status != 0) { @@ -563,7 +668,7 @@ HAL_REVPHFaults HAL_GetREVPHFaults(HAL_REVPHHandle handle, int32_t* status) { return faults; } - PH_status_0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); faults.channel0Fault = status0.channel_0_fault; faults.channel1Fault = status0.channel_1_fault; faults.channel2Fault = status0.channel_2_fault; @@ -599,7 +704,7 @@ HAL_REVPHStickyFaults HAL_GetREVPHStickyFaults(HAL_REVPHHandle handle, return stickyFaults; } - PH_status_1_t status1 = HAL_REV_ReadPHStatus1(ph->hcan, status); + PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); stickyFaults.compressorOverCurrent = status1.sticky_compressor_oc_fault; stickyFaults.compressorOpen = status1.sticky_compressor_open_fault; stickyFaults.solenoidOverCurrent = status1.sticky_solenoid_oc_fault; @@ -610,3 +715,15 @@ HAL_REVPHStickyFaults HAL_GetREVPHStickyFaults(HAL_REVPHHandle handle, return stickyFaults; } + +void HAL_ClearREVPHStickyFaults(HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + uint8_t packedData[8] = {0}; + HAL_WriteCANPacket(ph->hcan, packedData, PH_CLEAR_FAULTS_LENGTH, + PH_CLEAR_FAULTS_FRAME_API, status); +} diff --git a/hal/src/main/native/athena/rev/PHFrames.cpp b/hal/src/main/native/athena/rev/PHFrames.cpp index dc7180a9bc..d098c004fc 100644 --- a/hal/src/main/native/athena/rev/PHFrames.cpp +++ b/hal/src/main/native/athena/rev/PHFrames.cpp @@ -48,6 +48,14 @@ static inline uint8_t pack_left_shift_u16( return (uint8_t)((uint8_t)(value << shift) & mask); } +static inline uint8_t pack_left_shift_u32( + uint32_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value << shift) & mask); +} + static inline uint8_t pack_right_shift_u16( uint16_t value, uint8_t shift, @@ -56,6 +64,14 @@ static inline uint8_t pack_right_shift_u16( return (uint8_t)((uint8_t)(value >> shift) & mask); } +static inline uint8_t pack_right_shift_u32( + uint32_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint8_t)((uint8_t)(value >> shift) & mask); +} + static inline uint16_t unpack_left_shift_u16( uint8_t value, uint8_t shift, @@ -64,6 +80,14 @@ static inline uint16_t unpack_left_shift_u16( return (uint16_t)((uint16_t)(value & mask) << shift); } +static inline uint32_t unpack_left_shift_u32( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint32_t)((uint32_t)(value & mask) << shift); +} + static inline uint8_t unpack_right_shift_u8( uint8_t value, uint8_t shift, @@ -80,6 +104,14 @@ static inline uint16_t unpack_right_shift_u16( return (uint16_t)((uint16_t)(value & mask) >> shift); } +static inline uint32_t unpack_right_shift_u32( + uint8_t value, + uint8_t shift, + uint8_t mask) +{ + return (uint32_t)((uint32_t)(value & mask) >> shift); +} + int PH_compressor_config_pack( uint8_t *dst_p, const struct PH_compressor_config_t *src_p, @@ -1623,6 +1655,7 @@ int PH_status_1_pack( 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_fault, 0u, 0x01u); + dst_p[7] |= pack_left_shift_u8(src_p->supply_voltage_5_v, 1u, 0xfeu); return (8); } @@ -1650,6 +1683,7 @@ int PH_status_1_unpack( 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_fault = unpack_right_shift_u8(src_p[7], 0u, 0x01u); + dst_p->supply_voltage_5_v = unpack_right_shift_u8(src_p[7], 1u, 0xfeu); return (0); } @@ -1853,6 +1887,21 @@ bool PH_status_1_sticky_has_reset_fault_is_in_range(uint8_t value) return (value <= 1u); } +uint8_t PH_status_1_supply_voltage_5_v_encode(double value) +{ + return (uint8_t)((value - 4.5) / 0.0078125); +} + +double PH_status_1_supply_voltage_5_v_decode(uint8_t value) +{ + return (((double)value * 0.0078125) + 4.5); +} + +bool PH_status_1_supply_voltage_5_v_is_in_range(uint8_t value) +{ + return (value <= 128u); +} + int PH_clear_faults_pack( uint8_t *dst_p, const struct PH_clear_faults_t *src_p, @@ -1876,3 +1925,147 @@ int PH_clear_faults_unpack( return (0); } + +int PH_version_pack( + uint8_t *dst_p, + const struct PH_version_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 8); + + dst_p[0] |= pack_left_shift_u8(src_p->firmware_fix, 0u, 0xffu); + dst_p[1] |= pack_left_shift_u8(src_p->firmware_minor, 0u, 0xffu); + dst_p[2] |= pack_left_shift_u8(src_p->firmware_year, 0u, 0xffu); + dst_p[3] |= pack_left_shift_u8(src_p->hardware_minor, 0u, 0xffu); + dst_p[4] |= pack_left_shift_u8(src_p->hardware_major, 0u, 0xffu); + dst_p[5] |= pack_left_shift_u32(src_p->unique_id, 0u, 0xffu); + dst_p[6] |= pack_right_shift_u32(src_p->unique_id, 8u, 0xffu); + dst_p[7] |= pack_right_shift_u32(src_p->unique_id, 16u, 0xffu); + + return (8); +} + +int PH_version_unpack( + struct PH_version_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 8u) { + return (-EINVAL); + } + + dst_p->firmware_fix = unpack_right_shift_u8(src_p[0], 0u, 0xffu); + dst_p->firmware_minor = unpack_right_shift_u8(src_p[1], 0u, 0xffu); + dst_p->firmware_year = unpack_right_shift_u8(src_p[2], 0u, 0xffu); + dst_p->hardware_minor = unpack_right_shift_u8(src_p[3], 0u, 0xffu); + dst_p->hardware_major = unpack_right_shift_u8(src_p[4], 0u, 0xffu); + dst_p->unique_id = unpack_right_shift_u32(src_p[5], 0u, 0xffu); + dst_p->unique_id |= unpack_left_shift_u32(src_p[6], 8u, 0xffu); + dst_p->unique_id |= unpack_left_shift_u32(src_p[7], 16u, 0xffu); + + return (0); +} + +uint8_t PH_version_firmware_fix_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_firmware_fix_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_firmware_fix_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_version_firmware_minor_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_firmware_minor_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_firmware_minor_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_version_firmware_year_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_firmware_year_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_firmware_year_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_version_hardware_minor_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_hardware_minor_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_hardware_minor_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint8_t PH_version_hardware_major_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_version_hardware_major_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_version_hardware_major_is_in_range(uint8_t value) +{ + (void)value; + + return (true); +} + +uint32_t PH_version_unique_id_encode(double value) +{ + return (uint32_t)(value); +} + +double PH_version_unique_id_decode(uint32_t value) +{ + return ((double)value); +} + +bool PH_version_unique_id_is_in_range(uint32_t value) +{ + return (value <= 16777215u); +} diff --git a/hal/src/main/native/athena/rev/PHFrames.h b/hal/src/main/native/athena/rev/PHFrames.h index b75b06309e..20411a86b7 100644 --- a/hal/src/main/native/athena/rev/PHFrames.h +++ b/hal/src/main/native/athena/rev/PHFrames.h @@ -50,6 +50,7 @@ extern "C" { #define PH_STATUS_0_FRAME_ID (0x9051800u) #define PH_STATUS_1_FRAME_ID (0x9051840u) #define PH_CLEAR_FAULTS_FRAME_ID (0x9051b80u) +#define PH_VERSION_FRAME_ID (0x9052600u) /* Frame lengths in bytes. */ #define PH_COMPRESSOR_CONFIG_LENGTH (5u) @@ -58,6 +59,7 @@ extern "C" { #define PH_STATUS_0_LENGTH (8u) #define PH_STATUS_1_LENGTH (8u) #define PH_CLEAR_FAULTS_LENGTH (0u) +#define PH_VERSION_LENGTH (8u) /* Extended or standard frame types. */ #define PH_COMPRESSOR_CONFIG_IS_EXTENDED (1) @@ -66,6 +68,7 @@ extern "C" { #define PH_STATUS_0_IS_EXTENDED (1) #define PH_STATUS_1_IS_EXTENDED (1) #define PH_CLEAR_FAULTS_IS_EXTENDED (1) +#define PH_VERSION_IS_EXTENDED (1) /* Frame cycle times in milliseconds. */ @@ -781,6 +784,13 @@ struct PH_status_1_t { * Offset: 0 */ uint8_t sticky_has_reset_fault : 1; + + /** + * Range: 0..128 (4.5..5.5 V) + * Scale: 0.0078125 + * Offset: 4.5 + */ + uint8_t supply_voltage_5_v : 7; }; /** @@ -797,6 +807,57 @@ struct PH_clear_faults_t { uint8_t dummy; }; +/** + * Signals in message Version. + * + * Get the version of the PH + * + * All signal values are as on the CAN bus. + */ +struct PH_version_t { + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_fix : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_minor : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t firmware_year : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_minor : 8; + + /** + * Range: 0..255 (0..255 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t hardware_major : 8; + + /** + * Range: 0..16777215 (0..16777215 -) + * Scale: 1 + * Offset: 0 + */ + uint32_t unique_id : 24; +}; + /** * Pack message Compressor_Config. * @@ -3502,6 +3563,33 @@ double PH_status_1_sticky_has_reset_fault_decode(uint8_t value); */ bool PH_status_1_sticky_has_reset_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_status_1_supply_voltage_5_v_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status_1_supply_voltage_5_v_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_status_1_supply_voltage_5_v_is_in_range(uint8_t value); + /** * Pack message Clear_Faults. * @@ -3530,6 +3618,196 @@ int PH_clear_faults_unpack( const uint8_t *src_p, size_t size); +/** + * Pack message Version. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PH_version_pack( + uint8_t *dst_p, + const struct PH_version_t *src_p, + size_t size); + +/** + * Unpack message Version. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PH_version_unpack( + struct PH_version_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_firmware_fix_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_firmware_fix_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_firmware_fix_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_firmware_minor_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_firmware_minor_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_firmware_minor_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_firmware_year_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_firmware_year_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_firmware_year_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_hardware_minor_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_hardware_minor_decode(uint8_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_hardware_minor_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint8_t PH_version_hardware_major_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_hardware_major_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_version_hardware_major_is_in_range(uint8_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint32_t PH_version_unique_id_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_version_unique_id_decode(uint32_t value); + +/** + * Check that given signal is in allowed range. + * + * @param[in] value Signal to check. + * + * @return true if in range, false otherwise. + */ +bool PH_version_unique_id_is_in_range(uint32_t value); + #ifdef __cplusplus } diff --git a/hal/src/main/native/include/hal/REVPH.h b/hal/src/main/native/include/hal/REVPH.h index 7a1255a0a0..40b3e911ee 100644 --- a/hal/src/main/native/include/hal/REVPH.h +++ b/hal/src/main/native/include/hal/REVPH.h @@ -24,6 +24,18 @@ HAL_ENUM(HAL_REVPHCompressorConfigType){ HAL_REVPHCompressorConfigType_kHybrid = 3, }; +/** + * Storage for REV PH Version + */ +struct HAL_REVPHVersion { + uint32_t firmwareMajor; + uint32_t firmwareMinor; + uint32_t firmwareFix; + uint32_t hardwareMinor; + uint32_t hardwareMajor; + uint32_t uniqueId; +}; + /** * Storage for compressor config */ @@ -110,6 +122,11 @@ 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); +double HAL_GetREVPHVoltage(HAL_REVPHHandle handle, int32_t* status); +double HAL_GetREVPH5VVoltage(HAL_REVPHHandle handle, int32_t* status); +double HAL_GetREVPHSolenoidCurrent(HAL_REVPHHandle handle, int32_t* status); +double HAL_GetREVPHSolenoidVoltage(HAL_REVPHHandle handle, int32_t* status); +HAL_REVPHVersion HAL_GetREVPHVersion(HAL_REVPHHandle handle, 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, @@ -123,6 +140,8 @@ HAL_REVPHFaults HAL_GetREVPHFaults(HAL_REVPHHandle handle, int32_t* status); HAL_REVPHStickyFaults HAL_GetREVPHStickyFaults(HAL_REVPHHandle handle, int32_t* status); +void HAL_ClearREVPHStickyFaults(HAL_REVPHHandle handle, int32_t* status); + #ifdef __cplusplus } // extern "C" #endif