Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal] Add HAL functions for compressor config modes on REV PH (#3724)

Browse Source
This commit is contained in:
Jan-Felix Abellera
2021-11-20 03:02:23 -06:00
committed by GitHub
parent f0ab6df5b6
commit eb835598a4
4 changed files with 568 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
hal/src/main/native/athena/REVPH.cpp
View File

@@ -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<HAL_REVPHCompressorConfigType>(status0.compressor_config);
}
HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {

158
hal/src/main/native/athena/rev/PHFrames.cpp
View File

@@ -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);
}

289
hal/src/main/native/athena/rev/PHFrames.h
View File

@@ -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
}

37
hal/src/main/native/include/hal/REVPH.h
View File

@@ -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,