visus
/
teensy-usbhost
Watch 1
Star 0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
Browse Source

Merge pull request #14 from KurtE/PS3_Motion_WIP2-Bluetooth 

Ps3 motion wip2 bluetooth
main
KurtE 6 years ago
parent
3a467a8e88 14c1fe646d
commit
b39713aafc
No account linked to committer's email address
10 changed files with 545 additions and 217 deletions
Split View Show Diff Stats
  1. +7
    -5
      USBHost_t36.h
  2. +4
    -1
      bluetooth.cpp
  3. +15
    -13
      ehci.cpp
  4. +218
    -48
      examples/Bluetooth/JoystickBT/JoystickBT.ino
  5. +83
    -2
      examples/Bluetooth/JoystickBT/helperPS.ino
  6. +1
    -1
      examples/USBHost_viewer/USBHost_viewer.ino
  7. +1
    -1
      hid.cpp
  8. +214
    -145
      joystick.cpp
  9. +1
    -0
      keywords.txt
  10. +1
    -1
      mouse.cpp

+ 7
- 5
USBHost_t36.h View File

@@ -58,7 +58,7 @@


//#define USBHOST_PRINT_DEBUG
#define USBHDBGSerial Serial1
//#define USBHDBGSerial Serial1


#ifndef USBHDBGSerial
@@ -911,10 +911,11 @@ public:

// set functions functionality depends on underlying joystick.
bool setRumble(uint8_t lValue, uint8_t rValue, uint8_t timeout=0xff);
// setLEDs on PS4(RGB), PS3 simple LED setting (only uses lr)
bool setLEDs(uint8_t lr, uint8_t lg=0, uint8_t lb=0); // sets Leds,
// setLEDs on PS4(RGB), PS3 simple LED setting (only uses lb)
bool setLEDs(uint8_t lr, uint8_t lg, uint8_t lb); // sets Leds,
bool inline setLEDs(uint32_t leds) {return setLEDs((leds >> 16) & 0xff, (leds >> 8) & 0xff, leds & 0xff);} // sets Leds - passing one arg for all leds
enum { STANDARD_AXIS_COUNT = 10, ADDITIONAL_AXIS_COUNT = 54, TOTAL_AXIS_COUNT = (STANDARD_AXIS_COUNT+ADDITIONAL_AXIS_COUNT) };
typedef enum { UNKNOWN=0, PS3, PS4, XBOXONE, XBOX360} joytype_t;
typedef enum { UNKNOWN=0, PS3, PS4, XBOXONE, XBOX360, PS3_MOTION} joytype_t;
joytype_t joystickType() {return joystickType_;}

// PS3 pair function. hack, requires that it be connect4ed by USB and we have the address of the Bluetooth dongle...
@@ -954,6 +955,7 @@ private:
joytype_t mapVIDPIDtoJoystickType(uint16_t idVendor, uint16_t idProduct, bool exclude_hid_devices);
bool transmitPS4UserFeedbackMsg();
bool transmitPS3UserFeedbackMsg();
bool transmitPS3MotionUserFeedbackMsg();
bool mapNameToJoystickType(const uint8_t *remoteName);

bool anychange = false;
@@ -1685,7 +1687,7 @@ public:
const uint8_t* myBDAddr(void) {return my_bdaddr_;}

// BUGBUG version to allow some of the controlled objects to call?
enum {CONTROL_SCID=-1};
enum {CONTROL_SCID=-1, INTERRUPT_SCID=-2};
void sendL2CapCommand(uint8_t* data, uint8_t nbytes, int channel = (int)0x0001);

protected:

+ 4
- 1
bluetooth.cpp View File

@@ -31,7 +31,7 @@
#define println USBHost::println_//#define DEBUG_BT

//#define DEBUG_BT
#define DEBUG_BT_VERBOSE
//#define DEBUG_BT_VERBOSE

#ifndef DEBUG_BT
#undef DEBUG_BT_VERBOSE
@@ -1343,6 +1343,9 @@ void BluetoothController::sendL2CapCommand(uint8_t* data, uint8_t nbytes, int ch
case CONTROL_SCID:
channel_out = control_scid_;
break;
case INTERRUPT_SCID:
channel_out = interrupt_scid_;
break;
default:
channel_out = (uint16_t)channel;
}

+ 15
- 13
ehci.cpp View File

