Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal] Update DS API to new format (#7977)

Browse Source
This commit is contained in:
Thad House
2025-05-16 22:15:14 -07:00
committed by GitHub
parent 1596e2fd7a
commit 231ec348fe
35 changed files with 878 additions and 766 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
hal/src/main/native/cpp/proto/ControlDataProto.cpp
View File

@@ -2,9 +2,6 @@
// 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"
@@ -13,6 +10,8 @@ static_assert(sizeof(mrc::ControlFlags) == sizeof(uint32_t));
namespace {
constexpr uint32_t EnabledMask = 0x1;
constexpr uint32_t AutoMask = 0x2;
constexpr uint32_t TestMask = 0x4;
constexpr uint32_t EStopMask = 0x8;
constexpr uint32_t FmsConnectedMask = 0x10;
constexpr uint32_t DsConnectedMask = 0x20;
@@ -20,6 +19,8 @@ constexpr uint32_t WatchdogActiveMask = 0x40;
constexpr uint32_t AllianceMask = 0x1F80;
constexpr uint32_t EnabledShift = 0;
constexpr uint32_t AutoShift = 1;
constexpr uint32_t TestShift = 2;
constexpr uint32_t EStopShift = 3;
constexpr uint32_t FmsConnectedShift = 4;
constexpr uint32_t DsConnectedShift = 5;
@@ -31,6 +32,8 @@ constexpr uint32_t AllianceShift = 7;
constexpr uint32_t FromControlWord(mrc::ControlFlags Word) {
uint32_t Ret = 0;
WORD_TO_INT(Enabled);
WORD_TO_INT(Auto);
WORD_TO_INT(Test);
WORD_TO_INT(EStop);
WORD_TO_INT(FmsConnected);
WORD_TO_INT(DsConnected);
@@ -46,6 +49,8 @@ constexpr uint32_t FromControlWord(mrc::ControlFlags Word) {
constexpr mrc::ControlFlags ToControlWord(uint32_t Word) {
mrc::ControlFlags Ret = {};
INT_TO_WORD(Enabled);
INT_TO_WORD(Auto);
INT_TO_WORD(Test);
INT_TO_WORD(EStop);
INT_TO_WORD(FmsConnected);
INT_TO_WORD(DsConnected);
@@ -59,13 +64,12 @@ constexpr mrc::ControlFlags ToControlWord(uint32_t Word) {
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(),
.CurrentOpMode = 0,
};
if (!Stream.Decode(Msg)) {
@@ -73,16 +77,12 @@ std::optional<mrc::ControlData> wpi::Protobuf<mrc::ControlData>::Unpack(
}
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.CurrentOpMode = Msg.CurrentOpMode;
ControlData.SetJoystickCount(Joysticks.size());
for (size_t i = 0; i < ControlData.GetJoystickCount(); i++) {
@@ -94,8 +94,6 @@ std::optional<mrc::ControlData> wpi::Protobuf<mrc::ControlData>::Unpack(
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};
@@ -103,7 +101,7 @@ bool wpi::Protobuf<mrc::ControlData>::Pack(OutputStream& Stream,
.ControlWord = FromControlWord(Value.ControlWord),
.MatchTime = Value.MatchTime,
.Joysticks = Joysticks.Callback(),
.OpMode = OpModeCb.Callback(),
.CurrentOpMode = Value.CurrentOpMode,
};
return Stream.Encode(Msg);
@@ -111,14 +109,14 @@ bool wpi::Protobuf<mrc::ControlData>::Pack(OutputStream& Stream,
std::optional<mrc::Joystick> wpi::Protobuf<mrc::Joystick>::Unpack(
InputStream& Stream) {
wpi::UnpackCallback<float, MRC_MAX_NUM_AXES> AxesCb;
wpi::UnpackCallback<int16_t, MRC_MAX_NUM_POVS> PovsCb;
wpi::UnpackCallback<int16_t, MRC_MAX_NUM_AXES> AxesCb;
mrc_proto_ProtobufJoystickData Msg{
.ButtonCount = 0,
.Buttons = 0,
.Axes = AxesCb.Callback(),
.POVs = PovsCb.Callback(),
.POVCount = 0,
.POVs = 0,
};
if (!Stream.Decode(Msg)) {
@@ -126,7 +124,6 @@ std::optional<mrc::Joystick> wpi::Protobuf<mrc::Joystick>::Unpack(
}
auto Axes = AxesCb.Items();
auto Povs = PovsCb.Items();
mrc::Joystick Joystick;
Joystick.Axes.SetCount(Axes.size());
@@ -135,28 +132,36 @@ std::optional<mrc::Joystick> wpi::Protobuf<mrc::Joystick>::Unpack(
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;
Joystick.Povs.SetCount(Msg.POVCount);
uint32_t PovsStore = Msg.POVs;
for (size_t i = 0; i < Joystick.Povs.GetCount(); i++) {
uint8_t Val = PovsStore & 0xF;
PovsStore >>= 4;
Joystick.Povs.Povs()[i] = Val;
}
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()};
uint32_t PovsStore = 0;
for (size_t i = 0; i < Value.Povs.GetCount(); i++) {
PovsStore <<= 4;
PovsStore |= Value.Povs.Povs()[i] & 0xF;
}
mrc_proto_ProtobufJoystickData Msg{
.ButtonCount = static_cast<uint32_t>(Value.Buttons.GetCount()),
.Buttons = Value.Buttons.Buttons,
.Axes = AxesCb.Callback(),
.POVs = PovsCb.Callback(),
.POVCount = static_cast<uint32_t>(Value.Povs.GetCount()),
.POVs = PovsStore,
};
return Stream.Encode(Msg);