Add AddressableLED simulation support

This commit is contained in:
Peter Johnson
2019-11-17 15:05:56 -08:00
parent 8ed2059074
commit a4c9e4ec28
16 changed files with 750 additions and 10 deletions

View File

@@ -7,24 +7,119 @@
#include "hal/AddressableLED.h"
#include "DigitalInternal.h"
#include "HALInitializer.h"
#include "PortsInternal.h"
#include "hal/Errors.h"
#include "hal/handles/HandlesInternal.h"
#include "hal/handles/LimitedHandleResource.h"
#include "mockdata/AddressableLEDDataInternal.h"
using namespace hal;
namespace {
struct AddressableLED {
uint8_t index;
};
} // namespace
static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
kNumAddressableLEDs,
HAL_HandleEnum::AddressableLED>* ledHandles;
namespace hal {
namespace init {
void InitializeAddressableLED() {
static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
kNumAddressableLEDs,
HAL_HandleEnum::AddressableLED>
dcH;
ledHandles = &dcH;
}
} // namespace init
} // namespace hal
extern "C" {
HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
HAL_DigitalHandle outputPort, int32_t* status) {
return HAL_kInvalidHandle;
hal::init::CheckInit();
HAL_AddressableLEDHandle handle = ledHandles->Allocate();
if (handle == HAL_kInvalidHandle) {
*status = NO_AVAILABLE_RESOURCES;
return HAL_kInvalidHandle;
}
auto led = ledHandles->Get(handle);
if (!led) { // would only occur on thread issue
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
int16_t index = getHandleIndex(handle);
if (auto port = digitalChannelHandles->Get(outputPort, HAL_HandleEnum::PWM)) {
SimAddressableLEDData[index].outputPort = port->channel;
} else {
SimAddressableLEDData[index].outputPort = -1;
}
SimAddressableLEDData[index].length = 1;
SimAddressableLEDData[index].running = false;
SimAddressableLEDData[index].initialized = true;
led->index = index;
return handle;
}
void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {}
void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {
auto led = ledHandles->Get(handle);
ledHandles->Free(handle);
if (!led) return;
SimAddressableLEDData[led->index].running = false;
SimAddressableLEDData[led->index].initialized = false;
}
void HAL_SetAddressableLEDOutputPort(HAL_AddressableLEDHandle handle,
HAL_DigitalHandle outputPort,
int32_t* status) {}
int32_t* status) {
auto led = ledHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
if (auto port = digitalChannelHandles->Get(outputPort, HAL_HandleEnum::PWM)) {
SimAddressableLEDData[led->index].outputPort = port->channel;
} else {
SimAddressableLEDData[led->index].outputPort = -1;
}
}
void HAL_SetAddressableLEDLength(HAL_AddressableLEDHandle handle,
int32_t length, int32_t* status) {}
int32_t length, int32_t* status) {
auto led = ledHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
if (length > HAL_kAddressableLEDMaxLength) {
*status = PARAMETER_OUT_OF_RANGE;
return;
}
SimAddressableLEDData[led->index].length = length;
}
void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
const struct HAL_AddressableLEDData* data,
int32_t length, int32_t* status) {}
int32_t length, int32_t* status) {
auto led = ledHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
if (length > SimAddressableLEDData[led->index].length) {
*status = PARAMETER_OUT_OF_RANGE;
return;
}
SimAddressableLEDData[led->index].SetData(data, length);
}
void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
int32_t lowTime0NanoSeconds,
@@ -38,8 +133,22 @@ void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
int32_t* status) {}
void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
int32_t* status) {}
int32_t* status) {
auto led = ledHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
SimAddressableLEDData[led->index].running = true;
}
void HAL_StopAddressableLEDOutput(HAL_AddressableLEDHandle handle,
int32_t* status) {}
int32_t* status) {
auto led = ledHandles->Get(handle);
if (!led) {
*status = HAL_HANDLE_ERROR;
return;
}
SimAddressableLEDData[led->index].running = false;
}
} // extern "C"

View File

