[sim] Add callbacks for joysticks and match info (#2628)

This commit is contained in:
Peter Johnson
2020-08-03 22:38:48 -07:00
committed by GitHub
parent ceea1f9d44
commit 1b8ceb36fc
3 changed files with 346 additions and 142 deletions

View File

@@ -12,16 +12,6 @@
#include "DriverStationDataInternal.h"
#include "hal/DriverStation.h"
static constexpr int kNumJoysticks = 6;
namespace hal {
struct JoystickOutputStore {
int64_t outputs = 0;
int32_t leftRumble = 0;
int32_t rightRumble = 0;
};
} // namespace hal
using namespace hal;
namespace hal {
@@ -49,94 +39,114 @@ void DriverStationData::ResetData() {
{
std::scoped_lock lock(m_joystickDataMutex);
m_joystickAxes = std::make_unique<HAL_JoystickAxes[]>(kNumJoysticks);
m_joystickPOVs = std::make_unique<HAL_JoystickPOVs[]>(kNumJoysticks);
m_joystickButtons = std::make_unique<HAL_JoystickButtons[]>(kNumJoysticks);
m_joystickOutputs = std::make_unique<JoystickOutputStore[]>(kNumJoysticks);
m_joystickDescriptor =
std::make_unique<HAL_JoystickDescriptor[]>(kNumJoysticks);
m_joystickAxesCallbacks.Reset();
m_joystickPOVsCallbacks.Reset();
m_joystickButtonsCallbacks.Reset();
m_joystickOutputsCallbacks.Reset();
m_joystickDescriptorCallbacks.Reset();
for (int i = 0; i < kNumJoysticks; 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';
m_joystickData[i].axes = HAL_JoystickAxes{};
m_joystickData[i].povs = HAL_JoystickPOVs{};
m_joystickData[i].buttons = HAL_JoystickButtons{};
m_joystickData[i].descriptor = HAL_JoystickDescriptor{};
m_joystickData[i].descriptor.type = -1;
m_joystickData[i].descriptor.name[0] = '\0';
}
}
{
std::scoped_lock lock(m_matchInfoMutex);
m_matchInfo = std::make_unique<HAL_MatchInfo>();
m_matchInfoCallbacks.Reset();
m_matchInfo = HAL_MatchInfo{};
}
}
void DriverStationData::GetJoystickAxes(int32_t joystickNum,
HAL_JoystickAxes* axes) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
*axes = m_joystickAxes[joystickNum];
}
void DriverStationData::GetJoystickPOVs(int32_t joystickNum,
HAL_JoystickPOVs* povs) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
*povs = m_joystickPOVs[joystickNum];
}
void DriverStationData::GetJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
*buttons = m_joystickButtons[joystickNum];
}
#define DEFINE_CPPAPI_CALLBACKS(name, data, data2) \
int32_t DriverStationData::RegisterJoystick##name##Callback( \
int32_t joystickNum, HAL_Joystick##name##Callback callback, void* param, \
HAL_Bool initialNotify) { \
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return 0; \
std::scoped_lock lock(m_joystickDataMutex); \
int32_t uid = m_joystick##name##Callbacks.Register(callback, param); \
if (initialNotify) { \
callback(GetJoystick##name##Name(), param, joystickNum, \
&m_joystickData[joystickNum].data##data2); \
} \
return uid; \
} \
\
void DriverStationData::CancelJoystick##name##Callback(int32_t uid) { \
m_joystick##name##Callbacks.Cancel(uid); \
}
#define DEFINE_CPPAPI(name, data, data2) \
DEFINE_CPPAPI_CALLBACKS(name, data, data2) \
\
void DriverStationData::GetJoystick##name(int32_t joystickNum, \
HAL_Joystick##name* d) { \
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return; \
std::scoped_lock lock(m_joystickDataMutex); \
*d = m_joystickData[joystickNum].data##data2; \
} \
\
void DriverStationData::SetJoystick##name(int32_t joystickNum, \
const HAL_Joystick##name* d) { \
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return; \
std::scoped_lock lock(m_joystickDataMutex); \
m_joystickData[joystickNum].data##data2 = *d; \
m_joystick##name##Callbacks(joystickNum, d); \
}
DEFINE_CPPAPI(Axes, axes, )
DEFINE_CPPAPI(POVs, povs, )
DEFINE_CPPAPI(Buttons, buttons, )
DEFINE_CPPAPI_CALLBACKS(Descriptor, descriptor, )
void DriverStationData::GetJoystickDescriptor(
int32_t joystickNum, HAL_JoystickDescriptor* descriptor) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock 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) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock 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::scoped_lock lock(m_matchInfoMutex);
*info = *m_matchInfo;
}
void DriverStationData::SetJoystickAxes(int32_t joystickNum,
const HAL_JoystickAxes* axes) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickAxes[joystickNum] = *axes;
}
void DriverStationData::SetJoystickPOVs(int32_t joystickNum,
const HAL_JoystickPOVs* povs) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickPOVs[joystickNum] = *povs;
}
void DriverStationData::SetJoystickButtons(int32_t joystickNum,
const HAL_JoystickButtons* buttons) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickButtons[joystickNum] = *buttons;
*descriptor = m_joystickData[joystickNum].descriptor;
}
void DriverStationData::SetJoystickDescriptor(
int32_t joystickNum, const HAL_JoystickDescriptor* descriptor) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickDescriptor[joystickNum] = *descriptor;
m_joystickData[joystickNum].descriptor = *descriptor;
// Always ensure name is null terminated
m_joystickData[joystickNum].descriptor.name[255] = '\0';
m_joystickDescriptorCallbacks(joystickNum, descriptor);
}
int32_t DriverStationData::RegisterJoystickOutputsCallback(
int32_t joystickNum, HAL_JoystickOutputsCallback callback, void* param,
HAL_Bool initialNotify) {
if (joystickNum < 0 || joystickNum >= DriverStationData::kNumJoysticks)
return 0;
std::scoped_lock lock(m_joystickDataMutex);
int32_t uid = m_joystickOutputsCallbacks.Register(callback, param);
if (initialNotify) {
const auto& outputs = m_joystickData[joystickNum].outputs;
callback(DriverStationData::GetJoystickOutputsName(), param, joystickNum,
outputs.outputs, outputs.leftRumble, outputs.rightRumble);
}
return uid;
}
void DriverStationData::CancelJoystickOutputsCallback(int32_t uid) {
m_joystickOutputsCallbacks.Cancel(uid);
}
void DriverStationData::GetJoystickOutputs(int32_t joystickNum,
int64_t* outputs,
int32_t* leftRumble,
int32_t* rightRumble) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
*leftRumble = m_joystickData[joystickNum].outputs.leftRumble;
*outputs = m_joystickData[joystickNum].outputs.outputs;
*rightRumble = m_joystickData[joystickNum].outputs.rightRumble;
}
void DriverStationData::SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
@@ -144,15 +154,36 @@ void DriverStationData::SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
int32_t rightRumble) {
if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickOutputs[joystickNum].leftRumble = leftRumble;
m_joystickOutputs[joystickNum].outputs = outputs;
m_joystickOutputs[joystickNum].rightRumble = rightRumble;
m_joystickData[joystickNum].outputs.leftRumble = leftRumble;
m_joystickData[joystickNum].outputs.outputs = outputs;
m_joystickData[joystickNum].outputs.rightRumble = rightRumble;
m_joystickOutputsCallbacks(joystickNum, outputs, leftRumble, rightRumble);
}
int32_t DriverStationData::RegisterMatchInfoCallback(
HAL_MatchInfoCallback callback, void* param, HAL_Bool initialNotify) {
std::scoped_lock lock(m_matchInfoMutex);
int32_t uid = m_matchInfoCallbacks.Register(callback, param);
if (initialNotify) {
callback(GetMatchInfoName(), param, &m_matchInfo);
}
return uid;
}
void DriverStationData::CancelMatchInfoCallback(int32_t uid) {
m_matchInfoCallbacks.Cancel(uid);
}
void DriverStationData::GetMatchInfo(HAL_MatchInfo* info) {
std::scoped_lock lock(m_matchInfoMutex);
*info = m_matchInfo;
}
void DriverStationData::SetMatchInfo(const HAL_MatchInfo* info) {
std::scoped_lock lock(m_matchInfoMutex);
*m_matchInfo = *info;
*(std::end(m_matchInfo->eventName) - 1) = '\0';
m_matchInfo = *info;
*(std::end(m_matchInfo.eventName) - 1) = '\0';
m_matchInfoCallbacks(info);
}
void DriverStationData::NotifyNewData() { HAL_ReleaseDSMutex(); }
@@ -162,9 +193,10 @@ void DriverStationData::SetJoystickButton(int32_t stick, int32_t button,
if (stick < 0 || stick >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
if (state)
m_joystickButtons[stick].buttons |= 1 << (button - 1);
m_joystickData[stick].buttons.buttons |= 1 << (button - 1);
else
m_joystickButtons[stick].buttons &= ~(1 << (button - 1));
m_joystickData[stick].buttons.buttons &= ~(1 << (button - 1));
m_joystickButtonsCallbacks(stick, &m_joystickData[stick].buttons);
}
void DriverStationData::SetJoystickAxis(int32_t stick, int32_t axis,
@@ -172,7 +204,8 @@ void DriverStationData::SetJoystickAxis(int32_t stick, int32_t axis,
if (stick < 0 || stick >= kNumJoysticks) return;
if (axis < 0 || axis >= HAL_kMaxJoystickAxes) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickAxes[stick].axes[axis] = value;
m_joystickData[stick].axes.axes[axis] = value;
m_joystickAxesCallbacks(stick, &m_joystickData[stick].axes);
}
void DriverStationData::SetJoystickPOV(int32_t stick, int32_t pov,
@@ -180,53 +213,65 @@ void DriverStationData::SetJoystickPOV(int32_t stick, int32_t pov,
if (stick < 0 || stick >= kNumJoysticks) return;
if (pov < 0 || pov >= HAL_kMaxJoystickPOVs) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickPOVs[stick].povs[pov] = value;
m_joystickData[stick].povs.povs[pov] = value;
m_joystickPOVsCallbacks(stick, &m_joystickData[stick].povs);
}
void DriverStationData::SetJoystickButtons(int32_t stick, uint32_t buttons) {
if (stick < 0 || stick >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickButtons[stick].buttons = buttons;
m_joystickData[stick].buttons.buttons = buttons;
m_joystickButtonsCallbacks(stick, &m_joystickData[stick].buttons);
}
void DriverStationData::SetJoystickAxisCount(int32_t stick, int32_t count) {
if (stick < 0 || stick >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickAxes[stick].count = count;
m_joystickDescriptor[stick].axisCount = count;
m_joystickData[stick].axes.count = count;
m_joystickData[stick].descriptor.axisCount = count;
m_joystickAxesCallbacks(stick, &m_joystickData[stick].axes);
m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
}
void DriverStationData::SetJoystickPOVCount(int32_t stick, int32_t count) {
if (stick < 0 || stick >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickPOVs[stick].count = count;
m_joystickDescriptor[stick].povCount = count;
m_joystickData[stick].povs.count = count;
m_joystickData[stick].descriptor.povCount = count;
m_joystickPOVsCallbacks(stick, &m_joystickData[stick].povs);
m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
}
void DriverStationData::SetJoystickButtonCount(int32_t stick, int32_t count) {
if (stick < 0 || stick >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickButtons[stick].count = count;
m_joystickDescriptor[stick].buttonCount = count;
m_joystickData[stick].buttons.count = count;
m_joystickData[stick].descriptor.buttonCount = count;
m_joystickButtonsCallbacks(stick, &m_joystickData[stick].buttons);
m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
}
void DriverStationData::SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox) {
if (stick < 0 || stick >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickDescriptor[stick].isXbox = isXbox;
m_joystickData[stick].descriptor.isXbox = isXbox;
m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
}
void DriverStationData::SetJoystickType(int32_t stick, int32_t type) {
if (stick < 0 || stick >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickDescriptor[stick].type = type;
m_joystickData[stick].descriptor.type = type;
m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
}
void DriverStationData::SetJoystickName(int32_t stick, const char* name) {
if (stick < 0 || stick >= kNumJoysticks) return;
std::scoped_lock lock(m_joystickDataMutex);
std::strncpy(m_joystickDescriptor[stick].name, name,
sizeof(m_joystickDescriptor[stick].name) - 1);
std::strncpy(m_joystickData[stick].descriptor.name, name,
sizeof(m_joystickData[stick].descriptor.name) - 1);
*(std::end(m_joystickData[stick].descriptor.name) - 1) = '\0';
m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
}
void DriverStationData::SetJoystickAxisType(int32_t stick, int32_t axis,
@@ -234,37 +279,42 @@ void DriverStationData::SetJoystickAxisType(int32_t stick, int32_t axis,
if (stick < 0 || stick >= kNumJoysticks) return;
if (axis < 0 || axis >= HAL_kMaxJoystickAxes) return;
std::scoped_lock lock(m_joystickDataMutex);
m_joystickDescriptor[stick].axisTypes[axis] = type;
m_joystickData[stick].descriptor.axisTypes[axis] = type;
m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
}
void DriverStationData::SetGameSpecificMessage(const char* message) {
std::scoped_lock lock(m_matchInfoMutex);
std::strncpy(reinterpret_cast<char*>(m_matchInfo->gameSpecificMessage),
message, sizeof(m_matchInfo->gameSpecificMessage) - 1);
*(std::end(m_matchInfo->gameSpecificMessage) - 1) = '\0';
m_matchInfo->gameSpecificMessageSize = std::strlen(message);
std::strncpy(reinterpret_cast<char*>(m_matchInfo.gameSpecificMessage),
message, sizeof(m_matchInfo.gameSpecificMessage) - 1);
*(std::end(m_matchInfo.gameSpecificMessage) - 1) = '\0';
m_matchInfo.gameSpecificMessageSize = std::strlen(message);
m_matchInfoCallbacks(&m_matchInfo);
}
void DriverStationData::SetEventName(const char* name) {
std::scoped_lock lock(m_matchInfoMutex);
std::strncpy(m_matchInfo->eventName, name,
sizeof(m_matchInfo->eventName) - 1);
*(std::end(m_matchInfo->eventName) - 1) = '\0';
std::strncpy(m_matchInfo.eventName, name, sizeof(m_matchInfo.eventName) - 1);
*(std::end(m_matchInfo.eventName) - 1) = '\0';
m_matchInfoCallbacks(&m_matchInfo);
}
void DriverStationData::SetMatchType(HAL_MatchType type) {
std::scoped_lock lock(m_matchInfoMutex);
m_matchInfo->matchType = type;
m_matchInfo.matchType = type;
m_matchInfoCallbacks(&m_matchInfo);
}
void DriverStationData::SetMatchNumber(int32_t matchNumber) {
std::scoped_lock lock(m_matchInfoMutex);
m_matchInfo->matchNumber = matchNumber;
m_matchInfo.matchNumber = matchNumber;
m_matchInfoCallbacks(&m_matchInfo);
}
void DriverStationData::SetReplayNumber(int32_t replayNumber) {
std::scoped_lock lock(m_matchInfoMutex);
m_matchInfo->replayNumber = replayNumber;
m_matchInfo.replayNumber = replayNumber;
m_matchInfoCallbacks(&m_matchInfo);
}
extern "C" {
@@ -283,21 +333,42 @@ DEFINE_CAPI(HAL_Bool, DsAttached, dsAttached)
DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId, allianceStationId)
DEFINE_CAPI(double, MatchTime, matchTime)
void HALSIM_SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes) {
SimDriverStationData->SetJoystickAxes(joystickNum, axes);
#undef DEFINE_CAPI
#define DEFINE_CAPI(name, data) \
int32_t HALSIM_RegisterJoystick##name##Callback( \
int32_t joystickNum, HAL_Joystick##name##Callback callback, void* param, \
HAL_Bool initialNotify) { \
return SimDriverStationData->RegisterJoystick##name##Callback( \
joystickNum, callback, param, initialNotify); \
} \
\
void HALSIM_CancelJoystick##name##Callback(int32_t uid) { \
SimDriverStationData->CancelJoystick##name##Callback(uid); \
} \
\
void HALSIM_GetJoystick##name(int32_t joystickNum, HAL_Joystick##name* d) { \
SimDriverStationData->GetJoystick##name(joystickNum, d); \
} \
\
void HALSIM_SetJoystick##name(int32_t joystickNum, \
const HAL_Joystick##name* d) { \
SimDriverStationData->SetJoystick##name(joystickNum, d); \
}
DEFINE_CAPI(Axes, axes)
DEFINE_CAPI(POVs, povs)
DEFINE_CAPI(Buttons, buttons)
DEFINE_CAPI(Descriptor, descriptor)
int32_t HALSIM_RegisterJoystickOutputsCallback(
int32_t joystickNum, HAL_JoystickOutputsCallback callback, void* param,
HAL_Bool initialNotify) {
return SimDriverStationData->RegisterJoystickOutputsCallback(
joystickNum, callback, param, initialNotify);
}
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_CancelJoystickOutputsCallback(int32_t uid) {
SimDriverStationData->CancelJoystickOutputsCallback(uid);
}
void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
@@ -306,6 +377,26 @@ void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
rightRumble);
}
void HALSIM_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
int32_t leftRumble, int32_t rightRumble) {
SimDriverStationData->SetJoystickOutputs(joystickNum, outputs, leftRumble,
rightRumble);
}
int32_t HALSIM_RegisterMatchInfoCallback(HAL_MatchInfoCallback callback,
void* param, HAL_Bool initialNotify) {
return SimDriverStationData->RegisterMatchInfoCallback(callback, param,
initialNotify);
}
void HALSIM_CancelMatchInfoCallback(int32_t uid) {
SimDriverStationData->CancelMatchInfoCallback(uid);
}
void HALSIM_GetMatchInfo(HAL_MatchInfo* info) {
SimDriverStationData->GetMatchInfo(info);
}
void HALSIM_SetMatchInfo(const HAL_MatchInfo* info) {
SimDriverStationData->SetMatchInfo(info);
}