private StickParameters GetStickParameters()
{
if (stickParameters.Equals(default(StickParameters)))
{
// will eventually add detection for User generated config
stickParameters = controller.GetConfig().GetStickParameters();
}
return(stickParameters);
}
public StickParameters GetStickParameters()
{
SPIStream stream = spi.GetStream();
stream.Seek(0x6086, SeekOrigin.Begin);
byte[] data = stream.Read(0x11);
StickParameters param = new StickParameters();
param.deadzone = data[2];
param.rangeRatio = data[3];
return(param);
}
public Position ParseStickPosition(PacketData data, int id)
{
AnalogConfiguration config = GetAnalogConfiguration(id);
byte[] bytes = data.rawData;
int offset = GetStickDataOffset();
int posX = bytes[offset] | ((bytes[offset + 1] & 0xF) << 8);
int posY = (bytes[offset + 1] >> 4) | (bytes[offset + 2] << 4);
StickParameters stickParameters = GetStickParameters();
float xDiff = posX - config.xCenter;
float yDiff = posY - config.yCenter;
float posXf = xDiff / ((xDiff > 0) ? (config.xMax - config.xCenter) : (config.xCenter - config.xMin));
float posYf = yDiff / ((yDiff > 0) ? (config.yMax - config.yCenter) : (config.yCenter - config.yMin));
// distance from origin
if (Math.Sqrt(Math.Pow(xDiff, 2) + Math.Pow(yDiff, 2)) < stickParameters.deadzone)
{
posXf = 0;
posYf = 0;
}
posXf++;
posXf /= 2;
posXf = Math.Abs(posXf); // make sure it never goes negative because it will wrap around and glitch
posYf++;
posYf /= 2;
posYf = Math.Abs(posYf);
posYf = 1 - posYf; // easy way of inverting axis
posX = (int)(posXf * 35900f);
posY = (int)(posYf * 35900f);
return(new Position(posX, posY));
}