[hal] Add initial SystemServer support (#7463)

hal/src/main/native/cpp/proto/ControlDataProto.cpp

@@ -0,0 +1,163 @@
+// 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 <string>
+#include <utility>
+
+#include <wpi/protobuf/ProtobufCallbacks.h>
+
+#include "hal/proto/ControlData.h"
+
+static_assert(sizeof(mrc::ControlFlags) == sizeof(uint32_t));
+
+namespace {
+constexpr uint32_t EnabledMask = 0x1;
+constexpr uint32_t EStopMask = 0x8;
+constexpr uint32_t FmsConnectedMask = 0x10;
+constexpr uint32_t DsConnectedMask = 0x20;
+constexpr uint32_t WatchdogActiveMask = 0x40;
+constexpr uint32_t AllianceMask = 0x1F80;
+
+constexpr uint32_t EnabledShift = 0;
+constexpr uint32_t EStopShift = 3;
+constexpr uint32_t FmsConnectedShift = 4;
+constexpr uint32_t DsConnectedShift = 5;
+constexpr uint32_t WatchdogActiveShift = 6;
+constexpr uint32_t AllianceShift = 7;
+
+#define WORD_TO_INT(Name) Ret |= (Word.Name << Name##Shift)
+
+constexpr uint32_t FromControlWord(mrc::ControlFlags Word) {
+  uint32_t Ret = 0;
+  WORD_TO_INT(Enabled);
+  WORD_TO_INT(EStop);
+  WORD_TO_INT(FmsConnected);
+  WORD_TO_INT(DsConnected);
+  WORD_TO_INT(WatchdogActive);
+  WORD_TO_INT(Alliance);
+  return Ret;
+}
+
+#undef WORD_TO_INT
+
+#define INT_TO_WORD(Name) Ret.Name = ((Word & Name##Mask) >> Name##Shift)
+
+constexpr mrc::ControlFlags ToControlWord(uint32_t Word) {
+  mrc::ControlFlags Ret = {};
+  INT_TO_WORD(Enabled);
+  INT_TO_WORD(EStop);
+  INT_TO_WORD(FmsConnected);
+  INT_TO_WORD(DsConnected);
+  INT_TO_WORD(WatchdogActive);
+  INT_TO_WORD(Alliance);
+  return Ret;
+}
+
+}  // namespace
+
+std::optional<mrc::ControlData> wpi::Protobuf<mrc::ControlData>::Unpack(
+    InputStream& Stream) {
+  wpi::UnpackCallback<mrc::Joystick, MRC_MAX_NUM_JOYSTICKS> JoystickCb;
+  wpi::UnpackCallback<std::string> OpModeCb;
+
+  mrc_proto_ProtobufControlData Msg{
+      .ControlWord = 0,
+      .MatchTime = 0,
+      .Joysticks = JoystickCb.Callback(),
+      .OpMode = OpModeCb.Callback(),
+  };
+
+  if (!Stream.Decode(Msg)) {
+    return {};
+  }
+
+  auto Joysticks = JoystickCb.Items();
+  auto OpMode = OpModeCb.Items();
+
+  mrc::ControlData ControlData;
+
+  if (!OpMode.empty()) {
+    ControlData.MoveOpMode(std::move(OpMode[0]));
+  }
+
+  ControlData.ControlWord = ToControlWord(Msg.ControlWord);
+  ControlData.MatchTime = Msg.MatchTime;
+  ControlData.SetJoystickCount(Joysticks.size());
+
+  for (size_t i = 0; i < ControlData.GetJoystickCount(); i++) {
+    ControlData.Joysticks()[i] = Joysticks[i];
+  }
+
+  return ControlData;
+}
+
+bool wpi::Protobuf<mrc::ControlData>::Pack(OutputStream& Stream,
+                                           const mrc::ControlData& Value) {
+  std::string_view OpMode = Value.GetOpMode();
+  wpi::PackCallback OpModeCb{&OpMode};
+  std::span<const mrc::Joystick> Sticks = Value.Joysticks();
+  wpi::PackCallback Joysticks{Sticks};
+
+  mrc_proto_ProtobufControlData Msg{
+      .ControlWord = FromControlWord(Value.ControlWord),
+      .MatchTime = Value.MatchTime,
+      .Joysticks = Joysticks.Callback(),
+      .OpMode = OpModeCb.Callback(),
+  };
+
+  return Stream.Encode(Msg);
+}
+
+std::optional<mrc::Joystick> wpi::Protobuf<mrc::Joystick>::Unpack(
+    InputStream& Stream) {
+  wpi::UnpackCallback<float, MRC_MAX_NUM_AXES> AxesCb;
+  wpi::UnpackCallback<uint16_t, MRC_MAX_NUM_POVS> PovsCb;
+
+  mrc_proto_ProtobufJoystickData Msg{
+      .ButtonCount = 0,
+      .Buttons = 0,
+      .Axes = AxesCb.Callback(),
+      .POVs = PovsCb.Callback(),
+  };
+
+  if (!Stream.Decode(Msg)) {
+    return {};
+  }
+
+  auto Axes = AxesCb.Items();
+  auto Povs = PovsCb.Items();
+
+  mrc::Joystick Joystick;
+  Joystick.Axes.SetCount(Axes.size());
+
+  for (size_t i = 0; i < Joystick.Axes.GetCount(); i++) {
+    Joystick.Axes.Axes()[i] = Axes[i];
+  }
+
+  Joystick.Povs.SetCount(Povs.size());
+
+  for (size_t i = 0; i < Joystick.Povs.GetCount(); i++) {
+    Joystick.Povs.Povs()[i] = Povs[i];
+  }
+
+  Joystick.Buttons.SetCount(Msg.ButtonCount);
+  Joystick.Buttons.Buttons = Msg.Buttons;
+
+  return Joystick;
+}
+
+bool wpi::Protobuf<mrc::Joystick>::Pack(OutputStream& Stream,
+                                        const mrc::Joystick& Value) {
+  wpi::PackCallback AxesCb{Value.Axes.Axes()};
+  wpi::PackCallback PovsCb{Value.Povs.Povs()};
+
+  mrc_proto_ProtobufJoystickData Msg{
+      .ButtonCount = static_cast<uint32_t>(Value.Buttons.GetCount()),
+      .Buttons = Value.Buttons.Buttons,
+      .Axes = AxesCb.Callback(),
+      .POVs = PovsCb.Callback(),
+  };
+
+  return Stream.Encode(Msg);
+}

hal/src/main/native/cpp/proto/JoystickDescriptorProto.cpp

@@ -0,0 +1,68 @@
+// 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 <string>
+#include <utility>
+
+#include <wpi/protobuf/ProtobufCallbacks.h>
+
+#include "hal/proto/JoystickDescriptor.h"
+
+std::optional<mrc::JoystickDescriptor>
+wpi::Protobuf<mrc::JoystickDescriptor>::Unpack(InputStream& Stream) {
+  wpi::UnpackCallback<std::string> JoystickNameCb;
+  wpi::UnpackCallback<uint8_t, MRC_MAX_NUM_AXES> AxisTypesCb;
+
+  mrc_proto_ProtobufJoystickDescriptor Msg;
+  Msg.JoystickName = JoystickNameCb.Callback();
+  Msg.AxisTypes = AxisTypesCb.Callback();
+
+  if (!Stream.Decode(Msg)) {
+    return {};
+  }
+
+  auto JoystickName = JoystickNameCb.Items();
+  auto AxisTypes = AxisTypesCb.Items();
+
+  if (JoystickName.empty()) {
+    return {};
+  }
+
+  mrc::JoystickDescriptor OutputData;
+  OutputData.MoveName(std::move(JoystickName[0]));
+
+  OutputData.SetAxesCount(AxisTypes.size());
+
+  for (size_t i = 0; i < OutputData.GetAxesCount(); i++) {
+    OutputData.AxesTypes()[i] = AxisTypes[i];
+  }
+
+  OutputData.SetPovsCount(Msg.PovCount);
+  OutputData.SetButtonsCount(Msg.ButtonCount);
+
+  OutputData.IsXbox = Msg.IsXbox ? 1 : 0;
+  OutputData.Type = Msg.JoystickType;
+
+  return OutputData;
+}
+
+bool wpi::Protobuf<mrc::JoystickDescriptor>::Pack(
+    OutputStream& Stream, const mrc::JoystickDescriptor& Value) {
+  std::string_view JoystickName = Value.GetName();
+  wpi::PackCallback JoystickNameCb{&JoystickName};
+
+  std::span<const uint8_t> AxisTypes = Value.AxesTypes();
+  wpi::PackCallback AxisTypesCb{AxisTypes};
+
+  mrc_proto_ProtobufJoystickDescriptor Msg{
+      .JoystickName = JoystickNameCb.Callback(),
+      .AxisTypes = AxisTypesCb.Callback(),
+      .IsXbox = Value.IsXbox ? true : false,
+      .JoystickType = Value.Type,
+      .ButtonCount = static_cast<int32_t>(Value.GetButtonsCount()),
+      .PovCount = static_cast<int32_t>(Value.GetPovsCount()),
+  };
+
+  return Stream.Encode(Msg);
+}

hal/src/main/native/cpp/proto/JoystickOutputDataProto.cpp

@@ -0,0 +1,33 @@
+// 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 <wpi/protobuf/ProtobufCallbacks.h>
+
+#include "hal/proto/JoystickOutputData.h"
+
+std::optional<mrc::JoystickOutputData>
+wpi::Protobuf<mrc::JoystickOutputData>::Unpack(InputStream& Stream) {
+  mrc_proto_ProtobufJoystickOutputData Msg;
+
+  if (!Stream.Decode(Msg)) {
+    return {};
+  }
+
+  return mrc::JoystickOutputData{
+      .HidOutputs = Msg.HidOutputs,
+      .LeftRumble = Msg.LeftRumble,
+      .RightRumble = Msg.RightRumble,
+  };
+}
+
+bool wpi::Protobuf<mrc::JoystickOutputData>::Pack(
+    OutputStream& Stream, const mrc::JoystickOutputData& Value) {
+  mrc_proto_ProtobufJoystickOutputData Msg{
+      .HidOutputs = Value.HidOutputs,
+      .LeftRumble = Value.LeftRumble,
+      .RightRumble = Value.RightRumble,
+  };
+
+  return Stream.Encode(Msg);
+}

hal/src/main/native/cpp/proto/MatchInfoProto.cpp

@@ -0,0 +1,52 @@
+// 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 <string>
+#include <utility>
+
+#include <wpi/protobuf/ProtobufCallbacks.h>
+
+#include "hal/proto/MatchInfo.h"
+
+std::optional<mrc::MatchInfo> wpi::Protobuf<mrc::MatchInfo>::Unpack(
+    InputStream& Stream) {
+  wpi::UnpackCallback<std::string> NameCb;
+
+  mrc_proto_ProtobufMatchInfo Msg;
+  Msg.EventName = NameCb.Callback();
+
+  if (!Stream.Decode(Msg)) {
+    return {};
+  }
+
+  auto Name = NameCb.Items();
+
+  if (Name.empty()) {
+    return {};
+  }
+
+  mrc::MatchInfo OutputData;
+  OutputData.MoveEventName(std::move(Name[0]));
+
+  OutputData.MatchNumber = Msg.MatchNumber;
+  OutputData.ReplayNumber = Msg.ReplayNumber;
+  OutputData.Type = static_cast<mrc::MatchType>(Msg.MatchType);
+
+  return OutputData;
+}
+
+bool wpi::Protobuf<mrc::MatchInfo>::Pack(OutputStream& Stream,
+                                         const mrc::MatchInfo& Value) {
+  std::string_view EventNameStr = Value.GetEventName();
+  wpi::PackCallback EventName{&EventNameStr};
+
+  mrc_proto_ProtobufMatchInfo Msg{
+      .EventName = EventName.Callback(),
+      .MatchNumber = Value.MatchNumber,
+      .ReplayNumber = Value.ReplayNumber,
+      .MatchType = static_cast<int32_t>(Value.Type),
+  };
+
+  return Stream.Encode(Msg);
+}

hal/src/main/native/cpp/proto/VersionInfoProto.cpp

@@ -0,0 +1,56 @@
+// 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 <string>
+#include <utility>
+
+#include <wpi/protobuf/ProtobufCallbacks.h>
+
+#include "hal/proto/VersionInfo.h"
+
+std::optional<mrc::VersionInfo> wpi::Protobuf<mrc::VersionInfo>::Unpack(
+    InputStream& Stream) {
+  wpi::UnpackCallback<std::string> NameCb;
+  wpi::UnpackCallback<std::string> VersionCb;
+
+  mrc_proto_ProtobufVersionInfo Msg{
+      .DeviceId = 0,
+      .Name = NameCb.Callback(),
+      .Version = VersionCb.Callback(),
+  };
+
+  if (!Stream.Decode(Msg)) {
+    return {};
+  }
+
+  auto Name = NameCb.Items();
+  auto Version = VersionCb.Items();
+
+  if (Name.empty() || Version.empty()) {
+    return {};
+  }
+
+  mrc::VersionInfo ToRet;
+
+  ToRet.DeviceId = Msg.DeviceId;
+  ToRet.MoveName(std::move(Name[0]));
+  ToRet.MoveVersion(std::move(Version[0]));
+
+  return ToRet;
+}
+
+bool wpi::Protobuf<mrc::VersionInfo>::Pack(OutputStream& Stream,
+                                           const mrc::VersionInfo& Value) {
+  std::string_view NameView = Value.GetName();
+  std::string_view VersionView = Value.GetVersion();
+  wpi::PackCallback NameCb{&NameView};
+  wpi::PackCallback VersionCb{&VersionView};
+
+  mrc_proto_ProtobufVersionInfo Msg{
+      .DeviceId = Value.DeviceId,
+      .Name = NameCb.Callback(),
+      .Version = VersionCb.Callback(),
+  };
+  return Stream.Encode(Msg);
+}