wirtz
/
MicroDexed
mirror of https://codeberg.org/dcoredump/MicroDexed.git
1
1
Fork 0
Code Issues Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1329 Commits
2 Branches
1 Tag
401 MiB
 
 
Tag: Branch: Tree: dev
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'dev'
${ noResults }
MicroDexed/third-party/USBHost_t36/joystick.cpp

2408 lines
89 KiB
Raw Permalink Blame History

/* USB EHCI Host for Teensy 3.6
* Copyright 2017 Paul Stoffregen (paul@pjrc.com)
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be included
* in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <Arduino.h>
#include "USBHost_t36.h" // Read this header first for key info
#define print USBHost::print_
#define println USBHost::println_
//#define DEBUG_JOYSTICK
#ifdef DEBUG_JOYSTICK
#define DBGPrintf USBHDBGSerial.printf
#else
#define DBGPrintf(...)
#endif
template<class T>
const T& clamp(const T& x, const T& lower, const T& upper) {
return min(upper, max(x, lower));
}
bool ack_rvd = false;
bool JoystickController::queue_Data_Transfer_Debug(Pipe_t *pipe, void *buffer,
uint32_t len, USBDriver *driver, uint32_t line)
{
if ((pipe == nullptr) || (driver == nullptr) || ((len > 0) && (buffer == nullptr))) {
// something wrong:
DBGPrintf("\n !!!!!!!!!!! JoystickController::queue_Data_Transfer called with bad data line: %u\n", line);
DBGPrintf("\t pipe:%p buffer:%p len:%u driver:%p\n", pipe, buffer, len, driver);
return false;
}
return queue_Data_Transfer(pipe, buffer, len, driver);
}
// PID/VID to joystick mapping - Only the XBOXOne is used to claim the USB interface directly,
// The others are used after claim-hid code to know which one we have and to use it for
// doing other features.
JoystickController::product_vendor_mapping_t JoystickController::pid_vid_mapping[] = {
{ 0x045e, 0x02dd, XBOXONE, false }, // Xbox One Controller
{ 0x045e, 0x02ea, XBOXONE, false }, // Xbox One S Controller
{ 0x045e, 0x0b12, XBOXONE, false }, // Xbox Core Controller (Series S/X)
{ 0x045e, 0x0719, XBOX360, false},
{ 0x045e, 0x028E, SWITCH, false}, // Switch?
{ 0x057E, 0x2009, SWITCH, true}, // Switch Pro controller. // Let the swtich grab it, but...
{ 0x0079, 0x201C, SWITCH, false},
{ 0x054C, 0x0268, PS3, true},
{ 0x054C, 0x042F, PS3, true}, // PS3 Navigation controller
{ 0x054C, 0x03D5, PS3_MOTION, true}, // PS3 Motion controller
{ 0x054C, 0x05C4, PS4, true}, {0x054C, 0x09CC, PS4, true },
{ 0x0A5C, 0x21E8, PS4, true},
{ 0x046D, 0xC626, SpaceNav, true}, // 3d Connextion Space Navigator, 0x10008
{ 0x046D, 0xC628, SpaceNav, true} // 3d Connextion Space Navigator, 0x10008
};
//-----------------------------------------------------------------------------
// Switch controller Bluetooth config structure.
//-----------------------------------------------------------------------------
static uint8_t switch_packet_num = 0;
struct SWProBTSendConfigData {
uint8_t hid_hdr;
uint8_t id;
uint8_t gpnum; //GlobalPacketNumber
uint8_t rumbleDataL[4];
uint8_t rumbleDataR[4];
uint8_t subCommand;
uint8_t subCommandData[38];
} __attribute__((packed));
struct SWProBTSendConfigData1 {
uint8_t hid_hdr;
uint8_t id;
uint8_t gpnum; //GlobalPacketNumber
uint8_t subCommand;
uint8_t subCommandData[38];
} __attribute__((packed));
struct SWProIMUCalibration {
int16_t acc_offset[3];
int16_t acc_sensitivity[3] = {16384, 16384, 16384};
int16_t gyro_offset[3];
int16_t gyro_sensitivity[3] = {15335, 15335, 15335};
} __attribute__((packed));
struct SWProIMUCalibration SWIMUCal;
struct SWProStickCalibration {
int16_t rstick_center_x;
int16_t rstick_center_y;
int16_t rstick_x_min;
int16_t rstick_x_max;
int16_t rstick_y_min;
int16_t rstick_y_max;
int16_t lstick_center_x;
int16_t lstick_center_y;
int16_t lstick_x_min;
int16_t lstick_x_max;
int16_t lstick_y_min;
int16_t lstick_y_max;
int16_t deadzone_left;
int16_t deadzone_right;
} __attribute__((packed));
struct SWProStickCalibration SWStickCal;
//-----------------------------------------------------------------------------
void JoystickController::init()
{
contribute_Pipes(mypipes, sizeof(mypipes) / sizeof(Pipe_t));
contribute_Transfers(mytransfers, sizeof(mytransfers) / sizeof(Transfer_t));
contribute_String_Buffers(mystring_bufs, sizeof(mystring_bufs) / sizeof(strbuf_t));
driver_ready_for_device(this);
USBHIDParser::driver_ready_for_hid_collection(this);
BluetoothController::driver_ready_for_bluetooth(this);
}
//-----------------------------------------------------------------------------
JoystickController::joytype_t JoystickController::mapVIDPIDtoJoystickType(uint16_t idVendor, uint16_t idProduct, bool exclude_hid_devices)
{
for (uint8_t i = 0; i < (sizeof(pid_vid_mapping) / sizeof(pid_vid_mapping[0])); i++) {
if ((idVendor == pid_vid_mapping[i].idVendor) && (idProduct == pid_vid_mapping[i].idProduct)) {
println("Match PID/VID: ", i, DEC);
if (exclude_hid_devices && pid_vid_mapping[i].hidDevice) return UNKNOWN;
return pid_vid_mapping[i].joyType;
}
}
return UNKNOWN; // Not in our list
}
//*****************************************************************************
// Some simple query functions depend on which interface we are using...
//*****************************************************************************
uint16_t JoystickController::idVendor()
{
if (device != nullptr) return device->idVendor;
if (mydevice != nullptr) return mydevice->idVendor;
return 0;
}
uint16_t JoystickController::idProduct()
{
if (device != nullptr) return device->idProduct;
if (mydevice != nullptr) return mydevice->idProduct;
return 0;
}
const uint8_t *JoystickController::manufacturer()
{
if ((device != nullptr) && (device->strbuf != nullptr)) return &device->strbuf->buffer[device->strbuf->iStrings[strbuf_t::STR_ID_MAN]];
//if ((btdevice != nullptr) && (btdevice->strbuf != nullptr)) return &btdevice->strbuf->buffer[btdevice->strbuf->iStrings[strbuf_t::STR_ID_MAN]];
if ((mydevice != nullptr) && (mydevice->strbuf != nullptr)) return &mydevice->strbuf->buffer[mydevice->strbuf->iStrings[strbuf_t::STR_ID_MAN]];
return nullptr;
}
const uint8_t *JoystickController::product()
{
if ((device != nullptr) && (device->strbuf != nullptr)) return &device->strbuf->buffer[device->strbuf->iStrings[strbuf_t::STR_ID_PROD]];
if ((mydevice != nullptr) && (mydevice->strbuf != nullptr)) return &mydevice->strbuf->buffer[mydevice->strbuf->iStrings[strbuf_t::STR_ID_PROD]];
if (btconnect != nullptr) return btconnect->remote_name_;
return nullptr;
}
const uint8_t *JoystickController::serialNumber()
{
if ((device != nullptr) && (device->strbuf != nullptr)) return &device->strbuf->buffer[device->strbuf->iStrings[strbuf_t::STR_ID_SERIAL]];
if ((mydevice != nullptr) && (mydevice->strbuf != nullptr)) return &mydevice->strbuf->buffer[mydevice->strbuf->iStrings[strbuf_t::STR_ID_SERIAL]];
return nullptr;
}
static uint8_t rumble_counter = 0;
bool JoystickController::setRumble(uint8_t lValue, uint8_t rValue, uint8_t timeout)
{
// Need to know which joystick we are on. Start off with XBox support - maybe need to add some enum value for the known
// joystick types.
rumble_lValue_ = lValue;
rumble_rValue_ = rValue;
rumble_timeout_ = timeout;
switch (joystickType_) {
default:
break;
case PS3:
return transmitPS3UserFeedbackMsg();
case PS3_MOTION:
return transmitPS3MotionUserFeedbackMsg();
case PS4:
return transmitPS4UserFeedbackMsg();
case XBOXONE:
// Lets try sending a request to the XBox 1.
if (btdriver_) {
DBGPrintf("\nXBOXONE BT Joystick update Rumble %d %d %d\n", lValue, rValue, timeout);
txbuf_[0] = 0xA2; // HID BT DATA (0xA0) | Report Type (Output 0x02)
txbuf_[1] = 0x03; // ID 0x03
txbuf_[2] = 0x0F; // Rumble mask (what motors are activated) (0000 lT rT L R)
txbuf_[3] = map(lValue, 0, 1023, 0, 100); // lT force
txbuf_[4] = map(rValue, 0, 1023, 0, 100); // rT force
txbuf_[5] = map(lValue, 0, 1023, 0, 100); // L force
txbuf_[6] = map(rValue, 0, 1023, 0, 100); // R force
txbuf_[7] = 0xff; // Length of pulse
txbuf_[8] = 0x00; // Period between pulses
txbuf_[9] = 0x00; // Repeat
btdriver_->sendL2CapCommand(txbuf_, 10, BluetoothController::INTERRUPT_SCID);
return true;
}
txbuf_[0] = 0x9;
txbuf_[1] = 0x0;
txbuf_[2] = 0x0;
txbuf_[3] = 0x09; // Substructure (what substructure rest of this packet has)
txbuf_[4] = 0x00; // Mode
txbuf_[5] = 0x0f; // Rumble mask (what motors are activated) (0000 lT rT L R)
txbuf_[6] = 0x0; // lT force
txbuf_[7] = 0x0; // rT force
txbuf_[8] = lValue; // L force
txbuf_[9] = rValue; // R force
txbuf_[10] = 0xff; // Length of pulse
txbuf_[11] = 0x00; // Period between pulses
txbuf_[12] = 0x00; // Repeat
if (!queue_Data_Transfer_Debug(txpipe_, txbuf_, 13, this, __LINE__)) {
println("XBoxOne rumble transfer fail");
}
return true; //
case XBOX360:
txbuf_[0] = 0x00;
txbuf_[1] = 0x01;
txbuf_[2] = 0x0F;
txbuf_[3] = 0xC0;
txbuf_[4] = 0x00;
txbuf_[5] = lValue;
txbuf_[6] = rValue;
txbuf_[7] = 0x00;
txbuf_[8] = 0x00;
txbuf_[9] = 0x00;
txbuf_[10] = 0x00;
txbuf_[11] = 0x00;
if (!queue_Data_Transfer_Debug(txpipe_, txbuf_, 12, this, __LINE__)) {
println("XBox360 rumble transfer fail");
}
return true;
case SWITCH:
if (btdriver_) {
struct SWProBTSendConfigData *packet = (struct SWProBTSendConfigData *)txbuf_ ;
memset((void*)packet, 0, sizeof(struct SWProBTSendConfigData));
packet->hid_hdr = 0xA2; // HID BT Get_report (0xA0) | Report Type (Output)
packet->id = 0x10;
if(switch_packet_num > 0x10) switch_packet_num = 0;
packet->gpnum = switch_packet_num;
switch_packet_num = (switch_packet_num + 1) & 0x0f;
// 2-9 rumble data;
Serial.printf("Set Rumble data: %d, %d\n", lValue, rValue);
// 2-9 rumble data;
uint8_t rumble_on[8] = {0x28, 0x88, 0x60, 0x61, 0x28, 0x88, 0x60, 0x61};
uint8_t rumble_off[8] = {0x00, 0x01, 0x40, 0x40, 0x00, 0x01, 0x40, 0x40};
//if ((lValue == 0x00) && (rValue == 0x00)) {
// for(uint8_t i = 0; i < 4; i++) packet->rumbleDataR[i] = rumble_off[i];
// for(uint8_t i = 4; i < 8; i++) packet->rumbleDataL[i-4] = rumble_off[i];
//}
if ((lValue != 0x0) || (rValue != 0x0)) {
if (lValue != 0 && rValue == 0) {
for(uint8_t i = 0; i < 4; i++) packet->rumbleDataR[i] = rumble_off[i];
for(uint8_t i = 4; i < 8; i++) packet->rumbleDataL[i-4] = rumble_on[i];
} else if (rValue != 0 && lValue == 0) {
for(uint8_t i = 0; i < 4; i++) packet->rumbleDataR[i] = rumble_on[i];
for(uint8_t i = 4; i < 8; i++) packet->rumbleDataL[i-4] = rumble_off[i];
} else if (rValue != 0 && lValue != 0) {
for(uint8_t i = 0; i < 4; i++) packet->rumbleDataR[i] = rumble_on[i];
for(uint8_t i = 4; i < 8; i++) packet->rumbleDataL[i-4] = rumble_on[i];
}
}
packet->subCommand = 0x0;
packet->subCommandData[0] = 0;
btdriver_->sendL2CapCommand((uint8_t *)packet, sizeof(struct SWProBTSendConfigData), BluetoothController::INTERRUPT_SCID /*0x40*/);
return true;
}
Serial.printf("Set Rumble data (USB): %d, %d\n", lValue, rValue);
memset(txbuf_, 0, 18); // make sure it is cleared out
//txbuf_[0] = 0x80;
//txbuf_[1] = 0x92;
//txbuf_[3] = 0x31;
txbuf_[0] = 0x10; // Command
// Now add in subcommand data:
// Probably do this better soon
if(switch_packet_num > 0x10) switch_packet_num = 0;
txbuf_[1 + 0] = switch_packet_num;
switch_packet_num = (switch_packet_num + 1) & 0x0f; //
uint8_t rumble_on[8] = {0x28, 0x88, 0x60, 0x61, 0x28, 0x88, 0x60, 0x61};
uint8_t rumble_off[8] = {0x00, 0x01, 0x40, 0x40, 0x00, 0x01, 0x40, 0x40};
//if ((lValue == 0x00) && (rValue == 0x00)) {
// for(uint8_t i = 0; i < 4; i++) packet->rumbleDataR[i] = rumble_off[i];
// for(uint8_t i = 4; i < 8; i++) packet->rumbleDataL[i-4] = rumble_off[i];
//}
if ((lValue != 0x0) || (rValue != 0x0)) {
if (lValue != 0 && rValue == 0x00) {
for(uint8_t i = 0; i < 4; i++) txbuf_[i + 2] = rumble_off[i];
for(uint8_t i = 4; i < 8; i++) txbuf_[i - 4 + 6] = rumble_on[i];
} else if (rValue != 0 && lValue == 0x00) {
for(uint8_t i = 0; i < 4; i++) txbuf_[i + 2] = rumble_on[i];
for(uint8_t i = 4; i < 8; i++) txbuf_[i - 4 + 6] = rumble_off[i];
} else if (rValue != 0 && lValue != 0) {
for(uint8_t i = 0; i < 4; i++) txbuf_[i + 2] = rumble_on[i];
for(uint8_t i = 4; i < 8; i++) txbuf_[i - 4 + 6] = rumble_on[i];
}
}
txbuf_[11] = 0x00;
txbuf_[12] = 0x00;
if(driver_) {
driver_->sendPacket(txbuf_, 18);
} else if (txpipe_) {
if (!queue_Data_Transfer_Debug(txpipe_, txbuf_, 12, this, __LINE__)) {
println("switch transfer fail");
Serial.printf("Switch transfer fail\n");
}
}
return true;
}
return false;
}
bool JoystickController::setLEDs(uint8_t lr, uint8_t lg, uint8_t lb)
{
// Need to know which joystick we are on. Start off with XBox support - maybe need to add some enum value for the known
// joystick types.
DBGPrintf("::setLEDS(%x %x %x)\n", lr, lg, lb);
if ((leds_[0] != lr) || (leds_[1] != lg) || (leds_[2] != lb)) {
leds_[0] = lr;
leds_[1] = lg;
leds_[2] = lb;
switch (joystickType_) {
case PS3:
return transmitPS3UserFeedbackMsg();
case PS3_MOTION:
return transmitPS3MotionUserFeedbackMsg();
case PS4:
return transmitPS4UserFeedbackMsg();
case XBOX360:
// 0: off, 1: all blink then return to before
// 2-5(TL, TR, BL, BR) - blink on then stay on
// 6-9() - On
// ...
txbuf_[1] = 0x00;
txbuf_[2] = 0x08;
txbuf_[3] = 0x40 + lr;
txbuf_[4] = 0x00;
txbuf_[5] = 0x00;
txbuf_[6] = 0x00;
txbuf_[7] = 0x00;
txbuf_[8] = 0x00;
txbuf_[9] = 0x00;
txbuf_[10] = 0x00;
txbuf_[11] = 0x00;
if (!queue_Data_Transfer_Debug(txpipe_, txbuf_, 12, this, __LINE__)) {
println("XBox360 set leds fail");
}
return true;
case SWITCH:
if (btdriver_) {
DBGPrintf("Init LEDs\n");
struct SWProBTSendConfigData *packet = (struct SWProBTSendConfigData *)txbuf_ ;
memset((void*)packet, 0, sizeof(struct SWProBTSendConfigData));
packet->hid_hdr = 0xA2; // HID BT Get_report (0xA0) | Report Type (Output)
packet->id = 1;
packet->gpnum = switch_packet_num;
switch_packet_num = (switch_packet_num + 1) & 0x0f;
// 2-9 rumble data;
/*packet->rumbleDataL[0] = 0x00;
packet->rumbleDataL[1] = 0x01;
packet->rumbleDataL[2] = 0x40;
packet->rumbleDataL[3] = 0x00;
packet->rumbleDataR[0] = 0x00;
packet->rumbleDataR[1] = 0x01;
packet->rumbleDataR[2] = 0x40;
packet->rumbleDataR[3] = 0x00; */
packet->subCommand = 0x30; // Report ID
packet->subCommandData[0] = lr; // try full 0x30?; // Report ID
btdriver_->sendL2CapCommand((uint8_t *)packet, sizeof(struct SWProBTSendConfigData), BluetoothController::INTERRUPT_SCID /*0x40*/);
return true;
}
memset(txbuf_, 0, 20); // make sure it is cleared out
txbuf_[0] = 0x01; // Command
// Now add in subcommand data:
// Probably do this better soon
txbuf_[1 + 0] = rumble_counter++; //
txbuf_[1 + 1] = 0x00;
txbuf_[1 + 2] = 0x01;
txbuf_[1 + 3] = 0x40;
txbuf_[1 + 4] = 0x40;
txbuf_[1 + 5] = 0x00;
txbuf_[1 + 6] = 0x01;
txbuf_[1 + 7] = 0x40;
txbuf_[1 + 8] = 0x40;
txbuf_[1 + 9] = 0x30; // LED Command
txbuf_[1 + 10] = lr;
println("Switch set leds: driver? ", (uint32_t)driver_, HEX);
print_hexbytes((uint8_t*)txbuf_, 20);
if(driver_) {
driver_->sendPacket(txbuf_, 20);
} else if (txpipe_) {
if (!queue_Data_Transfer_Debug(txpipe_, txbuf_, 20, this, __LINE__)) {
println("switch transfer fail");
Serial.printf("Switch transfer fail\n");
}
}
case XBOXONE:
default:
return false;
}
}
return false;
}
bool JoystickController::transmitPS4UserFeedbackMsg() {
if (driver_) {
uint8_t packet[32];
memset(packet, 0, sizeof(packet));
packet[0] = 0x05; // Report ID
packet[1] = 0xFF;
packet[4] = rumble_lValue_; // Small Rumble
packet[5] = rumble_rValue_; // Big rumble
packet[6] = leds_[0]; // RGB value
packet[7] = leds_[1];
packet[8] = leds_[2];
// 9, 10 flash ON, OFF times in 100ths of second? 2.5 seconds = 255
DBGPrintf("Joystick update Rumble/LEDs\n");
return driver_->sendPacket(packet, 32);
} else if (btdriver_) {
uint8_t packet[79];
memset(packet, 0, sizeof(packet));
//0xa2, 0x11, 0xc0, 0x20, 0xf0, 0x04, 0x00
packet[0] = 0x52;
packet[1] = 0x11; // Report ID
packet[2] = 0x80;
//packet[3] = 0x20;
packet[4] = 0xFF;
packet[7] = rumble_lValue_; // Small Rumble
packet[8] = rumble_rValue_; // Big rumble
packet[9] = leds_[0]; // RGB value
packet[10] = leds_[1];
packet[11] = leds_[2];
// 12, 13 flash ON, OFF times in 100ths of sedond? 2.5 seconds = 255
DBGPrintf("Joystick update Rumble/LEDs\n");
btdriver_->sendL2CapCommand(packet, sizeof(packet), 0x40);
return true;
}
return false;
}
static const uint8_t PS3_USER_FEEDBACK_INIT[] = {
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0xff, 0x27, 0x10, 0x00, 0x32,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00
};
bool JoystickController::transmitPS3UserFeedbackMsg() {
if (driver_) {
memcpy(txbuf_, PS3_USER_FEEDBACK_INIT, 48);
txbuf_[1] = rumble_lValue_ ? rumble_timeout_ : 0;
txbuf_[2] = rumble_lValue_; // Small Rumble
txbuf_[3] = rumble_rValue_ ? rumble_timeout_ : 0;
txbuf_[4] = rumble_rValue_; // Big rumble
txbuf_[9] = leds_[2] << 1; // RGB value // using third led now...
//DBGPrintf("\nJoystick update Rumble/LEDs %d %d %d %d %d\n", txbuf_[1], txbuf_[2], txbuf_[3], txbuf_[4], txbuf_[9]);
return driver_->sendControlPacket(0x21, 9, 0x201, 0, 48, txbuf_);
} else if (btdriver_) {
txbuf_[0] = 0x52;
txbuf_[1] = 0x1;
memcpy(&txbuf_[2], PS3_USER_FEEDBACK_INIT, 48);
txbuf_[3] = rumble_lValue_ ? rumble_timeout_ : 0;
txbuf_[4] = rumble_lValue_; // Small Rumble
txbuf_[5] = rumble_rValue_ ? rumble_timeout_ : 0;
txbuf_[6] = rumble_rValue_; // Big rumble
txbuf_[11] = leds_[2] << 1; // RGB value
DBGPrintf("\nJoystick update Rumble/LEDs %d %d %d %d %d\n", txbuf_[3], txbuf_[4], txbuf_[5], txbuf_[6], txbuf_[11]);
btdriver_->sendL2CapCommand(txbuf_, 50, BluetoothController::CONTROL_SCID);
return true;
}
return false;
}
#define MOVE_REPORT_BUFFER_SIZE 7
#define MOVE_HID_BUFFERSIZE 50 // Size of the buffer for the Playstation Motion Controller
bool JoystickController::transmitPS3MotionUserFeedbackMsg() {
if (driver_) {
txbuf_[0] = 0x02; // Set report ID, this is needed for Move commands to work
txbuf_[2] = leds_[0];
txbuf_[3] = leds_[1];
txbuf_[4] = leds_[2];
txbuf_[6] = rumble_lValue_; // Set the rumble value into the write buffer
//return driver_->sendControlPacket(0x21, 9, 0x201, 0, MOVE_REPORT_BUFFER_SIZE, txbuf_);
return driver_->sendPacket(txbuf_, MOVE_REPORT_BUFFER_SIZE);
} else if (btdriver_) {
txbuf_[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
txbuf_[1] = 0x02; // Report ID
txbuf_[3] = leds_[0];
txbuf_[4] = leds_[1];
txbuf_[5] = leds_[2];
txbuf_[7] = rumble_lValue_;
btdriver_->sendL2CapCommand(txbuf_, MOVE_HID_BUFFERSIZE, BluetoothController::INTERRUPT_SCID);
return true;
}
return false;
}
//*****************************************************************************
// Support for Joysticks that Use HID data.
//*****************************************************************************
hidclaim_t JoystickController::claim_collection(USBHIDParser *driver, Device_t *dev, uint32_t topusage)
{
DBGPrintf("JoystickController::claim_collection(%p) Driver:%p(%u %u) Dev:%p Top:%x\n", this, driver,
driver->interfaceSubClass(), driver->interfaceProtocol(), dev, topusage);
// only claim Desktop/Joystick and Desktop/Gamepad
if (topusage != 0x10004 && topusage != 0x10005 && topusage != 0x10008) return CLAIM_NO;
// only claim from one physical device
if (mydevice != NULL && dev != mydevice) return CLAIM_NO;
// Also don't allow us to claim if it is used as a standard usb object (XBox...)
if (device != nullptr) return CLAIM_NO;
mydevice = dev;
collections_claimed++;
anychange = true; // always report values on first read
driver_ = driver; // remember the driver.
driver_->setTXBuffers(txbuf_, nullptr, sizeof(txbuf_));
connected_ = true; // remember that hardware is actually connected...
// Lets see if we know what type of joystick this is. That is, is it a PS3 or PS4 or ...
joystickType_ = mapVIDPIDtoJoystickType(mydevice->idVendor, mydevice->idProduct, false);
DBGPrintf("JoystickController::claim_collection joystickType_=%d\n", joystickType_);
switch (joystickType_) {
case PS3:
case PS3_MOTION: // not sure yet
additional_axis_usage_page_ = 0x1;
additional_axis_usage_start_ = 0x100;
additional_axis_usage_count_ = 39;
axis_change_notify_mask_ = (uint64_t) - 1; // Start off assume all bits
break;
case PS4:
additional_axis_usage_page_ = 0xFF00;
additional_axis_usage_start_ = 0x21;
additional_axis_usage_count_ = 54;
axis_change_notify_mask_ = (uint64_t)0xfffffffffffff3ffl; // Start off assume all bits - 10 and 11
break;
case SWITCH:
// bugbug set the hand shake...
{
DBGPrintf("Send Handshake\n");
sw_sendCmdUSB(0x02, SW_CMD_TIMEOUT);
initialPass_ = true;
connectedComplete_pending_ = 0;
}
// fall through
default:
additional_axis_usage_page_ = 0x09;
additional_axis_usage_start_ = 0x21;
additional_axis_usage_count_ = 5;
axis_change_notify_mask_ = 0x3ff; // Start off assume only the 10 bits...
}
//DBGPrintf("Claim Additional axis: %x %x %d\n", additional_axis_usage_page_, additional_axis_usage_start_, additional_axis_usage_count_);
USBHDBGSerial.printf("\tJoystickController claim collection\n");
return CLAIM_REPORT;
}
void JoystickController::disconnect_collection(Device_t *dev)
{
if (--collections_claimed == 0) {
mydevice = NULL;
driver_ = nullptr;
axis_mask_ = 0;
axis_changed_mask_ = 0;
}
}
void JoystickController::hid_input_begin(uint32_t topusage, uint32_t type, int lgmin, int lgmax)
{
// TODO: set up translation from logical min/max to consistent 16 bit scale
}
void JoystickController::hid_input_data(uint32_t usage, int32_t value)
{
DBGPrintf("joystickType_=%d\n", joystickType_);
DBGPrintf("Joystick: usage=%X, value=%d\n", usage, value);
uint32_t usage_page = usage >> 16;
usage &= 0xFFFF;
if (usage_page == 9 && usage >= 1 && usage <= 32) {
uint32_t bit = 1 << (usage - 1);
if (value == 0) {
if (buttons & bit) {
buttons &= ~bit;
anychange = true;
}
} else {
if (!(buttons & bit)) {
buttons |= bit;
anychange = true;
}
}
} else if (usage_page == 1 && usage >= 0x30 && usage <= 0x39) {
// TODO: need scaling of value to consistent API, 16 bit signed?
// TODO: many joysticks repeat slider usage. Detect & map to axis?
uint32_t i = usage - 0x30;
axis_mask_ |= (1 << i); // Keep record of which axis we have data on.
if (axis[i] != value) {
axis[i] = value;
axis_changed_mask_ |= (1 << i);
if (axis_changed_mask_ & axis_change_notify_mask_)
anychange = true;
}
} else if (usage_page == additional_axis_usage_page_) {
// see if the usage is witin range.
//DBGPrintf("UP: usage_page=%x usage=%x User: %x %d\n", usage_page, usage, user_buttons_usage_start, user_buttons_count_);
if ((usage >= additional_axis_usage_start_) && (usage < (additional_axis_usage_start_ + additional_axis_usage_count_))) {
// We are in the user range.
uint16_t usage_index = usage - additional_axis_usage_start_ + STANDARD_AXIS_COUNT;
if (usage_index < (sizeof(axis) / sizeof(axis[0]))) {
if (axis[usage_index] != value) {
axis[usage_index] = value;
if (usage_index > 63) usage_index = 63; // don't overflow our mask
axis_changed_mask_ |= ((uint64_t)1 << usage_index); // Keep track of which ones changed.
if (axis_changed_mask_ & axis_change_notify_mask_)
anychange = true; // We have changes...
}
axis_mask_ |= ((uint64_t)1 << usage_index); // Keep record of which axis we have data on.
}
//DBGPrintf("UB: index=%x value=%x\n", usage_index, value);
}
} else {
DBGPrintf("UP: usage_page=%x usage=%x add: %x %x %d\n", usage_page, usage, additional_axis_usage_page_, additional_axis_usage_start_, additional_axis_usage_count_);
}
// TODO: hat switch?
}
void JoystickController::hid_input_end()
{
if (anychange) {
joystickEvent = true;
}
}
bool JoystickController::hid_process_out_data(const Transfer_t *transfer)
{
DBGPrintf("JoystickController::hid_process_out_data\n");
return true;
}
//---------------------------------------------------
// Try to handle some of the startup code messages for Switch controll
// b
bool JoystickController::sw_handle_usb_init_of_joystick(uint8_t *buffer, uint16_t cb, bool timer_event)
{
if (buffer) {
if ((buffer[0] != 0x81) && (buffer[0] != 0x21))
return false; // was not an event message
driver_->stopTimer();
uint8_t ack_rpt = buffer[0];
if (ack_rpt == 0x81) {
uint8_t ack_81_subrpt = buffer[1];
DBGPrintf("\t(%u)CMD last sent: %x ack cmd: %x ", (uint32_t)em_sw_, sw_last_cmd_sent_, ack_81_subrpt);
switch(ack_81_subrpt) {
case 0x02: DBGPrintf("Handshake Complete......\n"); break;
case 0x03: DBGPrintf("Baud Rate Change Complete......\n"); break;
case 0x04: DBGPrintf("Disabled USB Timeout Complete......\n"); break;
default: DBGPrintf("???");
}
if (!initialPass_) return true; // don't need to process
if (sw_last_cmd_sent_ == ack_rpt) {
sw_last_cmd_repeat_count = 0;
connectedComplete_pending_++;
} else {
DBGPrintf("\tcmd != ack rpt count:%u ", sw_last_cmd_repeat_count);
if (sw_last_cmd_repeat_count) {
DBGPrintf("Skip to next\n");
sw_last_cmd_repeat_count = 0;
connectedComplete_pending_++;
} else {
DBGPrintf("Retry\n");
sw_last_cmd_repeat_count++;
}
}
} else {
// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
//21 0a 71 00 80 00 01 e8 7f 01 e8 7f 0c 80 40 00 00 00 00 00 00 ...
uint8_t ack_21_subrpt = buffer[14];
sw_parseAckMsg(buffer);
DBGPrintf("\t(%u)CMD Submd ack cmd: %x \n", (uint32_t)em_sw_, ack_21_subrpt);
switch (ack_21_subrpt) {
case 0x40: DBGPrintf("IMU Enabled......\n"); break;
case 0x48: DBGPrintf("Rumbled Enabled......\n"); break;
case 0x10: DBGPrintf("IMU Cal......\n"); break;
case 0x30: DBGPrintf("Std Rpt Enabled......\n"); break;
default: DBGPrintf("Other"); break;
}
if (!initialPass_) return true; // don't need to process
sw_last_cmd_repeat_count = 0;
connectedComplete_pending_++;
}
} else if (timer_event) {
if (!initialPass_) return true; // don't need to process
DBGPrintf("\t(%u)Timer event - advance\n", (uint32_t)em_sw_);
sw_last_cmd_repeat_count = 0;
connectedComplete_pending_++;
}
// Now lets Send out the next state
// Note: we don't increment it here, but let the ack or
// timeout increment it.
uint8_t packet_[8];
switch(connectedComplete_pending_) {
case 0:
//Change Baud
DBGPrintf("Change Baud\n");
sw_sendCmdUSB(0x03, SW_CMD_TIMEOUT);
break;
case 1:
DBGPrintf("Handshake2\n");
sw_sendCmdUSB(0x02, SW_CMD_TIMEOUT);
break;
case 2:
DBGPrintf("Try to get IMU Cal\n");
packet_[0] = 0x20;
packet_[1] = 0x60;
packet_[2] = 0x00;
packet_[3] = 0x00;
packet_[4] = (0x6037 - 0x6020 + 1);
sw_sendSubCmdUSB(0x10, packet_, 5, SW_CMD_TIMEOUT); // doesnt work wired
break;
case 3:
DBGPrintf("\nTry to Get IMU Horizontal Offset Data\n");
packet_[0] = 0x80;
packet_[1] = 0x60;
packet_[2] = 0x00;
packet_[3] = 0x00;
packet_[4] = (0x6085 - 0x6080 + 1);
sw_sendSubCmdUSB(0x10, packet_, 5, SW_CMD_TIMEOUT);
break;
case 4:
DBGPrintf("\n Read: Factory Analog stick calibration and Controller Colours\n");
packet_[0] = 0x3D;
packet_[1] = 0x60;
packet_[2] = 0x00;
packet_[3] = 0x00;
packet_[4] = (0x6055 - 0x603D + 1);
sw_sendSubCmdUSB(0x10, packet_, 5, SW_CMD_TIMEOUT);
break;
case 5:
connectedComplete_pending_++;
DBGPrintf("Enable IMU\n");
packet_[0] = 0x01;
sw_sendSubCmdUSB(0x40, packet_, 1, SW_CMD_TIMEOUT);
break;
case 6:
DBGPrintf("JC_USB_CMD_NO_TIMEOUT\n");
sw_sendCmdUSB(0x04, SW_CMD_TIMEOUT);
break;
case 7:
DBGPrintf("Enable Rumble\n");
packet_[0] = 0x01;
sw_sendSubCmdUSB(0x48, packet_, 1, SW_CMD_TIMEOUT);
break;
case 8:
DBGPrintf("Enable Std Rpt\n");
packet_[0] = 0x30;
sw_sendSubCmdUSB(0x03, packet_, 1, SW_CMD_TIMEOUT);
break;
case 9:
DBGPrintf("JC_USB_CMD_NO_TIMEOUT\n");
packet_[0] = 0x04;
sw_sendCmdUSB(0x04, SW_CMD_TIMEOUT);
break;
case 10:
connectedComplete_pending_ = 99;
initialPass_ = false;
}
return true;
}
bool JoystickController::hid_process_in_data(const Transfer_t *transfer)
{
uint8_t *buffer = (uint8_t *)transfer->buffer;
if (*buffer) report_id_ = *buffer;
uint8_t cnt = transfer->length;
if (!buffer || *buffer == 1) return false; // don't do report 1
DBGPrintf("hid_process_in_data %x %u %u %p %x %x:", transfer->buffer, transfer->length, joystickType_, txpipe_, initialPass_, connectedComplete_pending_);
for (uint8_t i = 0; i < cnt; i++) DBGPrintf(" %02x", buffer[i]);
DBGPrintf("\n");
if (joystickType_ == SWITCH) {
if (sw_handle_usb_init_of_joystick(buffer, cnt, false))
return true;
// the main HID parse code should handle it.
sw_process_HID_data(buffer, cnt);
return true; // don't let main hid code process this.
}
return false;
}
void JoystickController::hid_timer_event(USBDriverTimer *whichTimer)
{
DBGPrintf("JoystickController: Timer\n");
if (!driver_) return;
driver_->stopTimer();
sw_handle_usb_init_of_joystick(nullptr, 0, true);
}
void JoystickController::bt_hid_timer_event(USBDriverTimer *whichTimer)
{
DBGPrintf("Bluetooth JoystickController: Timer\n");
if (!btconnect) return;
btconnect->stopTimer();
sw_handle_bt_init_of_joystick(nullptr, 0, true);
}
bool JoystickController::hid_process_control(const Transfer_t *transfer) {
Serial.printf("USBHIDParser::control msg: %x %x : %x %u :", transfer->setup.word1, transfer->setup.word2, transfer->buffer, transfer->length);
if (transfer->buffer) {
uint16_t cnt = transfer->length;
if (cnt > 16) cnt = 16;
uint8_t *pb = (uint8_t*)transfer->buffer;
while (cnt--) Serial.printf(" %02x", *pb++);
}
Serial.printf("\n");
send_Control_packet_active_ = false;
return false;
}
void JoystickController::joystickDataClear() {
joystickEvent = false;
anychange = false;
axis_changed_mask_ = 0;
axis_mask_ = 0;
}
//*****************************************************************************
// Support for Joysticks that are class specific and do not use HID
// Example: XBox One controller.
//*****************************************************************************
static uint8_t xboxone_start_input[] = {0x05, 0x20, 0x00, 0x01, 0x00};
static uint8_t xbox360w_inquire_present[] = {0x08, 0x00, 0x0F, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
//static uint8_t switch_start_input[] = {0x19, 0x01, 0x03, 0x07, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10};
static uint8_t switch_start_input[] = {0x80, 0x02};
bool JoystickController::claim(Device_t *dev, int type, const uint8_t *descriptors, uint32_t len)
{
println("JoystickController claim this=", (uint32_t)this, HEX);
DBGPrintf("JoystickController::claim(%p) Dev:%p Type:%x\n", this, dev, type);
// Don't try to claim if it is used as USB device or HID device
if (mydevice != NULL) return false;
if (device != nullptr) return false;
// Try claiming at the interface level.
if (type != 1) return false;
print_hexbytes(descriptors, len);
JoystickController::joytype_t jtype = mapVIDPIDtoJoystickType(dev->idVendor, dev->idProduct, true);
DBGPrintf("\tVID:%x PID:%x Jype:%x\n", dev->idVendor, dev->idProduct, jtype);
println("Jtype=", (uint8_t)jtype, DEC);
if (jtype == UNKNOWN)
return false;
// XBOX One
// 0 1 2 3 4 5 6 7 8 *9 10 1 2 3 4 5 *6 7 8 9 20 1 2 3 4 5 6 7 8 9 30 1...
// 09 04 00 00 02 FF 47 D0 00 07 05 02 03 40 00 04 07 05 82 03 40 00 04 09 04 01 00 00 FF 47 D0 00
// Lets do some verifications to make sure.
// XBOX 360 wireless... Has 8 interfaces. 4 joysticks (1, 3, 5, 7) and 4 headphones assume 2,4,6, 8...
// Shows data for #1 only...
// Also they have some unknown data type we need to ignore between interface and end points.
// 0 1 2 3 4 5 6 7 8 *9 10 1 2 3 4 5 *6 7 8 9 20 1 2 3 4 5 6 7 8
// 09 04 00 00 02 FF 5D 81 00 14 22 00 01 13 81 1D 00 17 01 02 08 13 01 0C 00 0C 01 02 08
// 29 30 1 2 3 4 5 6 7 8 9 40 41 42
// 07 05 81 03 20 00 01 07 05 01 03 20 00 08
// Switch
// 09 04 00 00 02 FF 5D 01 00
// 10 21 10 01 01 24 81 14 03 00 03 13 02 00 03 00
// 07 05 81 03 20 00 08
// 07 05 02 03 20 00 08
if (len < 9 + 7 + 7) return false;
// Some common stuff for both XBoxs
uint32_t count_end_points = descriptors[4];
if (count_end_points < 2) return false;
if (descriptors[5] != 0xff) return false; // bInterfaceClass, 3 = HID
rx_ep_ = 0;
uint32_t txep = 0;
uint8_t rx_interval = 0;
uint8_t tx_interval = 0;
rx_size_ = 0;
tx_size_ = 0;
uint32_t descriptor_index = 9;
if (descriptors[descriptor_index + 1] == 0x22) {
if (descriptors[descriptor_index] != 0x14) return false; // only support specific versions...
descriptor_index += descriptors[descriptor_index]; // XBox360w ignore this unknown setup...
}
while ((rx_ep_ == 0) || txep == 0) {
print(" Index:", descriptor_index, DEC);
if (descriptor_index >= len) return false; // we ran off the end and did not get end points
// see if the next data is an end point descript
if ((descriptors[descriptor_index] == 7) && (descriptors[descriptor_index + 1] == 5)) {
if ((descriptors[descriptor_index + 3] == 3) // Type 3...
&& (descriptors[descriptor_index + 4] <= 64)
&& (descriptors[descriptor_index + 5] == 0)) {
// have a bulk EP size
if (descriptors[descriptor_index + 2] & 0x80 ) {
rx_ep_ = descriptors[descriptor_index + 2];
rx_size_ = descriptors[descriptor_index + 4];
rx_interval = descriptors[descriptor_index + 6];
} else {
txep = descriptors[descriptor_index + 2];
tx_size_ = descriptors[descriptor_index + 4];
tx_interval = descriptors[descriptor_index + 6];
}
}
}
descriptor_index += descriptors[descriptor_index]; // setup to look at next one...
}
if ((rx_ep_ == 0) || (txep == 0)) return false; // did not find two end points.
print("JoystickController, rx_ep_=", rx_ep_ & 15);
print("(", rx_size_);
print("), txep=", txep);
print("(", tx_size_);
println(")");
rxpipe_ = new_Pipe(dev, 3, rx_ep_ & 15, 1, rx_size_, rx_interval);
if (!rxpipe_) return false;
txpipe_ = new_Pipe(dev, 3, txep, 0, tx_size_, tx_interval);
if (!txpipe_) {
//free_Pipe(rxpipe_);
return false;
}
rxpipe_->callback_function = rx_callback;
queue_Data_Transfer_Debug(rxpipe_, rxbuf_, rx_size_, this, __LINE__);
txpipe_->callback_function = tx_callback;
if (jtype == XBOXONE) {
queue_Data_Transfer_Debug(txpipe_, xboxone_start_input, sizeof(xboxone_start_input), this, __LINE__);
connected_ = true; // remember that hardware is actually connected...
} else if (jtype == XBOX360) {
queue_Data_Transfer_Debug(txpipe_, xbox360w_inquire_present, sizeof(xbox360w_inquire_present), this, __LINE__);
connected_ = 0; // remember that hardware is actually connected...
} else if (jtype == SWITCH) {
queue_Data_Transfer_Debug(txpipe_, switch_start_input, sizeof(switch_start_input), this, __LINE__);
connected_ = true; // remember that hardware is actually connected...
}
memset(axis, 0, sizeof(axis)); // clear out any data.
joystickType_ = jtype; // remember we are an XBox One.
DBGPrintf(" JoystickController::claim joystickType_ %d\n", joystickType_);
return true;
}
void JoystickController::control(const Transfer_t *transfer)
{
}
/************************************************************/
// Interrupt-based Data Movement
/************************************************************/
void JoystickController::rx_callback(const Transfer_t *transfer)
{
if (!transfer->driver) return;
((JoystickController *)(transfer->driver))->rx_data(transfer);
}
void JoystickController::tx_callback(const Transfer_t *transfer)
{
if (!transfer->driver) return;
((JoystickController *)(transfer->driver))->tx_data(transfer);
}
/************************************************************/
// Interrupt-based Data Movement
// XBox one input data when type == 0x20
// Information came from several places on the web including:
// https://github.com/quantus/xbox-one-controller-protocol
/************************************************************/
// 20 00 C5 0E 00 00 00 00 00 00 F0 06 AD FB 7A 0A DD F7 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
// 20 00 E0 0E 40 00 00 00 00 00 F0 06 AD FB 7A 0A DD F7 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
typedef struct {
uint8_t type;
uint8_t const_0;
uint16_t id;
// From online references button order:
// sync, dummy, start, back, a, b, x, y
// dpad up, down left, right
// lb, rb, left stick, right stick
// Axis:
// lt, rt, lx, ly, rx, ry
//
uint16_t buttons;
int16_t axis[6];
} xbox1data20_t;
typedef struct {
uint8_t state;
uint8_t id_or_type;
uint16_t controller_status;
uint16_t unknown;
// From online references button order:
// sync, dummy, start, back, a, b, x, y
// dpad up, down left, right
// lb, rb, left stick, right stick
// Axis:
// lt, rt, lx, ly, rx, ry
//
uint16_t buttons;
uint8_t lt;
uint8_t rt;
int16_t axis[4];
} xbox360data_t;
typedef struct {
uint8_t state;
uint8_t id_or_type;
// From online references button order:
// sync, dummy, start, back, a, b, x, y
// dpad up, down left, right
// lb, rb, left stick, right stick
// Axis:
// lt, rt, lx, ly, rx, ry
//
uint8_t buttons_h;
uint8_t buttons_l;
uint8_t lt;
uint8_t rt;
int16_t axis[4];
} switchdataUSB_t;
static const uint8_t xbox_axis_order_mapping[] = {3, 4, 0, 1, 2, 5};
void JoystickController::rx_data(const Transfer_t *transfer)
{
#ifdef DEBUG_JOYSTICK
print("JoystickController::rx_data (", joystickType_, DEC);
print("): ");
print_hexbytes((uint8_t*)transfer->buffer, transfer->length);
#endif
if (joystickType_ == XBOXONE) {
// Process XBOX One data
axis_mask_ = 0x3f;
axis_changed_mask_ = 0; // assume none for now
xbox1data20_t *xb1d = (xbox1data20_t *)transfer->buffer;
if ((xb1d->type == 0x20) && (transfer->length >= sizeof (xbox1data20_t))) {
// We have a data transfer. Lets see what is new...
if (xb1d->buttons != buttons) {
buttons = xb1d->buttons;
anychange = true;
joystickEvent = true;
println(" Button Change: ", buttons, HEX);
}
for (uint8_t i = 0; i < sizeof (xbox_axis_order_mapping); i++) {
// The first two values were unsigned.
int axis_value = (i < 2) ? (int)(uint16_t)xb1d->axis[i] : xb1d->axis[i];
if (axis_value != axis[xbox_axis_order_mapping[i]]) {
axis[xbox_axis_order_mapping[i]] = axis_value;
axis_changed_mask_ |= (1 << xbox_axis_order_mapping[i]);
anychange = true;
}
}
joystickEvent = true;
}
} else if (joystickType_ == XBOX360) {
// First byte appears to status - if the byte is 0x8 it is a connect or disconnect of the controller.
xbox360data_t *xb360d = (xbox360data_t *)transfer->buffer;
if (xb360d->state == 0x08) {
if (xb360d->id_or_type != connected_) {
connected_ = xb360d->id_or_type; // remember it...
if (connected_) {
println("XBox360w - Connected type:", connected_, HEX);
// rx_ep_ should be 1, 3, 5, 7 for the wireless convert to 2-5 on led
setLEDs(2 + rx_ep_ / 2); // Right now hard coded to first joystick...
} else {
println("XBox360w - disconnected");
}
}
} else if ((xb360d->id_or_type == 0x00) && (xb360d->controller_status & 0x1300)) {
// Controller status report - Maybe we should save away and allow the user access?
println("XBox360w - controllerStatus: ", xb360d->controller_status, HEX);
} else if (xb360d->id_or_type == 0x01) { // Lets only process report 1.
//const uint8_t *pbuffer = (uint8_t*)transfer->buffer;
//for (uint8_t i = 0; i < transfer->length; i++) DBGPrintf("%02x ", pbuffer[i]);
//DBGPrintf("\n");
if (buttons != xb360d->buttons) {
buttons = xb360d->buttons;
anychange = true;
}
axis_mask_ = 0x3f;
axis_changed_mask_ = 0; // assume none for now
for (uint8_t i = 0; i < 4; i++) {
if (axis[i] != xb360d->axis[i]) {
axis[i] = xb360d->axis[i];
axis_changed_mask_ |= (1 << i);
anychange = true;
}
}
// the two triggers show up as 4 and 5
if (axis[4] != xb360d->lt) {
axis[4] = xb360d->lt;
axis_changed_mask_ |= (1 << 4);
anychange = true;
}
if (axis[5] != xb360d->rt) {
axis[5] = xb360d->rt;
axis_changed_mask_ |= (1 << 5);
anychange = true;
}
if (anychange) joystickEvent = true;
}
} else if (joystickType_ == SWITCH) {
uint8_t packet[8];
if(initialPass_ == true) {
switch(connectedComplete_pending_) {
case 0:
//setup handshake
DBGPrintf("Send Handshake\n");
sw_sendCmdUSB(0x02, SW_CMD_TIMEOUT);
connectedComplete_pending_ = 1;
break;
case 1:
DBGPrintf("Send Hid only\n");
sw_sendCmdUSB(0x04, SW_CMD_TIMEOUT);
connectedComplete_pending_ = 2;
break;
case 2:
//Send report type
DBGPrintf("Enable IMU\n");
packet[0] = 0x01;
sw_sendSubCmdUSB(0x40, packet, 1);
connectedComplete_pending_ = 3;
break;
case 3:
DBGPrintf("Enable Rumble\n");
packet[0] = 0x01;
sw_sendSubCmdUSB(0x48, packet, 1);
connectedComplete_pending_ = 4;
break;
case 4:
DBGPrintf("Enable Std Rpt\n");
packet[0] = 0x30;
sw_sendSubCmdUSB(0x3f, packet, 1);
connectedComplete_pending_ = 5;
case 5:
connectedComplete_pending_ = 0;
initialPass_ = false;
break;
}
}
switchdataUSB_t *switchd = (switchdataUSB_t *)transfer->buffer;
//uint32_t cur_buttons = switchd->buttons_l | (switchd->buttons_m << 8) | (switchd->buttons_h << 16);
uint16_t cur_buttons = (switchd->buttons_h << 8) | switchd->buttons_l;
if (buttons != cur_buttons) {
buttons = cur_buttons;
anychange = true;
}
axis_mask_ = 0x3f;
axis_changed_mask_ = 0; // assume none for now
for (uint8_t i = 0; i < 4; i++) {
if (axis[i] != switchd->axis[i]) {
axis[i] = switchd->axis[i];
axis_changed_mask_ |= (1 << i);
anychange = true;
}
}
//apply stick calibration
float xout, yout;
CalcAnalogStick(xout, yout, axis[0], axis[1], true);
//Serial.printf("Correctd Left Stick: %f, %f\n", xout , yout);
axis[0] = int(round(xout));
axis[1] = int(round(yout));
CalcAnalogStick(xout, yout, axis[2], axis[3], true);
axis[2] = int(round(xout));
axis[3] = int(round(yout));
// the two triggers show up as 4 and 5
if (axis[6] != switchd->lt) {
axis[6] = switchd->lt;
axis_changed_mask_ |= (1 << 4);
anychange = true;
}
if (axis[5] != switchd->rt) {
axis[5] = switchd->rt;
axis_changed_mask_ |= (1 << 5);
anychange = true;
}
if (anychange) joystickEvent = true;
}
queue_Data_Transfer_Debug(rxpipe_, rxbuf_, rx_size_, this, __LINE__);
}
void JoystickController::tx_data(const Transfer_t *transfer)
{
}
void JoystickController::disconnect()
{
axis_mask_ = 0;
axis_changed_mask_ = 0;
// TODO: free resources
}
hidclaim_t JoystickController::claim_bluetooth(BluetoothConnection *btconnection, uint32_t bluetooth_class, uint8_t *remoteName, int type)
{
USBHDBGSerial.printf("JoystickController::claim_bluetooth - Class %x %s\n", bluetooth_class, remoteName);
// If we are already in use than don't grab another one. Likewise don't grab if it is used as USB or HID object
if (btconnect && (btconnection != btconnect)) return CLAIM_NO;
if (mydevice != NULL) return CLAIM_NO;
if ((bluetooth_class & 0x0f00) == 0x500) {
bool name_maps_to_joystick_type = (remoteName && mapNameToJoystickType(remoteName));
if ((bluetooth_class & 0x3C) == 0x08) {
bool claim_interface = (type == 1) || (remoteName == nullptr);
if (name_maps_to_joystick_type) {
switch (joystickType_) {
//case SWITCH:
default:
// others will experiment with trying for HID.
break;
case PS3:
case PS3_MOTION:
special_process_required = SP_PS3_IDS; // PS3 maybe needs different IDS.
// fall through
case PS4:
case XBOXONE:
case SWITCH:
claim_interface = true;
break;
}
}
if (claim_interface) {
// They are telling me to grab it now. SO say yes
USBHDBGSerial.printf("JoystickController::claim_bluetooth Interface\n");
btconnect = btconnection;
btdevice = (Device_t*)btconnect->btController_; // remember this way
btdriver_ = btconnect->btController_;
btdriver_->useHIDProtocol(true);
// Another big hack try calling the connectionComplete to maybe update what reports we are working with
// if (name_maps_to_joystick_type) connectionComplete();
return CLAIM_INTERFACE;
}
}
return CLAIM_REPORT; // let them know we may be interested if there is a HID REport Descriptor
}
return CLAIM_NO;
}
bool JoystickController::process_bluetooth_HID_data(const uint8_t *data, uint16_t length)
{
// Example data from PS4 controller
//01 7e 7f 82 84 08 00 00 00 00
// LX LY RX RY BT BT PS LT RT
DBGPrintf("JoystickController::process_bluetooth_HID_data: data[0]=%x\n", data[0]);
// May have to look at this one with other controllers...
report_id_ = data[0];
if (data[0] == 1) {
//print(" Joystick Data: ");
// print_hexbytes(data, length);
if (length > TOTAL_AXIS_COUNT) length = TOTAL_AXIS_COUNT; // don't overflow arrays...
DBGPrintf(" Joystick Data: ");
for (uint16_t i = 0; i < length; i++) DBGPrintf("%02x ", data[i]);
DBGPrintf("\r\n");
if (joystickType_ == PS3) {
// Quick and dirty hack to match PS3 HID data
uint32_t cur_buttons = data[2] | ((uint16_t)data[3] << 8) | ((uint32_t)data[4] << 16);
if (cur_buttons != buttons) {
buttons = cur_buttons;
joystickEvent = true; // something changed.
}
uint64_t mask = 0x1;
axis_mask_ = 0x27; // assume bits 0, 1, 2, 5
for (uint16_t i = 0; i < 3; i++) {
if (axis[i] != data[i + 6]) {
axis_changed_mask_ |= mask;
axis[i] = data[i + 6];
}
mask <<= 1; // shift down the mask.
}
if (axis[5] != data[9]) {
axis_changed_mask_ |= (1 << 5);
axis[5] = data[9];
}
if (axis[3] != data[18]) {
axis_changed_mask_ |= (1 << 3);
axis[3] = data[18];
}
if (axis[4] != data[19]) {
axis_changed_mask_ |= (1 << 4);
axis[4] = data[19];
}
// Then rest of data
mask = 0x1 << 10; // setup for other bits
for (uint16_t i = 10; i < length; i++ ) {
axis_mask_ |= mask;
if (data[i] != axis[i]) {
axis_changed_mask_ |= mask;
axis[i] = data[i];
}
mask <<= 1; // shift down the mask.
}
} else if (joystickType_ == PS3_MOTION) {
// Quick and dirty PS3_Motion data.
uint32_t cur_buttons = data[1] | ((uint16_t)data[2] << 8) | ((uint32_t)data[3] << 16);
if (cur_buttons != buttons) {
buttons = cur_buttons;
joystickEvent = true; // something changed.
}
// Hard to know what is best here. for now just copy raw data over...
// will do this for now... Format of thought to be data.
// data[1-3] Buttons (mentioned 4 as well but appears to be counter
// axis[0-1] data[5] Trigger, Previous trigger value
// 2-5 Unknown probably place holders for Axis like data for other PS3
// 6 - Time stamp
// 7 - Battery
// 8-19 - Accel: XL, XH, YL, YH, ZL, ZH, XL2, XH2, YL2, YH2, ZL2, ZH2
// 20-31 - Gyro: Xl,Xh,Yl,Yh,Zl,Zh,Xl2,Xh2,Yl2,Yh2,Zl2,Zh2
// 32 - Temp High
// 33 - Temp Low (4 bits) Maybe Magneto x High on other??
uint64_t mask = 0x1;
axis_mask_ = 0; // assume bits 0, 1, 2, 5
// Then rest of data
mask = 0x1 << 10; // setup for other bits
for (uint16_t i = 5; i < length; i++ ) {
axis_mask_ |= mask;
if (data[i] != axis[i - 5]) {
axis_changed_mask_ |= mask;
axis[i - 5] = data[i];
}
mask <<= 1; // shift down the mask.
}
} else if (joystickType_ == XBOXONE) {
// Process XBOX One data
typedef struct __attribute__ ((packed)) {
uint8_t report_type; // 1
int16_t axis[6];
uint32_t buttons;
// From online references button order:
// sync, dummy, start, back, a, b, x, y
// dpad up, down left, right
// lb, rb, left stick, right stick
// Axis:
// lt, rt, lx, ly, rx, ry
//
} xbox1data20bt_t;
static const uint8_t xbox_bt_axis_order_mapping[] = { 0, 1, 2, 3, 4, 5};
axis_mask_ = 0x3f;
axis_changed_mask_ = 0; // assume none for now
xbox1data20bt_t *xb1d = (xbox1data20bt_t *)data;
//if ((xb1d->type == 0x20) && (length >= sizeof (xbox1data20bt_t))) {
// We have a data transfer. Lets see what is new...
if (xb1d->buttons != buttons) {
buttons = xb1d->buttons;
anychange = true;
joystickEvent = true;
println(" Button Change: ", buttons, HEX);
}
for (uint8_t i = 0; i < sizeof (xbox_axis_order_mapping); i++) {
// The first two values were unsigned.
int axis_value = (i < 4) ? (int)(uint16_t)xb1d->axis[i] : xb1d->axis[i];
//DBGPrintf(" axis value [ %d ] = %d \n", i, axis_value);
if (axis_value != axis[xbox_bt_axis_order_mapping[i]]) {
axis[xbox_bt_axis_order_mapping[i]] = axis_value;
axis_changed_mask_ |= (1 << xbox_bt_axis_order_mapping[i]);
anychange = true;
}
}
joystickEvent = true;
//}
} else {
uint64_t mask = 0x1;
axis_mask_ = 0;
for (uint16_t i = 0; i < length; i++ ) {
axis_mask_ |= mask;
if (data[i] != axis[i]) {
axis_changed_mask_ |= mask;
axis[i] = data[i];
}
mask <<= 1; // shift down the mask.
// DBGPrintf("%02x ", axis[i]);
}
}
if (axis_changed_mask_ & axis_change_notify_mask_)
joystickEvent = true;
connected_ = true;
return true;
} else if (data[0] == 0x11) {
DBGPrintf("\n Joystick Data: ");
uint64_t mask = 0x1;
axis_mask_ = 0;
axis_changed_mask_ = 0;
//This moves data to be equivalent to what we see for
//data[0] = 0x01
uint8_t tmp_data[length - 2];
for (uint16_t i = 0; i < (length - 2); i++ ) {
tmp_data[i] = 0;
tmp_data[i] = data[i + 2];
}
/*
* [1] LX, [2] = LY, [3] = RX, [4] = RY
* [5] combo, tri, cir, x, sqr, D-PAD (4bits, 0-3
* [6] R3,L3, opt, share, R2, L2, R1, L1
* [7] Counter (bit7-2), T-PAD, PS
* [8] Left Trigger, [9] Right Trigger
* [10-11] Timestamp
* [12] Battery (0 to 0xff)
* [13-14] acceleration x
* [15-16] acceleration y
* [17-18] acceleration z
* [19-20] gyro x
* [21-22] gyro y
* [23-24] gyro z
* [25-29] unknown
* [30] 0x00,phone,mic, usb, battery level (4bits)
* rest is trackpad? to do implement?
*/
//PS Bit
tmp_data[7] = (tmp_data[7] >> 0) & 1;
//set arrow buttons to axis[0]
tmp_data[10] = tmp_data[5] & ((1 << 4) - 1);
//set buttons for last 4bits in the axis[5]
tmp_data[5] = tmp_data[5] >> 4;
// Lets try mapping the DPAD buttons to high bits
// up up/right right R DN DOWN L DN Left LUP
static const uint32_t dpad_to_buttons[] = {0x10000, 0x30000, 0x20000, 0x60000, 0x40000, 0xC0000, 0x80000, 0x90000};
// Quick and dirty hack to match PS4 HID data
uint32_t cur_buttons = ((uint32_t)tmp_data[7] << 12) | (((uint32_t)tmp_data[6] * 0x10)) | ((uint16_t)tmp_data[5] ) ;
if (tmp_data[10] < 8) cur_buttons |= dpad_to_buttons[tmp_data[10]];
if (cur_buttons != buttons) {
buttons = cur_buttons;
joystickEvent = true; // something changed.
}
mask = 0x1;
axis_mask_ = 0x27; // assume bits 0, 1, 2, 5
for (uint16_t i = 0; i < 3; i++) {
if (axis[i] != tmp_data[i + 1]) {
axis_changed_mask_ |= mask;
axis[i] = tmp_data[i + 1];
}
mask <<= 1; // shift down the mask.
}
if (axis[5] != tmp_data[4]) {
axis_changed_mask_ |= (1 << 5);
axis[5] = tmp_data[4];
}
if (axis[3] != tmp_data[8]) {
axis_changed_mask_ |= (1 << 3);
axis[3] = tmp_data[8];
}
if (axis[4] != tmp_data[9]) {
axis_changed_mask_ |= (1 << 4);
axis[4] = tmp_data[9];
}
//limit for masking
mask = 0x1;
for (uint16_t i = 6; i < (64); i++ ) {
axis_mask_ |= mask;
if (tmp_data[i] != axis[i]) {
axis_changed_mask_ |= mask;
axis[i] = tmp_data[i];
}
mask <<= 1; // shift down the mask.
DBGPrintf("%02x ", axis[i]);
}
DBGPrintf("\n");
//DBGPrintf("Axis Mask (axis_mask_, axis_changed_mask_; %d, %d\n", axis_mask_,axis_changed_mask_);
joystickEvent = true;
connected_ = true;
} else if (joystickType_ == SWITCH) {
if (sw_handle_bt_init_of_joystick(data, length, false))
return true;
return sw_process_HID_data(data, length);
}
return false;
}
//-----------------------------------------------------------------------------
// Process SWITCH controller data messages - split out here as used both for HID
// and Bluetooth HID
//-----------------------------------------------------------------------------
bool JoystickController::sw_handle_bt_init_of_joystick(const uint8_t *data, uint16_t length, bool timer_event)
{
if (data) {
if (data[0] != 0x21) return false;
DBGPrintf("Joystick Acknowledge Command Rcvd! pending: %u SC: %x", connectedComplete_pending_, data[14]);
if (data[13] & 0x80) DBGPrintf(" ACK(%x)\n", data[13]);
else DBGPrintf(" ** NACK(%x) **\n", data[13]);
DBGPrintf(" Joystick Data: ");
for (uint16_t i = 0; i < length; i++) DBGPrintf("%02x ", data[i]);
DBGPrintf("\r\n");
btconnect->stopTimer();
sw_parseAckMsg(data);
DBGPrintf("==========> Connection Pending: %d\n",connectedComplete_pending_);
if (!initialPassBT_) return true; // don't need to process
// Shold maybe double check the right one...
connectedComplete_pending_++;
} else if (timer_event) {
if (!initialPassBT_) return true; // don't need to process
DBGPrintf("\t(%u)Timer event - advance\n", (uint32_t)em_sw_);
connectedComplete_pending_++;
}
DBGPrintf("==========> Connection Pending: %d\n",connectedComplete_pending_);
// only called by BT;
uint8_t packet_[8];
switch (connectedComplete_pending_) {
case 1:
DBGPrintf("\nSet Shipment Low Power State\n");
packet_[0] = 0x00;
sw_sendCmd(0x08, packet_, 1, SW_CMD_TIMEOUT );
break;
case 2:
DBGPrintf("\n Read Left joystick dead zone\n");
packet_[0] = 0x86;
packet_[1] = 0x60;
packet_[2] = 0x00;
packet_[3] = 0x00;
packet_[4] = (0x6097 - 0x6086 + 1);
sw_sendCmd(0x10, packet_, 5, SW_CMD_TIMEOUT);
break;
case 3:
DBGPrintf("\n Read Right stick dead zone\n");
packet_[0] = 0x98;
packet_[1] = 0x60;
packet_[2] = 0x00;
packet_[3] = 0x00;
packet_[4] = (0x60A9 - 0x6098 + 1);
sw_sendCmd(0x10, packet_, 5, SW_CMD_TIMEOUT);
break;
case 4:
DBGPrintf("\n Read: Factory Analog stick calibration\n");
packet_[0] = 0x3D;
packet_[1] = 0x60;
packet_[2] = 0x00;
packet_[3] = 0x00;
packet_[4] = (0x604E - 0x603D + 1);
sw_sendCmd(0x10, packet_, 5, SW_CMD_TIMEOUT);
break;
case 5:
DBGPrintf("\nTry to Get IMU Calibration Data\n");
packet_[0] = 0x20;
packet_[1] = 0x60;
packet_[2] = 0x00;
packet_[3] = 0x00;
packet_[4] = (0x6037 - 0x6020 + 1);
sw_sendCmd(0x10, packet_, 5, SW_CMD_TIMEOUT);
break;
case 6:
DBGPrintf("\nTry to Get IMU Horizontal Offset Data\n");
packet_[0] = 0x80;
packet_[1] = 0x60;
packet_[2] = 0x00;
packet_[3] = 0x00;
packet_[4] = (0x6097 - 0x6080 + 1);
sw_sendCmd(0x10, packet_, 5, SW_CMD_TIMEOUT);
break;
case 7:
DBGPrintf("\nTry to Enable IMU\n");
packet_[0] = 0x01;
sw_sendCmd(0x40, packet_, 1, SW_CMD_TIMEOUT); /* 0x40 IMU, note: 0x00 would disable */
break;
case 8:
DBGPrintf("\nTry to Enable Rumble\n");
packet_[0] = 0x01;
sw_sendCmd(0x48, packet_, 1, SW_CMD_TIMEOUT);
break;
case 9:
DBGPrintf("\nTry to set LEDS\n");
setLEDs(0x1, 0, 0);
break;
case 10:
DBGPrintf("\nSet Report Mode\n");
packet_[0] = 0x30; //0x3F;
sw_sendCmd(0x03, packet_, 1, SW_CMD_TIMEOUT);
break;
case 11:
DBGPrintf("\nTry to set Rumble\n");
setRumble(0xff, 0xff, 0xff);
initialPassBT_ = false;
connectedComplete_pending_ = 0xff;
break;
}
return true;
}
void JoystickController::sw_update_axis(uint8_t axis_index, int new_value)
{
if (axis[axis_index] != new_value) {
axis[axis_index] = new_value;
anychange = true;
axis_changed_mask_ |= (1 << axis_index);
}
}
bool JoystickController::sw_process_HID_data(const uint8_t *data, uint16_t length)
{
if (data[0] == 0x3f) {
// Assume switch:
//<<(02 15 21):48 20 11 00 0D 00 71 00 A1
// 16 bits buttons
// 4 bits hat
// 4 bits <constant>
// 16 bits X
// 16 bits Y
// 16 bits rx
// 16 bits ry
typedef struct __attribute__ ((packed)) {
uint8_t report_type; // 1
uint16_t buttons;
uint8_t hat;
int16_t axis[4];
// From online references button order:
// sync, dummy, start, back, a, b, x, y
// dpad up, down left, right
// lb, rb, left stick, right stick
// Axis:
// lt, rt, lx, ly, rx, ry
//
} switchbt_t;
static const uint8_t switch_bt_axis_order_mapping[] = { 0, 1, 2, 3};
axis_mask_ = 0x1ff;
axis_changed_mask_ = 0; // assume none for now
switchbt_t *sw1d = (switchbt_t *)data;
// We have a data transfer. Lets see what is new...
if (sw1d->buttons != buttons) {
buttons = sw1d->buttons;
anychange = true;
joystickEvent = true;
println(" Button Change: ", buttons, HEX);
}
// We will put the HAT into axis 9 for now..
if (sw1d->hat != axis[9]) {
axis[9] = sw1d->hat;
axis_changed_mask_ |= (1 << 9);
anychange = true;
}
//just a hack for a single joycon.
if(buttons == 0x8000) { //ZL
axis[6] = 1;
} else {
axis[6] = 0;
}
if(buttons == 0x8000) { //ZR
axis[7] = 1;
} else {
axis[7] = 0;
}
for (uint8_t i = 0; i < sizeof (switch_bt_axis_order_mapping); i++) {
// The first two values were unsigned.
int axis_value = (uint16_t)sw1d->axis[i];
//DBGPrintf(" axis value [ %d ] = %d \n", i, axis_value);
if (axis_value != axis[switch_bt_axis_order_mapping[i]]) {
axis[switch_bt_axis_order_mapping[i]] = axis_value;
axis_changed_mask_ |= (1 << switch_bt_axis_order_mapping[i]);
anychange = true;
}
}
joystickEvent = true;
} else if (data[0] == 0x30) {
// Assume switch full report
// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48
// 30 E0 80 00 00 00 D9 37 79 19 98 70 00 0D 0B F1 02 F0 0A 41 FE 25 FC 89 00 F8 0A F0 02 F2 0A 41 FE D9 FB 99 00 D4 0A F6 02 FC 0A 3C FE 69 FB B8 00
//<<(02 15 21):48 20 11 00 0D 00 71 00 A1
//static const uint8_t switch_bt_axis_order_mapping[] = { 0, 1, 2, 3};
axis_mask_ = 0x7fff; // have all of the fields.
axis_changed_mask_ = 0; // assume none for now
// We have a data transfer. Lets see what is new...
uint32_t cur_buttons = data[3] | (data[4] << 8) | (data[5] << 16);
//DBGPrintf("BUTTONS: %x\n", cur_buttons);
if(initialPassButton_ == true) {
if(cur_buttons == 0x8000) {
buttonOffset_ = 0x8000;
} else {
buttonOffset_ = 0;
}
initialPassButton_ = false;
}
cur_buttons = cur_buttons - buttonOffset_;
//Serial.printf("Buttons (3,4,5): %x, %x, %x, %x, %x, %x\n", buttonOffset_, cur_buttons, buttons, data[3], data[4], data[5]);
if (cur_buttons != buttons) {
buttons = cur_buttons;
anychange = true;
joystickEvent = true;
println(" Button Change: ", buttons, HEX);
}
// We will put the HAT into axis 9 for now..
/*
if (sw1d->hat != axis[9]) {
axis[9] = sw1d->hat;
axis_changed_mask_ |= (1 << 9);
anychange = true;
}
*/
uint16_t new_axis[14];
//Joystick data
new_axis[0] = data[6] | ((data[7] & 0xF) << 8); //xl
new_axis[1] = (data[7] >> 4) | (data[8] << 4); //yl
new_axis[2] = data[9] | ((data[10] & 0xF) << 8); //xr
new_axis[3] = (data[10] >> 4) | (data[11] << 4); //yr
//Kludge to get trigger buttons tripping
if(buttons == 0x40) { //R1
new_axis[5] = 1;
} else {
new_axis[5] = 0;
}
if(buttons == 0x400000) { //L1
new_axis[4] = 1;
} else {
new_axis[4] = 0;
}
if(buttons == 0x400040) {
new_axis[4] = 0xff;
new_axis[5] = 0xff;
}
if(buttons == 0x800000) { //ZL
new_axis[6] = 0xff;
} else {
new_axis[6] = 0;
}
if(buttons == 0x80) { //ZR
new_axis[7] = 0xff;
} else {
new_axis[7] = 0;
}
if(buttons == 0x800080) {
new_axis[6] = 0xff;
new_axis[7] = 0xff;
}
sw_update_axis(8, (int16_t)(data[13] | (data[14] << 8))); //ax
sw_update_axis(9, (int16_t)(data[15] | (data[16] << 8))); //ay
sw_update_axis(10, (int16_t)(data[17] | (data[18] << 8))); //az
sw_update_axis(11, (int16_t)(data[19] | (data[20] << 8))); //gx
sw_update_axis(12, (int16_t)(data[21] | (data[22] << 8))); //gy
sw_update_axis(13, (int16_t)(data[23] | (data[24] << 8))); //gz
sw_update_axis(14, data[2] >> 4); //Battery level, 8=full, 6=medium, 4=low, 2=critical, 0=empty
//map axes
for (uint8_t i = 0; i < 8; i++) {
// The first two values were unsigned.
if (new_axis[i] != axis[i]) {
axis[i] = new_axis[i];
axis_changed_mask_ |= (1 << i);
anychange = true;
}
}
//apply stick calibration
float xout, yout;
CalcAnalogStick(xout, yout, new_axis[0], new_axis[1], true);
//Serial.printf("Correctd Left Stick: %f, %f\n", xout , yout);
axis[0] = int(round(xout));
axis[1] = int(round(yout));
CalcAnalogStick(xout, yout, new_axis[2], new_axis[3], true);
axis[2] = int(round(xout));
axis[3] = int(round(yout));
joystickEvent = true;
initialPass_ = false;
}
return false;
}
hidclaim_t JoystickController::bt_claim_collection(BluetoothConnection *btconnection, uint32_t bluetooth_class, uint32_t topusage)
{
USBHDBGSerial.printf("JoystickController::bt_claim_collection(%p) Connection:%p class:%x Top:%x\n", this, btconnection, bluetooth_class, topusage);
if (mydevice != NULL) return CLAIM_NO; // claimed by some other...
if (btconnect && (btconnect != btconnection)) return CLAIM_NO;
// We will claim if BOOT Keyboard.
if (topusage != 0x10004 && topusage != 0x10005 && topusage != 0x10008) return CLAIM_NO;
// only claim from one physical device
USBHDBGSerial.printf("\tJoystickController claim collection\n");
btconnect = btconnection;
btdevice = (Device_t*)btconnect->btController_; // remember this way
// experiment? See if we can now tell system to maybe set which report we want
connectionComplete();
return CLAIM_REPORT;
}
void JoystickController::bt_hid_input_begin(uint32_t topusage, uint32_t type, int lgmin, int lgmax)
{
hid_input_begin(topusage, type, lgmin, lgmax);
}
void JoystickController::bt_hid_input_data(uint32_t usage, int32_t value)
{
hid_input_data(usage, value);
}
void JoystickController::bt_hid_input_end()
{
hid_input_end();
}
void JoystickController::bt_disconnect_collection(Device_t *dev)
{
disconnect_collection(dev);
}
bool JoystickController::mapNameToJoystickType(const uint8_t *remoteName)
{
// Sort of a hack, but try to map the name given from remote to a type...
if (strncmp((const char *)remoteName, "Wireless Controller", 19) == 0) {
DBGPrintf(" JoystickController::mapNameToJoystickType %s - set to PS4\n", remoteName);
joystickType_ = PS4;
} else if (strncmp((const char *)remoteName, "PLAYSTATION(R)3", 15) == 0) {
DBGPrintf(" JoystickController::mapNameToJoystickType %x %s - set to PS3\n", (uint32_t)this, remoteName);
joystickType_ = PS3;
} else if (strncmp((const char *)remoteName, "Navigation Controller", 21) == 0) {
DBGPrintf(" JoystickController::mapNameToJoystickType %x %s - set to PS3\n", (uint32_t)this, remoteName);
joystickType_ = PS3;
} else if (strncmp((const char *)remoteName, "Motion Controller", 17) == 0) {
DBGPrintf(" JoystickController::mapNameToJoystickType %x %s - set to PS3 Motion\n", (uint32_t)this, remoteName);
joystickType_ = PS3_MOTION;
} else if (strncmp((const char *)remoteName, "Xbox Wireless", 13) == 0) {
DBGPrintf(" JoystickController::mapNameToJoystickType %x %s - set to XBOXONE\n", (uint32_t)this, remoteName);
joystickType_ = XBOXONE;
} else if (strncmp((const char *)remoteName, "Pro Controller", 13) == 0) {
DBGPrintf(" JoystickController::mapNameToJoystickType %x %s - set to Nintendo Pro Controller\n", (uint32_t)this, remoteName);
joystickType_ = SWITCH;
} else if(strncmp((const char *)remoteName, "Joy-Con (R)", 11) == 0) {
DBGPrintf(" JoystickController::mapNameToJoystickType %x %s - set to Nintendo Joy-Con (R) Controller\n", (uint32_t)this, remoteName);
joystickType_ = SWITCH;
} else if(strncmp((const char *)remoteName, "Joy-Con (L)", 11) == 0) {
DBGPrintf(" JoystickController::mapNameToJoystickType %x %s - set to Nintendo Joy-Con (L) Controller\n", (uint32_t)this, remoteName);
joystickType_ = SWITCH;
} else {
DBGPrintf(" JoystickController::mapNameToJoystickType %s - Unknown\n", remoteName);
}
DBGPrintf(" Joystick Type: %d\n", joystickType_);
return true;
}
bool JoystickController::remoteNameComplete(const uint8_t *remoteName)
{
// Sort of a hack, but try to map the name given from remote to a type...
if (mapNameToJoystickType(remoteName)) {
switch (joystickType_) {
case PS4: special_process_required = SP_NEED_CONNECT; break;
case PS3: special_process_required = SP_PS3_IDS; break;
case PS3_MOTION: special_process_required = SP_PS3_IDS; break;
default:
break;
}
}
return true;
}
void JoystickController::connectionComplete()
{
connectedComplete_pending_ = 0;
DBGPrintf(" JoystickController::connectionComplete %x joystick type %d\n", (uint32_t)this, joystickType_);
switch (joystickType_) {
case PS4:
{
uint8_t packet[2];
packet[0] = 0x43; // HID BT Get_report (0x40) | Report Type (Feature 0x03)
packet[1] = 0x02; // Report ID
DBGPrintf("Set PS4 report\n");
delay(1);
btdriver_->sendL2CapCommand(packet, sizeof(packet), BluetoothController::CONTROL_SCID /*0x40*/);
}
break;
case PS3:
{
uint8_t packet[6];
packet[0] = 0x53; // HID BT Set_report (0x50) | Report Type (Feature 0x03)
packet[1] = 0xF4; // Report ID
packet[2] = 0x42; // Special PS3 Controller enable commands
packet[3] = 0x03;
packet[4] = 0x00;
packet[5] = 0x00;
DBGPrintf("enable six axis\n");
delay(1);
btdriver_->sendL2CapCommand(packet, sizeof(packet), BluetoothController::CONTROL_SCID);
}
break;
case PS3_MOTION:
setLEDs(0, 0xff, 0); // Maybe try setting to green?
break;
case SWITCH:
{
// See if we can set a specific report
#if 1
uint8_t packet_[8];
DBGPrintf("Request Device Info......\n");
packet_[0] = 0x00;
sw_sendCmd(0x02, packet_, 1, SW_CMD_TIMEOUT);
connectedComplete_pending_ = 0;
DBGPrintf("Config Complete!\n");
#endif
}
default:
break;
}
}
void JoystickController::release_bluetooth()
{
btdevice = nullptr; // remember this way
btdriver_ = nullptr;
connected_ = false;
special_process_required = false;
}
bool JoystickController::PS3Pair(uint8_t* bdaddr) {
if (!driver_) return false;
if (joystickType_ == PS3) {
/* Set the internal Bluetooth address */
txbuf_[0] = 0x01;
txbuf_[1] = 0x00;
for (uint8_t i = 0; i < 6; i++)
txbuf_[i + 2] = bdaddr[5 - i]; // Copy into buffer, has to be written reversed, so it is MSB first
// bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF5), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
return driver_->sendControlPacket(0x21, 9, 0x3f5, 0, 8, txbuf_);
} else if (joystickType_ == PS3_MOTION) {
// Slightly different than other PS3 units...
txbuf_[0] = 0x05;
for (uint8_t i = 0; i < 6; i++)
txbuf_[i + 1] = bdaddr[i]; // Order different looks like LSB First?
txbuf_[7] = 0x10;
txbuf_[8] = 0x01;
txbuf_[9] = 0x02;
txbuf_[10] = 0x12;
// bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF5), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
return driver_->sendControlPacket(0x21, 9, 0x305, 0, 11, txbuf_);
}
return false;
}
//=============================================================================
// Retrieve the current pairing information for a PS4...
//=============================================================================
bool JoystickController::PS4GetCurrentPairing(uint8_t* bdaddr) {
if (!driver_ || (joystickType_ != PS4)) return false;
// Try asking PS4 for information
memset(txbuf_, 0, 0x10);
send_Control_packet_active_ = true;
if (!driver_->sendControlPacket(0xA1, 1, 0x312, 0, 0x10, txbuf_))
return false;
elapsedMillis em = 0;
while ((em < 500) && send_Control_packet_active_) ;
memcpy(bdaddr, &txbuf_[10], 6);
return true;
}
bool JoystickController::PS4Pair(uint8_t* bdaddr) {
if (!driver_ || (joystickType_ != PS4)) return false;
// Lets try to setup a message to send...
static const uint8_t ps4_pair_msg[] PROGMEM = {0x13, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0x56, 0xE8, 0x81, 0x38, 0x08, 0x06, 0x51, 0x41, 0xC0, 0x7F, 0x12, 0xAA, 0xD9, 0x66, 0x3C, 0xCE
};
// Note the above 0xff sare place holders for the bdaddr
memcpy(txbuf_, ps4_pair_msg, sizeof(ps4_pair_msg));
for (uint8_t i = 0; i < 6; i++)
txbuf_[i + 1] = bdaddr[i];
send_Control_packet_active_ = true;
return driver_->sendControlPacket(0x21, 0x09, 0x0313, 0, sizeof(ps4_pair_msg), txbuf_);
}
//Nintendo Switch functions
void JoystickController::sw_sendCmd(uint8_t cmd, uint8_t *data, uint16_t size, uint32_t timeout) {
struct SWProBTSendConfigData *packet = (struct SWProBTSendConfigData *)txbuf_ ;
memset((void*)packet, 0, sizeof(struct SWProBTSendConfigData));
packet->hid_hdr = 0xA2; // HID BT Get_report (0xA0) | Report Type (Output)
packet->id = 1;
packet->gpnum = switch_packet_num;
switch_packet_num = (switch_packet_num + 1) & 0x0f;
// 2-9 rumble data;
packet->rumbleDataL[0] = 0x00;
packet->rumbleDataL[1] = 0x01;
packet->rumbleDataL[2] = 0x40;
packet->rumbleDataL[3] = 0x40;
packet->rumbleDataR[0] = 0x00;
packet->rumbleDataR[1] = 0x01;
packet->rumbleDataR[2] = 0x40;
packet->rumbleDataR[3] = 0x40;
packet->subCommand = cmd; // Report ID
for(uint16_t i = 0; i < size; i++) {
packet->subCommandData[i] = data[i];
}
if (btdriver_) {
if (timeout != 0) {
btconnect->startTimer(timeout);
}
btdriver_->sendL2CapCommand((uint8_t *)packet, sizeof(struct SWProBTSendConfigData), BluetoothController::INTERRUPT_SCID /*0x40*/);
}
else Serial.printf("\n####### sw_sendCMD(%x %p %u) called with btdriver_ == 0 ", cmd, data, size);
em_sw_ = 0;
}
void JoystickController::sw_sendCmdUSB(uint8_t cmd, uint32_t timeout) {
DBGPrintf("sw_sendCmdUSB: cmd:%x, timeout:%x\n", cmd, timeout);
//sub-command
txbuf_[0] = 0x80;
txbuf_[1] = cmd;
sw_last_cmd_sent_ = cmd; // remember which command we sent
if(driver_) {
if (timeout != 0) {
driver_->startTimer(timeout);
}
driver_->sendPacket(txbuf_, 2);
em_sw_ = 0;
} else {
if (!queue_Data_Transfer_Debug(txpipe_, txbuf_, 18, this, __LINE__)) {
println("switch transfer fail");
}
}
}
void JoystickController::sw_sendSubCmdUSB(uint8_t sub_cmd, uint8_t *data, uint8_t size, uint32_t timeout) {
DBGPrintf("sw_sendSubCmdUSB(%x, %p, %u): ", sub_cmd, size);
for (uint8_t i = 0; i < size; i++) DBGPrintf(" %02x", data[i]);
DBGPrintf("\n");
memset(txbuf_, 0, 32); // make sure it is cleared out
txbuf_[0] = 0x01;
// Now add in subcommand data:
// Probably do this better soon
txbuf_[ 1] = switch_packet_num = (switch_packet_num + 1) & 0x0f; //
txbuf_[ 2] = 0x00;
txbuf_[ 3] = 0x01;
txbuf_[ 4] = 0x40;
txbuf_[ 5] = 0x40;
txbuf_[ 6] = 0x00;
txbuf_[ 7] = 0x01;
txbuf_[ 8] = 0x40;
txbuf_[ 9] = 0x40;
txbuf_[ 10] = sub_cmd;
//sub-command
for(uint16_t i = 0; i < size; i++) {
txbuf_[i + 11] = data[i];
}
println("USB send sub cmd: driver? ", (uint32_t)driver_, HEX);
print_hexbytes((uint8_t*)txbuf_, 32);
if(driver_) {
driver_->sendPacket(txbuf_, 32);
if (timeout != 0) {
driver_->startTimer(timeout);
}
} else if (txpipe_) {
if (!queue_Data_Transfer_Debug(txpipe_, txbuf_, 32, this, __LINE__)) {
println("switch transfer fail");
}
}
em_sw_ = 0;
if (!timeout) delay(100);
}
void JoystickController::sw_parseAckMsg(const uint8_t *buf_)
{
int16_t data[6];
uint8_t offset = 20;
uint8_t icount = 0;
//uint8_t packet_[8];
if((buf_[14] == 0x10 && buf_[15] == 0x20 && buf_[16] == 0x60)) {
//parse IMU calibration
DBGPrintf("===> IMU Calibration \n");
for(uint8_t i = 0; i < 3; i++) {
SWIMUCal.acc_offset[i] = (int16_t)(buf_[icount+offset] | (buf_[icount+offset+1] << 8));
SWIMUCal.acc_sensitivity[i] = (int16_t)(buf_[icount+offset+6] | (buf_[icount+offset+1+6] << 8));
SWIMUCal.gyro_offset[i] = (int16_t)(buf_[icount+offset+12] | (buf_[icount+offset+1+12] << 8));
SWIMUCal.gyro_sensitivity[i] = (int16_t)(buf_[icount+offset+18] | (buf_[icount+offset+1+18] << 8));
icount = i * 2;
}
for(uint8_t i = 0; i < 3; i++) {
DBGPrintf("\t %d, %d, %d, %d\n", SWIMUCal.acc_offset[i], SWIMUCal.acc_sensitivity[i],
SWIMUCal.gyro_offset[i], SWIMUCal.gyro_sensitivity[i]);
}
} else if((buf_[14] == 0x10 && buf_[15] == 0x80 && buf_[16] == 0x60)) {
//parse IMU calibration
DBGPrintf("===> IMU Calibration Offsets \n");
for(uint8_t i = 0; i < 3; i++) {
SWIMUCal.acc_offset[i] = (int16_t)(buf_[i+offset] | (buf_[i+offset+1] << 8));
}
for(uint8_t i = 0; i < 3; i++) {
DBGPrintf("\t %d\n", SWIMUCal.acc_offset[i]);
}
} else if((buf_[14] == 0x10 && buf_[15] == 0x3D && buf_[16] == 0x60)){ //left stick
offset = 20;
data[0] = ((buf_[1+offset] << 8) & 0xF00) | buf_[0+offset];
data[1] = (buf_[2+offset] << 4) | (buf_[1+offset] >> 4);
data[2] = ((buf_[4+offset] << 8) & 0xF00) | buf_[3+offset];
data[3] = (buf_[5+offset] << 4) | (buf_[4+offset] >> 4);
data[4] = ((buf_[7+offset] << 8) & 0xF00) | buf_[6+offset];
data[5] = (buf_[8+offset] << 4) | (buf_[7+offset] >> 4);
SWStickCal.lstick_center_x = data[2];
SWStickCal.lstick_center_y = data[3];
SWStickCal.lstick_x_min = SWStickCal.lstick_center_x - data[0];
SWStickCal.lstick_x_max = SWStickCal.lstick_center_x + data[4];
SWStickCal.lstick_y_min = SWStickCal.lstick_center_y - data[1];
SWStickCal.lstick_y_max = SWStickCal.lstick_center_y + data[5];
DBGPrintf("Left Stick Calibrataion\n");
DBGPrintf("center: %d, %d\n", SWStickCal.lstick_center_x, SWStickCal.lstick_center_y );
DBGPrintf("min/max x: %d, %d\n", SWStickCal.lstick_x_min, SWStickCal.lstick_x_max);
DBGPrintf("min/max y: %d, %d\n", SWStickCal.lstick_y_min, SWStickCal.lstick_y_max);
//right stick
offset = 29;
data[0] = ((buf_[1+offset] << 8) & 0xF00) | buf_[0+offset];
data[1] = (buf_[2+offset] << 4) | (buf_[1+offset] >> 4);
data[2] = ((buf_[4+offset] << 8) & 0xF00) | buf_[3+offset];
data[3] = (buf_[5+offset] << 4) | (buf_[4+offset] >> 4);
data[4] = ((buf_[7+offset] << 8) & 0xF00) | buf_[6+offset];
data[5] = (buf_[8+offset] << 4) | (buf_[7+offset] >> 4);
SWStickCal.rstick_center_x = data[0];
SWStickCal.rstick_center_y = data[1];
SWStickCal.rstick_x_min = SWStickCal.rstick_center_x - data[2];
SWStickCal.rstick_x_max = SWStickCal.rstick_center_x + data[4];
SWStickCal.rstick_y_min = SWStickCal.rstick_center_y - data[3];
SWStickCal.rstick_y_max = SWStickCal.rstick_center_y + data[5];
DBGPrintf("\nRight Stick Calibrataion\n");
DBGPrintf("center: %d, %d\n", SWStickCal.rstick_center_x, SWStickCal.rstick_center_y );
DBGPrintf("min/max x: %d, %d\n", SWStickCal.rstick_x_min, SWStickCal.rstick_x_max);
DBGPrintf("min/max y: %d, %d\n", SWStickCal.rstick_y_min, SWStickCal.rstick_y_max);
} else if((buf_[14] == 0x10 && buf_[15] == 0x86 && buf_[16] == 0x60)){ //left stick deadzone_left
offset = 20;
SWStickCal.deadzone_left = (((buf_[4 + offset] << 8) & 0xF00) | buf_[3 + offset]);
DBGPrintf("\nLeft Stick Deadzone\n");
DBGPrintf("deadzone: %d\n", SWStickCal.deadzone_left);
} else if((buf_[14] == 0x10 && buf_[15] == 0x98 && buf_[16] == 0x60)){ //left stick deadzone_left
offset = 20;
SWStickCal.deadzone_left = (((buf_[4 + offset] << 8) & 0xF00) | buf_[3 + offset]);
DBGPrintf("\nRight Stick Deadzone\n");
DBGPrintf("deadzone: %d\n", SWStickCal.deadzone_right);
} else if((buf_[14] == 0x10 && buf_[15] == 0x10 && buf_[16] == 0x80)){
DBGPrintf("\nUser Calibration Rcvd!\n");
}
}
bool JoystickController::sw_getIMUCalValues(float *accel, float *gyro)
{
// Fail if we don't have actually have those fields. We need axis 8-13 for this
if ((axis_mask_ & 0x3f00) != 0x3f00) return false;
for(uint8_t i = 0; i < 3; i++) {
accel[i] = (float)(axis[8+i] - SWIMUCal.acc_offset[i]) * (1.0f / (float)SWIMUCal.acc_sensitivity[i]) * 4.0f;
gyro[i] = (float)(axis[11+i] - SWIMUCal.gyro_offset[i]) * (816.0f / (float)SWIMUCal.gyro_sensitivity[i]);
}
return true;
}
#define sw_scale 2048
void JoystickController::CalcAnalogStick
(
float &pOutX, // out: resulting stick X value
float &pOutY, // out: resulting stick Y value
int16_t x, // in: initial stick X value
int16_t y, // in: initial stick Y value
bool isLeft // are we dealing with left or right Joystick
)
{
// uint16_t x_calc[3], // calc -X, CenterX, +X
// uint16_t y_calc[3] // calc -Y, CenterY, +Y
int16_t min_x;
int16_t max_x;
int16_t center_x;
int16_t min_y; // analog joystick calibration
int16_t max_y;
int16_t center_y;
if(isLeft) {
min_x = SWStickCal.lstick_x_min;
max_x = SWStickCal.lstick_x_max;
center_x = SWStickCal.lstick_center_x;
min_y = SWStickCal.lstick_y_min;
max_y = SWStickCal.lstick_y_max;
center_y = SWStickCal.lstick_center_y;
} else {
min_x = SWStickCal.rstick_x_min;
max_x = SWStickCal.rstick_x_max;
center_x = SWStickCal.rstick_center_x;
min_y = SWStickCal.rstick_y_min;
max_y = SWStickCal.rstick_y_max;
center_y = SWStickCal.rstick_center_y;
}
float x_f, y_f;
// Apply Joy-Con center deadzone. 0xAE translates approx to 15%. Pro controller has a 10% () deadzone
float deadZoneCenter = 0.15f;
// Add a small ammount of outer deadzone to avoid edge cases or machine variety.
float deadZoneOuter = 0.0f;
// convert to float based on calibration and valid ranges per +/-axis
x = clamp(x, min_x, max_x);
y = clamp(y, min_y, max_y);
if (x >= center_x) {
x_f = (float)(x - center_x) / (float)(max_x - center_x);
} else {
x_f = -((float)(x - center_x) / (float)(min_x - center_x));
}
if (y >= center_y) {
y_f = (float)(y - center_y) / (float)(max_y - center_y);
} else {
y_f = -((float)(y - center_y) / (float)(min_y - center_y));
}
// Interpolate zone between deadzones
float mag = sqrtf(x_f*x_f + y_f*y_f);
if (mag > deadZoneCenter) {
// scale such that output magnitude is in the range [0.0f, 1.0f]
float legalRange = 1.0f - deadZoneOuter - deadZoneCenter;
float normalizedMag = min(1.0f, (mag - deadZoneCenter) / legalRange);
float scale = normalizedMag / mag;
pOutX = (x_f * scale * sw_scale);
pOutY = (y_f * scale * sw_scale);
} else {
// stick is in the inner dead zone
pOutX = 0.0f;
pOutY = 0.0f;
}
}