diff --git a/hal/include/HAL/DriverStation.h b/hal/include/HAL/DriverStation.h new file mode 100644 index 0000000000..7574fba0b5 --- /dev/null +++ b/hal/include/HAL/DriverStation.h @@ -0,0 +1,116 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2013-2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "HAL/Types.h" + +#define HAL_IO_CONFIG_DATA_SIZE 32 +#define HAL_SYS_STATUS_DATA_SIZE 44 +#define HAL_USER_STATUS_DATA_SIZE \ + (984 - HAL_IO_CONFIG_DATA_SIZE - HAL_SYS_STATUS_DATA_SIZE) + +#define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Input 17 +#define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Output 18 +#define HALFRC_NetworkCommunication_DynamicType_Kinect_Header 19 +#define HALFRC_NetworkCommunication_DynamicType_Kinect_Extra1 20 +#define HALFRC_NetworkCommunication_DynamicType_Kinect_Vertices1 21 +#define HALFRC_NetworkCommunication_DynamicType_Kinect_Extra2 22 +#define HALFRC_NetworkCommunication_DynamicType_Kinect_Vertices2 23 +#define HALFRC_NetworkCommunication_DynamicType_Kinect_Joystick 24 +#define HALFRC_NetworkCommunication_DynamicType_Kinect_Custom 25 + +struct HAL_ControlWord { + uint32_t enabled : 1; + uint32_t autonomous : 1; + uint32_t test : 1; + uint32_t eStop : 1; + uint32_t fmsAttached : 1; + uint32_t dsAttached : 1; + uint32_t control_reserved : 26; +}; + +enum HAL_AllianceStationID { + HAL_AllianceStationID_kRed1, + HAL_AllianceStationID_kRed2, + HAL_AllianceStationID_kRed3, + HAL_AllianceStationID_kBlue1, + HAL_AllianceStationID_kBlue2, + HAL_AllianceStationID_kBlue3, +}; + +/* The maximum number of axes that will be stored in a single HALJoystickAxes + * struct. This is used for allocating buffers, not bounds checking, since + * there are usually less axes in practice. + */ +#define HAL_kMaxJoystickAxes 12 +#define HAL_kMaxJoystickPOVs 12 + +struct HAL_JoystickAxes { + uint16_t count; + float axes[HAL_kMaxJoystickAxes]; +}; + +struct HAL_JoystickPOVs { + uint16_t count; + int16_t povs[HAL_kMaxJoystickPOVs]; +}; + +struct HAL_JoystickButtons { + uint32_t buttons; + uint8_t count; +}; + +struct HAL_JoystickDescriptor { + uint8_t isXbox; + uint8_t type; + char name[256]; + uint8_t axisCount; + uint8_t axisTypes[HAL_kMaxJoystickAxes]; + uint8_t buttonCount; + uint8_t povCount; +}; + +#ifdef __cplusplus +extern "C" { +#endif +int32_t HAL_SetErrorData(const char* errors, int32_t errorsLength, + int32_t wait_ms); +int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, + const char* details, const char* location, + const char* callStack, HAL_Bool printMsg); + +int32_t HAL_GetControlWord(HAL_ControlWord* controlWord); +HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status); +int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes); +int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs); +int32_t HAL_GetJoystickButtons(int32_t joystickNum, + HAL_JoystickButtons* buttons); +int32_t HAL_GetJoystickDescriptor(int32_t joystickNum, + HAL_JoystickDescriptor* desc); +HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum); +int32_t HAL_GetJoystickType(int32_t joystickNum); +char* HAL_GetJoystickName(int32_t joystickNum); +int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis); +int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs, + int32_t leftRumble, int32_t rightRumble); +double HAL_GetMatchTime(int32_t* status); + +void HAL_WaitForDSData(void); +void HAL_InitializeDriverStation(void); + +void HAL_ObserveUserProgramStarting(void); +void HAL_ObserveUserProgramDisabled(void); +void HAL_ObserveUserProgramAutonomous(void); +void HAL_ObserveUserProgramTeleop(void); +void HAL_ObserveUserProgramTest(void); +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/hal/include/HAL/HAL.h b/hal/include/HAL/HAL.h index 49511b147f..92d59f04ed 100644 --- a/hal/include/HAL/HAL.h +++ b/hal/include/HAL/HAL.h @@ -20,6 +20,7 @@ #include "HAL/Constants.h" #include "HAL/Counter.h" #include "HAL/DIO.h" +#include "HAL/DriverStation.h" #include "HAL/Errors.h" #include "HAL/I2C.h" #include "HAL/Interrupts.h" @@ -37,72 +38,6 @@ namespace HALUsageReporting = nUsageReporting; -#define HAL_IO_CONFIG_DATA_SIZE 32 -#define HAL_SYS_STATUS_DATA_SIZE 44 -#define HAL_USER_STATUS_DATA_SIZE \ - (984 - HAL_IO_CONFIG_DATA_SIZE - HAL_SYS_STATUS_DATA_SIZE) - -#define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Input 17 -#define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Output 18 -#define HALFRC_NetworkCommunication_DynamicType_Kinect_Header 19 -#define HALFRC_NetworkCommunication_DynamicType_Kinect_Extra1 20 -#define HALFRC_NetworkCommunication_DynamicType_Kinect_Vertices1 21 -#define HALFRC_NetworkCommunication_DynamicType_Kinect_Extra2 22 -#define HALFRC_NetworkCommunication_DynamicType_Kinect_Vertices2 23 -#define HALFRC_NetworkCommunication_DynamicType_Kinect_Joystick 24 -#define HALFRC_NetworkCommunication_DynamicType_Kinect_Custom 25 - -struct HAL_ControlWord { - uint32_t enabled : 1; - uint32_t autonomous : 1; - uint32_t test : 1; - uint32_t eStop : 1; - uint32_t fmsAttached : 1; - uint32_t dsAttached : 1; - uint32_t control_reserved : 26; -}; - -enum HAL_AllianceStationID { - HAL_AllianceStationID_kRed1, - HAL_AllianceStationID_kRed2, - HAL_AllianceStationID_kRed3, - HAL_AllianceStationID_kBlue1, - HAL_AllianceStationID_kBlue2, - HAL_AllianceStationID_kBlue3, -}; - -/* The maximum number of axes that will be stored in a single HALJoystickAxes - * struct. This is used for allocating buffers, not bounds checking, since - * there are usually less axes in practice. - */ -static constexpr size_t HAL_kMaxJoystickAxes = 12; -static constexpr size_t HAL_kMaxJoystickPOVs = 12; - -struct HAL_JoystickAxes { - uint16_t count; - float axes[HAL_kMaxJoystickAxes]; -}; - -struct HAL_JoystickPOVs { - uint16_t count; - int16_t povs[HAL_kMaxJoystickPOVs]; -}; - -struct HAL_JoystickButtons { - uint32_t buttons; - uint8_t count; -}; - -struct HAL_JoystickDescriptor { - uint8_t isXbox; - uint8_t type; - char name[256]; - uint8_t axisCount; - uint8_t axisTypes[HAL_kMaxJoystickAxes]; - uint8_t buttonCount; - uint8_t povCount; -}; - #ifdef __cplusplus extern "C" { #endif @@ -116,39 +51,10 @@ uint64_t HAL_GetFPGATime(int32_t* status); HAL_Bool HAL_GetFPGAButton(int32_t* status); -int32_t HAL_SetErrorData(const char* errors, int32_t errorsLength, - int32_t wait_ms); -int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, - const char* details, const char* location, - const char* callStack, HAL_Bool printMsg); - -int32_t HAL_GetControlWord(HAL_ControlWord* controlWord); -HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status); -int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes); -int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs); -int32_t HAL_GetJoystickButtons(int32_t joystickNum, - HAL_JoystickButtons* buttons); -int32_t HAL_GetJoystickDescriptor(int32_t joystickNum, - HAL_JoystickDescriptor* desc); -HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum); -int32_t HAL_GetJoystickType(int32_t joystickNum); -char* HAL_GetJoystickName(int32_t joystickNum); -int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis); -int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs, - int32_t leftRumble, int32_t rightRumble); -double HAL_GetMatchTime(int32_t* status); - -void HAL_WaitForDSData(void); - HAL_Bool HAL_GetSystemActive(int32_t* status); HAL_Bool HAL_GetBrownedOut(int32_t* status); int32_t HAL_Initialize(int32_t mode); -void HAL_ObserveUserProgramStarting(void); -void HAL_ObserveUserProgramDisabled(void); -void HAL_ObserveUserProgramAutonomous(void); -void HAL_ObserveUserProgramTeleop(void); -void HAL_ObserveUserProgramTest(void); // ifdef's definition is to allow for default parameters in C++. #ifdef __cplusplus diff --git a/hal/lib/athena/HALAthena.cpp b/hal/lib/athena/HALAthena.cpp index 961b274dad..7b08fda605 100644 --- a/hal/lib/athena/HALAthena.cpp +++ b/hal/lib/athena/HALAthena.cpp @@ -6,7 +6,6 @@ /*----------------------------------------------------------------------------*/ #include "HAL/HAL.h" -#include "HAL/cpp/priority_mutex.h" #include // linux for kill #include @@ -23,7 +22,6 @@ #include "FRC_NetworkCommunication/FRCComm.h" #include "FRC_NetworkCommunication/LoadOut.h" #include "HAL/Errors.h" -#include "HAL/cpp/priority_condition_variable.h" #include "HAL/cpp/priority_mutex.h" #include "HAL/handles/HandlesInternal.h" #include "ctre/ctre.h" @@ -33,14 +31,10 @@ static tGlobal* global = nullptr; static tSysWatchdog* watchdog = nullptr; static priority_mutex timeMutex; -static priority_mutex msgMutex; static uint32_t timeEpoch = 0; static uint32_t prevFPGATime = 0; static HAL_NotifierHandle rolloverNotifier = 0; -static priority_condition_variable newDSDataAvailableCond; -static priority_mutex newDSDataAvailableMutex; - using namespace hal; extern "C" { @@ -231,57 +225,6 @@ HAL_Bool HAL_GetFPGAButton(int32_t* status) { return global->readUserButton(status); } -int32_t HAL_SetErrorData(const char* errors, int32_t errorsLength, - int32_t wait_ms) { - return setErrorData(errors, errorsLength, wait_ms); -} - -int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, - const char* details, const char* location, - const char* callStack, HAL_Bool printMsg) { - // Avoid flooding console by keeping track of previous 5 error - // messages and only printing again if they're longer than 1 second old. - static constexpr int KEEP_MSGS = 5; - std::lock_guard lock(msgMutex); - static std::string prev_msg[KEEP_MSGS]; - static uint64_t prev_msg_time[KEEP_MSGS] = {0, 0, 0}; - - int32_t status = 0; - uint64_t curTime = HAL_GetFPGATime(&status); - int i; - for (i = 0; i < KEEP_MSGS; ++i) { - if (prev_msg[i] == details) break; - } - int retval = 0; - if (i == KEEP_MSGS || (curTime - prev_msg_time[i]) >= 1000000) { - retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode, - details, location, callStack); - if (printMsg) { - if (location && location[0] != '\0') { - fprintf(stderr, "%s at %s: ", isError ? "Error" : "Warning", location); - } - fprintf(stderr, "%s\n", details); - if (callStack && callStack[0] != '\0') { - fprintf(stderr, "%s\n", callStack); - } - } - if (i == KEEP_MSGS) { - // replace the oldest one - i = 0; - uint64_t first = prev_msg_time[0]; - for (int j = 1; j < KEEP_MSGS; ++j) { - if (prev_msg_time[j] < first) { - first = prev_msg_time[j]; - i = j; - } - } - prev_msg[i] = details; - } - prev_msg_time[i] = curTime; - } - return retval; -} - HAL_Bool HAL_GetSystemActive(int32_t* status) { if (!watchdog) { *status = NiFpga_Status_ResourceNotInitialized; @@ -313,14 +256,6 @@ static void timerRollover(uint64_t currentTime, void*) { &status); } -/** - * Waits for the newest DS packet to arrive. Note that this is a blocking call. - */ -void HAL_WaitForDSData() { - std::unique_lock lock(newDSDataAvailableMutex); - newDSDataAvailableCond.wait(lock); -} - /** * Call this to start up HAL. This is required for robot programs. */ @@ -388,8 +323,7 @@ int32_t HAL_Initialize(int32_t mode) { fs << pid << std::endl; fs.close(); - // Set our DS new data condition variable. - setNewDataSem(newDSDataAvailableCond.native_handle()); + HAL_InitializeDriverStation(); return 1; } diff --git a/hal/lib/shared/HAL.cpp b/hal/lib/shared/FRCDriverStation.cpp similarity index 71% rename from hal/lib/shared/HAL.cpp rename to hal/lib/shared/FRCDriverStation.cpp index 488c5b29fe..89d3458686 100644 --- a/hal/lib/shared/HAL.cpp +++ b/hal/lib/shared/FRCDriverStation.cpp @@ -12,13 +12,69 @@ #include #include "FRC_NetworkCommunication/FRCComm.h" +#include "HAL/cpp/priority_condition_variable.h" +#include "HAL/cpp/priority_mutex.h" struct HAL_JoystickAxesInt { uint16_t count; int16_t axes[HAL_kMaxJoystickAxes]; }; +static priority_mutex msgMutex; +static priority_condition_variable newDSDataAvailableCond; +static priority_mutex newDSDataAvailableMutex; + extern "C" { +int32_t HAL_SetErrorData(const char* errors, int32_t errorsLength, + int32_t wait_ms) { + return setErrorData(errors, errorsLength, wait_ms); +} + +int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, + const char* details, const char* location, + const char* callStack, HAL_Bool printMsg) { + // Avoid flooding console by keeping track of previous 5 error + // messages and only printing again if they're longer than 1 second old. + static constexpr int KEEP_MSGS = 5; + std::lock_guard lock(msgMutex); + static std::string prev_msg[KEEP_MSGS]; + static uint64_t prev_msg_time[KEEP_MSGS] = {0, 0, 0}; + + int32_t status = 0; + uint64_t curTime = HAL_GetFPGATime(&status); + int i; + for (i = 0; i < KEEP_MSGS; ++i) { + if (prev_msg[i] == details) break; + } + int retval = 0; + if (i == KEEP_MSGS || (curTime - prev_msg_time[i]) >= 1000000) { + retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode, + details, location, callStack); + if (printMsg) { + if (location && location[0] != '\0') { + fprintf(stderr, "%s at %s: ", isError ? "Error" : "Warning", location); + } + fprintf(stderr, "%s\n", details); + if (callStack && callStack[0] != '\0') { + fprintf(stderr, "%s\n", callStack); + } + } + if (i == KEEP_MSGS) { + // replace the oldest one + i = 0; + uint64_t first = prev_msg_time[0]; + for (int j = 1; j < KEEP_MSGS; ++j) { + if (prev_msg_time[j] < first) { + first = prev_msg_time[j]; + i = j; + } + } + prev_msg[i] = details; + } + prev_msg_time[i] = curTime; + } + return retval; +} int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) { std::memset(controlWord, 0, sizeof(HAL_ControlWord)); @@ -175,4 +231,17 @@ void HAL_ObserveUserProgramTest(void) { FRC_NetworkCommunication_observeUserProgramTest(); } +/** + * Waits for the newest DS packet to arrive. Note that this is a blocking call. + */ +void HAL_WaitForDSData(void) { + std::unique_lock lock(newDSDataAvailableMutex); + newDSDataAvailableCond.wait(lock); +} + +void HAL_InitializeDriverStation(void) { + // Set our DS new data condition variable. + setNewDataSem(newDSDataAvailableCond.native_handle()); +} + } // extern "C"