159 lines
4.9 KiB
C++
159 lines
4.9 KiB
C++
#include "XboxControllerNotificationParser.h"
|
|
|
|
#define XBOX_CONTROLLER_INDEX_BUTTONS_DIR 12
|
|
#define XBOX_CONTROLLER_INDEX_BUTTONS_MAIN 13
|
|
#define XBOX_CONTROLLER_INDEX_BUTTONS_CENTER 14
|
|
#define XBOX_CONTROLLER_INDEX_BUTTONS_SHARE 15
|
|
|
|
XboxControllerNotificationParser::XboxControllerNotificationParser() {
|
|
btnA = btnB = btnX = btnY = false;
|
|
btnShare = btnStart = btnSelect = btnXbox = false;
|
|
btnLB = btnRB = btnLS = btnRS = false;
|
|
btnDirUp = btnDirLeft = btnDirRight = btnDirDown = false;
|
|
joyLHori = joyLVert = joyRHori = joyRVert = 0xffff / 2;
|
|
trigLT = trigRT = 0;
|
|
}
|
|
|
|
uint8_t XboxControllerNotificationParser::update(uint8_t* data, size_t length) {
|
|
if (length != expectedDataLen) {
|
|
return XBOX_CONTROLLER_ERROR_INVALID_LENGTH;
|
|
}
|
|
for (uint8_t i = 0; i < expectedDataLen; i++)
|
|
{
|
|
rx_buf[i]=data[i];
|
|
}
|
|
uint8_t btnBits;
|
|
btnBits = data[XBOX_CONTROLLER_INDEX_BUTTONS_MAIN];
|
|
btnA = btnBits & 0b00000001;
|
|
btnB = btnBits & 0b00000010;
|
|
btnX = btnBits & 0b00001000;
|
|
btnY = btnBits & 0b00010000;
|
|
btnLB = btnBits & 0b01000000;
|
|
btnRB = btnBits & 0b10000000;
|
|
|
|
btnBits = data[XBOX_CONTROLLER_INDEX_BUTTONS_CENTER];
|
|
btnSelect = btnBits & 0b00000100;
|
|
btnStart = btnBits & 0b00001000;
|
|
btnXbox = btnBits & 0b00010000;
|
|
btnLS = btnBits & 0b00100000;
|
|
btnRS = btnBits & 0b01000000;
|
|
|
|
btnBits = data[XBOX_CONTROLLER_INDEX_BUTTONS_SHARE];
|
|
btnShare = btnBits & 0b00000001;
|
|
|
|
btnBits = data[XBOX_CONTROLLER_INDEX_BUTTONS_DIR];
|
|
btnDirUp = btnBits == 1 || btnBits == 2 || btnBits == 8;
|
|
btnDirRight = 2 <= btnBits && btnBits <= 4;
|
|
btnDirDown = 4 <= btnBits && btnBits <= 6;
|
|
btnDirLeft = 6 <= btnBits && btnBits <= 8;
|
|
|
|
joyLHori = (uint16_t)data[0] | ((uint16_t)data[1] << 8);
|
|
joyLVert = (uint16_t)data[2] | ((uint16_t)data[3] << 8);
|
|
joyRHori = (uint16_t)data[4] | ((uint16_t)data[5] << 8);
|
|
joyRVert = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
|
|
trigLT = (uint16_t)data[8] | ((uint16_t)data[9] << 8);
|
|
trigRT = (uint16_t)data[10] | ((uint16_t)data[11] << 8);
|
|
return 0;
|
|
}
|
|
|
|
void XboxControllerNotificationParser::convertU16TU8Arr(uint16_t vU16,
|
|
uint8_t* data) {
|
|
data[0] = vU16 & 0xff;
|
|
data[1] = vU16 >> 8;
|
|
}
|
|
|
|
uint8_t XboxControllerNotificationParser::toArr(uint8_t* data, size_t length) {
|
|
if (length < expectedDataLen) {
|
|
return XBOX_CONTROLLER_ERROR_INVALID_LENGTH;
|
|
}
|
|
convertU16TU8Arr(joyLHori, &data[0]);
|
|
convertU16TU8Arr(joyLVert, &data[2]);
|
|
convertU16TU8Arr(joyRHori, &data[4]);
|
|
convertU16TU8Arr(joyRVert, &data[6]);
|
|
convertU16TU8Arr(trigLT, &data[8]);
|
|
convertU16TU8Arr(trigRT, &data[10]);
|
|
{
|
|
uint8_t btnBits = 0;
|
|
if (btnA) btnBits |= 0b00000001;
|
|
if (btnB) btnBits |= 0b00000010;
|
|
if (btnX) btnBits |= 0b00001000;
|
|
if (btnY) btnBits |= 0b00010000;
|
|
if (btnLB) btnBits |= 0b01000000;
|
|
if (btnRB) btnBits |= 0b10000000;
|
|
data[XBOX_CONTROLLER_INDEX_BUTTONS_MAIN] = btnBits;
|
|
}
|
|
{
|
|
uint8_t btnBits = 0;
|
|
if (btnSelect) btnBits |= 0b00000100;
|
|
if (btnStart) btnBits |= 0b00001000;
|
|
if (btnXbox) btnBits |= 0b00010000;
|
|
if (btnLS) btnBits |= 0b00100000;
|
|
if (btnRS) btnBits |= 0b01000000;
|
|
data[XBOX_CONTROLLER_INDEX_BUTTONS_CENTER] = btnBits;
|
|
}
|
|
{
|
|
uint8_t btnBits = 0;
|
|
if (btnShare) btnBits |= 0b00000001;
|
|
data[XBOX_CONTROLLER_INDEX_BUTTONS_SHARE] = btnBits;
|
|
}
|
|
{
|
|
uint8_t btnBits = 0;
|
|
if (btnDirUp) {
|
|
if (btnDirRight) {
|
|
btnBits = 2;
|
|
} else if (btnDirLeft) {
|
|
btnBits = 8;
|
|
} else {
|
|
btnBits = 1;
|
|
}
|
|
} else if (btnDirDown) {
|
|
if (btnDirRight) {
|
|
btnBits = 4;
|
|
} else if (btnDirLeft) {
|
|
btnBits = 6;
|
|
} else {
|
|
btnBits = 5;
|
|
}
|
|
} else {
|
|
if (btnDirRight) {
|
|
btnBits = 3;
|
|
} else if (btnDirLeft) {
|
|
btnBits = 7;
|
|
} else {
|
|
btnBits = 0;
|
|
}
|
|
}
|
|
data[XBOX_CONTROLLER_INDEX_BUTTONS_DIR] = btnBits;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
String XboxControllerNotificationParser::toString() {
|
|
// clang-format off
|
|
String str = String("") +
|
|
"btnY: " + String(btnY) + " " +
|
|
"btnX: " + String(btnX) + " " +
|
|
"btnB: " + String(btnB) + " " +
|
|
"btnA: " + String(btnA) + " " +
|
|
"btnLB: " + String(btnLB) + " " +
|
|
"btnRB: " + String(btnRB) + "\n" +
|
|
"btnSelect: " + String(btnSelect) + " " +
|
|
"btnStart: " + String(btnStart) + " " +
|
|
"btnXbox: " + String(btnXbox) + " " +
|
|
"btnShare: " + String(btnShare) + " " +
|
|
"btnLS: " + String(btnLS) + " " +
|
|
"btnRS: " + String(btnRS) + "\n" +
|
|
"btnDirUp: " + String(btnDirUp) + " " +
|
|
"btnDirRight: " + String(btnDirRight) + " " +
|
|
"btnDirDown: " + String(btnDirDown) + " " +
|
|
"btnDirLeft: " + String(btnDirLeft) + "\n"
|
|
"joyLHori: " + String(joyLHori) + "\n" +
|
|
"joyLVert: " + String(joyLVert) + "\n" +
|
|
"joyRHori: " + String(joyRHori) + "\n" +
|
|
"joyRVert: " + String(joyRVert) + "\n" +
|
|
"trigLT: " + String(trigLT) + "\n" +
|
|
"trigRT: " + String(trigRT) + "\n";
|
|
// clang-format on
|
|
return str;
|
|
}
|