@@ -25,6 +25,7 @@ namespace hal {
namespace init {
void InitializeHAL() {
InitializeAccelerometerData();
InitializeAddressableLEDData();
InitializeAnalogGyroData();
InitializeAnalogInData();
InitializeAnalogOutData();
@@ -46,6 +47,7 @@ void InitializeHAL() {
InitializeSPIAccelerometerData();
InitializeSPIData();
InitializeAccelerometer();
InitializeAddressableLED();
InitializeAnalogAccumulator();
InitializeAnalogGyro();
InitializeAnalogInput();

View File

@@ -19,6 +19,7 @@ static inline void CheckInit() {
}
extern void InitializeAccelerometerData();
extern void InitializeAddressableLEDData();
extern void InitializeAnalogGyroData();
extern void InitializeAnalogInData();
extern void InitializeAnalogOutData();
@@ -41,6 +42,7 @@ extern void InitializeSimDeviceData();
extern void InitializeSPIAccelerometerData();
extern void InitializeSPIData();
extern void InitializeAccelerometer();
extern void InitializeAddressableLED();
extern void InitializeAnalogAccumulator();
extern void InitializeAnalogGyro();
extern void InitializeAnalogInput();

View File

@@ -37,4 +37,5 @@ int32_t HAL_GetNumSolenoidChannels(void) { return kNumSolenoidChannels; }
int32_t HAL_GetNumPDPModules(void) { return kNumPDPModules; }
int32_t HAL_GetNumPDPChannels(void) { return kNumPDPChannels; }
int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; }
int32_t HAL_GetNumAddressableLEDs(void) { return kNumAddressableLEDs; }
} // extern "C"

View File

@@ -29,4 +29,5 @@ constexpr int32_t kNumSolenoidChannels = 8;
constexpr int32_t kNumPDPModules = 63;
constexpr int32_t kNumPDPChannels = 16;
constexpr int32_t kNumDutyCycles = 8;
constexpr int32_t kNumAddressableLEDs = 1;
} // namespace hal

View File

@@ -0,0 +1,293 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <jni.h>
#include "CallbackStore.h"
#include "ConstBufferCallbackStore.h"
#include "edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI.h"
#include "mockdata/AddressableLEDData.h"
static_assert(sizeof(jbyte) * 4 == sizeof(HAL_AddressableLEDData));
using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: registerInitializedCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerInitializedCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(
env, index, callback, initialNotify,
&HALSIM_RegisterAddressableLEDInitializedCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: cancelInitializedCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelInitializedCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelAddressableLEDInitializedCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: getInitialized
* Signature: (I)Z
*/
JNIEXPORT jboolean JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getInitialized
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetAddressableLEDInitialized(index);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: setInitialized
* Signature: (IZ)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setInitialized
(JNIEnv*, jclass, jint index, jboolean value)
{
HALSIM_SetAddressableLEDInitialized(index, value);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: registerOutputPortCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerOutputPortCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(
env, index, callback, initialNotify,
&HALSIM_RegisterAddressableLEDOutputPortCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: cancelOutputPortCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelOutputPortCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelAddressableLEDOutputPortCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: getOutputPort
* Signature: (I)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getOutputPort
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetAddressableLEDOutputPort(index);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: setOutputPort
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setOutputPort
(JNIEnv*, jclass, jint index, jint value)
{
HALSIM_SetAddressableLEDOutputPort(index, value);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: registerLengthCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerLengthCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(env, index, callback, initialNotify,
&HALSIM_RegisterAddressableLEDLengthCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: cancelLengthCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelLengthCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelAddressableLEDLengthCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: getLength
* Signature: (I)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getLength
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetAddressableLEDLength(index);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: setLength
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setLength
(JNIEnv*, jclass, jint index, jint value)
{
HALSIM_SetAddressableLEDLength(index, value);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: registerRunningCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerRunningCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(env, index, callback, initialNotify,
&HALSIM_RegisterAddressableLEDRunningCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: cancelRunningCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelRunningCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelAddressableLEDRunningCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: getRunning
* Signature: (I)Z
*/
JNIEXPORT jboolean JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getRunning
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetAddressableLEDRunning(index);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: setRunning
* Signature: (IZ)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setRunning
(JNIEnv*, jclass, jint index, jboolean value)
{
HALSIM_SetAddressableLEDRunning(index, value);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: registerDataCallback
* Signature: (ILjava/lang/Object;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerDataCallback
(JNIEnv* env, jclass, jint index, jobject callback)
{
return sim::AllocateConstBufferCallback(
env, index, callback, &HALSIM_RegisterAddressableLEDDataCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: cancelDataCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelDataCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
sim::FreeConstBufferCallback(env, handle, index,
&HALSIM_CancelAddressableLEDDataCallback);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: getData
* Signature: (I)[B
*/
JNIEXPORT jbyteArray JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getData
(JNIEnv* env, jclass, jint index)
{
auto data =
std::make_unique<HAL_AddressableLEDData[]>(HAL_kAddressableLEDMaxLength);
int32_t length = HALSIM_GetAddressableLEDData(index, data.get());
return MakeJByteArray(
env, wpi::ArrayRef(reinterpret_cast<jbyte*>(data.get()), length * 4));
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: setData
* Signature: (I[B)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setData
(JNIEnv* env, jclass, jint index, jbyteArray arr)
{
JByteArrayRef jArrRef{env, arr};
auto arrRef = jArrRef.array();
HALSIM_SetAddressableLEDData(
index, reinterpret_cast<const HAL_AddressableLEDData*>(arrRef.data()),
arrRef.size() / 4);
}
/*
* Class: edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
* Method: resetData
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_resetData
(JNIEnv*, jclass, jint index)
{
HALSIM_ResetAddressableLEDData(index);
}
} // extern "C"

View File

@@ -0,0 +1,96 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <algorithm>
#include <cstring>
#include "../PortsInternal.h"
#include "AddressableLEDDataInternal.h"
using namespace hal;
namespace hal {
namespace init {
void InitializeAddressableLEDData() {
static AddressableLEDData sad[kNumAddressableLEDs];
::hal::SimAddressableLEDData = sad;
}
} // namespace init
} // namespace hal
AddressableLEDData* hal::SimAddressableLEDData;
void AddressableLEDData::ResetData() {
initialized.Reset(false);
outputPort.Reset(-1);
length.Reset(1);
running.Reset(false);
data.Reset();
}
void AddressableLEDData::SetData(const HAL_AddressableLEDData* d, int32_t len) {
len = (std::min)(HAL_kAddressableLEDMaxLength, len);
{
std::scoped_lock lock(m_dataMutex);
std::memcpy(m_data, d, len * sizeof(d[0]));
}
data(reinterpret_cast<const uint8_t*>(d), len * sizeof(d[0]));
}
int32_t AddressableLEDData::GetData(HAL_AddressableLEDData* d) {
std::scoped_lock lock(m_dataMutex);
int32_t len = length;
if (d) std::memcpy(d, m_data, len * sizeof(d[0]));
return len;
}
extern "C" {
void HALSIM_ResetAddressableLEDData(int32_t index) {
SimAddressableLEDData[index].ResetData();
}
int32_t HALSIM_GetAddressableLEDData(int32_t index,
struct HAL_AddressableLEDData* data) {
return SimAddressableLEDData[index].GetData(data);
}
void HALSIM_SetAddressableLEDData(int32_t index,
const struct HAL_AddressableLEDData* data,
int32_t length) {
SimAddressableLEDData[index].SetData(data, length);
}
#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME, \
SimAddressableLEDData, LOWERNAME)
DEFINE_CAPI(HAL_Bool, Initialized, initialized)
DEFINE_CAPI(int32_t, OutputPort, outputPort)
DEFINE_CAPI(int32_t, Length, length)
DEFINE_CAPI(HAL_Bool, Running, running)
#undef DEFINE_CAPI
#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME, \
SimAddressableLEDData, LOWERNAME)
DEFINE_CAPI(HAL_ConstBufferCallback, Data, data)
#define REGISTER(NAME) \
SimAddressableLEDData[index].NAME.RegisterCallback(callback, param, \
initialNotify)
void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {
REGISTER(initialized);
REGISTER(outputPort);
REGISTER(length);
REGISTER(running);
}
} // extern "C"

View File

@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <vector>
#include <wpi/spinlock.h>
#include "mockdata/AddressableLEDData.h"
#include "mockdata/SimCallbackRegistry.h"
#include "mockdata/SimDataValue.h"
namespace hal {
class AddressableLEDData {
HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
HAL_SIMDATAVALUE_DEFINE_NAME(OutputPort)
HAL_SIMDATAVALUE_DEFINE_NAME(Length)
HAL_SIMDATAVALUE_DEFINE_NAME(Running)
HAL_SIMDATAVALUE_DEFINE_NAME(Data)
wpi::recursive_spinlock m_dataMutex;
HAL_AddressableLEDData m_data[HAL_kAddressableLEDMaxLength];
public:
void SetData(const HAL_AddressableLEDData* d, int32_t len);
int32_t GetData(HAL_AddressableLEDData* d);
SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
false};
SimDataValue<int32_t, HAL_MakeInt, GetOutputPortName> outputPort{-1};
SimDataValue<int32_t, HAL_MakeInt, GetLengthName> length{1};
SimDataValue<HAL_Bool, HAL_MakeBoolean, GetRunningName> running{false};
SimCallbackRegistry<HAL_ConstBufferCallback, GetDataName> data;
void ResetData();
};
extern AddressableLEDData* SimAddressableLEDData;
} // namespace hal