diff --git a/hal/src/main/native/athena/REVPH.cpp b/hal/src/main/native/athena/REVPH.cpp index 6099c663bc..0161cf5dc8 100644 --- a/hal/src/main/native/athena/REVPH.cpp +++ b/hal/src/main/native/athena/REVPH.cpp @@ -41,6 +41,8 @@ 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_STATUS0_FRAME_API = APIFromExtId(PH_STATUS0_FRAME_ID); static constexpr uint32_t PH_STATUS1_FRAME_API = @@ -266,6 +268,88 @@ HAL_Bool HAL_GetREVPHClosedLoopControl(HAL_REVPHHandle handle, return false; // TODO } +void HAL_SetREVPHCompressorConfig(HAL_REVPHHandle handle, + HAL_REVPHCompressorConfig config, + int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + PH_compressor_config_t frameData; + frameData.minimum_tank_pressure = config.minAnalogVoltage; + frameData.maximum_tank_pressure = config.maxAnalogVoltage; + frameData.force_disable = config.forceDisable; + frameData.use_digital = config.useDigital; + + uint8_t packedData[PH_COMPRESSOR_CONFIG_LENGTH] = {0}; + PH_compressor_config_pack(packedData, &frameData, + PH_COMPRESSOR_CONFIG_LENGTH); + HAL_WriteCANPacket(ph->hcan, packedData, PH_COMPRESSOR_CONFIG_LENGTH, + PH_COMPRESSOR_CONFIG_API, status); +} + +void HAL_SetREVPHClosedLoopControlDisabled(HAL_REVPHHandle handle, + int32_t* status) { + HAL_REVPHCompressorConfig config = {0, 0, 0, 0}; + config.forceDisable = true; + + HAL_SetREVPHCompressorConfig(handle, config, status); +} + +void HAL_SetREVPHClosedLoopControlDigital(HAL_REVPHHandle handle, + int32_t* status) { + HAL_REVPHCompressorConfig config = {0, 0, 0, 0}; + config.useDigital = true; + + HAL_SetREVPHCompressorConfig(handle, config, status); +} + +void HAL_SetREVPHClosedLoopControlAnalog(HAL_REVPHHandle handle, + double minAnalogVoltage, + double maxAnalogVoltage, + int32_t* status) { + HAL_REVPHCompressorConfig config = {0, 0, 0, 0}; + config.minAnalogVoltage = + PH_compressor_config_minimum_tank_pressure_encode(minAnalogVoltage); + config.maxAnalogVoltage = + PH_compressor_config_maximum_tank_pressure_encode(maxAnalogVoltage); + + HAL_SetREVPHCompressorConfig(handle, config, status); +} + +void HAL_SetREVPHClosedLoopControlHybrid(HAL_REVPHHandle handle, + double minAnalogVoltage, + double maxAnalogVoltage, + int32_t* status) { + HAL_REVPHCompressorConfig config = {0, 0, 0, 0}; + config.minAnalogVoltage = + PH_compressor_config_minimum_tank_pressure_encode(minAnalogVoltage); + config.maxAnalogVoltage = + PH_compressor_config_maximum_tank_pressure_encode(maxAnalogVoltage); + config.useDigital = true; + + HAL_SetREVPHCompressorConfig(handle, config, status); +} + +HAL_REVPHCompressorConfigType HAL_GetREVPHCompressorConfig( + HAL_REVPHHandle handle, int32_t* status) { + auto ph = REVPHHandles->Get(handle); + if (ph == nullptr) { + *status = HAL_HANDLE_ERROR; + return HAL_REVPHCompressorConfigType_kDisabled; + } + + PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status); + + if (*status != 0) { + return HAL_REVPHCompressorConfigType_kDisabled; + } + + return static_cast(status0.compressor_config); +} + HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) { auto ph = REVPHHandles->Get(handle); if (ph == nullptr) { diff --git a/hal/src/main/native/athena/rev/PHFrames.cpp b/hal/src/main/native/athena/rev/PHFrames.cpp index e7f77e0054..7b674f16fd 100644 --- a/hal/src/main/native/athena/rev/PHFrames.cpp +++ b/hal/src/main/native/athena/rev/PHFrames.cpp @@ -80,6 +80,106 @@ static inline uint16_t unpack_right_shift_u16( return (uint16_t)((uint16_t)(value & mask) >> shift); } +int PH_compressor_config_pack( + uint8_t *dst_p, + const struct PH_compressor_config_t *src_p, + size_t size) +{ + if (size < 5u) { + return (-EINVAL); + } + + memset(&dst_p[0], 0, 5); + + dst_p[0] |= pack_left_shift_u16(src_p->minimum_tank_pressure, 0u, 0xffu); + dst_p[1] |= pack_right_shift_u16(src_p->minimum_tank_pressure, 8u, 0xffu); + dst_p[2] |= pack_left_shift_u16(src_p->maximum_tank_pressure, 0u, 0xffu); + dst_p[3] |= pack_right_shift_u16(src_p->maximum_tank_pressure, 8u, 0xffu); + dst_p[4] |= pack_left_shift_u8(src_p->force_disable, 0u, 0x01u); + dst_p[4] |= pack_left_shift_u8(src_p->use_digital, 1u, 0x02u); + + return (5); +} + +int PH_compressor_config_unpack( + struct PH_compressor_config_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + if (size < 5u) { + return (-EINVAL); + } + + dst_p->minimum_tank_pressure = unpack_right_shift_u16(src_p[0], 0u, 0xffu); + dst_p->minimum_tank_pressure |= unpack_left_shift_u16(src_p[1], 8u, 0xffu); + dst_p->maximum_tank_pressure = unpack_right_shift_u16(src_p[2], 0u, 0xffu); + dst_p->maximum_tank_pressure |= unpack_left_shift_u16(src_p[3], 8u, 0xffu); + dst_p->force_disable = unpack_right_shift_u8(src_p[4], 0u, 0x01u); + dst_p->use_digital = unpack_right_shift_u8(src_p[4], 1u, 0x02u); + + return (0); +} + +uint16_t PH_compressor_config_minimum_tank_pressure_encode(double value) +{ + return (uint16_t)(value / 0.001); +} + +double PH_compressor_config_minimum_tank_pressure_decode(uint16_t value) +{ + return ((double)value * 0.001); +} + +bool PH_compressor_config_minimum_tank_pressure_is_in_range(uint16_t value) +{ + return (value <= 5000u); +} + +uint16_t PH_compressor_config_maximum_tank_pressure_encode(double value) +{ + return (uint16_t)(value / 0.001); +} + +double PH_compressor_config_maximum_tank_pressure_decode(uint16_t value) +{ + return ((double)value * 0.001); +} + +bool PH_compressor_config_maximum_tank_pressure_is_in_range(uint16_t value) +{ + return (value <= 5000u); +} + +uint8_t PH_compressor_config_force_disable_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_compressor_config_force_disable_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_compressor_config_force_disable_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_compressor_config_use_digital_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_compressor_config_use_digital_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_compressor_config_use_digital_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + int PH_set_all_pack( uint8_t *dst_p, const struct PH_set_all_t *src_p, @@ -755,6 +855,8 @@ int PH_status0_pack( 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); + dst_p[7] |= pack_left_shift_u8(src_p->robo_rio_present, 2u, 0x04u); + dst_p[7] |= pack_left_shift_u8(src_p->compressor_config, 3u, 0x18u); return (8); } @@ -811,6 +913,8 @@ int PH_status0_unpack( 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); + dst_p->robo_rio_present = unpack_right_shift_u8(src_p[7], 2u, 0x04u); + dst_p->compressor_config = unpack_right_shift_u8(src_p[7], 3u, 0x18u); return (0); } @@ -1464,6 +1568,36 @@ bool PH_status0_system_enabled_is_in_range(uint8_t value) return (value <= 1u); } +uint8_t PH_status0_robo_rio_present_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_robo_rio_present_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_robo_rio_present_is_in_range(uint8_t value) +{ + return (value <= 1u); +} + +uint8_t PH_status0_compressor_config_encode(double value) +{ + return (uint8_t)(value); +} + +double PH_status0_compressor_config_decode(uint8_t value) +{ + return ((double)value); +} + +bool PH_status0_compressor_config_is_in_range(uint8_t value) +{ + return (value <= 3u); +} + int PH_status1_pack( uint8_t *dst_p, const struct PH_status1_t *src_p, @@ -1718,3 +1852,27 @@ bool PH_status1_sticky_has_reset_is_in_range(uint8_t value) { return (value <= 1u); } + +int PH_clear_faults_pack( + uint8_t *dst_p, + const struct PH_clear_faults_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} + +int PH_clear_faults_unpack( + struct PH_clear_faults_t *dst_p, + const uint8_t *src_p, + size_t size) +{ + (void)dst_p; + (void)src_p; + (void)size; + + return (0); +} diff --git a/hal/src/main/native/athena/rev/PHFrames.h b/hal/src/main/native/athena/rev/PHFrames.h index 295be0a31f..71b868d7a9 100644 --- a/hal/src/main/native/athena/rev/PHFrames.h +++ b/hal/src/main/native/athena/rev/PHFrames.h @@ -44,22 +44,28 @@ extern "C" { #endif /* Frame ids. */ +#define PH_COMPRESSOR_CONFIG_FRAME_ID (0x9050840u) #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) +#define PH_CLEAR_FAULTS_FRAME_ID (0x9051b80u) /* Frame lengths in bytes. */ +#define PH_COMPRESSOR_CONFIG_LENGTH (5u) #define PH_SET_ALL_LENGTH (4u) #define PH_PULSE_ONCE_LENGTH (4u) #define PH_STATUS0_LENGTH (8u) #define PH_STATUS1_LENGTH (8u) +#define PH_CLEAR_FAULTS_LENGTH (0u) /* Extended or standard frame types. */ +#define PH_COMPRESSOR_CONFIG_IS_EXTENDED (1) #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) +#define PH_CLEAR_FAULTS_IS_EXTENDED (1) /* Frame cycle times in milliseconds. */ @@ -67,6 +73,43 @@ extern "C" { /* Signal choices. */ +/** + * Signals in message Compressor_Config. + * + * Configures compressor to use digitial/analog sensors + * + * All signal values are as on the CAN bus. + */ +struct PH_compressor_config_t { + /** + * Range: 0..5000 (0..5 V) + * Scale: 0.001 + * Offset: 0 + */ + uint16_t minimum_tank_pressure : 16; + + /** + * Range: 0..5000 (0..5 V) + * Scale: 0.001 + * Offset: 0 + */ + uint16_t maximum_tank_pressure : 16; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t force_disable : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t use_digital : 1; +}; + /** * Signals in message SetAll. * @@ -624,6 +667,20 @@ struct PH_status0_t { * Offset: 0 */ uint8_t system_enabled : 1; + + /** + * Range: 0..1 (0..1 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t robo_rio_present : 1; + + /** + * Range: 0..3 (0..3 -) + * Scale: 1 + * Offset: 0 + */ + uint8_t compressor_config : 2; }; /** @@ -726,6 +783,156 @@ struct PH_status1_t { uint8_t sticky_has_reset : 1; }; +/** + * Signals in message ClearFaults. + * + * Clear sticky faults on the device + * + * All signal values are as on the CAN bus. + */ +struct PH_clear_faults_t { + /** + * Dummy signal in empty message. + */ + uint8_t dummy; +}; + +/** + * Pack message Compressor_Config. + * + * @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_compressor_config_pack( + uint8_t *dst_p, + const struct PH_compressor_config_t *src_p, + size_t size); + +/** + * Unpack message Compressor_Config. + * + * @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_compressor_config_unpack( + struct PH_compressor_config_t *dst_p, + const uint8_t *src_p, + size_t size); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PH_compressor_config_minimum_tank_pressure_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_compressor_config_minimum_tank_pressure_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_compressor_config_minimum_tank_pressure_is_in_range(uint16_t value); + +/** + * Encode given signal by applying scaling and offset. + * + * @param[in] value Signal to encode. + * + * @return Encoded signal. + */ +uint16_t PH_compressor_config_maximum_tank_pressure_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_compressor_config_maximum_tank_pressure_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_compressor_config_maximum_tank_pressure_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_compressor_config_force_disable_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_compressor_config_force_disable_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_compressor_config_force_disable_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_compressor_config_use_digital_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_compressor_config_use_digital_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_compressor_config_use_digital_is_in_range(uint8_t value); + /** * Pack message SetAll. * @@ -2862,6 +3069,60 @@ double PH_status0_system_enabled_decode(uint8_t value); */ bool PH_status0_system_enabled_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_robo_rio_present_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_robo_rio_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_status0_robo_rio_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_status0_compressor_config_encode(double value); + +/** + * Decode given signal by applying scaling and offset. + * + * @param[in] value Signal to decode. + * + * @return Decoded signal. + */ +double PH_status0_compressor_config_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_config_is_in_range(uint8_t value); + /** * Pack message Status1. * @@ -3241,6 +3502,34 @@ double PH_status1_sticky_has_reset_decode(uint8_t value); */ bool PH_status1_sticky_has_reset_is_in_range(uint8_t value); +/** + * Pack message ClearFaults. + * + * @param[out] dst_p Buffer to pack the message into. + * @param[in] src_p Data to pack. + * @param[in] size Size of dst_p. + * + * @return Size of packed data, or negative error code. + */ +int PH_clear_faults_pack( + uint8_t *dst_p, + const struct PH_clear_faults_t *src_p, + size_t size); + +/** + * Unpack message ClearFaults. + * + * @param[out] dst_p Object to unpack the message into. + * @param[in] src_p Message to unpack. + * @param[in] size Size of src_p. + * + * @return zero(0) or negative error code. + */ +int PH_clear_faults_unpack( + struct PH_clear_faults_t *dst_p, + const uint8_t *src_p, + size_t size); + #ifdef __cplusplus } diff --git a/hal/src/main/native/include/hal/REVPH.h b/hal/src/main/native/include/hal/REVPH.h index 0cab15e090..19984c88fb 100644 --- a/hal/src/main/native/include/hal/REVPH.h +++ b/hal/src/main/native/include/hal/REVPH.h @@ -14,6 +14,26 @@ * @{ */ +/** + * The compressor configuration type + */ +HAL_ENUM(HAL_REVPHCompressorConfigType){ + HAL_REVPHCompressorConfigType_kDisabled = 0, + HAL_REVPHCompressorConfigType_kDigital = 1, + HAL_REVPHCompressorConfigType_kAnalog = 2, + HAL_REVPHCompressorConfigType_kHybrid = 3, +}; + +/** + * Storage for compressor config + */ +struct HAL_REVPHCompressorConfig { + double minAnalogVoltage; + double maxAnalogVoltage; + HAL_Bool forceDisable; + HAL_Bool useDigital; +}; + #ifdef __cplusplus extern "C" { #endif @@ -31,6 +51,23 @@ 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); +void HAL_SetREVPHCompressorConfig(HAL_REVPHHandle handle, + HAL_REVPHCompressorConfig config, + int32_t* status); +void HAL_SetREVPHClosedLoopControlDisabled(HAL_REVPHHandle handle, + int32_t* status); +void HAL_SetREVPHClosedLoopControlDigital(HAL_REVPHHandle handle, + int32_t* status); +void HAL_SetREVPHClosedLoopControlAnalog(HAL_REVPHHandle handle, + double minAnalogVoltage, + double maxAnalogVoltage, + int32_t* status); +void HAL_SetREVPHClosedLoopControlHybrid(HAL_REVPHHandle handle, + double minAnalogVoltage, + double maxAnalogVoltage, + int32_t* status); +HAL_REVPHCompressorConfigType HAL_GetREVPHCompressorConfig( + 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,