@@ -745,21 +745,23 @@ bool USBHost::queue_Data_Transfer(Pipe_t *pipe, void *buffer, uint32_t len, USBD
transfer = allocate_Transfer();
if (!transfer) return false;
data = transfer;
for (count=(len >> 14); count; count--) {
next = allocate_Transfer();
if (!next) {
// free already-allocated qTDs
while (1) {
next = (Transfer_t *)transfer->qtd.next;
free_Transfer(transfer);
if (transfer == data) break;
transfer = next;
if (len) {
for (count=((len-1) >> 14); count; count--) {
next = allocate_Transfer();
if (!next) {
// free already-allocated qTDs
while (1) {
next = (Transfer_t *)transfer->qtd.next;
free_Transfer(transfer);
if (transfer == data) break;
transfer = next;
}
return false;
}
return false;
data->qtd.next = (uint32_t)next;
data = next;
}
data->qtd.next = (uint32_t)next;
data = next;
}
}
// last qTD needs info for followup
data->qtd.next = 1;
data->pipe = pipe;

+ 218
- 48
examples/Bluetooth/JoystickBT/JoystickBT.ino View File

@@ -37,6 +37,13 @@ USBHIDInput *hiddrivers[] = {&joystick1, &rawhid1, &rawhid2};
const char * hid_driver_names[CNT_DEVICES] = {"Joystick1", "RawHid1", "RawHid2"};

bool hid_driver_active[CNT_DEVICES] = {false, false, false};

BTHIDInput *bthiddrivers[] = {&joystick1};
#define CNT_BTHIDDEVICES (sizeof(bthiddrivers)/sizeof(bthiddrivers[0]))
const char * bthid_driver_names[CNT_HIDDEVICES] = {"joystick"};
bool bthid_driver_active[CNT_HIDDEVICES] = {false};


bool show_changed_only = false;
bool show_raw_data = false;
bool show_changed_data = false;
@@ -49,6 +56,15 @@ int psAxis[64];
bool first_joystick_message = true;
uint8_t last_bdaddr[6] = {0, 0, 0, 0, 0, 0};

// ps3 motion on USB does not do much, but see if we can pair it and maybe change
// color of bulb...
uint32_t PS3_MOTION_timer = 0;
uint8_t PS3_MOTION_tried_to_pair_state = 0;
#define PS3_MOTION_PERIOD 2500 // not sure yet what would be good period for this..

//=============================================================================
// Setup
//=============================================================================
void setup()
{

@@ -65,7 +81,9 @@ void setup()
rawhid2.attachReceive(OnReceiveHidData);
}


//=============================================================================
// Loop
//=============================================================================
void loop()
{
myusb.Task();
@@ -90,51 +108,9 @@ void loop()
}
}
}

for (uint8_t i = 0; i < CNT_DEVICES; i++) {
if (*drivers[i] != driver_active[i]) {
if (driver_active[i]) {
Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
driver_active[i] = false;
} else {
Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
driver_active[i] = true;

const uint8_t *psz = drivers[i]->manufacturer();
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
psz = drivers[i]->product();
if (psz && *psz) Serial.printf(" product: %s\n", psz);
psz = drivers[i]->serialNumber();
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);

if (drivers[i] == &bluet) {
const uint8_t *bdaddr = bluet.myBDAddr();
// remember it...
Serial.printf(" BDADDR: %x:%x:%x:%x:%x:%x\n", bdaddr[0], bdaddr[1], bdaddr[2], bdaddr[3], bdaddr[4], bdaddr[5]);
for (uint8_t i = 0; i < 6; i++) last_bdaddr[i] = bdaddr[i];
}
}
}
}

for (uint8_t i = 0; i < CNT_HIDDEVICES; i++) {
if (*hiddrivers[i] != hid_driver_active[i]) {
if (hid_driver_active[i]) {
Serial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);
hid_driver_active[i] = false;
} else {
Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
hid_driver_active[i] = true;

const uint8_t *psz = hiddrivers[i]->manufacturer();
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
psz = hiddrivers[i]->product();
if (psz && *psz) Serial.printf(" product: %s\n", psz);
psz = hiddrivers[i]->serialNumber();
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);
}
}
}
// check to see if the device list has changed:
UpdateActiveDeviceInfo();
processPS3MotionTimer();

