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

[hal, wpilib] Add OpMode support (#7744)

Browse Source 
User code:
- OpModeRobot used as the robot base class
- LinearOpMode and PeriodicOpMode are provided opmode base classes
- In Java, annotations can be used to automatically register opmode classes

Additional user code functionality:
- OpMode (string) is available in addition to the overall
auto/teleop/test robot mode
- OpMode does not indicate enable (enable/disable is still separate)
- The HAL API uses integer UIDs; these are exposed at the user API level
as well for faster checks
- User code creates opmodes on startup (these have name, category,
description, etc).

DS:
- DS will present opmode selection lists for auto and teleop for
match/practice. During a match, the DS will automatically activate the
selected opmode in the corresponding match period.
- For testing, an overall mode is selected (e.g. teleop/auto/test) and a
single opmode is selected

Future work:
- Command framework support/integration
- Python annotation support
- Unit tests (needs race-free DS sim updates)
- Porting of examples

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Peter Johnson
2025-12-12 21:25:57 -07:00
committed by GitHub
parent 2a41b80e00
commit dacded37e5
163 changed files with 7454 additions and 2175 deletions
Download Patch File Download Diff File Expand all files Collapse all files

162
hal/src/main/native/cpp/DashboardOpMode.cpp Normal file
View File

@@ -0,0 +1,162 @@
// 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/hal/DashboardOpMode.hpp"
#include <atomic>
#include <string>
#include <vector>
#include <fmt/format.h>
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/nt/NetworkTableInstance.hpp"
#include "wpi/nt/NetworkTableListener.hpp"
#include "wpi/nt/StringArrayTopic.hpp"
#include "wpi/nt/StringTopic.hpp"
#include "wpi/nt/ntcore_cpp.hpp"
#include "wpi/util/StringMap.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/string.h"
using namespace wpi;
namespace {
class DashboardOpModeSender {
public:
void Start(nt::NetworkTableInstance inst, std::string_view tableName) {
m_typeTopic = inst.GetStringTopic(fmt::format("{}/.type", tableName));
m_optionsTopic =
inst.GetStringArrayTopic(fmt::format("{}/options", tableName));
m_activeTopic = inst.GetStringTopic(fmt::format("{}/active", tableName));
m_selectedSub = inst.GetStringTopic(fmt::format("{}/selected", tableName))
.Subscribe("");
m_selectedListener = nt::NetworkTableListener::CreateListener(
m_selectedSub, NT_EVENT_VALUE_ALL | NT_EVENT_IMMEDIATE,
[this](const nt::Event& event) {
if (auto data = event.GetValueEventData()) {
if (data->value.IsString()) {
m_activePub.Set(data->value.GetString());
}
}
});
}
void Enable() {
m_typePub = m_typeTopic.Publish();
m_typePub.Set("String Chooser");
m_optionsPub = m_optionsTopic.Publish();
m_optionsPub.Set(m_options);
m_activePub = m_activeTopic.Publish();
m_activePub.Set("");
}
void SetOptions(std::span<const HAL_OpModeOption> options,
HAL_RobotMode mode) {
m_optionMap.clear();
m_options.clear();
for (auto&& option : options) {
if (HAL_OpMode_GetRobotMode(option.id) == mode) {
auto name = util::to_string_view(&option.name);
m_optionMap[name] = option.id;
m_options.emplace_back(name);
}
}
if (m_optionsPub) {
m_optionsPub.Set(m_options);
}
}
int64_t GetSelected() const {
auto it = m_optionMap.find(m_selectedSub.Get());
if (it == m_optionMap.end()) {
return 0;
}
return it->second;
}
private:
nt::StringTopic m_typeTopic;
nt::StringPublisher m_typePub;
nt::StringArrayTopic m_optionsTopic;
nt::StringArrayPublisher m_optionsPub;
nt::StringTopic m_activeTopic;
nt::StringPublisher m_activePub;
nt::StringSubscriber m_selectedSub;
nt::NetworkTableListener m_selectedListener;
util::StringMap<int64_t> m_optionMap;
std::vector<std::string> m_options;
};
struct DashboardOpModeInstance {
void Start(nt::NetworkTableInstance inst) {
autoOpModes.Start(inst, "/SmartDashboard/Auto OpMode");
teleopOpModes.Start(inst, "/SmartDashboard/Teleop OpMode");
testOpModes.Start(inst, "/SmartDashboard/Test OpMode");
}
util::mutex mutex;
DashboardOpModeSender autoOpModes;
DashboardOpModeSender teleopOpModes;
DashboardOpModeSender testOpModes;
};
} // namespace
static DashboardOpModeInstance* gInstance;
static std::atomic_flag gStarted{};
static std::atomic_flag gEnabled{};
void hal::InitializeDashboardOpMode() {
static DashboardOpModeInstance inst;
gInstance = &inst;
}
void hal::SetDashboardOpModeOptions(std::span<const HAL_OpModeOption> options) {
std::scoped_lock lock{gInstance->mutex};
gInstance->autoOpModes.SetOptions(options, HAL_ROBOTMODE_AUTONOMOUS);
gInstance->teleopOpModes.SetOptions(options, HAL_ROBOTMODE_TELEOPERATED);
gInstance->testOpModes.SetOptions(options, HAL_ROBOTMODE_TEST);
}
void hal::StartDashboardOpMode() {
if (gStarted.test_and_set()) {
return;
}
std::scoped_lock lock{gInstance->mutex};
gInstance->Start(nt::NetworkTableInstance::GetDefault());
}
void hal::EnableDashboardOpMode() {
if (gEnabled.test_and_set()) {
return;
}
StartDashboardOpMode();
std::scoped_lock lock{gInstance->mutex};
gInstance->autoOpModes.Enable();
gInstance->teleopOpModes.Enable();
gInstance->testOpModes.Enable();
}
int64_t hal::GetDashboardSelectedOpMode(HAL_RobotMode robotMode) {
if (!gEnabled.test()) {
return 0;
}
std::scoped_lock lock{gInstance->mutex};
switch (robotMode) {
case HAL_ROBOTMODE_AUTONOMOUS:
return gInstance->autoOpModes.GetSelected();
case HAL_ROBOTMODE_TELEOPERATED:
return gInstance->teleopOpModes.GetSelected();
case HAL_ROBOTMODE_TEST:
return gInstance->testOpModes.GetSelected();
default:
return 0;
}
}

105
hal/src/main/native/cpp/jni/DriverStationJNI.cpp
View File

@@ -5,6 +5,8 @@
#include <jni.h>
#include <cassert>
#include <utility>
#include <vector>
#include <fmt/format.h>
@@ -61,68 +63,77 @@ Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramStarting
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: observeUserProgramDisabled
* Signature: ()V
* Method: observeUserProgram
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramDisabled
(JNIEnv*, jclass)
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgram
(JNIEnv*, jclass, jlong word)
{
HAL_ObserveUserProgramDisabled();
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: observeUserProgramAutonomous
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramAutonomous
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramAutonomous();
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: observeUserProgramTeleop
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramTeleop
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramTeleop();
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: observeUserProgramTest
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramTest
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramTest();
HAL_ObserveUserProgram({.value = word});
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: nativeGetControlWord
* Signature: ()I
* Signature: ()J
*/
JNIEXPORT jint JNICALL
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_nativeGetControlWord
(JNIEnv*, jclass)
{
static_assert(sizeof(HAL_ControlWord) == sizeof(jint),
static_assert(sizeof(HAL_ControlWord) == sizeof(jlong),
"Java int must match the size of control word");
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
jint retVal = 0;
std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord));
return retVal;
return controlWord.value;
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: nativeGetUncachedControlWord
* Signature: ()J
*/
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_nativeGetUncachedControlWord
(JNIEnv*, jclass)
{
static_assert(sizeof(HAL_ControlWord) == sizeof(jlong),
"Java int must match the size of control word");
HAL_ControlWord controlWord;
HAL_GetUncachedControlWord(&controlWord);
return controlWord.value;
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: setOpModeOptions
* Signature: ([Ljava/lang/Object;)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_setOpModeOptions
(JNIEnv* env, jclass, jobjectArray options)
{
std::vector<HAL_OpModeOption> coptions;
if (options != nullptr) {
jsize length = env->GetArrayLength(options);
coptions.reserve(length);
for (jsize i = 0; i < length; i++) {
JLocal<jobject> option{env, env->GetObjectArrayElement(options, i)};
if (!option) {
ThrowIllegalArgumentException(env, "Null OpModeOption passed in array");
return;
}
auto coption = CreateOpModeOptionFromJava(env, option);
if (coption.id == 0) {
// exception thrown
return;
}
coptions.emplace_back(std::move(coption));
}
}
int32_t status = HAL_SetOpModeOptions(coptions.data(), coptions.size());
CheckStatusForceThrow(env, status);
}
/*

67
hal/src/main/native/cpp/jni/HALUtil.cpp
View File

@@ -53,6 +53,7 @@ static JClass matchInfoDataCls;
static JClass canReceiveMessageCls;
static JClass canStreamMessageCls;
static JClass halValueCls;
static JClass opModeOptionCls;
static JClass revPHVersionCls;
static JClass canStreamOverflowExCls;
@@ -63,6 +64,7 @@ static const JClassInit classes[] = {
{"org/wpilib/hardware/hal/MatchInfoData", &matchInfoDataCls},
{"org/wpilib/hardware/hal/can/CANReceiveMessage", &canReceiveMessageCls},
{"org/wpilib/hardware/hal/can/CANStreamMessage", &canStreamMessageCls},
{"org/wpilib/hardware/hal/OpModeOption", &opModeOptionCls},
{"org/wpilib/hardware/hal/HALValue", &halValueCls},
{"org/wpilib/hardware/hal/REVPHVersion", &revPHVersionCls},
{"org/wpilib/hardware/hal/can/CANStreamOverflowException",
@@ -188,6 +190,71 @@ void ThrowBoundaryException(JNIEnv* env, double value, double lower,
env->Throw(static_cast<jthrowable>(ex));
}
jobject CreateOpModeOption(JNIEnv* env, const HAL_OpModeOption& option) {
static jmethodID constructor = env->GetMethodID(
opModeOptionCls, "<init>",
"(JLjava/lang/String;L/java/lang/String;Ljava/lang/String;II)V");
JLocal<jstring> name{
env, MakeJString(env, wpi::util::to_string_view(&option.name))};
JLocal<jstring> group{
env, MakeJString(env, wpi::util::to_string_view(&option.group))};
JLocal<jstring> desc{
env, MakeJString(env, wpi::util::to_string_view(&option.description))};
return env->NewObject(opModeOptionCls, constructor,
static_cast<jlong>(option.id), name.obj(), group.obj(),
desc.obj(), static_cast<jint>(option.textColor),
static_cast<jint>(option.backgroundColor));
}
jobjectArray CreateOpModeOptionArray(
JNIEnv* env, std::span<const HAL_OpModeOption> options) {
jobjectArray arr =
env->NewObjectArray(options.size(), opModeOptionCls, nullptr);
if (!arr) {
return nullptr;
}
size_t i = 0;
for (auto& option : options) {
JLocal<jobject> elem{env, CreateOpModeOption(env, option)};
env->SetObjectArrayElement(arr, i++, elem);
}
return arr;
}
HAL_OpModeOption CreateOpModeOptionFromJava(JNIEnv* env, jobject option) {
static jfieldID idField = env->GetFieldID(opModeOptionCls, "id", "J");
static jfieldID nameField =
env->GetFieldID(opModeOptionCls, "name", "Ljava/lang/String;");
static jfieldID groupField =
env->GetFieldID(opModeOptionCls, "group", "Ljava/lang/String;");
static jfieldID descriptionField =
env->GetFieldID(opModeOptionCls, "description", "Ljava/lang/String;");
static jfieldID textColorField =
env->GetFieldID(opModeOptionCls, "textColor", "I");
static jfieldID backgroundColorField =
env->GetFieldID(opModeOptionCls, "backgroundColor", "I");
if (!idField || !nameField || !groupField || !descriptionField ||
!textColorField || !backgroundColorField) {
ThrowIllegalArgumentException(env, "Missing field in OpModeOption");
return {0, {}, {}, {}, 0, 0};
}
int64_t id = env->GetLongField(option, idField);
JLocal<jstring> name{
env, static_cast<jstring>(env->GetObjectField(option, nameField))};
JLocal<jstring> group{
env, static_cast<jstring>(env->GetObjectField(option, groupField))};
JLocal<jstring> description{
env, static_cast<jstring>(env->GetObjectField(option, descriptionField))};
int32_t textColor = env->GetIntField(option, textColorField);
int32_t backgroundColor = env->GetIntField(option, backgroundColorField);
return {id,
wpi::util::alloc_wpi_string(JStringRef{env, name}),
wpi::util::alloc_wpi_string(JStringRef{env, group}),
wpi::util::alloc_wpi_string(JStringRef{env, description}),
textColor,
backgroundColor};
}
jobject CreateREVPHVersion(JNIEnv* env, uint32_t firmwareMajor,
uint32_t firmwareMinor, uint32_t firmwareFix,
uint32_t hardwareMinor, uint32_t hardwareMajor,

7
hal/src/main/native/cpp/jni/HALUtil.h
View File

@@ -7,9 +7,11 @@
#include <jni.h>
#include <stdint.h>
#include <span>
#include <string_view>
struct HAL_MatchInfo;
struct HAL_OpModeOption;
struct HAL_Value;
namespace wpi::hal {
@@ -49,6 +51,11 @@ void ThrowIndexOutOfBoundsException(JNIEnv* env, std::string_view msg);
void ThrowBoundaryException(JNIEnv* env, double value, double lower,
double upper);
jobject CreateOpModeOption(JNIEnv* env, const HAL_OpModeOption& option);
jobjectArray CreateOpModeOptionArray(JNIEnv* env,
std::span<const HAL_OpModeOption> options);
HAL_OpModeOption CreateOpModeOptionFromJava(JNIEnv* env, jobject option);
jobject CreateREVPHVersion(JNIEnv* env, uint32_t firmwareMajor,
uint32_t firmwareMinor, uint32_t firmwareFix,
uint32_t hardwareMinor, uint32_t hardwareMajor,

178
hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp
View File

@@ -4,7 +4,10 @@
#include <jni.h>
#include "../HALUtil.h"
#include "CallbackStore.h"
#include "OpModeOptionsCallbackStore.h"
#include "SimulatorJNI.h"
#include "org_wpilib_hardware_hal_simulation_DriverStationDataJNI.h"
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/hal/simulation/MockHooks.h"
@@ -69,103 +72,53 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setEnabled
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: registerAutonomousCallback
* Method: registerRobotModeCallback
* Signature: (Ljava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerAutonomousCallback
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerRobotModeCallback
(JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallbackNoIndex(
env, callback, initialNotify,
&HALSIM_RegisterDriverStationAutonomousCallback);
&HALSIM_RegisterDriverStationRobotModeCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: cancelAutonomousCallback
* Method: cancelRobotModeCallback
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelAutonomousCallback
(JNIEnv* env, jclass, jint handle)
{
return sim::FreeCallbackNoIndex(
env, handle, &HALSIM_CancelDriverStationAutonomousCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: getAutonomous
* Signature: ()Z
*/
JNIEXPORT jboolean JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getAutonomous
(JNIEnv*, jclass)
{
return HALSIM_GetDriverStationAutonomous();
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: setAutonomous
* Signature: (Z)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setAutonomous
(JNIEnv*, jclass, jboolean value)
{
HALSIM_SetDriverStationAutonomous(value);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: registerTestCallback
* Signature: (Ljava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerTestCallback
(JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallbackNoIndex(
env, callback, initialNotify, &HALSIM_RegisterDriverStationTestCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: cancelTestCallback
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelTestCallback
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelRobotModeCallback
(JNIEnv* env, jclass, jint handle)
{
return sim::FreeCallbackNoIndex(env, handle,
&HALSIM_CancelDriverStationTestCallback);
&HALSIM_CancelDriverStationRobotModeCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: getTest
* Signature: ()Z
* Method: nativeGetRobotMode
* Signature: ()I
*/
JNIEXPORT jboolean JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getTest
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_nativeGetRobotMode
(JNIEnv*, jclass)
{
return HALSIM_GetDriverStationTest();
return HALSIM_GetDriverStationRobotMode();
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: setTest
* Signature: (Z)V
* Method: nativeSetRobotMode
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setTest
(JNIEnv*, jclass, jboolean value)
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_nativeSetRobotMode
(JNIEnv*, jclass, jint value)
{
HALSIM_SetDriverStationTest(value);
HALSIM_SetDriverStationRobotMode(static_cast<HAL_RobotMode>(value));
}
/*
@@ -423,6 +376,99 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setMatchTime
HALSIM_SetDriverStationMatchTime(value);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: registerOpModeCallback
* Signature: (Ljava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerOpModeCallback
(JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallbackNoIndex(
env, callback, initialNotify,
&HALSIM_RegisterDriverStationOpModeCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: cancelOpModeCallback
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelOpModeCallback
(JNIEnv* env, jclass, jint handle)
{
return sim::FreeCallbackNoIndex(env, handle,
&HALSIM_CancelDriverStationOpModeCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: getOpMode
* Signature: ()J
*/
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getOpMode
(JNIEnv* env, jclass)
{
return HALSIM_GetDriverStationOpMode();
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: setOpMode
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setOpMode
(JNIEnv* env, jclass, jlong value)
{
HALSIM_SetDriverStationOpMode(value);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: registerOpModeOptionsCallback
* Signature: (Ljava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerOpModeOptionsCallback
(JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
{
return sim::AllocateOpModeOptionsCallback(
env, callback, initialNotify, &HALSIM_RegisterOpModeOptionsCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: cancelOpModeOptionsCallback
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelOpModeOptionsCallback
(JNIEnv* env, jclass, jint handle)
{
sim::FreeOpModeOptionsCallback(env, handle,
&HALSIM_CancelOpModeOptionsCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: getOpModeOptions
* Signature: ()[Ljava/lang/Object;
*/
JNIEXPORT jobjectArray JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getOpModeOptions
(JNIEnv* env, jclass)
{
int32_t count;
HAL_OpModeOption* options = HALSIM_GetOpModeOptions(&count);
auto rv = CreateOpModeOptionArray(env, {options, options + count});
HALSIM_FreeOpModeOptionsArray(options, count);
return rv;
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: setJoystickAxes

121
hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.cpp Normal file
View File

@@ -0,0 +1,121 @@
// 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 "OpModeOptionsCallbackStore.h"
#include <jni.h>
#include <cstdio>
#include <memory>
#include "../HALUtil.h"
#include "SimulatorJNI.h"
#include "wpi/hal/Types.h"
#include "wpi/hal/handles/UnlimitedHandleResource.h"
#include "wpi/util/jni_util.hpp"
using namespace wpi::hal;
using namespace wpi::hal::sim;
using namespace wpi::util::java;
static UnlimitedHandleResource<SIM_JniHandle, OpModeOptionsCallbackStore,
HAL_HandleEnum::SimulationJni>* callbackHandles;
namespace wpi::hal::sim {
void InitializeOpModeOptionsStore() {
static UnlimitedHandleResource<SIM_JniHandle, OpModeOptionsCallbackStore,
HAL_HandleEnum::SimulationJni>
cb;
callbackHandles = &cb;
}
} // namespace wpi::hal::sim
void OpModeOptionsCallbackStore::create(JNIEnv* env, jobject obj) {
m_call = JGlobal<jobject>(env, obj);
}
void OpModeOptionsCallbackStore::performCallback(
const char* name, const HAL_OpModeOption* opmodes, int32_t count) {
JNIEnv* env;
JavaVM* vm = sim::GetJVM();
bool didAttachThread = false;
int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
if (tryGetEnv == JNI_EDETACHED) {
// Thread not attached
didAttachThread = true;
if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
// Failed to attach, log and return
std::puts("Failed to attach");
std::fflush(stdout);
return;
}
} else if (tryGetEnv == JNI_EVERSION) {
std::puts("Invalid JVM Version requested");
std::fflush(stdout);
}
JLocal<jobjectArray> toCallbackArr{
env, CreateOpModeOptionArray(env, {opmodes, opmodes + count})};
env->CallVoidMethod(m_call, sim::GetBiConsumerCallback(),
MakeJString(env, name), toCallbackArr.obj());
if (env->ExceptionCheck()) {
env->ExceptionDescribe();
}
if (didAttachThread) {
vm->DetachCurrentThread();
}
}
void OpModeOptionsCallbackStore::free(JNIEnv* env) {
m_call.free(env);
}
SIM_JniHandle sim::AllocateOpModeOptionsCallback(
JNIEnv* env, jobject callback, jboolean initialNotify,
RegisterOpModeOptionsCallbackFunc createCallback) {
auto callbackStore = std::make_shared<OpModeOptionsCallbackStore>();
auto handle = callbackHandles->Allocate(callbackStore);
if (handle == HAL_kInvalidHandle) {
return -1;
}
uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
callbackStore->create(env, callback);
auto callbackFunc = [](const char* name, void* param,
const HAL_OpModeOption* opmodes, int32_t count) {
uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
auto data = callbackHandles->Get(handle);
if (!data) {
return;
}
data->performCallback(name, opmodes, count);
};
auto id = createCallback(callbackFunc, handleAsVoidPtr, initialNotify);
callbackStore->setCallbackId(id);
return handle;
}
void sim::FreeOpModeOptionsCallback(
JNIEnv* env, SIM_JniHandle handle,
FreeOpModeOptionsCallbackFunc freeCallback) {
auto callback = callbackHandles->Free(handle);
if (callback == nullptr) {
return;
}
freeCallback(callback->getCallbackId());
callback->free(env);
}

40
hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.h Normal file
View File

@@ -0,0 +1,40 @@
// 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.
#pragma once
#include <jni.h>
#include "SimulatorJNI.h"
#include "wpi/hal/Types.h"
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/util/jni_util.hpp"
namespace wpi::hal::sim {
class OpModeOptionsCallbackStore {
public:
void create(JNIEnv* env, jobject obj);
void performCallback(const char* name, const HAL_OpModeOption* opmodes,
int32_t count);
void free(JNIEnv* env);
void setCallbackId(int32_t id) { callbackId = id; }
int32_t getCallbackId() { return callbackId; }
private:
wpi::util::java::JGlobal<jobject> m_call;
int32_t callbackId;
};
void InitializeOpModeOptionsStore();
using RegisterOpModeOptionsCallbackFunc = int32_t (*)(
HAL_OpModeOptionsCallback callback, void* param, HAL_Bool initialNotify);
using FreeOpModeOptionsCallbackFunc = void (*)(int32_t uid);
SIM_JniHandle AllocateOpModeOptionsCallback(
JNIEnv* env, jobject callback, jboolean initialNotify,
RegisterOpModeOptionsCallbackFunc createCallback);
void FreeOpModeOptionsCallback(JNIEnv* env, SIM_JniHandle handle,
FreeOpModeOptionsCallbackFunc freeCallback);
} // namespace wpi::hal::sim

100
hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp
View File

@@ -7,6 +7,7 @@
#include "BufferCallbackStore.h"
#include "CallbackStore.h"
#include "ConstBufferCallbackStore.h"
#include "OpModeOptionsCallbackStore.h"
#include "SimDeviceDataJNI.h"
#include "org_wpilib_hardware_hal_simulation_SimulatorJNI.h"
#include "wpi/hal/HAL.h"
@@ -20,9 +21,35 @@ static JavaVM* jvm = nullptr;
static JClass notifyCallbackCls;
static JClass bufferCallbackCls;
static JClass constBufferCallbackCls;
static JClass biConsumerCls;
static jmethodID notifyCallbackCallback;
static jmethodID bufferCallbackCallback;
static jmethodID constBufferCallbackCallback;
static jmethodID biConsumerCallback;
static const JClassInit classes[] = {
{"org/wpilib/hardware/hal/simulation/NotifyCallback", &notifyCallbackCls},
{"org/wpilib/hardware/hal/simulation/BufferCallback", &bufferCallbackCls},
{"org/wpilib/hardware/hal/simulation/ConstBufferCallback",
&constBufferCallbackCls},
{"java/util/function/BiConsumer", &biConsumerCls},
};
static const struct JMethodInit {
JClass* cls;
const char* name;
const char* sig;
jmethodID* method;
} methods[] = {
{&notifyCallbackCls, "callbackNative", "(Ljava/lang/String;IJD)V",
&notifyCallbackCallback},
{&bufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V",
&bufferCallbackCallback},
{&constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V",
&constBufferCallbackCallback},
{&biConsumerCls, "accept", "(Ljava/lang/Object;Ljava/lang/Object;)V",
&biConsumerCallback},
};
namespace wpi::hal::sim {
jint SimOnLoad(JavaVM* vm, void* reserved) {
@@ -33,45 +60,24 @@ jint SimOnLoad(JavaVM* vm, void* reserved) {
return JNI_ERR;
}
notifyCallbackCls =
JClass(env, "org/wpilib/hardware/hal/simulation/NotifyCallback");
if (!notifyCallbackCls) {
return JNI_ERR;
for (auto& c : classes) {
*c.cls = JClass(env, c.name);
if (!*c.cls) {
return JNI_ERR;
}
}
notifyCallbackCallback = env->GetMethodID(notifyCallbackCls, "callbackNative",
"(Ljava/lang/String;IJD)V");
if (!notifyCallbackCallback) {
return JNI_ERR;
}
bufferCallbackCls =
JClass(env, "org/wpilib/hardware/hal/simulation/BufferCallback");
if (!bufferCallbackCls) {
return JNI_ERR;
}
bufferCallbackCallback = env->GetMethodID(bufferCallbackCls, "callback",
"(Ljava/lang/String;[BI)V");
if (!bufferCallbackCallback) {
return JNI_ERR;
}
constBufferCallbackCls =
JClass(env, "org/wpilib/hardware/hal/simulation/ConstBufferCallback");
if (!constBufferCallbackCls) {
return JNI_ERR;
}
constBufferCallbackCallback = env->GetMethodID(
constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V");
if (!constBufferCallbackCallback) {
return JNI_ERR;
for (auto& m : methods) {
*m.method = env->GetMethodID(*m.cls, m.name, m.sig);
if (!*m.method) {
return JNI_ERR;
}
}
InitializeStore();
InitializeBufferStore();
InitializeConstBufferStore();
InitializeOpModeOptionsStore();
if (!InitializeSimDeviceDataJNI(env)) {
return JNI_ERR;
}
@@ -88,6 +94,7 @@ void SimOnUnload(JavaVM* vm, void* reserved) {
notifyCallbackCls.free(env);
bufferCallbackCls.free(env);
constBufferCallbackCls.free(env);
biConsumerCls.free(env);
FreeSimDeviceDataJNI(env);
jvm = nullptr;
}
@@ -108,6 +115,9 @@ jmethodID GetConstBufferCallback() {
return constBufferCallbackCallback;
}
jmethodID GetBiConsumerCallback() {
return biConsumerCallback;
}
} // namespace wpi::hal::sim
extern "C" {
@@ -159,6 +169,32 @@ Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_getProgramStarted
return HALSIM_GetProgramStarted();
}
/*
* Class: org_wpilib_hardware_hal_simulation_SimulatorJNI
* Method: setProgramState
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_setProgramState
(JNIEnv*, jclass, jlong word)
{
HALSIM_SetProgramState({word});
}
/*
* Class: org_wpilib_hardware_hal_simulation_SimulatorJNI
* Method: nativeGetProgramState
* Signature: ()J
*/
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_nativeGetProgramState
(JNIEnv*, jclass)
{
HAL_ControlWord word;
HALSIM_GetProgramState(&word);
return word.value;
}
/*
* Class: org_wpilib_hardware_hal_simulation_SimulatorJNI
* Method: restartTiming

1
hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h
View File

@@ -15,4 +15,5 @@ JavaVM* GetJVM();
jmethodID GetNotifyCallback();
jmethodID GetBufferCallback();
jmethodID GetConstBufferCallback();
jmethodID GetBiConsumerCallback();
} // namespace wpi::hal::sim

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

@@ -11,8 +11,7 @@ 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 RobotModeMask = 0x6;
constexpr uint32_t EStopMask = 0x8;
constexpr uint32_t FmsConnectedMask = 0x10;
constexpr uint32_t DsConnectedMask = 0x20;
@@ -20,8 +19,7 @@ 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 RobotModeShift = 1;
constexpr uint32_t EStopShift = 3;
constexpr uint32_t FmsConnectedShift = 4;
constexpr uint32_t DsConnectedShift = 5;
@@ -33,8 +31,7 @@ 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(RobotMode);
WORD_TO_INT(EStop);
WORD_TO_INT(FmsConnected);
WORD_TO_INT(DsConnected);
@@ -50,8 +47,7 @@ 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(RobotMode);
INT_TO_WORD(EStop);
INT_TO_WORD(FmsConnected);
INT_TO_WORD(DsConnected);
@@ -67,10 +63,10 @@ std::optional<mrc::ControlData> wpi::util::Protobuf<mrc::ControlData>::Unpack(
wpi::util::UnpackCallback<mrc::Joystick, MRC_MAX_NUM_JOYSTICKS> JoystickCb;
mrc_proto_ProtobufControlData Msg{
.ControlWord = 0,
.MatchTime = 0,
.Joysticks = JoystickCb.Callback(),
.CurrentOpMode = 0,
.ControlWord = 0,
};
if (!Stream.Decode(Msg)) {
@@ -99,10 +95,10 @@ bool wpi::util::Protobuf<mrc::ControlData>::Pack(
wpi::util::PackCallback Joysticks{Sticks};
mrc_proto_ProtobufControlData Msg{
.ControlWord = FromControlWord(Value.ControlWord),
.MatchTime = Value.MatchTime,
.Joysticks = Joysticks.Callback(),
.CurrentOpMode = Value.CurrentOpMode.ToValue(),
.ControlWord = FromControlWord(Value.ControlWord),
};
return Stream.Encode(Msg);

30
hal/src/main/native/cpp/proto/OpModeProto.cpp
View File

@@ -12,36 +12,52 @@
std::optional<mrc::OpMode> wpi::util::Protobuf<mrc::OpMode>::Unpack(
InputStream& Stream) {
wpi::util::UnpackCallback<std::string> NameCb;
wpi::util::UnpackCallback<std::string> GroupCb;
wpi::util::UnpackCallback<std::string> DescriptionCb;
mrc_proto_ProtobufOpMode Msg;
Msg.Name = NameCb.Callback();
Msg.Group = GroupCb.Callback();
Msg.Description = DescriptionCb.Callback();
if (!Stream.Decode(Msg)) {
return {};
}
auto Name = NameCb.Items();
auto Group = GroupCb.Items();
auto Description = DescriptionCb.Items();
if (Name.empty()) {
if (Name.empty() || Group.empty() || Description.empty()) {
return {};
}
mrc::OpMode OutputData;
OutputData.MoveName(std::move(Name[0]));
OutputData.Hash = mrc::OpModeHash::FromValue(Msg.Hash);
return OutputData;
return mrc::OpMode{
mrc::OpModeHash::FromValue(Msg.Hash),
std::move(Name[0]),
std::move(Group[0]),
std::move(Description[0]),
Msg.TextColor,
Msg.BackgroundColor,
};
}
bool wpi::util::Protobuf<mrc::OpMode>::Pack(OutputStream& Stream,
const mrc::OpMode& Value) {
std::string_view EventNameStr = Value.GetName();
wpi::util::PackCallback EventName{&EventNameStr};
std::string_view EventGroupStr = Value.GetGroup();
wpi::util::PackCallback EventGroup{&EventGroupStr};
std::string_view EventDescriptionStr = Value.GetDescription();
wpi::util::PackCallback EventDescription{&EventDescriptionStr};
mrc_proto_ProtobufOpMode Msg{
.Hash = Value.Hash.ToValue(),
.Name = EventName.Callback(),
.Group = EventGroup.Callback(),
.Description = EventDescription.Callback(),
.TextColor = Value.GetTextColor(),
.BackgroundColor = Value.GetBackgroundColor(),
};
return Stream.Encode(Msg);

19
hal/src/main/native/include/wpi/hal/DashboardOpMode.hpp Normal file
View File

@@ -0,0 +1,19 @@
// 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.
#pragma once
#include <span>
#include "wpi/hal/DriverStationTypes.h"
namespace wpi::hal {
void InitializeDashboardOpMode();
void SetDashboardOpModeOptions(std::span<const HAL_OpModeOption> options);
void StartDashboardOpMode();
void EnableDashboardOpMode();
int64_t GetDashboardSelectedOpMode(HAL_RobotMode robotMode);
} // namespace wpi::hal

67
hal/src/main/native/include/wpi/hal/DriverStation.h
View File

@@ -64,6 +64,28 @@ int32_t HAL_SendConsoleLine(const char* line);
*/
int32_t HAL_GetControlWord(HAL_ControlWord* controlWord);
/**
* Gets the current control word of the driver station. Unlike
* HAL_GetControlWord, this function gets the latest value rather than using the
* value cached by HAL_RefreshDSData().
*
* The control word contains the robot state.
*
* @param controlWord the control word (out)
* @return the error code, or 0 for success
*/
int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord);
/**
* Sets operating mode options.
*
* @param options array of operating mode options
* @param count number of options in the array
* @return the error code, or 0 for success
*/
int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count);
/**
* Gets the current alliance station ID.
*
@@ -264,42 +286,31 @@ void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle);
void HAL_ObserveUserProgramStarting(void);
/**
* Sets the disabled flag in the DS.
* Sets the control word state returned to the DS.
*
* This is used for the DS to ensure the robot is properly responding to its
* state request. Ensure this gets called about every 50ms, or the robot will be
* disabled by the DS.
*/
void HAL_ObserveUserProgramDisabled(void);
/**
* Sets the autonomous enabled flag in the DS.
*
* This is used for the DS to ensure the robot is properly responding to its
* state request. Ensure this gets called about every 50ms, or the robot will be
* disabled by the DS.
* @param word control word returned by HAL_GetControlWord
*/
void HAL_ObserveUserProgramAutonomous(void);
/**
* Sets the teleoperated enabled flag in the DS.
*
* This is used for the DS to ensure the robot is properly responding to its
* state request. Ensure this gets called about every 50ms, or the robot will be
* disabled by the DS.
*/
void HAL_ObserveUserProgramTeleop(void);
/**
* Sets the test mode flag in the DS.
*
* This is used for the DS to ensure the robot is properly responding to its
* state request. Ensure this gets called about every 50ms, or the robot will be
* disabled by the DS.
*/
void HAL_ObserveUserProgramTest(void);
void HAL_ObserveUserProgram(HAL_ControlWord word);
#ifdef __cplusplus
} // extern "C"
namespace wpi::hal {
inline ControlWord GetControlWord() {
HAL_ControlWord word;
HAL_GetControlWord(&word);
return ControlWord{word};
}
inline ControlWord GetUncachedControlWord() {
HAL_ControlWord word;
HAL_GetUncachedControlWord(&word);
return ControlWord{word};
}
} // namespace wpi::hal
#endif
/** @} */

364
hal/src/main/native/include/wpi/hal/DriverStationTypes.h
View File

@@ -7,6 +7,11 @@
#include <stdint.h>
#include "wpi/hal/Types.h"
#include "wpi/util/string.h"
#ifdef __cplusplus
#include "wpi/util/struct/Struct.hpp"
#endif // __cplusplus
/**
* @defgroup hal_driverstation Driver Station Functions
@@ -14,15 +19,16 @@
* @{
*/
#define HAL_CONTROLWORD_OPMODE_HASH_MASK 0x00FFFFFFFFFFFFFFLL
#define HAL_CONTROLWORD_ROBOT_MODE_MASK 0x0300000000000000LL
#define HAL_CONTROLWORD_ROBOT_MODE_SHIFT 56
#define HAL_CONTROLWORD_ENABLED_MASK 0x0400000000000000LL
#define HAL_CONTROLWORD_ESTOP_MASK 0x0800000000000000LL
#define HAL_CONTROLWORD_FMS_ATTACHED_MASK 0x1000000000000000LL
#define HAL_CONTROLWORD_DS_ATTACHED_MASK 0x2000000000000000LL
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 watchdogEnabled : 1;
uint32_t control_reserved : 25;
int64_t value;
};
typedef struct HAL_ControlWord HAL_ControlWord;
@@ -50,6 +56,13 @@ HAL_ENUM(HAL_MatchType) {
HAL_kMatchType_elimination,
};
HAL_ENUM(HAL_RobotMode) {
HAL_ROBOTMODE_UNKNOWN = 0,
HAL_ROBOTMODE_AUTONOMOUS,
HAL_ROBOTMODE_TELEOPERATED,
HAL_ROBOTMODE_TEST,
};
/**
* The maximum number of touchpads that will be stored in a single
* HAL_JoystickTouchpads struct. This is used for allocating buffers, not
@@ -157,4 +170,339 @@ struct HAL_MatchInfo {
uint16_t gameSpecificMessageSize;
};
typedef struct HAL_MatchInfo HAL_MatchInfo;
#define HAL_OPMODE_HASH_MASK HAL_CONTROLWORD_OPMODE_HASH_MASK
#define HAL_OPMODE_ROBOT_MODE_MASK HAL_CONTROLWORD_ROBOT_MODE_MASK
#define HAL_OPMODE_ROBOT_MODE_SHIFT HAL_CONTROLWORD_ROBOT_MODE_SHIFT
struct HAL_OpModeOption {
int64_t id; // encodes robot mode in bits 57-56, LSB 56 bits is hash of name
struct WPI_String name;
struct WPI_String group;
struct WPI_String description;
int32_t textColor; // 0x00RRGGBB or -1 for default
int32_t backgroundColor; // 0x00RRGGBB or -1 for default
};
typedef struct HAL_OpModeOption HAL_OpModeOption;
#ifdef __cplusplus
extern "C" {
#endif // __cplusplus
inline HAL_ControlWord HAL_MakeControlWord(int64_t opModeHash,
HAL_RobotMode robotMode,
HAL_Bool enabled, HAL_Bool eStop,
HAL_Bool fmsAttached,
HAL_Bool dsAttached) {
HAL_ControlWord word;
word.value =
(opModeHash & HAL_CONTROLWORD_OPMODE_HASH_MASK) |
(((uint64_t)(robotMode) << HAL_CONTROLWORD_ROBOT_MODE_SHIFT) & // NOLINT
HAL_CONTROLWORD_ROBOT_MODE_MASK) |
(enabled ? HAL_CONTROLWORD_ENABLED_MASK : 0) |
(eStop ? HAL_CONTROLWORD_ESTOP_MASK : 0) |
(fmsAttached ? HAL_CONTROLWORD_FMS_ATTACHED_MASK : 0) |
(dsAttached ? HAL_CONTROLWORD_DS_ATTACHED_MASK : 0);
return word;
}
inline int64_t HAL_ControlWord_GetOpModeHash(HAL_ControlWord word) {
return word.value & HAL_CONTROLWORD_OPMODE_HASH_MASK;
}
inline int64_t HAL_ControlWord_GetOpModeId(HAL_ControlWord word) {
// if the hash portion is zero, return 0
if ((word.value & HAL_CONTROLWORD_OPMODE_HASH_MASK) == 0) {
return 0;
}
// otherwise return the full ID (which includes the robot mode)
return word.value &
(HAL_CONTROLWORD_OPMODE_HASH_MASK | HAL_CONTROLWORD_ROBOT_MODE_MASK);
}
inline void HAL_ControlWord_SetOpModeId(HAL_ControlWord* word, int64_t id) {
// clear out the old hash and robot mode
word->value &=
~(HAL_CONTROLWORD_OPMODE_HASH_MASK | HAL_CONTROLWORD_ROBOT_MODE_MASK);
// set the new id
word->value |=
id & (HAL_CONTROLWORD_OPMODE_HASH_MASK | HAL_CONTROLWORD_ROBOT_MODE_MASK);
}
inline HAL_RobotMode HAL_ControlWord_GetRobotMode(HAL_ControlWord word) {
// NOLINTBEGIN
return (HAL_RobotMode)((word.value & HAL_CONTROLWORD_ROBOT_MODE_MASK) >>
HAL_CONTROLWORD_ROBOT_MODE_SHIFT);
// NOLINTEND
}
inline HAL_Bool HAL_ControlWord_IsEnabled(HAL_ControlWord word) {
return (word.value & HAL_CONTROLWORD_ENABLED_MASK) != 0;
}
inline HAL_Bool HAL_ControlWord_IsEStopped(HAL_ControlWord word) {
return (word.value & HAL_CONTROLWORD_ESTOP_MASK) != 0;
}
inline HAL_Bool HAL_ControlWord_IsFMSAttached(HAL_ControlWord word) {
return (word.value & HAL_CONTROLWORD_FMS_ATTACHED_MASK) != 0;
}
inline HAL_Bool HAL_ControlWord_IsDSAttached(HAL_ControlWord word) {
return (word.value & HAL_CONTROLWORD_DS_ATTACHED_MASK) != 0;
}
// NOLINTBEGIN
// for use at compile time
#define HAL_MAKE_OPMODEID(mode, hash) \
((((int64_t)(mode) << HAL_OPMODE_ROBOT_MODE_SHIFT) & \
HAL_OPMODE_ROBOT_MODE_MASK) | \
((hash) & HAL_OPMODE_HASH_MASK))
// NOLINTEND
inline int64_t HAL_MakeOpModeId(HAL_RobotMode mode, int64_t hash) {
return HAL_MAKE_OPMODEID(mode, hash);
}
inline HAL_RobotMode HAL_OpMode_GetRobotMode(int64_t id) {
return (HAL_RobotMode)((id & HAL_OPMODE_ROBOT_MODE_MASK) >> // NOLINT
HAL_OPMODE_ROBOT_MODE_SHIFT);
}
inline int64_t HAL_OpMode_GetHash(int64_t id) {
return id & HAL_OPMODE_HASH_MASK;
}
#ifdef __cplusplus
} // extern "C"
#endif // __cplusplus
/** @} */
#ifdef __cplusplus
namespace wpi::hal {
/**
* The overall robot mode (not including enabled state).
*/
enum class RobotMode {
/// Unknown.
UNKNOWN = HAL_ROBOTMODE_UNKNOWN,
/// Autonomous.
AUTONOMOUS = HAL_ROBOTMODE_AUTONOMOUS,
/// Qualification.
TELEOPERATED = HAL_ROBOTMODE_TELEOPERATED,
/// Elimination.
TEST = HAL_ROBOTMODE_TEST
};
/**
* A wrapper around Driver Station control word.
*/
class ControlWord {
public:
/**
* Default constructor.
*/
ControlWord() = default;
/**
* Constructs from state values.
*
* @param opModeHash opmode hash
* @param robotMode robot mode
* @param enabled enabled
* @param eStop emergency stopped
* @param fmsAttached FMS attached
* @param dsAttached DS attached
*/
ControlWord(int64_t opModeHash, RobotMode robotMode, bool enabled, bool eStop,
bool fmsAttached, bool dsAttached)
: m_word{HAL_MakeControlWord(opModeHash,
static_cast<HAL_RobotMode>(robotMode),
enabled, eStop, fmsAttached, dsAttached)} {}
/**
* Constructs from the native HAL value.
*
* @param word value
*/
explicit ControlWord(HAL_ControlWord word) : m_word{word} {}
ControlWord(const ControlWord&) = default;
ControlWord& operator=(const ControlWord&) = default;
/**
* Updates from state values.
*
* @param opModeHash opmode hash
* @param robotMode robot mode
* @param enabled enabled
* @param eStop emergency stopped
* @param fmsAttached FMS attached
* @param dsAttached DS attached
*/
void Update(int64_t opModeHash, RobotMode robotMode, bool enabled, bool eStop,
bool fmsAttached, bool dsAttached) {
m_word =
HAL_MakeControlWord(opModeHash, static_cast<HAL_RobotMode>(robotMode),
enabled, eStop, fmsAttached, dsAttached);
}
/**
* Updates from the native HAL value.
*
* @param word value
*/
void Update(HAL_ControlWord word) { m_word = word; }
/**
* Check if the DS has enabled the robot.
*
* @return True if the robot is enabled and the DS is connected
*/
bool IsEnabled() const { return HAL_ControlWord_IsEnabled(m_word); }
/**
* Gets the current robot mode.
*
* <p>Note that this does not indicate whether the robot is enabled or
* disabled.
*
* @return robot mode
*/
RobotMode GetRobotMode() const {
return static_cast<RobotMode>(HAL_ControlWord_GetRobotMode(m_word));
}
/**
* Gets the current opmode ID.
*
* @return opmode
*/
int64_t GetOpModeId() const { return HAL_ControlWord_GetOpModeId(m_word); }
/**
* Sets the opmode ID.
*
* @param id opmode ID
*/
void SetOpModeId(int64_t id) { HAL_ControlWord_SetOpModeId(&m_word, id); }
/**
* Check if the robot is e-stopped.
*
* @return True if the robot is e-stopped
*/
bool IsEStopped() const { return HAL_ControlWord_IsEStopped(m_word); }
/**
* Is the driver station attached to a Field Management System?
*
* @return True if the robot is competing on a field being controlled by a
* Field Management System
*/
bool IsFMSAttached() const { return HAL_ControlWord_IsFMSAttached(m_word); }
/**
* Check if the DS is attached.
*
* @return True if the DS is connected to the robot
*/
bool IsDSAttached() const { return HAL_ControlWord_IsDSAttached(m_word); }
/**
* Check if the DS is commanding autonomous mode.
*
* @return True if the robot is being commanded to be in autonomous mode
*/
bool IsAutonomous() const { return GetRobotMode() == RobotMode::AUTONOMOUS; }
/**
* Check if the DS is commanding autonomous mode and if it has enabled the
* robot.
*
* @return True if the robot is being commanded to be in autonomous mode and
* enabled.
*/
bool IsAutonomousEnabled() const {
return IsAutonomous() && IsEnabled() && IsDSAttached();
}
/**
* Check if the DS is commanding teleop mode.
*
* @return True if the robot is being commanded to be in teleop mode
*/
bool IsTeleop() const { return GetRobotMode() == RobotMode::TELEOPERATED; }
/**
* Check if the DS is commanding teleop mode and if it has enabled the robot.
*
* @return True if the robot is being commanded to be in teleop mode and
* enabled.
*/
bool IsTeleopEnabled() const {
return IsTeleop() && IsEnabled() && IsDSAttached();
}
/**
* Check if the DS is commanding test mode.
*
* @return True if the robot is being commanded to be in test mode
*/
bool IsTest() const { return GetRobotMode() == RobotMode::TEST; }
/**
* Check if the DS is commanding test mode and if it has enabled the robot.
*
* @return True if the robot is being commanded to be in test mode and
* enabled.
*/
bool IsTestEnabled() const {
return IsTest() && IsEnabled() && IsDSAttached();
}
/**
* Get the HAL raw value.
*
* @return Control word value
*/
HAL_ControlWord GetValue() const { return m_word; }
private:
HAL_ControlWord m_word{.value = 0};
};
inline bool operator==(const ControlWord& lhs, const ControlWord& rhs) {
return lhs.GetValue().value == rhs.GetValue().value;
}
inline bool operator!=(const ControlWord& lhs, const ControlWord& rhs) {
return !(lhs == rhs);
}
} // namespace wpi::hal
template <>
struct wpi::util::Struct<wpi::hal::ControlWord> {
static constexpr std::string_view GetTypeName() { return "ControlWord"; }
static constexpr size_t GetSize() { return 8; }
static constexpr std::string_view GetSchema() {
return "uint64 opModeHash:56;"
"enum{unknown=0,autonomous=1,teleoperated=2,test=3}"
"uint64 robotMode:2;"
"bool enabled:1;bool eStop:1;bool fmsAttached:1;bool dsAttached:1;";
}
static inline wpi::hal::ControlWord Unpack(std::span<const uint8_t> data) {
return wpi::hal::ControlWord{
{.value = wpi::util::UnpackStruct<int64_t>(data)}};
}
static inline void Pack(std::span<uint8_t> data,
wpi::hal::ControlWord value) {
wpi::util::PackStruct(data, value.GetValue().value);
}
};
static_assert(wpi::util::StructSerializable<wpi::hal::ControlWord>);
#endif // __cplusplus

34
hal/src/main/native/include/wpi/hal/simulation/DriverStationData.h
View File

@@ -11,6 +11,9 @@
#include "wpi/hal/simulation/NotifyListener.h"
#include "wpi/util/string.h"
typedef void (*HAL_OpModeOptionsCallback)(const char* name, void* param,
const HAL_OpModeOption* opmodes,
int32_t count);
typedef void (*HAL_JoystickAxesCallback)(const char* name, void* param,
int32_t joystickNum,
const HAL_JoystickAxes* axes);
@@ -39,6 +42,7 @@ extern "C" {
#endif
void HALSIM_ResetDriverStationData(void);
int32_t HALSIM_RegisterDriverStationEnabledCallback(HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify);
@@ -46,18 +50,11 @@ void HALSIM_CancelDriverStationEnabledCallback(int32_t uid);
HAL_Bool HALSIM_GetDriverStationEnabled(void);
void HALSIM_SetDriverStationEnabled(HAL_Bool enabled);
int32_t HALSIM_RegisterDriverStationAutonomousCallback(
int32_t HALSIM_RegisterDriverStationRobotModeCallback(
HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
void HALSIM_CancelDriverStationAutonomousCallback(int32_t uid);
HAL_Bool HALSIM_GetDriverStationAutonomous(void);
void HALSIM_SetDriverStationAutonomous(HAL_Bool autonomous);
int32_t HALSIM_RegisterDriverStationTestCallback(HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify);
void HALSIM_CancelDriverStationTestCallback(int32_t uid);
HAL_Bool HALSIM_GetDriverStationTest(void);
void HALSIM_SetDriverStationTest(HAL_Bool test);
void HALSIM_CancelDriverStationRobotModeCallback(int32_t uid);
HAL_RobotMode HALSIM_GetDriverStationRobotMode(void);
void HALSIM_SetDriverStationRobotMode(HAL_RobotMode mode);
int32_t HALSIM_RegisterDriverStationEStopCallback(HAL_NotifyCallback callback,
void* param,
@@ -91,6 +88,21 @@ void HALSIM_CancelDriverStationMatchTimeCallback(int32_t uid);
double HALSIM_GetDriverStationMatchTime(void);
void HALSIM_SetDriverStationMatchTime(double matchTime);
int32_t HALSIM_RegisterDriverStationOpModeCallback(HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify);
void HALSIM_CancelDriverStationOpModeCallback(int32_t uid);
int64_t HALSIM_GetDriverStationOpMode(void);
void HALSIM_SetDriverStationOpMode(int64_t opmode);
int32_t HALSIM_RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback,
void* param,
HAL_Bool initialNotify);
void HALSIM_CancelOpModeOptionsCallback(int32_t uid);
struct HAL_OpModeOption* HALSIM_GetOpModeOptions(int32_t* len);
void HALSIM_FreeOpModeOptionsArray(struct HAL_OpModeOption* arr, size_t length);
int32_t HALSIM_RegisterJoystickAxesCallback(int32_t joystickNum,
HAL_JoystickAxesCallback callback,
void* param,

21
hal/src/main/native/include/wpi/hal/simulation/MockHooks.h
View File

@@ -4,14 +4,21 @@
#pragma once
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/HALBase.h"
#include "wpi/hal/Types.h"
#ifdef __cplusplus
extern "C" {
#endif
void HALSIM_SetRuntimeType(HAL_RuntimeType type);
void HALSIM_WaitForProgramStart(void);
void HALSIM_SetProgramStarted(HAL_Bool started);
HAL_Bool HALSIM_GetProgramStarted(void);
void HALSIM_SetProgramState(HAL_ControlWord controlWord);
void HALSIM_GetProgramState(HAL_ControlWord* controlWord);
void HALSIM_RestartTiming(void);
void HALSIM_PauseTiming(void);
void HALSIM_ResumeTiming(void);
@@ -38,4 +45,18 @@ void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid);
void HALSIM_CancelAllSimPeriodicCallbacks(void);
#ifdef __cplusplus
} // extern "C"
namespace wpi::hal::sim {
inline void SetProgramState(const ControlWord& controlWord) {
HALSIM_SetProgramState(controlWord.GetValue());
}
inline ControlWord GetProgramState() {
HAL_ControlWord word;
HALSIM_GetProgramState(&word);
return ControlWord{word};
}
} // namespace wpi::hal::sim
#endif // __cplusplus

67
hal/src/main/native/sim/DriverStation.cpp
View File

@@ -18,11 +18,13 @@
#include "HALInitializer.h"
#include "mockdata/DriverStationDataInternal.h"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/Errors.h"
#include "wpi/hal/cpp/fpga_clock.h"
#include "wpi/hal/simulation/MockHooks.h"
#include "wpi/util/EventVector.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/string.h"
static wpi::util::mutex msgMutex;
static std::atomic<HALSIM_SendErrorHandler> sendErrorHandler{nullptr};
@@ -69,15 +71,10 @@ void JoystickDataCache::Update() {
allianceStation = SimDriverStationData->allianceStationId;
matchTime = SimDriverStationData->matchTime;
HAL_ControlWord tmpControlWord;
std::memset(&tmpControlWord, 0, sizeof(tmpControlWord));
tmpControlWord.enabled = SimDriverStationData->enabled;
tmpControlWord.autonomous = SimDriverStationData->autonomous;
tmpControlWord.test = SimDriverStationData->test;
tmpControlWord.eStop = SimDriverStationData->eStop;
tmpControlWord.fmsAttached = SimDriverStationData->fmsAttached;
tmpControlWord.dsAttached = SimDriverStationData->dsAttached;
this->controlWord = tmpControlWord;
controlWord = HAL_MakeControlWord(
SimDriverStationData->opMode, SimDriverStationData->robotMode,
SimDriverStationData->enabled, SimDriverStationData->eStop,
SimDriverStationData->fmsAttached, SimDriverStationData->dsAttached);
}
#define CHECK_JOYSTICK_NUMBER(stickNum) \
@@ -221,6 +218,7 @@ int32_t HAL_SendConsoleLine(const char* line) {
int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
if (gShutdown) {
controlWord->value = 0;
return INCOMPATIBLE_STATE;
}
std::scoped_lock lock{driverStation->cacheMutex};
@@ -228,6 +226,35 @@ int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
return 0;
}
int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) {
if (gShutdown) {
controlWord->value = 0;
return INCOMPATIBLE_STATE;
}
bool dsAttached = SimDriverStationData->dsAttached;
if (dsAttached) {
*controlWord = HAL_MakeControlWord(
SimDriverStationData->opMode, SimDriverStationData->robotMode,
SimDriverStationData->enabled, SimDriverStationData->eStop,
SimDriverStationData->fmsAttached, SimDriverStationData->dsAttached);
} else {
controlWord->value = 0;
}
return 0;
}
int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) {
if (gShutdown) {
return 0;
}
if (count < 0 || count > 1000 || (count != 0 && !options)) {
return PARAMETER_OUT_OF_RANGE;
}
SimDriverStationData->SetOpModeOptions({options, options + count});
return 0;
}
HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
if (gShutdown) {
return HAL_AllianceStationID_kUnknown;
@@ -370,20 +397,8 @@ void HAL_ObserveUserProgramStarting(void) {
HALSIM_SetProgramStarted(true);
}
void HAL_ObserveUserProgramDisabled(void) {
// TODO
}
void HAL_ObserveUserProgramAutonomous(void) {
// TODO
}
void HAL_ObserveUserProgramTeleop(void) {
// TODO
}
void HAL_ObserveUserProgramTest(void) {
// TODO
void HAL_ObserveUserProgram(HAL_ControlWord word) {
HALSIM_SetProgramState(word);
}
HAL_Bool HAL_RefreshDSData(void) {
@@ -415,8 +430,7 @@ HAL_Bool HAL_RefreshDSData(void) {
// Also, when the DS has never been connected the rest of the fields
// in control word are garbage, so we also need to zero out in that
// case too
std::memset(&currentRead->controlWord, 0,
sizeof(currentRead->controlWord));
currentRead->controlWord.value = 0;
}
newestControlWord = currentRead->controlWord;
}
@@ -450,7 +464,8 @@ HAL_Bool HAL_GetOutputsEnabled(void) {
return false;
}
std::scoped_lock lock{driverStation->cacheMutex};
return newestControlWord.enabled && newestControlWord.dsAttached;
return HAL_ControlWord_IsEnabled(newestControlWord) &&
HAL_ControlWord_IsDSAttached(newestControlWord);
}
} // extern "C"

9
hal/src/main/native/sim/MockHooks.cpp
View File

@@ -15,6 +15,7 @@
#include "wpi/util/timestamp.h"
static std::atomic<bool> programStarted{false};
static std::atomic<int64_t> programState{0};
static std::atomic<uint64_t> programStartTime{0};
static std::atomic<uint64_t> programPauseTime{0};
@@ -98,6 +99,14 @@ HAL_Bool HALSIM_GetProgramStarted(void) {
return GetProgramStarted();
}
void HALSIM_SetProgramState(HAL_ControlWord controlWord) {
programState = controlWord.value;
}
void HALSIM_GetProgramState(HAL_ControlWord* controlWord) {
controlWord->value = programState;
}
void HALSIM_RestartTiming(void) {
RestartTiming();
}

123
hal/src/main/native/sim/mockdata/DriverStationData.cpp
View File

@@ -2,14 +2,30 @@
// 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/hal/simulation/DriverStationData.h"
#include <algorithm>
#include <cstring>
#include <string>
#include <vector>
#include <fmt/format.h>
#include "DriverStationDataInternal.h"
#include "wpi/hal/DashboardOpMode.hpp"
#include "wpi/hal/DriverStationTypes.h"
using namespace wpi::hal;
static void FreeOpModeOption(HAL_OpModeOption& option) {
WPI_FreeString(&option.name);
WPI_FreeString(&option.group);
WPI_FreeString(&option.description);
}
namespace wpi::hal::init {
void InitializeDriverStationData() {
wpi::hal::InitializeDashboardOpMode();
static DriverStationData dsd;
::wpi::hal::SimDriverStationData = &dsd;
}
@@ -21,15 +37,21 @@ DriverStationData::DriverStationData() {
ResetData();
}
DriverStationData::~DriverStationData() {
for (auto&& option : m_opModeOptions) {
FreeOpModeOption(option);
}
}
void DriverStationData::ResetData() {
enabled.Reset(false);
autonomous.Reset(false);
test.Reset(false);
robotMode.Reset(HAL_ROBOTMODE_UNKNOWN);
eStop.Reset(false);
fmsAttached.Reset(false);
dsAttached.Reset(false);
allianceStationId.Reset(static_cast<HAL_AllianceStationID>(0));
matchTime.Reset(-1.0);
opMode.Reset(0);
{
std::scoped_lock lock(m_joystickDataMutex);
@@ -61,7 +83,72 @@ void DriverStationData::ResetData() {
m_matchInfoCallbacks.Reset();
m_matchInfo = HAL_MatchInfo{};
}
{
std::scoped_lock lock{m_opModeMutex};
m_opModeOptionsCallbacks.Reset();
// XXX: do not clear options vector as it comes from robot code?
}
m_newDataCallbacks.Reset();
wpi::hal::SetDashboardOpModeOptions({});
}
void DriverStationData::SetOpModeOptions(
std::span<const HAL_OpModeOption> options) {
std::scoped_lock lock{m_opModeMutex};
for (auto&& option : m_opModeOptions) {
FreeOpModeOption(option);
}
m_opModeOptions.clear();
m_opModeOptions.reserve(options.size());
for (const auto& option : options) {
if (option.id == 0) {
continue;
}
m_opModeOptions.emplace_back(
HAL_OpModeOption{static_cast<int64_t>(option.id),
wpi::util::copy_wpi_string(option.name),
wpi::util::copy_wpi_string(option.group),
wpi::util::copy_wpi_string(option.description),
option.textColor, option.backgroundColor});
}
m_opModeOptionsCallbacks.Invoke(m_opModeOptions.data(),
m_opModeOptions.size());
wpi::hal::SetDashboardOpModeOptions(options);
}
int32_t DriverStationData::RegisterOpModeOptionsCallback(
HAL_OpModeOptionsCallback callback, void* param, HAL_Bool initialNotify) {
std::scoped_lock lock(m_opModeMutex);
int32_t uid = m_opModeOptionsCallbacks.Register(callback, param);
if (initialNotify) {
callback(GetOpModeOptionsName(), param, m_opModeOptions.data(),
m_opModeOptions.size());
}
return uid;
}
void DriverStationData::CancelOpModeOptionsCallback(int32_t uid) {
m_opModeOptionsCallbacks.Cancel(uid);
}
HAL_OpModeOption* DriverStationData::GetOpModeOptions(int32_t* len) {
std::scoped_lock lock(m_opModeMutex);
*len = 0;
if (m_opModeOptions.empty()) {
return nullptr;
}
auto options = static_cast<HAL_OpModeOption*>(
std::malloc(sizeof(HAL_OpModeOption) * m_opModeOptions.size()));
std::copy(m_opModeOptions.begin(), m_opModeOptions.end(), options);
*len = m_opModeOptions.size();
for (auto&& option : std::span{options, m_opModeOptions.size()}) {
option.name = wpi::util::copy_wpi_string(option.name);
option.group = wpi::util::copy_wpi_string(option.group);
option.description = wpi::util::copy_wpi_string(option.description);
}
return options;
}
#define DEFINE_CPPAPI_CALLBACKS(name, data, data2) \
@@ -495,13 +582,38 @@ void HALSIM_ResetDriverStationData(void) {
SimDriverStationData, LOWERNAME)
DEFINE_CAPI(HAL_Bool, Enabled, enabled)
DEFINE_CAPI(HAL_Bool, Autonomous, autonomous)
DEFINE_CAPI(HAL_Bool, Test, test)
DEFINE_CAPI(HAL_RobotMode, RobotMode, robotMode)
DEFINE_CAPI(HAL_Bool, EStop, eStop)
DEFINE_CAPI(HAL_Bool, FmsAttached, fmsAttached)
DEFINE_CAPI(HAL_Bool, DsAttached, dsAttached)
DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId, allianceStationId)
DEFINE_CAPI(double, MatchTime, matchTime)
DEFINE_CAPI(int64_t, OpMode, opMode)
int32_t HALSIM_RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback,
void* param,
HAL_Bool initialNotify) {
return SimDriverStationData->RegisterOpModeOptionsCallback(callback, param,
initialNotify);
}
void HALSIM_CancelOpModeOptionsCallback(int32_t uid) {
return SimDriverStationData->CancelOpModeOptionsCallback(uid);
}
struct HAL_OpModeOption* HALSIM_GetOpModeOptions(int32_t* len) {
return SimDriverStationData->GetOpModeOptions(len);
}
void HALSIM_FreeOpModeOptionsArray(struct HAL_OpModeOption* arr,
size_t length) {
for (size_t i = 0; i < length; ++i) {
WPI_FreeString(&arr[i].name);
WPI_FreeString(&arr[i].group);
WPI_FreeString(&arr[i].description);
}
std::free(arr);
}
#undef DEFINE_CAPI
#define DEFINE_CAPI(name, data) \
@@ -704,8 +816,7 @@ void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {
REGISTER(enabled);
REGISTER(autonomous);
REGISTER(test);
REGISTER(robotMode);
REGISTER(eStop);
REGISTER(fmsAttached);
REGISTER(dsAttached);

36
hal/src/main/native/sim/mockdata/DriverStationDataInternal.h
View File

@@ -4,8 +4,13 @@
#pragma once
#include <memory>
#include <stdint.h>
#include <span>
#include <string_view>
#include <vector>
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/hal/simulation/SimCallbackRegistry.h"
#include "wpi/hal/simulation/SimDataValue.h"
@@ -15,13 +20,14 @@ namespace wpi::hal {
class DriverStationData {
HAL_SIMDATAVALUE_DEFINE_NAME(Enabled)
HAL_SIMDATAVALUE_DEFINE_NAME(Autonomous)
HAL_SIMDATAVALUE_DEFINE_NAME(Test)
HAL_SIMDATAVALUE_DEFINE_NAME(RobotMode)
HAL_SIMDATAVALUE_DEFINE_NAME(EStop)
HAL_SIMDATAVALUE_DEFINE_NAME(FmsAttached)
HAL_SIMDATAVALUE_DEFINE_NAME(DsAttached)
HAL_SIMDATAVALUE_DEFINE_NAME(AllianceStationId)
HAL_SIMDATAVALUE_DEFINE_NAME(MatchTime)
HAL_SIMDATAVALUE_DEFINE_NAME(OpMode)
HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(OpModeOptions)
HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickAxes)
HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickPOVs)
HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickButtons)
@@ -36,11 +42,25 @@ class DriverStationData {
MakeAllianceStationIdValue(HAL_AllianceStationID value) {
return HAL_MakeEnum(value);
}
static LLVM_ATTRIBUTE_ALWAYS_INLINE HAL_Value
MakeRobotModeValue(HAL_RobotMode value) {
return HAL_MakeEnum(value);
}
public:
DriverStationData();
~DriverStationData();
DriverStationData(const DriverStationData&) = delete;
DriverStationData& operator=(const DriverStationData&) = delete;
void ResetData();
void SetOpModeOptions(std::span<const HAL_OpModeOption> options);
int32_t RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback,
void* param, HAL_Bool initialNotify);
void CancelOpModeOptionsCallback(int32_t uid);
HAL_OpModeOption* GetOpModeOptions(int32_t* len);
int32_t RegisterJoystickAxesCallback(int32_t joystickNum,
HAL_JoystickAxesCallback callback,
void* param, HAL_Bool initialNotify);
@@ -141,8 +161,8 @@ class DriverStationData {
void SetReplayNumber(int32_t replayNumber);
SimDataValue<HAL_Bool, HAL_MakeBoolean, GetEnabledName> enabled{false};
SimDataValue<HAL_Bool, HAL_MakeBoolean, GetAutonomousName> autonomous{false};
SimDataValue<HAL_Bool, HAL_MakeBoolean, GetTestName> test{false};
SimDataValue<HAL_RobotMode, MakeRobotModeValue, GetRobotModeName> robotMode{
HAL_ROBOTMODE_UNKNOWN};
SimDataValue<HAL_Bool, HAL_MakeBoolean, GetEStopName> eStop{false};
SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFmsAttachedName> fmsAttached{
false};
@@ -151,8 +171,11 @@ class DriverStationData {
GetAllianceStationIdName>
allianceStationId{static_cast<HAL_AllianceStationID>(0)};
SimDataValue<double, HAL_MakeDouble, GetMatchTimeName> matchTime{-1.0};
SimDataValue<int64_t, HAL_MakeLong, GetOpModeName> opMode{0};
private:
SimCallbackRegistry<HAL_OpModeOptionsCallback, GetOpModeOptionsName>
m_opModeOptionsCallbacks;
SimCallbackRegistry<HAL_JoystickAxesCallback, GetJoystickAxesName>
m_joystickAxesCallbacks;
SimCallbackRegistry<HAL_JoystickPOVsCallback, GetJoystickPOVsName>
@@ -194,6 +217,9 @@ class DriverStationData {
wpi::util::spinlock m_matchInfoMutex;
HAL_MatchInfo m_matchInfo;
wpi::util::spinlock m_opModeMutex;
std::vector<HAL_OpModeOption> m_opModeOptions;
};
extern DriverStationData* SimDriverStationData;
} // namespace wpi::hal

147
hal/src/main/native/systemcore/FRCDriverStation.cpp
View File

@@ -18,7 +18,9 @@
#include "HALInitializer.h"
#include "SystemServerInternal.h"
#include "mrc/NtNetComm.h"
#include "wpi/hal/DashboardOpMode.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/Errors.h"
#include "wpi/hal/proto/ControlData.h"
#include "wpi/hal/proto/ErrorInfo.h"
@@ -37,6 +39,7 @@
#include "wpi/util/SmallVector.hpp"
#include "wpi/util/condition_variable.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/string.h"
#include "wpi/util/timestamp.h"
static_assert(sizeof(int32_t) >= sizeof(int),
@@ -86,9 +89,7 @@ struct SystemServerDriverStation {
MRC_MAX_NUM_JOYSTICKS>
joystickOutputTopics;
wpi::nt::ProtobufPublisher<std::vector<mrc::OpMode>> teleopOpModes;
wpi::nt::ProtobufPublisher<std::vector<mrc::OpMode>> autoOpModes;
wpi::nt::ProtobufPublisher<std::vector<mrc::OpMode>> testOpModes;
wpi::nt::ProtobufPublisher<std::vector<mrc::OpMode>> opModeOptionsPublisher;
wpi::nt::IntegerPublisher traceOpModePublisher;
NT_Listener controlDataListener;
@@ -151,33 +152,11 @@ struct SystemServerDriverStation {
ROBOT_JOYSTICK_DESCRIPTORS_PATH)
.Subscribe({});
teleopOpModes = ntInst
.GetProtobufTopic<std::vector<mrc::OpMode>>(
ROBOT_TELEOP_OP_MODES_PATH)
.Publish();
autoOpModes = ntInst
.GetProtobufTopic<std::vector<mrc::OpMode>>(
ROBOT_AUTO_OP_MODES_PATH)
.Publish();
testOpModes = ntInst
.GetProtobufTopic<std::vector<mrc::OpMode>>(
ROBOT_TEST_OP_MODES_PATH)
.Publish();
std::vector<mrc::OpMode> staticTeleopOpModes;
staticTeleopOpModes.emplace_back(
mrc::OpMode{"TeleOp", mrc::OpModeHash::MakeTele(2)});
teleopOpModes.Set(staticTeleopOpModes);
std::vector<mrc::OpMode> staticAutoOpModes;
staticAutoOpModes.emplace_back(
mrc::OpMode{"Auto", mrc::OpModeHash::MakeAuto(1)});
autoOpModes.Set(staticAutoOpModes);
std::vector<mrc::OpMode> staticTestOpModes;
staticTestOpModes.emplace_back(
mrc::OpMode{"Test", mrc::OpModeHash::MakeTest(3)});
testOpModes.Set(staticTestOpModes);
opModeOptionsPublisher = ntInst
.GetProtobufTopic<std::vector<mrc::OpMode>>(
ROBOT_OP_MODE_OPTIONS_PATH)
.Publish();
opModeOptionsPublisher.Set({});
controlDataListener = ntInst.AddListener(
controlDataSubscriber, NT_EVENT_VALUE_REMOTE | NT_EVENT_UNPUBLISH,
@@ -243,13 +222,20 @@ void JoystickDataCache::Update(const mrc::ControlData& data) {
allianceInt += 1;
allianceStation = static_cast<HAL_AllianceStationID>(allianceInt);
std::memset(&controlWord, 0, sizeof(controlWord));
controlWord.enabled = data.ControlWord.Enabled;
controlWord.fmsAttached = data.ControlWord.FmsConnected;
controlWord.dsAttached = data.ControlWord.DsConnected;
controlWord.eStop = data.ControlWord.EStop;
controlWord.test = data.ControlWord.Test;
controlWord.autonomous = data.ControlWord.Auto;
if (data.ControlWord.SupportsOpModes) {
controlWord = HAL_MakeControlWord(
data.CurrentOpMode.ToValue(),
static_cast<HAL_RobotMode>(data.ControlWord.RobotMode),
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
} else {
wpi::hal::EnableDashboardOpMode();
auto robotMode = static_cast<HAL_RobotMode>(data.ControlWord.RobotMode);
controlWord = HAL_MakeControlWord(
wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode,
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
}
auto sticks = data.Joysticks();
@@ -373,7 +359,8 @@ void TcpCache::Update() {
namespace wpi::hal::init {
void InitializeFRCDriverStation() {
std::memset(&newestControlWord, 0, sizeof(newestControlWord));
InitializeDashboardOpMode();
newestControlWord.value = 0;
static FRCDriverStation ds;
driverStation = &ds;
}
@@ -478,6 +465,63 @@ int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
return 0;
}
int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) {
mrc::ControlData data;
int64_t dataTime{0};
bool dataValid = systemServerDs->GetLastControlData(&data, &dataTime);
if (dataValid && data.ControlWord.DsConnected) {
if (data.ControlWord.SupportsOpModes) {
*controlWord = HAL_MakeControlWord(
data.CurrentOpMode.ToValue(),
static_cast<HAL_RobotMode>(data.ControlWord.RobotMode),
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
} else {
wpi::hal::EnableDashboardOpMode();
auto robotMode = static_cast<HAL_RobotMode>(data.ControlWord.RobotMode);
*controlWord = HAL_MakeControlWord(
wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode,
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
}
} else {
// DS disconnected. Clear the control word
controlWord->value = 0;
}
return 0;
}
int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) {
if (count < 0 || count > 1000 || (count != 0 && !options)) {
return PARAMETER_OUT_OF_RANGE;
}
std::vector<mrc::OpMode> newOptions;
newOptions.reserve(count);
if (count != 0) {
for (auto&& option : std::span{options, options + count}) {
if (option.id == 0) {
continue;
}
newOptions.emplace_back(mrc::OpModeHash::FromValue(option.id),
wpi::util::to_string_view(&option.name),
wpi::util::to_string_view(&option.group),
wpi::util::to_string_view(&option.description),
option.textColor, option.backgroundColor);
}
}
{
std::scoped_lock lock{tcpCacheMutex};
systemServerDs->opModeOptionsPublisher.Set(newOptions);
}
wpi::hal::SetDashboardOpModeOptions({options, options + count});
return 0;
}
int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
@@ -619,24 +663,11 @@ void HAL_ObserveUserProgramStarting(void) {
systemServerDs->hasUserCodeReadyPublisher.Set(true);
}
void HAL_ObserveUserProgramDisabled(void) {
systemServerDs->traceOpModePublisher.Set(
mrc::OpModeHash::MakeTele(1, false).ToValue());
}
void HAL_ObserveUserProgramAutonomous(void) {
auto tVal = mrc::OpModeHash::MakeAuto(2, true).ToValue();
systemServerDs->traceOpModePublisher.Set(tVal);
}
void HAL_ObserveUserProgramTeleop(void) {
auto tVal = mrc::OpModeHash::MakeTele(1, true).ToValue();
systemServerDs->traceOpModePublisher.Set(tVal);
}
void HAL_ObserveUserProgramTest(void) {
systemServerDs->traceOpModePublisher.Set(
mrc::OpModeHash::MakeTest(3, true).ToValue());
void HAL_ObserveUserProgram(HAL_ControlWord word) {
systemServerDs->traceOpModePublisher.Set(word.value &
(HAL_CONTROLWORD_OPMODE_HASH_MASK |
HAL_CONTROLWORD_ROBOT_MODE_MASK |
HAL_CONTROLWORD_ENABLED_MASK));
}
HAL_Bool HAL_RefreshDSData(void) {
@@ -657,8 +688,7 @@ HAL_Bool HAL_RefreshDSData(void) {
updatedData = true;
} else {
// DS disconnected. Clear the control word
std::memset(&cacheToUpdate->controlWord, 0,
sizeof(cacheToUpdate->controlWord));
cacheToUpdate->controlWord.value = 0;
}
{
@@ -694,6 +724,7 @@ HAL_Bool HAL_GetSystemTimeValid(int32_t* status) {
namespace wpi::hal {
void InitializeDriverStation() {
StartDashboardOpMode();
systemServerDs = new ::SystemServerDriverStation{wpi::hal::GetSystemServer()};
}

21
hal/src/main/native/systemcore/mockdata/DriverStationData.cpp
View File

@@ -4,6 +4,7 @@
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/simulation/SimDataValue.h"
extern "C" {
@@ -14,14 +15,30 @@ void HALSIM_ResetDriverStationData(void) {}
RETURN)
DEFINE_CAPI(HAL_Bool, Enabled, false)
DEFINE_CAPI(HAL_Bool, Autonomous, false)
DEFINE_CAPI(HAL_Bool, Test, false)
DEFINE_CAPI(HAL_RobotMode, RobotMode, HAL_ROBOTMODE_UNKNOWN)
DEFINE_CAPI(HAL_Bool, EStop, false)
DEFINE_CAPI(HAL_Bool, FmsAttached, false)
DEFINE_CAPI(HAL_Bool, DsAttached, false)
DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId,
HAL_AllianceStationID_kRed1)
DEFINE_CAPI(double, MatchTime, 0)
DEFINE_CAPI(int64_t, OpMode, 0)
int32_t HALSIM_RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback,
void* param,
HAL_Bool initialNotify) {
return 0;
}
void HALSIM_CancelOpModeOptionsCallback(int32_t uid) {}
struct HAL_OpModeOption* HALSIM_GetOpModeOptions(int32_t* len) {
*len = 0;
return nullptr;
}
void HALSIM_FreeOpModeOptionsArray(struct HAL_OpModeOption* arr,
size_t length) {}
#undef DEFINE_CAPI
#define DEFINE_CAPI(name, data) \

6
hal/src/main/native/systemcore/mockdata/MockHooks.cpp
View File

@@ -16,6 +16,12 @@ HAL_Bool HALSIM_GetProgramStarted(void) {
return false;
}
void HALSIM_SetProgramState(HAL_ControlWord controlWord) {}
void HALSIM_GetProgramState(HAL_ControlWord* controlWord) {
controlWord->value = 0;
}
void HALSIM_RestartTiming(void) {}
void HALSIM_PauseTiming(void) {}