| // 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/HAL.h" |
| |
| #include <cstdio> |
| #include <vector> |
| |
| #include <wpi/mutex.h> |
| #include <wpi/spinlock.h> |
| |
| #ifdef _WIN32 |
| #include <Windows.h> |
| #pragma comment(lib, "Winmm.lib") |
| #endif // _WIN32 |
| |
| #include "ErrorsInternal.h" |
| #include "HALInitializer.h" |
| #include "MockHooksInternal.h" |
| #include "hal/DriverStation.h" |
| #include "hal/Errors.h" |
| #include "hal/Extensions.h" |
| #include "hal/handles/HandlesInternal.h" |
| #include "hal/simulation/DriverStationData.h" |
| #include "hal/simulation/SimCallbackRegistry.h" |
| #include "mockdata/RoboRioDataInternal.h" |
| |
| using namespace hal; |
| |
| namespace { |
| class SimPeriodicCallbackRegistry : public impl::SimCallbackRegistryBase { |
| public: |
| int32_t Register(HALSIM_SimPeriodicCallback callback, void* param) { |
| std::scoped_lock lock(m_mutex); |
| return DoRegister(reinterpret_cast<RawFunctor>(callback), param); |
| } |
| |
| void operator()() const { |
| #ifdef _MSC_VER // work around VS2019 16.4.0 bug |
| std::scoped_lock<wpi::recursive_spinlock> lock(m_mutex); |
| #else |
| std::scoped_lock lock(m_mutex); |
| #endif |
| if (m_callbacks) { |
| for (auto&& cb : *m_callbacks) { |
| reinterpret_cast<HALSIM_SimPeriodicCallback>(cb.callback)(cb.param); |
| } |
| } |
| } |
| }; |
| } // namespace |
| |
| static HAL_RuntimeType runtimeType{HAL_Runtime_Simulation}; |
| static wpi::spinlock gOnShutdownMutex; |
| static std::vector<std::pair<void*, void (*)(void*)>> gOnShutdown; |
| static SimPeriodicCallbackRegistry gSimPeriodicBefore; |
| static SimPeriodicCallbackRegistry gSimPeriodicAfter; |
| |
| namespace hal { |
| void InitializeDriverStation(); |
| } // namespace hal |
| |
| namespace hal::init { |
| void InitializeHAL() { |
| InitializeAccelerometerData(); |
| InitializeAddressableLEDData(); |
| InitializeAnalogGyroData(); |
| InitializeAnalogInData(); |
| InitializeAnalogOutData(); |
| InitializeAnalogTriggerData(); |
| InitializeCanData(); |
| InitializeCANAPI(); |
| InitializeDigitalPWMData(); |
| InitializeDutyCycleData(); |
| InitializeDIOData(); |
| InitializeDriverStationData(); |
| InitializeEncoderData(); |
| InitializeI2CData(); |
| InitializeCTREPCMData(); |
| InitializeREVPHData(); |
| InitializePowerDistributionData(); |
| InitializePWMData(); |
| InitializeRelayData(); |
| InitializeRoboRioData(); |
| InitializeSimDeviceData(); |
| InitializeSPIAccelerometerData(); |
| InitializeSPIData(); |
| InitializeAccelerometer(); |
| InitializeAddressableLED(); |
| InitializeAnalogAccumulator(); |
| InitializeAnalogGyro(); |
| InitializeAnalogInput(); |
| InitializeAnalogInternal(); |
| InitializeAnalogOutput(); |
| InitializeAnalogTrigger(); |
| InitializeCAN(); |
| InitializeConstants(); |
| InitializeCounter(); |
| InitializeDigitalInternal(); |
| InitializeDIO(); |
| InitializeDutyCycle(); |
| InitializeDriverStation(); |
| InitializeEncoder(); |
| InitializeExtensions(); |
| InitializeI2C(); |
| InitializeInterrupts(); |
| InitializeMain(); |
| InitializeMockHooks(); |
| InitializeNotifier(); |
| InitializePowerDistribution(); |
| InitializePorts(); |
| InitializePower(); |
| InitializeCTREPCM(); |
| InitializeREVPH(); |
| InitializePWM(); |
| InitializeRelay(); |
| InitializeSerialPort(); |
| InitializeSimDevice(); |
| InitializeSPI(); |
| InitializeThreads(); |
| } |
| } // namespace hal::init |
| |
| extern "C" { |
| |
| HAL_PortHandle HAL_GetPort(int32_t channel) { |
| // Dont allow a number that wouldn't fit in a uint8_t |
| if (channel < 0 || channel >= 255) { |
| return HAL_kInvalidHandle; |
| } |
| return createPortHandle(channel, 1); |
| } |
| |
| HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel) { |
| // Dont allow a number that wouldn't fit in a uint8_t |
| if (channel < 0 || channel >= 255) { |
| return HAL_kInvalidHandle; |
| } |
| if (module < 0 || module >= 255) { |
| return HAL_kInvalidHandle; |
| } |
| return createPortHandle(channel, module); |
| } |
| |
| const char* HAL_GetErrorMessage(int32_t code) { |
| switch (code) { |
| case 0: |
| return ""; |
| case CTR_RxTimeout: |
| return CTR_RxTimeout_MESSAGE; |
| case CTR_TxTimeout: |
| return CTR_TxTimeout_MESSAGE; |
| case CTR_InvalidParamValue: |
| return CTR_InvalidParamValue_MESSAGE; |
| case CTR_UnexpectedArbId: |
| return CTR_UnexpectedArbId_MESSAGE; |
| case CTR_TxFailed: |
| return CTR_TxFailed_MESSAGE; |
| case CTR_SigNotUpdated: |
| return CTR_SigNotUpdated_MESSAGE; |
| case NiFpga_Status_FifoTimeout: |
| return NiFpga_Status_FifoTimeout_MESSAGE; |
| case NiFpga_Status_TransferAborted: |
| return NiFpga_Status_TransferAborted_MESSAGE; |
| case NiFpga_Status_MemoryFull: |
| return NiFpga_Status_MemoryFull_MESSAGE; |
| case NiFpga_Status_SoftwareFault: |
| return NiFpga_Status_SoftwareFault_MESSAGE; |
| case NiFpga_Status_InvalidParameter: |
| return NiFpga_Status_InvalidParameter_MESSAGE; |
| case NiFpga_Status_ResourceNotFound: |
| return NiFpga_Status_ResourceNotFound_MESSAGE; |
| case NiFpga_Status_ResourceNotInitialized: |
| return NiFpga_Status_ResourceNotInitialized_MESSAGE; |
| case NiFpga_Status_HardwareFault: |
| return NiFpga_Status_HardwareFault_MESSAGE; |
| case NiFpga_Status_IrqTimeout: |
| return NiFpga_Status_IrqTimeout_MESSAGE; |
| case SAMPLE_RATE_TOO_HIGH: |
| return SAMPLE_RATE_TOO_HIGH_MESSAGE; |
| case VOLTAGE_OUT_OF_RANGE: |
| return VOLTAGE_OUT_OF_RANGE_MESSAGE; |
| case LOOP_TIMING_ERROR: |
| return LOOP_TIMING_ERROR_MESSAGE; |
| case SPI_WRITE_NO_MOSI: |
| return SPI_WRITE_NO_MOSI_MESSAGE; |
| case SPI_READ_NO_MISO: |
| return SPI_READ_NO_MISO_MESSAGE; |
| case SPI_READ_NO_DATA: |
| return SPI_READ_NO_DATA_MESSAGE; |
| case INCOMPATIBLE_STATE: |
| return INCOMPATIBLE_STATE_MESSAGE; |
| case NO_AVAILABLE_RESOURCES: |
| return NO_AVAILABLE_RESOURCES_MESSAGE; |
| case RESOURCE_IS_ALLOCATED: |
| return RESOURCE_IS_ALLOCATED_MESSAGE; |
| case RESOURCE_OUT_OF_RANGE: |
| return RESOURCE_OUT_OF_RANGE_MESSAGE; |
| case HAL_INVALID_ACCUMULATOR_CHANNEL: |
| return HAL_INVALID_ACCUMULATOR_CHANNEL_MESSAGE; |
| case HAL_HANDLE_ERROR: |
| return HAL_HANDLE_ERROR_MESSAGE; |
| case NULL_PARAMETER: |
| return NULL_PARAMETER_MESSAGE; |
| case ANALOG_TRIGGER_LIMIT_ORDER_ERROR: |
| return ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE; |
| case ANALOG_TRIGGER_PULSE_OUTPUT_ERROR: |
| return ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE; |
| case PARAMETER_OUT_OF_RANGE: |
| return PARAMETER_OUT_OF_RANGE_MESSAGE; |
| case HAL_COUNTER_NOT_SUPPORTED: |
| return HAL_COUNTER_NOT_SUPPORTED_MESSAGE; |
| case HAL_ERR_CANSessionMux_InvalidBuffer: |
| return ERR_CANSessionMux_InvalidBuffer_MESSAGE; |
| case HAL_ERR_CANSessionMux_MessageNotFound: |
| return ERR_CANSessionMux_MessageNotFound_MESSAGE; |
| case HAL_WARN_CANSessionMux_NoToken: |
| return WARN_CANSessionMux_NoToken_MESSAGE; |
| case HAL_ERR_CANSessionMux_NotAllowed: |
| return ERR_CANSessionMux_NotAllowed_MESSAGE; |
| case HAL_ERR_CANSessionMux_NotInitialized: |
| return ERR_CANSessionMux_NotInitialized_MESSAGE; |
| case VI_ERROR_SYSTEM_ERROR: |
| return VI_ERROR_SYSTEM_ERROR_MESSAGE; |
| case VI_ERROR_INV_OBJECT: |
| return VI_ERROR_INV_OBJECT_MESSAGE; |
| case VI_ERROR_RSRC_LOCKED: |
| return VI_ERROR_RSRC_LOCKED_MESSAGE; |
| case VI_ERROR_RSRC_NFOUND: |
| return VI_ERROR_RSRC_NFOUND_MESSAGE; |
| case VI_ERROR_INV_RSRC_NAME: |
| return VI_ERROR_INV_RSRC_NAME_MESSAGE; |
| case VI_ERROR_QUEUE_OVERFLOW: |
| return VI_ERROR_QUEUE_OVERFLOW_MESSAGE; |
| case VI_ERROR_IO: |
| return VI_ERROR_IO_MESSAGE; |
| case VI_ERROR_ASRL_PARITY: |
| return VI_ERROR_ASRL_PARITY_MESSAGE; |
| case VI_ERROR_ASRL_FRAMING: |
| return VI_ERROR_ASRL_FRAMING_MESSAGE; |
| case VI_ERROR_ASRL_OVERRUN: |
| return VI_ERROR_ASRL_OVERRUN_MESSAGE; |
| case VI_ERROR_RSRC_BUSY: |
| return VI_ERROR_RSRC_BUSY_MESSAGE; |
| case VI_ERROR_INV_PARAMETER: |
| return VI_ERROR_INV_PARAMETER_MESSAGE; |
| case HAL_PWM_SCALE_ERROR: |
| return HAL_PWM_SCALE_ERROR_MESSAGE; |
| case HAL_CAN_TIMEOUT: |
| return HAL_CAN_TIMEOUT_MESSAGE; |
| case HAL_SIM_NOT_SUPPORTED: |
| return HAL_SIM_NOT_SUPPORTED_MESSAGE; |
| case HAL_CAN_BUFFER_OVERRUN: |
| return HAL_CAN_BUFFER_OVERRUN_MESSAGE; |
| case HAL_LED_CHANNEL_ERROR: |
| return HAL_LED_CHANNEL_ERROR_MESSAGE; |
| case HAL_USE_LAST_ERROR: |
| return HAL_USE_LAST_ERROR_MESSAGE; |
| case HAL_CONSOLE_OUT_ENABLED_ERROR: |
| return HAL_CONSOLE_OUT_ENABLED_ERROR_MESSAGE; |
| default: |
| return "Unknown error status"; |
| } |
| } |
| |
| HAL_RuntimeType HAL_GetRuntimeType(void) { |
| return runtimeType; |
| } |
| |
| void HALSIM_SetRuntimeType(HAL_RuntimeType type) { |
| runtimeType = type; |
| } |
| |
| int32_t HAL_GetFPGAVersion(int32_t* status) { |
| return 2018; // Automatically script this at some point |
| } |
| |
| int64_t HAL_GetFPGARevision(int32_t* status) { |
| return 0; // TODO: Find a better number to return; |
| } |
| |
| size_t HAL_GetSerialNumber(char* buffer, size_t size) { |
| return HALSIM_GetRoboRioSerialNumber(buffer, size); |
| } |
| |
| size_t HAL_GetComments(char* buffer, size_t size) { |
| return HALSIM_GetRoboRioComments(buffer, size); |
| } |
| |
| uint64_t HAL_GetFPGATime(int32_t* status) { |
| return hal::GetFPGATime(); |
| } |
| |
| uint64_t HAL_ExpandFPGATime(uint32_t unexpandedLower, int32_t* status) { |
| // Capture the current FPGA time. This will give us the upper half of the |
| // clock. |
| uint64_t fpgaTime = HAL_GetFPGATime(status); |
| if (*status != 0) { |
| return 0; |
| } |
| |
| // Now, we need to detect the case where the lower bits rolled over after we |
| // sampled. In that case, the upper bits will be 1 bigger than they should |
| // be. |
| |
| // Break it into lower and upper portions. |
| uint32_t lower = fpgaTime & 0xffffffffull; |
| uint64_t upper = (fpgaTime >> 32) & 0xffffffff; |
| |
| // The time was sampled *before* the current time, so roll it back. |
| if (lower < unexpandedLower) { |
| --upper; |
| } |
| |
| return (upper << 32) + static_cast<uint64_t>(unexpandedLower); |
| } |
| |
| HAL_Bool HAL_GetFPGAButton(int32_t* status) { |
| return SimRoboRioData[0].fpgaButton; |
| } |
| |
| HAL_Bool HAL_GetSystemActive(int32_t* status) { |
| return HALSIM_GetDriverStationEnabled(); |
| } |
| |
| HAL_Bool HAL_GetBrownedOut(int32_t* status) { |
| return false; // Figure out if we need to detect a brownout condition |
| } |
| |
| HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { |
| static std::atomic_bool initialized{false}; |
| static wpi::mutex initializeMutex; |
| // Initial check, as if it's true initialization has finished |
| if (initialized) { |
| return true; |
| } |
| |
| std::scoped_lock lock(initializeMutex); |
| // Second check in case another thread was waiting |
| if (initialized) { |
| return true; |
| } |
| |
| hal::init::InitializeHAL(); |
| |
| hal::init::HAL_IsInitialized.store(true); |
| |
| hal::RestartTiming(); |
| hal::InitializeDriverStation(); |
| |
| initialized = true; |
| |
| // Set Timer Precision to 1ms on Windows |
| #ifdef _WIN32 |
| TIMECAPS tc; |
| if (timeGetDevCaps(&tc, sizeof(tc)) == TIMERR_NOERROR) { |
| UINT target = min(1, tc.wPeriodMin); |
| timeBeginPeriod(target); |
| std::atexit([]() { |
| TIMECAPS tc; |
| if (timeGetDevCaps(&tc, sizeof(tc)) == TIMERR_NOERROR) { |
| UINT target = min(1, tc.wPeriodMin); |
| timeEndPeriod(target); |
| } |
| }); |
| } |
| #endif // _WIN32 |
| |
| #ifndef _WIN32 |
| setlinebuf(stdin); |
| setlinebuf(stdout); |
| #endif |
| |
| if (HAL_LoadExtensions() < 0) { |
| return false; |
| } |
| |
| return true; // Add initialization if we need to at a later point |
| } |
| |
| void HAL_Shutdown(void) { |
| std::vector<std::pair<void*, void (*)(void*)>> funcs; |
| { |
| std::scoped_lock lock(gOnShutdownMutex); |
| funcs.swap(gOnShutdown); |
| } |
| for (auto&& func : funcs) { |
| func.second(func.first); |
| } |
| } |
| |
| void HAL_OnShutdown(void* param, void (*func)(void*)) { |
| std::scoped_lock lock(gOnShutdownMutex); |
| gOnShutdown.emplace_back(param, func); |
| } |
| |
| void HAL_SimPeriodicBefore(void) { |
| gSimPeriodicBefore(); |
| } |
| |
| void HAL_SimPeriodicAfter(void) { |
| gSimPeriodicAfter(); |
| } |
| |
| int32_t HALSIM_RegisterSimPeriodicBeforeCallback( |
| HALSIM_SimPeriodicCallback callback, void* param) { |
| return gSimPeriodicBefore.Register(callback, param); |
| } |
| |
| void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) { |
| gSimPeriodicBefore.Cancel(uid); |
| } |
| |
| int32_t HALSIM_RegisterSimPeriodicAfterCallback( |
| HALSIM_SimPeriodicCallback callback, void* param) { |
| return gSimPeriodicAfter.Register(callback, param); |
| } |
| |
| void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) { |
| gSimPeriodicAfter.Cancel(uid); |
| } |
| |
| void HALSIM_CancelAllSimPeriodicCallbacks(void) { |
| gSimPeriodicBefore.Reset(); |
| gSimPeriodicAfter.Reset(); |
| } |
| |
| int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context, |
| const char* feature) { |
| return 0; // Do nothing for now |
| } |
| |
| } // extern "C" |