if (joystick1.available()) {
if (first_joystick_message) {
@@ -164,6 +140,9 @@ void loop()
case JoystickController::PS3:
displayPS3Data();
break;
case JoystickController::PS3_MOTION:
displayPS3MotionData();
break;
case JoystickController::XBOXONE:
case JoystickController::XBOX360:
displayXBoxData();
@@ -196,13 +175,96 @@ void loop()
}
}

//=============================================================================
// UpdateActiveDeviceInfo
//=============================================================================
void UpdateActiveDeviceInfo() {
for (uint8_t i = 0; i < CNT_DEVICES; i++) {
if (*drivers[i] != driver_active[i]) {
if (driver_active[i]) {
Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
driver_active[i] = false;
} else {
Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
driver_active[i] = true;

const uint8_t *psz = drivers[i]->manufacturer();
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
psz = drivers[i]->product();
if (psz && *psz) Serial.printf(" product: %s\n", psz);
psz = drivers[i]->serialNumber();
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);

if (drivers[i] == &bluet) {
const uint8_t *bdaddr = bluet.myBDAddr();
// remember it...
Serial.printf(" BDADDR: %x:%x:%x:%x:%x:%x\n", bdaddr[0], bdaddr[1], bdaddr[2], bdaddr[3], bdaddr[4], bdaddr[5]);
for (uint8_t i = 0; i < 6; i++) last_bdaddr[i] = bdaddr[i];
}
}
}
}

for (uint8_t i = 0; i < CNT_HIDDEVICES; i++) {
if (*hiddrivers[i] != hid_driver_active[i]) {
if (hid_driver_active[i]) {
Serial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);
hid_driver_active[i] = false;
} else {
Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
hid_driver_active[i] = true;

const uint8_t *psz = hiddrivers[i]->manufacturer();
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
psz = hiddrivers[i]->product();
if (psz && *psz) Serial.printf(" product: %s\n", psz);
psz = hiddrivers[i]->serialNumber();
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);

// See if this is our joystick object...
if (hiddrivers[i] == &joystick1) {
Serial.printf(" Joystick type: %d\n", joystick1.joystickType());
if (joystick1.joystickType() == JoystickController::PS3_MOTION) {
Serial.println(" PS3 Motion detected");
PS3_MOTION_timer = millis(); // set time for last event
PS3_MOTION_tried_to_pair_state = 0;
}
}

}
}
}
// Then Bluetooth devices
for (uint8_t i = 0; i < CNT_BTHIDDEVICES; i++) {
if (*bthiddrivers[i] != bthid_driver_active[i]) {
if (bthid_driver_active[i]) {
Serial.printf("*** BTHID Device %s - disconnected ***\n", hid_driver_names[i]);
bthid_driver_active[i] = false;
} else {
Serial.printf("*** BTHID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
bthid_driver_active[i] = true;

const uint8_t *psz = bthiddrivers[i]->manufacturer();
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
psz = bthiddrivers[i]->product();
if (psz && *psz) Serial.printf(" product: %s\n", psz);
psz = bthiddrivers[i]->serialNumber();
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);
}
}
}
}

//=============================================================================
// displayPS4Data
//=============================================================================
void displayPS4Data()
{
buttons = joystick1.getButtons();
Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[0], psAxis[1], psAxis[2], psAxis[5]);
Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[3], psAxis[4]);
Serial.printf("Buttons: %x\r\n", buttons);
Serial.printf("Battery Status: %d\n", ((psAxis[30] & (1 << 4) - 1)*10));
Serial.printf("Battery Status: %d\n", ((psAxis[30] & ((1 << 4) - 1))*10));
printAngles();
Serial.println();

@@ -244,6 +306,9 @@ void displayPS4Data()

}

//=============================================================================
// displayPS3Data
//=============================================================================
void displayPS3Data()

{
@@ -305,7 +370,61 @@ void displayPS3Data()
buttons_prev = buttons;
}
}
//=============================================================================
// displayPS3MotionData
//=============================================================================
void displayPS3MotionData()

