| // 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 <thread> |
| |
| #include <fmt/format.h> |
| |
| #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 kDefaultCompressorDuty = 255; |
| static constexpr uint8_t kDefaultPressureTarget = 120; |
| static constexpr uint8_t kDefaultPressureHysteresis = 60; |
| |
| #define HAL_REVPH_MAX_PULSE_TIME 65534 |
| |
| static constexpr uint32_t APIFromExtId(uint32_t extId) { |
| return (extId >> 6) & 0x3FF; |
| } |
| |
| 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; |
| |
| namespace { |
| |
| struct REV_PHObj { |
| int32_t controlPeriod; |
| PH_set_all_t desiredSolenoidsState; |
| wpi::mutex solenoidLock; |
| HAL_CANHandle hcan; |
| std::string previousAllocation; |
| HAL_REVPHVersion versionInfo; |
| }; |
| |
| } // namespace |
| |
| static IndexedHandleResource<HAL_REVPHHandle, REV_PHObj, 63, |
| HAL_HandleEnum::REVPH>* REVPHHandles; |
| |
| namespace hal::init { |
| void InitializeREVPH() { |
| static IndexedHandleResource<HAL_REVPHHandle, REV_PHObj, kNumREVPHModules, |
| HAL_HandleEnum::REVPH> |
| rH; |
| REVPHHandles = &rH; |
| } |
| } // namespace hal::init |
| |
| 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; |
| PH_status_0_t result = {}; |
| |
| HAL_ReadCANPacketTimeout(hcan, PH_STATUS_0_FRAME_API, packedData, &length, |
| ×tamp, kPHFrameStatus0Timeout * 2, status); |
| |
| if (*status != 0) { |
| return result; |
| } |
| |
| PH_status_0_unpack(&result, packedData, PH_STATUS_0_LENGTH); |
| |
| return result; |
| } |
| |
| 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; |
| PH_status_1_t result = {}; |
| |
| HAL_ReadCANPacketTimeout(hcan, PH_STATUS_1_FRAME_API, packedData, &length, |
| ×tamp, kPHFrameStatus1Timeout * 2, status); |
| |
| if (*status != 0) { |
| return result; |
| } |
| |
| PH_status_1_unpack(&result, packedData, PH_STATUS_1_LENGTH); |
| |
| return result; |
| } |
| |
| enum REV_SolenoidState { |
| kSolenoidDisabled = 0, |
| kSolenoidEnabled, |
| kSolenoidControlledViaPulse |
| }; |
| |
| static void HAL_UpdateDesiredREVPHSolenoidState(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_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_CheckREVPHPulseTime(int32_t time) { |
| return ((time > 0) && (time <= HAL_REVPH_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; |
| std::memset(&hph->desiredSolenoidsState, 0, |
| sizeof(hph->desiredSolenoidsState)); |
| std::memset(&hph->versionInfo, 0, sizeof(hph->versionInfo)); |
| |
| // Start closed-loop compressor control by starting solenoid state updates |
| HAL_SendREVPHSolenoidsState(hph.get(), status); |
| |
| 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_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); |
| |
| if (*status != 0) { |
| return false; |
| } |
| |
| return status0.compressor_on; |
| } |
| |
| void HAL_SetREVPHCompressorConfig(HAL_REVPHHandle handle, |
| const 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 = |
| PH_compressor_config_minimum_tank_pressure_encode( |
| config->minAnalogVoltage); |
| frameData.maximum_tank_pressure = |
| PH_compressor_config_maximum_tank_pressure_encode( |
| 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 = minAnalogVoltage; |
| config.maxAnalogVoltage = 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 = minAnalogVoltage; |
| config.maxAnalogVoltage = 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_status_0_t status0 = HAL_ReadREVPHStatus0(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) { |
| *status = HAL_HANDLE_ERROR; |
| return false; |
| } |
| |
| PH_status_0_t status0 = HAL_ReadREVPHStatus0(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_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status); |
| |
| if (*status != 0) { |
| return 0; |
| } |
| |
| return PH_status_1_compressor_current_decode(status1.compressor_current); |
| } |
| |
| double HAL_GetREVPHAnalogVoltage(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_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status); |
| |
| if (*status != 0) { |
| return 0; |
| } |
| |
| if (channel == 0) { |
| return PH_status_0_analog_0_decode(status0.analog_0); |
| } |
| 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); |
| } |
| |
| void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version, |
| int32_t* status) { |
| 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; |
| } |
| |
| if (ph->versionInfo.firmwareMajor > 0) { |
| version->firmwareMajor = ph->versionInfo.firmwareMajor; |
| version->firmwareMinor = ph->versionInfo.firmwareMinor; |
| version->firmwareFix = ph->versionInfo.firmwareFix; |
| version->hardwareMajor = ph->versionInfo.hardwareMajor; |
| version->hardwareMinor = ph->versionInfo.hardwareMinor; |
| version->uniqueId = ph->versionInfo.uniqueId; |
| |
| *status = 0; |
| return; |
| } |
| |
| HAL_WriteCANRTRFrame(ph->hcan, PH_VERSION_LENGTH, PH_VERSION_FRAME_API, |
| status); |
| |
| if (*status != 0) { |
| return; |
| } |
| |
| uint32_t timeoutMs = 100; |
| for (uint32_t i = 0; i <= timeoutMs; i++) { |
| HAL_ReadCANPacketNew(ph->hcan, PH_VERSION_FRAME_API, packedData, &length, |
| ×tamp, status); |
| if (*status == 0) { |
| break; |
| } |
| std::this_thread::sleep_for(std::chrono::milliseconds(1)); |
| } |
| |
| if (*status != 0) { |
| return; |
| } |
| |
| 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; |
| |
| ph->versionInfo = *version; |
| } |
| |
| 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_status_0_t status0 = HAL_ReadREVPHStatus0(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_UpdateDesiredREVPHSolenoidState(ph.get(), solenoid, |
| desiredSolenoidState); |
| } |
| } |
| HAL_SendREVPHSolenoidsState(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_CheckREVPHPulseTime(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_UpdateDesiredREVPHSolenoidState(ph.get(), index, |
| kSolenoidControlledViaPulse); |
| HAL_SendREVPHSolenoidsState(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); |
| } |
| |
| void HAL_GetREVPHFaults(HAL_REVPHHandle handle, HAL_REVPHFaults* faults, |
| int32_t* status) { |
| std::memset(faults, 0, sizeof(*faults)); |
| auto ph = REVPHHandles->Get(handle); |
| if (ph == nullptr) { |
| *status = HAL_HANDLE_ERROR; |
| return; |
| } |
| |
| 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; |
| faults->channel3Fault = status0.channel_3_fault; |
| faults->channel4Fault = status0.channel_4_fault; |
| faults->channel5Fault = status0.channel_5_fault; |
| faults->channel6Fault = status0.channel_6_fault; |
| faults->channel7Fault = status0.channel_7_fault; |
| faults->channel8Fault = status0.channel_8_fault; |
| faults->channel9Fault = status0.channel_9_fault; |
| faults->channel10Fault = status0.channel_10_fault; |
| faults->channel11Fault = status0.channel_11_fault; |
| faults->channel12Fault = status0.channel_12_fault; |
| faults->channel13Fault = status0.channel_13_fault; |
| faults->channel14Fault = status0.channel_14_fault; |
| faults->channel15Fault = status0.channel_15_fault; |
| faults->compressorOverCurrent = status0.compressor_oc_fault; |
| faults->compressorOpen = status0.compressor_open_fault; |
| faults->solenoidOverCurrent = status0.solenoid_oc_fault; |
| faults->brownout = status0.brownout_fault; |
| faults->canWarning = status0.can_warning_fault; |
| faults->hardwareFault = status0.hardware_fault; |
| } |
| |
| void HAL_GetREVPHStickyFaults(HAL_REVPHHandle handle, |
| HAL_REVPHStickyFaults* stickyFaults, |
| int32_t* status) { |
| std::memset(stickyFaults, 0, sizeof(*stickyFaults)); |
| auto ph = REVPHHandles->Get(handle); |
| if (ph == nullptr) { |
| *status = HAL_HANDLE_ERROR; |
| return; |
| } |
| |
| 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; |
| stickyFaults->brownout = status1.sticky_brownout_fault; |
| stickyFaults->canWarning = status1.sticky_can_warning_fault; |
| stickyFaults->canBusOff = status1.sticky_can_bus_off_fault; |
| stickyFaults->hasReset = status1.sticky_has_reset_fault; |
| } |
| |
| 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); |
| } |