541 lines
No EOL
13 KiB
C++
541 lines
No EOL
13 KiB
C++
// A lot of this code is made because of the work of https://github.com/capull0/SkyDumper and https://github.com/silicontrip/SkyReader
|
|
|
|
#include "SkyPortalSubsystem.h"
|
|
#include "Engine/Engine.h"
|
|
|
|
#include "HAL/RunnableThread.h"
|
|
#include "Misc/ScopeLock.h"
|
|
|
|
|
|
void USkyPortalSubsystem::Initialize(FSubsystemCollectionBase& Collection)
|
|
{
|
|
|
|
Super::Initialize(Collection);
|
|
// Start the status checker thread
|
|
StatusChecker = new FPortalStatusChecker(this, 0.1f); // Check every 5 seconds
|
|
StatusCheckerThread = FRunnableThread::Create(StatusChecker, TEXT("PortalStatusCheckerThread"), 0, TPri_AboveNormal);
|
|
|
|
UE_LOG(LogTemp, Log, TEXT("SkyPortalSubsystem Initialized"));
|
|
|
|
}
|
|
|
|
void USkyPortalSubsystem::Deinitialize()
|
|
{
|
|
// Stop the thread when the subsystem is deinitialized
|
|
if (StatusChecker)
|
|
{
|
|
StatusChecker->Stop();
|
|
StatusCheckerThread->WaitForCompletion();
|
|
|
|
delete StatusCheckerThread;
|
|
delete StatusChecker;
|
|
StatusChecker = nullptr;
|
|
StatusCheckerThread = nullptr;
|
|
}
|
|
|
|
// Disconnect portal
|
|
if (PortalHandle) {
|
|
PortalHandle->Close();
|
|
}
|
|
hid_exit();
|
|
|
|
|
|
UE_LOG(LogTemp, Log, TEXT("SkyPortalSubsystem Deinitialized"));
|
|
Super::Deinitialize();
|
|
}
|
|
|
|
|
|
void USkyPortalSubsystem::PortalActivate(const bool bShouldActivate)
|
|
{
|
|
FWriteBlock command;
|
|
|
|
memset(command.data, 0, write_buf_size);
|
|
command.data[1] = 'A';
|
|
command.data[2] = bShouldActivate;
|
|
uint8* output;
|
|
do {
|
|
PortalHandle->Write(&command);
|
|
output = PortalHandle->Read();
|
|
} while (output[0] != 'A');
|
|
|
|
}
|
|
|
|
|
|
|
|
void USkyPortalSubsystem::PortalReady()
|
|
{
|
|
FWriteBlock command;
|
|
|
|
memset(command.data, 0, write_buf_size); //maybe not needed here
|
|
command.data[1] = 'R';
|
|
uint8* output;
|
|
do {
|
|
PortalHandle->Write(&command);
|
|
output = PortalHandle->Read();
|
|
} while (output[0] != 'R');
|
|
|
|
uint16 _PortalId = ((output[1] << 8) + output[2]);
|
|
PortalId = _PortalId; //Need a conversion as uint16 is not supported in BP
|
|
|
|
}
|
|
|
|
|
|
|
|
#pragma region Color functions
|
|
|
|
void USkyPortalSubsystem::ChangePortalColor(const FLinearColor& Color)
|
|
{
|
|
|
|
unsigned char r = FMath::Clamp(Color.R * 100, 0.0f, 255.0f);
|
|
unsigned char g = FMath::Clamp(Color.G * 100, 0.0f, 255.0f);
|
|
unsigned char b = FMath::Clamp(Color.B * 100, 0.0f, 255.0f);
|
|
|
|
FWriteBlock command;
|
|
|
|
memset(command.data, 0, write_buf_size);
|
|
|
|
command.data[1] = 'C';
|
|
command.data[2] = r; // R
|
|
command.data[3] = g; // G
|
|
command.data[4] = b; // B
|
|
|
|
// no response for this one.
|
|
PortalHandle->Write(&command);
|
|
}
|
|
|
|
void USkyPortalSubsystem::ChangePortalColorSide(const FLinearColor& Color, const EPortalSide PortalSide, const float BlendTime)
|
|
{
|
|
|
|
unsigned char r = FMath::Clamp(Color.R * 100, 0, 255);
|
|
unsigned char g = FMath::Clamp(Color.G * 100, 0, 255);
|
|
unsigned char b = FMath::Clamp(Color.B * 100, 0, 255);
|
|
EPortalSide _portalside;
|
|
FWriteBlock command;
|
|
|
|
memset(command.data, 0, write_buf_size);
|
|
|
|
if (PortalSide == EPortalSide::BOTH) {
|
|
_portalside = EPortalSide::LEFT;
|
|
}
|
|
|
|
switch (_portalside) {
|
|
case EPortalSide::LEFT:
|
|
command.data[1] = 'J';
|
|
command.data[2] = 0x00;
|
|
case EPortalSide::RIGHT:
|
|
command.data[1] = 'J';
|
|
command.data[2] = 0x02;
|
|
case EPortalSide::TRAP:
|
|
command.data[1] = 'L';
|
|
command.data[2] = 0x01;
|
|
command.data[3] = FMath::Max3(r, g, b); // calculate brightness
|
|
PortalHandle->Write(&command); //since it's a color command for trap, only 3 bytes are needed, no response.
|
|
return;
|
|
}
|
|
|
|
command.data[3] = r; // R
|
|
command.data[4] = g; // G
|
|
command.data[5] = b; // B
|
|
|
|
//Convert the time in millisecond into two bytes
|
|
uint16_t _time = BlendTime;
|
|
uint8_t _time_low = _time & 0xFF; // Get the low byte by masking the least significant 8 bits
|
|
uint8_t _time_high = (_time >> 8) & 0xFF; // Get the high byte extracted by shifting the bits 8 positions to the right and masking the result
|
|
command.data[6] = _time_low;
|
|
command.data[7] = _time_high;
|
|
|
|
uint8* output;
|
|
do {
|
|
PortalHandle->Write(&command);
|
|
output = PortalHandle->Read();
|
|
} while (output[0] != 'J');
|
|
|
|
if (PortalSide == EPortalSide::BOTH) {
|
|
ChangePortalColorSide(Color, EPortalSide::RIGHT, BlendTime); // send a second command for the right side
|
|
}
|
|
}
|
|
#pragma endregion
|
|
|
|
|
|
|
|
|
|
|
|
FPortalStatusData USkyPortalSubsystem::ParsePortalStatus(const FWriteBlock& ResponseBlock)
|
|
{
|
|
FPortalStatusData PortalStatusData;
|
|
|
|
// Parse the figure status array (little-endian 32-bit integer)
|
|
uint32 FigureStatusArray = 0;
|
|
// Reading the 32-bit integer (character status array) from the buffer
|
|
FigureStatusArray |= ResponseBlock.buf[1]; // 1st byte
|
|
FigureStatusArray |= (ResponseBlock.buf[2] << 8); // 2nd byte
|
|
FigureStatusArray |= (ResponseBlock.buf[3] << 16); // 3rd byte
|
|
FigureStatusArray |= (ResponseBlock.buf[4] << 24); // 4th byte
|
|
|
|
TStaticArray<EFigureStatus, 16> tempArray;
|
|
// For each of the 16 entries, extract the 2-bit status and map it to EFigureStatus
|
|
for (int32 i = 0; i < 16; ++i)
|
|
{
|
|
uint8 StatusBits = (FigureStatusArray >> (i * 2)) & 0b11; // Extract 2 bits
|
|
EFigureStatus FigureStatus;
|
|
|
|
switch (StatusBits)
|
|
{
|
|
case 0b00:
|
|
FigureStatus = EFigureStatus::NOT_PRESENT;
|
|
break;
|
|
case 0b01:
|
|
FigureStatus = EFigureStatus::PRESENT;
|
|
break;
|
|
case 0b11:
|
|
FigureStatus = EFigureStatus::ADDED;
|
|
break;
|
|
case 0b10:
|
|
FigureStatus = EFigureStatus::REMOVED;
|
|
break;
|
|
default:
|
|
FigureStatus = EFigureStatus::NOT_PRESENT; // Default case
|
|
break;
|
|
}
|
|
|
|
// Add to the array of figure statuses
|
|
//PortalStatusData.StatusArray.Insert(FigureStatus, i);
|
|
tempArray[i] = FigureStatus;
|
|
}
|
|
PortalStatusData.StatusArray.SetNum(0);
|
|
PortalStatusData.StatusArray.Append(tempArray);
|
|
// The next byte is the response counter
|
|
PortalStatusData.Counter = ResponseBlock.buf[5];
|
|
|
|
// The last byte is the boolean indicating whether the portal is ready
|
|
PortalStatusData.bIsReady = ResponseBlock.buf[6] != 0; // 0 means not ready, non-zero means ready
|
|
|
|
return PortalStatusData;
|
|
}
|
|
|
|
void USkyPortalSubsystem::Sleep(int sleepMs) {
|
|
FPlatformProcess::Sleep(sleepMs * 0.0001);
|
|
}
|
|
|
|
void USkyPortalSubsystem::CheckComplexResponse() {
|
|
|
|
if (!PortalDevice) {
|
|
return;
|
|
}
|
|
|
|
RWBlock req, res;
|
|
|
|
memset(command.data, 0, rw_buf_size);
|
|
command.data[1] = 'S';
|
|
Write(&req);
|
|
|
|
int BuffResponse = hid_read_timeout(PortalDevice, res.buf, rw_buf_size, TIMEOUT);
|
|
if (BuffResponse < 0) {
|
|
UE_LOG(LogSkyportalIO, Error, TEXT("Error.\n %s"), hid_error(PortalDevice));
|
|
return;
|
|
}
|
|
EPortalCommand CommandResponse = GetPortalCommandFromChar(res.buf[0]);
|
|
switch (CommandResponse)
|
|
{
|
|
case A:
|
|
break;
|
|
case C:
|
|
break;
|
|
case J:
|
|
break;
|
|
case L:
|
|
break;
|
|
case M:
|
|
break;
|
|
case Q:
|
|
break;
|
|
case R:
|
|
break;
|
|
case S:
|
|
CurrentStatusData = ParsePortalStatus(res);
|
|
|
|
//Send delegate when new informations are received
|
|
if (OldStatusData != CurrentStatusData) {
|
|
|
|
for (int i = 0; i < CurrentStatusData.StatusArray.Num(); i++) {
|
|
if (CurrentStatusData.StatusArray[i] != OldStatusData.StatusArray[i]) {
|
|
if (
|
|
//!FalsePositive() //filter conflicting infos
|
|
true) {
|
|
FigureDataBlock FigureData;
|
|
switch (CurrentStatusData.StatusArray[i])
|
|
{
|
|
case EFigureStatus::NOT_PRESENT:
|
|
OnSkylanderRemoved.Broadcast(00, i);
|
|
break;
|
|
case EFigureStatus::PRESENT:
|
|
FigureData = ReadFigureBlocks(i);
|
|
OnSkylanderAdded.Broadcast(GetFigureID(FigureData), i);
|
|
break;
|
|
case EFigureStatus::ADDED:
|
|
FigureData = ReadFigureBlocks(i);
|
|
OnSkylanderAdded.Broadcast(GetFigureID(FigureData), i);
|
|
case EFigureStatus::REMOVED:
|
|
OnSkylanderRemoved.Broadcast(00, i);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
OldStatusData = CurrentStatusData;
|
|
}
|
|
|
|
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
TArray<uint8> USkyPortalSubsystem::QueryBlock(uint8 characterIndex, uint8 block)
|
|
{
|
|
TArray<uint8> QueryCommand;
|
|
QueryCommand.SetNum(0x21);
|
|
QueryCommand[1] = 'Q';
|
|
QueryCommand[2] = characterIndex;
|
|
QueryCommand[3] = block;
|
|
|
|
TArray<uint8> Output;
|
|
|
|
do {
|
|
portalConnection->Write(QueryCommand);
|
|
output = portalConnection->Read();
|
|
} while (output[0] != 'Q' || (output[1] % 0x10 != characterIndex && output[1] != 0x01) || output[2] != block);
|
|
|
|
return output;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* Verify the command response, when only a character is sended by the portal.
|
|
*
|
|
|
|
|
|
bool USkyPortalSubsystem::CheckResponse(RWBlock* res, char expect)
|
|
{
|
|
if (!PortalDevice) {
|
|
return false;
|
|
}
|
|
|
|
int b = hid_read_timeout(PortalDevice, res->buf, rw_buf_size, TIMEOUT);
|
|
|
|
if (b < 0) {
|
|
UE_LOG(LogSkyportalIO, Error, TEXT("Error.\n %s"), hid_error(PortalDevice));
|
|
return false;
|
|
}
|
|
|
|
res->BytesTransferred = b;
|
|
|
|
|
|
|
|
// found wireless USB but portal is not connected
|
|
if (res->buf[0] == 'Z')
|
|
{
|
|
UE_LOG(LogSkyportalIO, Error, TEXT("found wireless USB but portal is not connected"));
|
|
return false;
|
|
}
|
|
|
|
// Status says no skylander on portal
|
|
if (res->buf[0] == 'Q' && res->buf[1] == 0) {
|
|
UE_LOG(LogSkyportalIO, Warning, TEXT("Status says no skylander on portal"));
|
|
}
|
|
|
|
if (res->buf[0] == 'R' && res->buf[1] == 0) {
|
|
UE_LOG(LogSkyportalIO, Warning, TEXT("Status says no skylander on portal"));
|
|
}
|
|
|
|
return (res->buf[0] != expect);
|
|
}
|
|
*/
|
|
|
|
|
|
|
|
|
|
EPortalCommand USkyPortalSubsystem::GetPortalCommandFromChar(unsigned char Char)
|
|
{
|
|
switch (Char)
|
|
{
|
|
case 'A':
|
|
return EPortalCommand::A;
|
|
case 'C':
|
|
return EPortalCommand::C;
|
|
case 'J':
|
|
return EPortalCommand::J;
|
|
case 'L':
|
|
return EPortalCommand::L;
|
|
case 'M':
|
|
return EPortalCommand::M;
|
|
case 'Q':
|
|
return EPortalCommand::Q;
|
|
case 'R':
|
|
return EPortalCommand::R;
|
|
case 'S':
|
|
return EPortalCommand::S;
|
|
default:
|
|
// Handle the case when the character doesn't match any enum
|
|
// Return a default or invalid value, or handle the error
|
|
UE_LOG(LogSkyportalIO, Warning, TEXT("Invalid character for Portal Command: %c"), TCHAR(Char));
|
|
return EPortalCommand::S; // 'S' for Status as a default
|
|
}
|
|
|
|
}
|
|
|
|
bool USkyPortalSubsystem::ReadBlock(unsigned int block, unsigned char data[0x10], int skylanderIndex) {
|
|
RWBlock req, res; //request and response buffers
|
|
unsigned char followup;
|
|
|
|
UE_LOG(LogSkyportalIO, Verbose, TEXT("PortalIO:ReadBlock :%X"), block);
|
|
|
|
|
|
// Checking if the block is not out of range
|
|
if (!ensure(block < 0x40)) {
|
|
UE_LOG(LogSkyportalIO, Error, TEXT("PortalIO:ReadBlock failed, block out of range"));
|
|
return false; // Early return instead of throwing an exception
|
|
}
|
|
|
|
// Send query request
|
|
|
|
// Trying to read data 15x
|
|
for (int attempt = 0; attempt < 15; attempt++)
|
|
{
|
|
int i = 0;
|
|
|
|
memset(command.data, 0, rw_buf_size); // Clear the request buffer (initialize all bytes to zero)
|
|
command.data[1] = 'Q';
|
|
followup = 0x10 + skylanderIndex;
|
|
command.data[2] = followup;
|
|
if (block == 0) {
|
|
command.data[2] = followup + 0x10; // may not be needed
|
|
}
|
|
command.data[3] = (unsigned char)block;
|
|
|
|
memset(&(res.buf), 0, rw_buf_size); // Clear the response buffer to prepare for incoming data
|
|
|
|
|
|
do { Write(&req); } while (CheckResponse(&res, 'Q'));
|
|
|
|
if (res.buf[0] == 'Q' && res.buf[2] == (unsigned char)block) {
|
|
// Got our query back
|
|
if (res.buf[1] == followup) {
|
|
// got the query back with no error
|
|
memcpy(data, res.buf + 3, 0x10);
|
|
UE_LOG(LogSkyportalIO, Verbose, TEXT("PortalIO:ReadBlock success"));
|
|
return true;
|
|
}
|
|
}
|
|
|
|
|
|
} // retries
|
|
|
|
UE_LOG(LogSkyportalIO, Fatal, TEXT("PortalIO:ReadBlock failed after retries"));
|
|
ensureMsgf(false, TEXT("PortalIO: Failed to read block after multiple retries"));
|
|
return false;
|
|
}
|
|
|
|
bool USkyPortalSubsystem::PortalConnect()
|
|
{
|
|
PortalHandle = NewObject<USkyPortalIO>();
|
|
if (IsValidLowLevelFast(PortalHandle)) {
|
|
UE_LOG(LogSkyportalIO, Log, TEXT("Portal connected: "));
|
|
PortalReady();
|
|
PortalActivate(1);
|
|
|
|
Sleep(500);
|
|
ChangePortalColor(FLinearColor(0.5, 0.5, 0.5));
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool USkyPortalSubsystem::bIsPortalReady()
|
|
{
|
|
return false;
|
|
}
|
|
|
|
void USkyPortalSubsystem::SendPortalCommand(EPortalCommand Command)
|
|
{
|
|
|
|
}
|
|
|
|
void USkyPortalSubsystem::SendPortalSound(USoundWave* Sound)
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
FigureDataBlock USkyPortalSubsystem::ReadFigureBlocks(uint8 FigureIndex)
|
|
{
|
|
FigureDataBlock FigureData;
|
|
FigureData.error = false; // Initialize error flag
|
|
|
|
RWBlock req, res;
|
|
|
|
// Loop over all 64 blocks
|
|
for (uint8 BlockIndex = 0; BlockIndex < FIGURE_TOTAL_BLOCKS; ++BlockIndex)
|
|
{
|
|
// Prepare the request buffer
|
|
memset(command.data, 0, rw_buf_size);
|
|
command.data[1] = 'Q'; // Command character
|
|
command.data[2] = FigureIndex; // Figure index (0x00 to 0x0F)
|
|
command.data[3] = BlockIndex; // Block index (0x00 to 0x3F)
|
|
|
|
// Send the request
|
|
Write(&req);
|
|
|
|
// Read the response
|
|
int BuffResponse = hid_read_timeout(PortalDevice, res.buf, rw_buf_size, TIMEOUT);
|
|
|
|
// Check if the response was received successfully
|
|
if (BuffResponse < rw_buf_size)
|
|
{
|
|
FigureData.error = true; // Mark error flag
|
|
UE_LOG(LogSkyportalIO, Warning, TEXT("Error receiving data for block %d of figure %d"), BlockIndex, FigureIndex);
|
|
break;
|
|
}
|
|
|
|
// Parse the response status
|
|
uint8 StatusByte = res.buf[2];
|
|
uint8 ReturnedBlockIndex = res.buf[3];
|
|
|
|
// Check for errors in the status byte
|
|
if (StatusByte == 0x01) // Error in response
|
|
{
|
|
FigureData.error = true;
|
|
UE_LOG(LogSkyportalIO, Warning, TEXT("Error reading block %d for figure %d"), BlockIndex, FigureIndex);
|
|
break;
|
|
}
|
|
else if (StatusByte != (0x10 + 'Q')) // Unexpected status byte
|
|
{
|
|
FigureData.error = true;
|
|
UE_LOG(LogSkyportalIO, Warning, TEXT("Unexpected status byte 0x%02X for block %d"), StatusByte, BlockIndex);
|
|
break;
|
|
}
|
|
|
|
// Verify that the block index matches the requested block
|
|
if (ReturnedBlockIndex != BlockIndex)
|
|
{
|
|
FigureData.error = true;
|
|
UE_LOG(LogSkyportalIO, Warning, TEXT("Mismatched block index. Expected %d, got %d"), BlockIndex, ReturnedBlockIndex);
|
|
break;
|
|
}
|
|
|
|
// Copy block data into FigureData
|
|
memcpy(FigureData.blockdata[BlockIndex], &res.buf[4], FIGURE_BLOCK_SIZE);
|
|
}
|
|
|
|
return FigureData; // Return the complete figure data
|
|
} |