{
buttons = joystick1.getButtons();

// 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??

// Use Select button to choose raw or not
if ((buttons & 0x01) && !(buttons_prev & 0x01)) show_raw_data = !show_raw_data;
if ((buttons & 0x04) && !(buttons_prev & 0x04)) show_changed_data = !show_changed_data;

if (show_raw_data) {
displayRawData();
} else {
uint64_t changed_mask = joystick1.axisChangedMask();
Serial.printf("Changed: %08x Buttons: %x: Trig: %d\r\n", (uint32_t)changed_mask, buttons, psAxis[0]);
Serial.printf("Battery Status: %d\n", psAxis[7]);
printPS3MotionAngles();
Serial.println();
}

uint8_t ltv = psAxis[0];

if ((ltv != joystick_left_trigger_value) ) {
joystick_left_trigger_value = ltv;
Serial.printf("Rumbling: %d\r\n", ltv);
joystick1.setRumble(ltv, 0);
}

if (buttons != buttons_prev) {
uint8_t ledsR = (buttons & 0x8000)? 0xff : 0; //Srq
uint8_t ledsG = (buttons & 0x2000)? 0xff : 0; //Cir
uint8_t ledsB = (buttons & 0x1000)? 0xff : 0; //Tri
Serial.printf("Set Leds %x %x %x\r\n", ledsR, ledsG, ledsB );
joystick1.setLEDs(ledsR, ledsG, ledsB);
buttons_prev = buttons;
}
}

//=============================================================================
// displayXBoxData
//=============================================================================
void displayXBoxData()
{
buttons = joystick1.getButtons();
@@ -345,6 +464,9 @@ void displayXBoxData()
}
}

//=============================================================================
// displayRawData
//=============================================================================
void displayRawData() {
uint64_t axis_mask = joystick1.axisMask();
uint64_t changed_mask = joystick1.axisChangedMask();
@@ -380,6 +502,9 @@ void displayRawData() {
}


//=============================================================================
// OnReceiveHidData
//=============================================================================
bool OnReceiveHidData(uint32_t usage, const uint8_t *data, uint32_t len) {
// Called for maybe both HIDS for rawhid basic test. One is for the Teensy
// to output to Serial. while still having Raw Hid...
@@ -413,4 +538,49 @@ bool OnReceiveHidData(uint32_t usage, const uint8_t *data, uint32_t len) {
}

return true;
}
}

//=============================================================================
// processPS3MotionTimer
//=============================================================================
static const uint32_t PS3_MOTION_colors[] = {0, 0xff, 0xff00, 0xff0000, 0xffff, 0xff00ff, 0xffff00, 0xffffff};

uint8_t PS3_MOTION_colors_index = 0;

void processPS3MotionTimer() {
// See if we have a PS3_MOTION connected and we have run for a certain amount of time

if (PS3_MOTION_timer && ((millis()-PS3_MOTION_timer) >= PS3_MOTION_PERIOD)) {
Serial.println("PS3 Motion Timer"); Serial.flush();
if (joystick1) {
PS3_MOTION_timer = millis(); // joystick not there any more...

// We will first try to set feedback color for the PS3, maybe alternate colors
if (++PS3_MOTION_colors_index >= sizeof(PS3_MOTION_colors)/sizeof(PS3_MOTION_colors[0])) PS3_MOTION_colors_index = 0;
joystick1.setLEDs(PS3_MOTION_colors[PS3_MOTION_colors_index]);

// Next see if we can try to pair.
if (PS3_MOTION_tried_to_pair_state == 0) {
Serial.println("PS3_MOTION Connected");
if (!last_bdaddr[0] && !last_bdaddr[1] && !last_bdaddr[2] && !last_bdaddr[3] && !last_bdaddr[4] && !last_bdaddr[5]) {
Serial.println(" - No Bluetooth adapter has been plugged in - so will not try to pair");
PS3_MOTION_tried_to_pair_state = 1;
}
}
if ((PS3_MOTION_tried_to_pair_state < 2) &&
(last_bdaddr[0] || last_bdaddr[1] || last_bdaddr[2] || last_bdaddr[3] || last_bdaddr[4] || last_bdaddr[5])) {
Serial.println(" - Bluetooth device detected, will try to pair");
// Lets try to pair
if (! joystick1.PS3Pair(last_bdaddr)) {
Serial.println(" - Pairing call Failed");
} else {
Serial.println(" - Pairing complete (I hope), make sure Bluetooth adapter is plugged in and try PS3 without USB");
}
PS3_MOTION_tried_to_pair_state = 2; // don't try again...
}
} else {
Serial.println("PS3 Motion Joystick no longer detected");
PS3_MOTION_timer = 0; // joystick not there any more...
}
}
}

+ 83
- 2
examples/Bluetooth/JoystickBT/helperPS.ino View File

