diff --git a/hal/src/main/native/include/MockData/DriverStationData.h b/hal/src/main/native/include/MockData/DriverStationData.h index 95b9b0d7e3..9db11b3add 100644 --- a/hal/src/main/native/include/MockData/DriverStationData.h +++ b/hal/src/main/native/include/MockData/DriverStationData.h @@ -7,6 +7,7 @@ #pragma once +#include "HAL/DriverStation.h" #include "HAL/HAL.h" #include "NotifyListener.h" @@ -67,12 +68,24 @@ void HALSIM_CancelDriverStationMatchTimeCallback(int32_t uid); double HALSIM_GetDriverStationMatchTime(); void HALSIM_SetDriverStationMatchTime(double matchTime); -void HALSIM_NotifyDriverStationNewData(void); +void HALSIM_SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes); +void HALSIM_SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs); +void HALSIM_SetJoystickButtons(int32_t joystickNum, + const HAL_JoystickButtons* buttons); +void HALSIM_SetJoystickDescriptor(int32_t joystickNum, + const HAL_JoystickDescriptor* descriptor); + +void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs, + int32_t* leftRumble, int32_t* rightRumble); + +void HALSIM_SetMatchInfo(const HAL_MatchInfo* info); void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify); +void HALSIM_NotifyDriverStationNewData(void); + #ifdef __cplusplus } // extern "C" #endif diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index 7ee4c82877..23abbae9c9 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -99,15 +99,18 @@ HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) { } int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) { + SimDriverStationData.GetJoystickAxes(joystickNum, axes); return 0; } int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) { + SimDriverStationData.GetJoystickPOVs(joystickNum, povs); return 0; } int32_t HAL_GetJoystickButtons(int32_t joystickNum, HAL_JoystickButtons* buttons) { + SimDriverStationData.GetJoystickButtons(joystickNum, buttons); return 0; } /** @@ -123,16 +126,29 @@ int32_t HAL_GetJoystickButtons(int32_t joystickNum, */ int32_t HAL_GetJoystickDescriptor(int32_t joystickNum, HAL_JoystickDescriptor* desc) { + SimDriverStationData.GetJoystickDescriptor(joystickNum, desc); return 0; } -HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) { return false; } +HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) { + HAL_JoystickDescriptor desc; + SimDriverStationData.GetJoystickDescriptor(joystickNum, &desc); + return desc.isXbox; +} -int32_t HAL_GetJoystickType(int32_t joystickNum) { return 0; } +int32_t HAL_GetJoystickType(int32_t joystickNum) { + HAL_JoystickDescriptor desc; + SimDriverStationData.GetJoystickDescriptor(joystickNum, &desc); + return desc.type; +} char* HAL_GetJoystickName(int32_t joystickNum) { - char* name = static_cast(std::malloc(1)); - name[0] = '\0'; + HAL_JoystickDescriptor desc; + SimDriverStationData.GetJoystickDescriptor(joystickNum, &desc); + size_t len = std::strlen(desc.name); + char* name = static_cast(std::malloc(len + 1)); + std::strncpy(name, desc.name, len); + name[len] = '\0'; return name; } @@ -142,6 +158,8 @@ int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) { return 0; } int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs, int32_t leftRumble, int32_t rightRumble) { + SimDriverStationData.SetJoystickOutputs(joystickNum, outputs, leftRumble, + rightRumble); return 0; } @@ -150,20 +168,12 @@ double HAL_GetMatchTime(int32_t* status) { } int HAL_GetMatchInfo(HAL_MatchInfo* info) { - info->eventName = static_cast(std::malloc(1)); - info->eventName[0] = '\0'; - info->matchNumber = 0; - info->replayNumber = 0; - info->gameSpecificMessage = static_cast(std::malloc(1)); - info->gameSpecificMessage[0] = '\0'; + SimDriverStationData.GetMatchInfo(info); return 0; } void HAL_FreeMatchInfo(HAL_MatchInfo* info) { - std::free(info->eventName); - std::free(info->gameSpecificMessage); - info->eventName = nullptr; - info->gameSpecificMessage = nullptr; + SimDriverStationData.FreeMatchInfo(info); } void HAL_ObserveUserProgramStarting(void) { HALSIM_SetProgramStarted(); } diff --git a/hal/src/main/native/sim/MockData/DriverStationData.cpp b/hal/src/main/native/sim/MockData/DriverStationData.cpp index 0b7b497f13..43c22b1caa 100644 --- a/hal/src/main/native/sim/MockData/DriverStationData.cpp +++ b/hal/src/main/native/sim/MockData/DriverStationData.cpp @@ -5,13 +5,35 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +#include +#include +#include + #include "DriverStationDataInternal.h" +#include "HAL/cpp/make_unique.h" #include "NotifyCallbackHelpers.h" +namespace hal { +struct JoystickOutputStore { + int64_t outputs = 0; + int32_t leftRumble = 0; + int32_t rightRumble = 0; +}; +struct MatchInfoDataStore { + std::string eventName; + std::string gameSpecificMessage; + int32_t replayNumber = 0; + int32_t matchNumber = 0; + HAL_MatchType matchType = HAL_MatchType::HAL_kMatchType_none; +}; +} // namespace hal + using namespace hal; DriverStationData hal::SimDriverStationData; +DriverStationData::DriverStationData() { ResetData(); } + void DriverStationData::ResetData() { m_enabled = false; m_enabledCallbacks = nullptr; @@ -25,6 +47,29 @@ void DriverStationData::ResetData() { m_fmsAttachedCallbacks = nullptr; m_dsAttached = false; m_dsAttachedCallbacks = nullptr; + + { + std::lock_guard lock(m_joystickDataMutex); + m_joystickAxes = std::make_unique(6); + m_joystickPOVs = std::make_unique(6); + m_joystickButtons = std::make_unique(6); + m_joystickOutputs = std::make_unique(6); + m_joystickDescriptor = std::make_unique(6); + + for (int i = 0; i < 6; i++) { + m_joystickAxes[i].count = 0; + m_joystickPOVs[i].count = 0; + m_joystickButtons[i].count = 0; + m_joystickDescriptor[i].isXbox = 0; + m_joystickDescriptor[i].type = -1; + m_joystickDescriptor[i].name[0] = '\0'; + } + } + { + std::lock_guard lock(m_matchInfoMutex); + + m_matchInfo = std::make_unique(); + } } int32_t DriverStationData::RegisterEnabledCallback(HAL_NotifyCallback callback, @@ -316,6 +361,97 @@ void DriverStationData::SetMatchTime(double matchTime) { } } +void DriverStationData::GetJoystickAxes(int32_t joystickNum, + HAL_JoystickAxes* axes) { + std::lock_guard lock(m_joystickDataMutex); + *axes = m_joystickAxes[joystickNum]; +} +void DriverStationData::GetJoystickPOVs(int32_t joystickNum, + HAL_JoystickPOVs* povs) { + std::lock_guard lock(m_joystickDataMutex); + *povs = m_joystickPOVs[joystickNum]; +} +void DriverStationData::GetJoystickButtons(int32_t joystickNum, + HAL_JoystickButtons* buttons) { + std::lock_guard lock(m_joystickDataMutex); + *buttons = m_joystickButtons[joystickNum]; +} +void DriverStationData::GetJoystickDescriptor( + int32_t joystickNum, HAL_JoystickDescriptor* descriptor) { + std::lock_guard lock(m_joystickDataMutex); + *descriptor = m_joystickDescriptor[joystickNum]; + // Always ensure name is null terminated + descriptor->name[255] = '\0'; +} +void DriverStationData::GetJoystickOutputs(int32_t joystickNum, + int64_t* outputs, + int32_t* leftRumble, + int32_t* rightRumble) { + std::lock_guard lock(m_joystickDataMutex); + *leftRumble = m_joystickOutputs[joystickNum].leftRumble; + *outputs = m_joystickOutputs[joystickNum].outputs; + *rightRumble = m_joystickOutputs[joystickNum].rightRumble; +} +void DriverStationData::GetMatchInfo(HAL_MatchInfo* info) { + std::lock_guard lock(m_matchInfoMutex); + auto eventLen = m_matchInfo->eventName.size(); + info->eventName = static_cast(std::malloc(eventLen + 1)); + std::memcpy(info->eventName, m_matchInfo->eventName.c_str(), eventLen); + auto gameLen = m_matchInfo->gameSpecificMessage.size(); + info->gameSpecificMessage = static_cast(std::malloc(gameLen + 1)); + std::memcpy(info->gameSpecificMessage, + m_matchInfo->gameSpecificMessage.c_str(), gameLen); + info->gameSpecificMessage[gameLen] = '\0'; + info->eventName[eventLen] = '\0'; + info->matchNumber = m_matchInfo->matchNumber; + info->replayNumber = m_matchInfo->replayNumber; + info->matchType = m_matchInfo->matchType; +} +void DriverStationData::FreeMatchInfo(const HAL_MatchInfo* info) { + std::free(info->eventName); + std::free(info->gameSpecificMessage); +} + +void DriverStationData::SetJoystickAxes(int32_t joystickNum, + const HAL_JoystickAxes* axes) { + std::lock_guard lock(m_joystickDataMutex); + m_joystickAxes[joystickNum] = *axes; +} +void DriverStationData::SetJoystickPOVs(int32_t joystickNum, + const HAL_JoystickPOVs* povs) { + std::lock_guard lock(m_joystickDataMutex); + m_joystickPOVs[joystickNum] = *povs; +} +void DriverStationData::SetJoystickButtons(int32_t joystickNum, + const HAL_JoystickButtons* buttons) { + std::lock_guard lock(m_joystickDataMutex); + m_joystickButtons[joystickNum] = *buttons; +} + +void DriverStationData::SetJoystickDescriptor( + int32_t joystickNum, const HAL_JoystickDescriptor* descriptor) { + std::lock_guard lock(m_joystickDataMutex); + m_joystickDescriptor[joystickNum] = *descriptor; +} + +void DriverStationData::SetJoystickOutputs(int32_t joystickNum, int64_t outputs, + int32_t leftRumble, + int32_t rightRumble) { + std::lock_guard lock(m_joystickDataMutex); + m_joystickOutputs[joystickNum].leftRumble = leftRumble; + m_joystickOutputs[joystickNum].outputs = outputs; + m_joystickOutputs[joystickNum].rightRumble = rightRumble; +} + +void DriverStationData::SetMatchInfo(const HAL_MatchInfo* info) { + std::lock_guard lock(m_matchInfoMutex); + m_matchInfo->eventName = info->eventName; + m_matchInfo->gameSpecificMessage = info->gameSpecificMessage; + m_matchInfo->matchNumber = info->matchNumber; + m_matchInfo->matchType = info->matchType; + m_matchInfo->replayNumber = info->replayNumber; +} + void DriverStationData::NotifyNewData() { HAL_ReleaseDSMutex(); } extern "C" { @@ -445,6 +581,33 @@ void HALSIM_SetDriverStationMatchTime(double matchTime) { SimDriverStationData.SetMatchTime(matchTime); } +void HALSIM_SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes) { + SimDriverStationData.SetJoystickAxes(joystickNum, axes); +} + +void HALSIM_SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs) { + SimDriverStationData.SetJoystickPOVs(joystickNum, povs); +} + +void HALSIM_SetJoystickButtons(int32_t joystickNum, + const HAL_JoystickButtons* buttons) { + SimDriverStationData.SetJoystickButtons(joystickNum, buttons); +} +void HALSIM_SetJoystickDescriptor(int32_t joystickNum, + const HAL_JoystickDescriptor* descriptor) { + SimDriverStationData.SetJoystickDescriptor(joystickNum, descriptor); +} + +void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs, + int32_t* leftRumble, int32_t* rightRumble) { + SimDriverStationData.GetJoystickOutputs(joystickNum, outputs, leftRumble, + rightRumble); +} + +void HALSIM_SetMatchInfo(const HAL_MatchInfo* info) { + SimDriverStationData.SetMatchInfo(info); +} + void HALSIM_NotifyDriverStationNewData(void) { SimDriverStationData.NotifyNewData(); } diff --git a/hal/src/main/native/sim/MockData/DriverStationDataInternal.h b/hal/src/main/native/sim/MockData/DriverStationDataInternal.h index 3044ba77ac..b79ebc96f9 100644 --- a/hal/src/main/native/sim/MockData/DriverStationDataInternal.h +++ b/hal/src/main/native/sim/MockData/DriverStationDataInternal.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include @@ -14,8 +15,12 @@ #include "MockData/NotifyListenerVector.h" namespace hal { +struct JoystickOutputStore; +struct MatchInfoDataStore; + class DriverStationData { public: + DriverStationData(); void ResetData(); int32_t RegisterEnabledCallback(HAL_NotifyCallback callback, void* param, @@ -75,6 +80,26 @@ class DriverStationData { double GetMatchTime(); void SetMatchTime(double matchTime); + void GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes); + void GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs); + void GetJoystickButtons(int32_t joystickNum, HAL_JoystickButtons* buttons); + void GetJoystickDescriptor(int32_t joystickNum, + HAL_JoystickDescriptor* descriptor); + void GetJoystickOutputs(int32_t joystickNum, int64_t* outputs, + int32_t* leftRumble, int32_t* rightRumble); + void GetMatchInfo(HAL_MatchInfo* info); + void FreeMatchInfo(const HAL_MatchInfo* info); + + void SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes); + void SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs); + void SetJoystickButtons(int32_t joystickNum, + const HAL_JoystickButtons* buttons); + void SetJoystickDescriptor(int32_t joystickNum, + const HAL_JoystickDescriptor* descriptor); + void SetJoystickOutputs(int32_t joystickNum, int64_t outputs, + int32_t leftRumble, int32_t rightRumble); + void SetMatchInfo(const HAL_MatchInfo* info); + void NotifyNewData(); private: @@ -96,6 +121,17 @@ class DriverStationData { std::shared_ptr m_allianceStationIdCallbacks = nullptr; std::atomic m_matchTime{0.0}; std::shared_ptr m_matchTimeCallbacks = nullptr; + + std::mutex m_joystickDataMutex; + std::mutex m_matchInfoMutex; + + std::unique_ptr m_joystickAxes; + std::unique_ptr m_joystickPOVs; + std::unique_ptr m_joystickButtons; + + std::unique_ptr m_joystickOutputs; + std::unique_ptr m_joystickDescriptor; + std::unique_ptr m_matchInfo; }; extern DriverStationData SimDriverStationData; } // namespace hal diff --git a/hal/src/test/native/cpp/DriverStationTests.cpp b/hal/src/test/native/cpp/DriverStationTests.cpp new file mode 100644 index 0000000000..2ed3dc5607 --- /dev/null +++ b/hal/src/test/native/cpp/DriverStationTests.cpp @@ -0,0 +1,139 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2015-2017 FIRST. 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. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "HAL/HAL.h" +#include "HAL/Solenoid.h" +#include "MockData/DriverStationData.h" +#include "gtest/gtest.h" + +namespace hal { + +TEST(DriverStationTests, JoystickTests) { + HAL_JoystickAxes axes; + HAL_JoystickPOVs povs; + HAL_JoystickButtons buttons; + + // Check default values before anything has been set + for (int joystickNum = 0; joystickNum < 6; ++joystickNum) { + HAL_GetJoystickAxes(joystickNum, &axes); + HAL_GetJoystickPOVs(joystickNum, &povs); + HAL_GetJoystickButtons(joystickNum, &buttons); + + EXPECT_EQ(0, axes.count); + for (int i = 0; i < HAL_kMaxJoystickAxes; ++i) { + EXPECT_EQ(0, axes.axes[i]); + } + + EXPECT_EQ(0, povs.count); + for (int i = 0; i < HAL_kMaxJoystickPOVs; ++i) { + EXPECT_EQ(0, povs.povs[i]); + } + + EXPECT_EQ(0, buttons.count); + EXPECT_EQ(0u, buttons.buttons); + } + + HAL_JoystickAxes set_axes; + std::memset(&set_axes, 0, sizeof(HAL_JoystickAxes)); + HAL_JoystickPOVs set_povs; + std::memset(&set_povs, 0, sizeof(HAL_JoystickPOVs)); + HAL_JoystickButtons set_buttons; + std::memset(&set_buttons, 0, sizeof(HAL_JoystickButtons)); + + // Set values + int joystickUnderTest = 4; + set_axes.count = 5; + for (int i = 0; i < set_axes.count; ++i) { + set_axes.axes[i] = i * .125; + } + + set_povs.count = 3; + for (int i = 0; i < set_povs.count; ++i) { + set_povs.povs[i] = i * 15 + 12; + } + + set_buttons.count = 8; + set_buttons.buttons = 0xDEADBEEF; + + HALSIM_SetJoystickAxes(joystickUnderTest, &set_axes); + HALSIM_SetJoystickPOVs(joystickUnderTest, &set_povs); + HALSIM_SetJoystickButtons(joystickUnderTest, &set_buttons); + + // Check the set values + HAL_GetJoystickAxes(joystickUnderTest, &axes); + HAL_GetJoystickPOVs(joystickUnderTest, &povs); + HAL_GetJoystickButtons(joystickUnderTest, &buttons); + + EXPECT_EQ(5, axes.count); + EXPECT_NEAR(0.000, axes.axes[0], 0.000001); + EXPECT_NEAR(0.125, axes.axes[1], 0.000001); + EXPECT_NEAR(0.250, axes.axes[2], 0.000001); + EXPECT_NEAR(0.375, axes.axes[3], 0.000001); + EXPECT_NEAR(0.500, axes.axes[4], 0.000001); + EXPECT_NEAR(0, axes.axes[5], 0.000001); // Should not have been set, still 0 + EXPECT_NEAR(0, axes.axes[6], 0.000001); // Should not have been set, still 0 + + EXPECT_EQ(3, povs.count); + EXPECT_EQ(12, povs.povs[0]); + EXPECT_EQ(27, povs.povs[1]); + EXPECT_EQ(42, povs.povs[2]); + EXPECT_EQ(0, povs.povs[3]); // Should not have been set, still 0 + EXPECT_EQ(0, povs.povs[4]); // Should not have been set, still 0 + EXPECT_EQ(0, povs.povs[5]); // Should not have been set, still 0 + EXPECT_EQ(0, povs.povs[6]); // Should not have been set, still 0 + + EXPECT_EQ(8, buttons.count); + EXPECT_EQ(0xDEADBEEFu, buttons.buttons); + + // Reset + HALSIM_ResetDriverStationData(); + for (int joystickNum = 0; joystickNum < 6; ++joystickNum) { + HAL_GetJoystickAxes(joystickNum, &axes); + HAL_GetJoystickPOVs(joystickNum, &povs); + HAL_GetJoystickButtons(joystickNum, &buttons); + + EXPECT_EQ(0, axes.count); + for (int i = 0; i < HAL_kMaxJoystickAxes; ++i) { + EXPECT_EQ(0, axes.axes[i]); + } + + EXPECT_EQ(0, povs.count); + for (int i = 0; i < HAL_kMaxJoystickPOVs; ++i) { + EXPECT_EQ(0, povs.povs[i]); + } + + EXPECT_EQ(0, buttons.count); + EXPECT_EQ(0u, buttons.buttons); + } +} + +TEST(DriverStationTests, EventInfoTest) { + std::string eventName = "UnitTest"; + std::string gameData = "Insert game specific info here :D"; + HAL_MatchInfo info; + info.eventName = const_cast(eventName.c_str()); + info.gameSpecificMessage = const_cast(gameData.c_str()); + info.matchNumber = 5; + info.matchType = HAL_MatchType::HAL_kMatchType_qualification; + info.replayNumber = 42; + HALSIM_SetMatchInfo(&info); + + HAL_MatchInfo dataBack; + HAL_GetMatchInfo(&dataBack); + + EXPECT_STREQ(eventName.c_str(), dataBack.eventName); + EXPECT_STREQ(gameData.c_str(), dataBack.gameSpecificMessage); + EXPECT_EQ(5, dataBack.matchNumber); + EXPECT_EQ(HAL_MatchType::HAL_kMatchType_qualification, dataBack.matchType); + EXPECT_EQ(42, dataBack.replayNumber); + + HAL_FreeMatchInfo(&dataBack); +} + +} // namespace hal