@@ -1,10 +1,17 @@
float pitch, roll;
float gx, gy, gz;
int16_t mx, my, mz;
uint16_t xc, yc;
uint8_t isTouch;
float ax, ay, az;
int16_t xc_old, yc_old;

/* Decode 12-bit signed value (assuming two's complement) */
#define TWELVE_BIT_SIGNED(x) (((x) & 0x800)?(-(((~(x)) & 0xFFF) + 1)):(x))

#define Model_ZCM1 1
//#define Model_ZCM2 0

void printAngles(){
//test function calls
float gx, gy, gz;
@@ -22,8 +29,6 @@ void printAngles(){
void getCoords(uint16_t &xc, uint16_t &yc, uint8_t &isTouch){

//uint8_t finger = 0; //only getting finger 1
uint8_t Id = 0;


// Trackpad touch 1: id, active, x, y
xc = ((psAxis[37] & 0x0f) << 8) | psAxis[36];
@@ -62,4 +67,80 @@ void getGyro(float &gx, float &gy, float &gz){
gy = (float) gyroy * RAD_TO_DEG/1024;
gz = (float) gyroz * RAD_TO_DEG/1024;
}


void printPS3MotionAngles(){
//test function calls
float gx, gy, gz;
getPS3MotionAccel(ax, ay, az);
Serial.printf("Accel-g's: %f, %f, %f\n", ax, ay, az);
getPS3MotionGyro(gx, gy, gz);
Serial.printf("Gyro-deg/sec: %f, %f, %f\n", gx, gy, gz);
getPS3MotionMag(mx, my, mz);
Serial.printf("Mag: %d, %d, %d\n", mx, my, mz);

getPS3MotionAngles(pitch, roll);
Serial.printf("Pitch/Roll: %f, %f\n", pitch, roll);
}


void getPS3MotionAccel( float &ax, float &ay, float &az){
int accelx = (psAxis[15]<<8 | psAxis[14]);
int accely = (psAxis[17]<<8 | psAxis[16]);
int accelz = (psAxis[19]<<8 | psAxis[18]);
#if defined(Model_ZCM1)
accelx = accelx-0x8000;
accely = accely-0x8000;
accelz = accelz-0x8000;
#elif defined(Model_ZCM2)
accelx = (accelx & 0x8000) ? (-(~accelx & 0xFFFF) + 1) : accelx;
accely = (accely & 0x8000) ? (-(~accely & 0xFFFF) + 1) : accely;
accelz = (accelz & 0x8000) ? (-(~accelz & 0xFFFF) + 1) : accelz;
#endif
ax = (float) accelx/4096;
ay = (float) accely/4096;
az = (float) accelz/4096;
}

void getPS3MotionAngles(float &p, float &r){
getAccel( ax, ay, az);
p = (atan2f(ay, az) + PI) * RAD_TO_DEG;
r = (atan2f(ax, az) + PI) * RAD_TO_DEG;
}

void getPS3MotionGyro(float &gx, float &gy, float &gz){
int gyrox = (psAxis[21]<<8 | psAxis[20]);
int gyroy = (psAxis[23]<<8 | psAxis[22]);
int gyroz = (psAxis[25]<<8 | psAxis[24]);
#if defined(Model_ZCM1)
gyrox = gyrox-0x8000;
gyroy = gyroy-0x8000;
gyroz = gyroz-0x8000;
#elif defined(Model_ZCM2)
gyrox = (gyrox & 0x8000) ? (-(~gyrox & 0xFFFF) + 1) : gyrox;
gyroy = (gyroy & 0x8000) ? (-(~gyroy & 0xFFFF) + 1) : gyroy;
gyroz = (gyroz & 0x8000) ? (-(~gyroz & 0xFFFF) + 1) : gyroz;
#endif

gx = (float) gyrox * RAD_TO_DEG/1024;
gy = (float) gyroy * RAD_TO_DEG/1024;
gz = (float) gyroz * RAD_TO_DEG/1024;
}
void getPS3MotionMag(int16_t &mx, int16_t &my, int16_t &mz){
#if defined(Model_ZCM1)
mx = TWELVE_BIT_SIGNED(((psAxis[33] & 0x0F) << 8) |
psAxis[34]);

my = TWELVE_BIT_SIGNED((psAxis[35] << 4) |
(psAxis[36] & 0xF0) >> 4);

mz = TWELVE_BIT_SIGNED(((psAxis[36] & 0x0F) << 8) |
psAxis[37]);
#elif defined(Model_ZCM2)
// NOTE: This model does not have magnetometers
Serial.println("Not avail for ZCM2!");
#endif
}

+ 1
- 1
examples/USBHost_viewer/USBHost_viewer.ino View File

@@ -251,7 +251,7 @@ void UpdateActiveDeviceInfo() {
if (*bthiddrivers[i] != bthid_driver_active[i]) {
if (bthid_driver_active[i]) {
Serial.printf("*** BTHID Device %s - disconnected ***\n", hid_driver_names[i]);
hid_driver_active[i] = false;
bthid_driver_active[i] = false;
} else {
new_device_detected = true;
Serial.printf("*** BTHID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());

+ 1
- 1
hid.cpp View File

@@ -166,7 +166,7 @@ void USBHIDParser::control(const Transfer_t *transfer)
parse();
queue_Data_Transfer(in_pipe, report, in_size, this);
if (device->idVendor == 0x054C &&
((device->idProduct == 0x0268) || (device->idProduct == 0x042F) || (device->idProduct == 0x03D5))) {
((device->idProduct == 0x0268) || (device->idProduct == 0x042F)/* || (device->idProduct == 0x03D5)*/)) {
println("send special PS3 feature command");
mk_setup(setup, 0x21, 9, 0x03F4, 0, 4); // ps3 tell to send report 1?
static uint8_t ps3_feature_F4_report[] = {0x42, 0x0c, 0x00, 0x00};

+ 214
- 145
joystick.cpp View File

@@ -42,7 +42,7 @@ JoystickController::product_vendor_mapping_t JoystickController::pid_vid_mapping
{ 0x045e, 0x0719, XBOX360, false},
{ 0x054C, 0x0268, PS3, true},
{ 0x054C, 0x042F, PS3, true}, // PS3 Navigation controller
{ 0x054C, 0x03D5, PS3, true}, // PS3 Motion controller
{ 0x054C, 0x03D5, PS3_MOTION, true}, // PS3 Motion controller
{ 0x054C, 0x05C4, PS4, true}, {0x054C, 0x09CC, PS4, true }
};

@@ -127,6 +127,8 @@ bool JoystickController::setRumble(uint8_t lValue, uint8_t rValue, uint8_t timeo
break;
case PS3:
return transmitPS3UserFeedbackMsg();
case PS3_MOTION:
return transmitPS3MotionUserFeedbackMsg();
case PS4:
return transmitPS4UserFeedbackMsg();
case XBOXONE:
@@ -169,6 +171,7 @@ bool JoystickController::setRumble(uint8_t lValue, uint8_t rValue, uint8_t timeo
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
@@ -181,6 +184,8 @@ bool JoystickController::setLEDs(uint8_t lr, uint8_t lg, uint8_t lb)
switch (joystickType_) {
case PS3:
return transmitPS3UserFeedbackMsg();
case PS3_MOTION:
return transmitPS3MotionUserFeedbackMsg();
case PS4:
return transmitPS4UserFeedbackMsg();
case XBOX360:
@@ -272,7 +277,7 @@ bool JoystickController::transmitPS3UserFeedbackMsg() {
txbuf_[2] = rumble_lValue_; // Small Rumble
txbuf_[3] = rumble_rValue_? rumble_timeout_ : 0;
txbuf_[4] = rumble_rValue_; // Big rumble
txbuf_[9] = leds_[0] << 1; // RGB value
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_) {
@@ -284,7 +289,7 @@ bool JoystickController::transmitPS3UserFeedbackMsg() {
txbuf_[4] = rumble_lValue_; // Small Rumble
txbuf_[5] = rumble_rValue_? rumble_timeout_ : 0;
txbuf_[6] = rumble_rValue_; // Big rumble
txbuf_[11] = leds_[0] << 1; // RGB value
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;
@@ -292,6 +297,33 @@ bool JoystickController::transmitPS3UserFeedbackMsg() {
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.
//*****************************************************************************
@@ -318,6 +350,7 @@ hidclaim_t JoystickController::claim_collection(USBHIDParser *driver, Device_t *
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;
@@ -711,7 +744,7 @@ bool JoystickController::claim_bluetooth(BluetoothController *driver, uint32_t b
}

if (remoteName && mapNameToJoystickType(remoteName)) {
if (joystickType_ == PS3) {
if ((joystickType_ == PS3) || (joystickType_ == PS3_MOTION)) {
DBGPrintf("JoystickController::claim_bluetooth TRUE PS3 hack...\n");
btdriver_ = driver;
btdevice = (Device_t*)driver; // remember this way
@@ -728,13 +761,15 @@ bool JoystickController::process_bluetooth_HID_data(const uint8_t *data, uint16_
// 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\n");
DBGPrintf("JoystickController::process_bluetooth_HID_data: data[0]=%x\n", data[0]);
// May have to look at this one with other controllers...
if (data[0] == 1) {
//print(" Joystick Data: ");
//print_hexbytes(data, length);
// DBGPrintf(" 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);
@@ -777,6 +812,37 @@ bool JoystickController::process_bluetooth_HID_data(const uint8_t *data, uint16_
}
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 {
uint64_t mask = 0x1;
@@ -800,116 +866,92 @@ bool JoystickController::process_bluetooth_HID_data(const uint8_t *data, uint16_
return true;

} else if(data[0] == 0x11){
if (data[0] == 1) {
//print(" Joystick Data: ");
//print_hexbytes(data, length);
// DBGPrintf(" Joystick Data: ");
uint64_t mask = 0x1;
axis_mask_ = 0;
axis_changed_mask_ = 0;

if (length > TOTAL_AXIS_COUNT) length = TOTAL_AXIS_COUNT; // don't overflow arrays...
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]);
}
// DBGPrintf("\n");
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;
DBGPrintf("\n Joystick Data: ");
uint64_t mask = 0x1;
axis_mask_ = 0;
axis_changed_mask_ = 0;
// Quick and dirty hack to match PS4 HID data
uint32_t cur_buttons = tmp_data[7] | (tmp_data[10]) | ((tmp_data[6]*10)) | ((uint16_t)tmp_data[5] << 16) ;
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];
//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;
// Quick and dirty hack to match PS4 HID data
uint32_t cur_buttons = tmp_data[7] | (tmp_data[10]) | ((tmp_data[6]*10)) | ((uint16_t)tmp_data[5] << 16) ;
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];
}
//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]);
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];
}
DBGPrintf("\n");
//DBGPrintf("Axis Mask (axis_mask_, axis_changed_mask_; %d, %d\n", axis_mask_,axis_changed_mask_);
joystickEvent = true;
connected_ = true;
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;
}
return false;
}
@@ -926,6 +968,9 @@ bool JoystickController::mapNameToJoystickType(const uint8_t *remoteName)
} 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;
@@ -944,6 +989,7 @@ bool JoystickController::remoteNameComplete(const uint8_t *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;
}
@@ -954,28 +1000,37 @@ bool JoystickController::remoteNameComplete(const uint8_t *remoteName)
void JoystickController::connectionComplete()
{
DBGPrintf(" JoystickController::connectionComplete %x joystick type %d\n", (uint32_t)this, joystickType_);
if (joystickType_ == 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), 0x40);

} else if (joystickType_ == 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);
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), 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?
default:
break;
}

}

void JoystickController::release_bluetooth()
@@ -989,15 +1044,29 @@ void JoystickController::release_bluetooth()


bool JoystickController::PS3Pair(uint8_t* bdaddr) {
if ((joystickType_ != PS3) || !driver_) return false; // not a PS2 nor plugged into USB...

/* 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_);
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;
}

+ 1
- 0
keywords.txt View File

@@ -90,6 +90,7 @@ setRumbleOn KEYWORD2
setLEDs KEYWORD2
joystickType KEYWORD2
PS3 LITERAL1
PS3_MOTION LITERAL1
PS4 LITERAL1
XBOXONE LITERAL1
XBOX360 LITERAL1

+ 1
- 1
mouse.cpp View File

@@ -35,7 +35,7 @@ void MouseController::init()
hidclaim_t MouseController::claim_collection(USBHIDParser *driver, Device_t *dev, uint32_t topusage)
{
// only claim Desktop/Mouse
if (topusage != 0x10002) return CLAIM_NO;
if ((topusage != 0x10002) && (topusage != 0x10001)) return CLAIM_NO;
// only claim from one physical device
if (mydevice != NULL && dev != mydevice) return CLAIM_NO;
mydevice = dev;

Write Preview
Loading…
Cancel
Save