diff --git a/BTD.h b/BTD.h index 238b8959..c314e6cd 100755 --- a/BTD.h +++ b/BTD.h @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -134,17 +134,17 @@ /** All Bluetooth services should include this class. */ class BluetoothService { public: - /** - * Used to pass acldata to the Bluetooth service. - * @param ACLData Pointer to the incoming acldata. - */ - virtual void ACLData(uint8_t* ACLData); - /** Used to run the different state machines in the Bluetooth service. */ - virtual void Run(); - /** Used to reset the Bluetooth service. */ - virtual void Reset(); - /** Used to disconnect both the L2CAP Channel and the HCI Connection for the Bluetooth service. */ - virtual void disconnect(); + /** + * Used to pass acldata to the Bluetooth service. + * @param ACLData Pointer to the incoming acldata. + */ + virtual void ACLData(uint8_t* ACLData); + /** Used to run the different state machines in the Bluetooth service. */ + virtual void Run(); + /** Used to reset the Bluetooth service. */ + virtual void Reset(); + /** Used to disconnect both the L2CAP Channel and the HCI Connection for the Bluetooth service. */ + virtual void disconnect(); }; /** @@ -153,299 +153,312 @@ public: */ class BTD : public USBDeviceConfig, public UsbConfigXtracter { public: - /** - * Constructor for the BTD class. - * @param p Pointer to USB class instance. - */ - BTD(USB *p); - - /** @name USBDeviceConfig implementation */ - /** - * Initialize the Bluetooth dongle. - * @param parent Hub number. - * @param port Port number on the hub. - * @param lowspeed Speed of the device. - * @return 0 on success. - */ - virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed); - /** - * Release the USB device. - * @return 0 on success. - */ - virtual uint8_t Release(); - /** - * Poll the USB Input endpoins and run the state machines. - * @return 0 on success. - */ - virtual uint8_t Poll(); - /** - * Get the device address. - * @return The device address. - */ - virtual uint8_t GetAddress() { return bAddress; }; - /** - * Used to check if the dongle has been initialized. - * @return True if it's ready. - */ - virtual bool isReady() { return bPollEnable; }; - /**@}*/ - - /** @name UsbConfigXtracter implementation */ - /** - * UsbConfigXtracter implementation, used to extract endpoint information. - * @param conf Configuration value. - * @param iface Interface number. - * @param alt Alternate setting. - * @param proto Interface Protocol. - * @param ep Endpoint Descriptor. - */ - virtual void EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *ep); - /**@}*/ + /** + * Constructor for the BTD class. + * @param p Pointer to USB class instance. + */ + BTD(USB *p); - /** Disconnects both the L2CAP Channel and the HCI Connection for all Bluetooth services. */ - void disconnect() { - for (uint8_t i=0; idisconnect(); - }; - /** - * Register bluetooth dongle members/services. - * @param pService Pointer to BluetoothService class instance. - * @return The serice ID on succes or -1 on fail. - */ - int8_t registerServiceClass(BluetoothService *pService) { - for (uint8_t i=0; idisconnect(); + }; + + /** + * Register bluetooth dongle members/services. + * @param pService Pointer to BluetoothService class instance. + * @return The serice ID on succes or -1 on fail. + */ + int8_t registerServiceClass(BluetoothService *pService) { + for(uint8_t i = 0; i < BTD_NUMSERVICES; i++) { + if(!btService[i]) { + btService[i] = pService; + return i; // Return ID + } + } + return -1; // ErrorregisterServiceClass + }; + + /** @name HCI Commands */ + /** + * Used to send a HCI Command. + * @param data Data to send. + * @param nbytes Number of bytes to send. + */ + void HCI_Command(uint8_t* data, uint16_t nbytes); + /** Reset the Bluetooth dongle. */ + void hci_reset(); + /** Read the Bluetooth address of the dongle. */ + void hci_read_bdaddr(); + /** Read the HCI Version of the Bluetooth dongle. */ + void hci_read_local_version_information(); + /** + * Set the local name of the Bluetooth dongle. + * @param name Desired name. + */ + void hci_set_local_name(const char* name); + /** Enable visibility to other Bluetooth devices. */ + void hci_write_scan_enable(); + /** Disable visibility to other Bluetooth devices. */ + void hci_write_scan_disable(); + /** Read the remote devices name. */ + void hci_remote_name(); + /** Accept the connection with the Bluetooth device. */ + void hci_accept_connection(); + /** + * Disconnect the HCI connection. + * @param handle The HCI Handle for the connection. + */ + void hci_disconnect(uint16_t handle); + /** + * Respond with the pin for the connection. + * The pin is automatically set for the Wii library, + * but can be customized for the SPP library. + */ + void hci_pin_code_request_reply(); + /** Respons when no pin was set. */ + void hci_pin_code_negative_request_reply(); + /** + * Command is used to reply to a Link Key Request event from the BR/EDR Controller + * if the Host does not have a stored Link Key for the connection. + */ + void hci_link_key_request_negative_reply(); + /** Used to try to authenticate with the remote device. */ + void hci_authentication_request(); + /** Start a HCI inquiry. */ + void hci_inquiry(); + /** Cancel a HCI inquiry. */ + void hci_inquiry_cancel(); + /** Connect to a device. */ + void hci_connect(); + /** Used to a set the class of the device. */ + void hci_write_class_of_device(); + /**@}*/ + + /** @name L2CAP Commands */ + /** + * Used to send L2CAP Commands. + * @param handle HCI Handle. + * @param data Data to send. + * @param nbytes Number of bytes to send. + * @param channelLow,channelHigh Low and high byte of channel to send to. + * If argument is omitted then the Standard L2CAP header: Channel ID (0x01) for ACL-U will be used. + */ + void L2CAP_Command(uint16_t handle, uint8_t* data, uint8_t nbytes, uint8_t channelLow = 0x01, uint8_t channelHigh = 0x00); + /** + * L2CAP Connection Request. + * @param handle HCI handle. + * @param rxid Identifier. + * @param scid Source Channel Identifier. + * @param psm Protocol/Service Multiplexer - see: https://www.bluetooth.org/Technical/AssignedNumbers/logical_link.htm. + */ + void l2cap_connection_request(uint16_t handle, uint8_t rxid, uint8_t* scid, uint16_t psm); + /** + * L2CAP Connection Response. + * @param handle HCI handle. + * @param rxid Identifier. + * @param dcid Destination Channel Identifier. + * @param scid Source Channel Identifier. + * @param result Result - First send ::PENDING and then ::SUCCESSFUL. + */ + void l2cap_connection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result); + /** + * L2CAP Config Request. + * @param handle HCI Handle. + * @param rxid Identifier. + * @param dcid Destination Channel Identifier. + */ + void l2cap_config_request(uint16_t handle, uint8_t rxid, uint8_t* dcid); + /** + * L2CAP Config Response. + * @param handle HCI Handle. + * @param rxid Identifier. + * @param scid Source Channel Identifier. + */ + void l2cap_config_response(uint16_t handle, uint8_t rxid, uint8_t* scid); + /** + * L2CAP Disconnection Request. + * @param handle HCI Handle. + * @param rxid Identifier. + * @param dcid Device Channel Identifier. + * @param scid Source Channel Identifier. + */ + void l2cap_disconnection_request(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid); + /** + * L2CAP Disconnection Response. + * @param handle HCI Handle. + * @param rxid Identifier. + * @param dcid Device Channel Identifier. + * @param scid Source Channel Identifier. + */ + void l2cap_disconnection_response(uint16_t handle, uint8_t rxid, uint8_t* dcid, uint8_t* scid); + /** + * L2CAP Information Response. + * @param handle HCI Handle. + * @param rxid Identifier. + * @param infoTypeLow,infoTypeHigh Infotype. + */ + void l2cap_information_response(uint16_t handle, uint8_t rxid, uint8_t infoTypeLow, uint8_t infoTypeHigh); + /**@}*/ + + /** Use this to see if it is waiting for a incoming connection. */ + bool watingForConnection; + /** This is used by the service to know when to store the device information. */ + bool l2capConnectionClaimed; + /** This is used by the SPP library to claim the current SDP incoming request. */ + bool sdpConnectionClaimed; + /** This is used by the SPP library to claim the current RFCOMM incoming request. */ + bool rfcommConnectionClaimed; + + /** The name you wish to make the dongle show up as. It is set automatically by the SPP library. */ + const char* btdName; + /** The pin you wish to make the dongle use for authentication. It is set automatically by the SPP library. */ + const char* btdPin; + + /** The bluetooth dongles Bluetooth address. */ + uint8_t my_bdaddr[6]; + /** HCI handle for the last connection. */ + uint16_t hci_handle; + /** Last incoming devices Bluetooth address. */ + uint8_t disc_bdaddr[6]; + /** First 30 chars of last remote name. */ + uint8_t remote_name[30]; + /** + * The supported HCI Version read from the Bluetooth dongle. + * Used by the PS3BT library to check the HCI Version of the Bluetooth dongle, + * it should be at least 3 to work properly with the library. + */ + uint8_t hci_version; + + /** Call this function to pair with a Wiimote */ + void pairWithWiimote() { + pairWithWii = true; + hci_state = HCI_CHECK_WII_SERVICE; + }; + /** Used to only send the ACL data to the wiimote. */ + bool connectToWii; + /** True if a Wiimote is connecting. */ + bool incomingWii; + /** True when it should pair with the incoming Wiimote. */ + bool pairWithWii; + /** True if it's the new Wiimote with the Motion Plus Inside or a Wii U Pro Controller. */ + bool motionPlusInside; + /** True if it's a Wii U Pro Controller. */ + bool wiiUProController; + + /** + * Read the poll interval taken from the endpoint descriptors. + * @return The poll interval in ms. + */ + uint8_t readPollInterval() { + return pollInterval; + }; - /** - * Read the poll interval taken from the endpoint descriptors. - * @return The poll interval in ms. - */ - uint8_t readPollInterval() { return pollInterval; }; - protected: - /** Pointer to USB class instance. */ - USB *pUsb; - /** Device address. */ - uint8_t bAddress; - /** Endpoint info structure. */ - EpInfo epInfo[BTD_MAX_ENDPOINTS]; - - /** Configuration number. */ - uint8_t bConfNum; - /** Total number of endpoints in the configuration. */ - uint8_t bNumEP; - /** Next poll time based on poll interval taken from the USB descriptor. */ - uint32_t qNextPollTime; - - /** Bluetooth dongle control endpoint. */ - static const uint8_t BTD_CONTROL_PIPE; - /** HCI event endpoint index. */ - static const uint8_t BTD_EVENT_PIPE; - /** ACL In endpoint index. */ - static const uint8_t BTD_DATAIN_PIPE; - /** ACL Out endpoint index. */ - static const uint8_t BTD_DATAOUT_PIPE; - - /** - * Used to print the USB Endpoint Descriptor. - * @param ep_ptr Pointer to USB Endpoint Descriptor. - */ - void PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr); - + /** Pointer to USB class instance. */ + USB *pUsb; + /** Device address. */ + uint8_t bAddress; + /** Endpoint info structure. */ + EpInfo epInfo[BTD_MAX_ENDPOINTS]; + + /** Configuration number. */ + uint8_t bConfNum; + /** Total number of endpoints in the configuration. */ + uint8_t bNumEP; + /** Next poll time based on poll interval taken from the USB descriptor. */ + uint32_t qNextPollTime; + + /** Bluetooth dongle control endpoint. */ + static const uint8_t BTD_CONTROL_PIPE; + /** HCI event endpoint index. */ + static const uint8_t BTD_EVENT_PIPE; + /** ACL In endpoint index. */ + static const uint8_t BTD_DATAIN_PIPE; + /** ACL Out endpoint index. */ + static const uint8_t BTD_DATAOUT_PIPE; + + /** + * Used to print the USB Endpoint Descriptor. + * @param ep_ptr Pointer to USB Endpoint Descriptor. + */ + void PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr); + private: - BluetoothService* btService[BTD_NUMSERVICES]; - - bool bPollEnable; - uint8_t pollInterval; - - /* Variables used by high level HCI task */ - uint8_t hci_state; //current state of bluetooth hci connection - uint16_t hci_counter; // counter used for bluetooth hci reset loops - uint8_t hci_num_reset_loops; // this value indicate how many times it should read before trying to reset - uint16_t hci_event_flag; // hci flags of received bluetooth events - uint8_t inquiry_counter; - - uint8_t hcibuf[BULK_MAXPKTSIZE];//General purpose buffer for hci data - uint8_t l2capinbuf[BULK_MAXPKTSIZE];//General purpose buffer for l2cap in data - uint8_t l2capoutbuf[BULK_MAXPKTSIZE];//General purpose buffer for l2cap out data - - /* State machines */ - void HCI_event_task(); // Poll the HCI event pipe - void HCI_task(); // HCI state machine - void ACL_event_task(); // ACL input pipe - - /* Used to set the Bluetooth Address internally to the PS3 Controllers */ - void setBdaddr(uint8_t* BDADDR); - void setMoveBdaddr(uint8_t* BDADDR); + BluetoothService* btService[BTD_NUMSERVICES]; + + bool bPollEnable; + uint8_t pollInterval; + + /* Variables used by high level HCI task */ + uint8_t hci_state; //current state of bluetooth hci connection + uint16_t hci_counter; // counter used for bluetooth hci reset loops + uint8_t hci_num_reset_loops; // this value indicate how many times it should read before trying to reset + uint16_t hci_event_flag; // hci flags of received bluetooth events + uint8_t inquiry_counter; + + uint8_t hcibuf[BULK_MAXPKTSIZE]; //General purpose buffer for hci data + uint8_t l2capinbuf[BULK_MAXPKTSIZE]; //General purpose buffer for l2cap in data + uint8_t l2capoutbuf[BULK_MAXPKTSIZE]; //General purpose buffer for l2cap out data + + /* State machines */ + void HCI_event_task(); // Poll the HCI event pipe + void HCI_task(); // HCI state machine + void ACL_event_task(); // ACL input pipe + + /* Used to set the Bluetooth Address internally to the PS3 Controllers */ + void setBdaddr(uint8_t* BDADDR); + void setMoveBdaddr(uint8_t* BDADDR); }; #endif diff --git a/PS3BT.cpp b/PS3BT.cpp index 87214ab3..1912808c 100644 --- a/PS3BT.cpp +++ b/PS3BT.cpp @@ -21,540 +21,542 @@ //#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers const uint8_t OUTPUT_REPORT_BUFFER[] PROGMEM = { - 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 + 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 }; -PS3BT::PS3BT(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0): +PS3BT::PS3BT(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0) : pBtd(p) // pointer to USB class instance - mandatory { - if (pBtd) - pBtd->registerServiceClass(this); // Register it as a Bluetooth service + if (pBtd) + pBtd->registerServiceClass(this); // Register it as a Bluetooth service - pBtd->my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead - pBtd->my_bdaddr[4] = btadr4; - pBtd->my_bdaddr[3] = btadr3; - pBtd->my_bdaddr[2] = btadr2; - pBtd->my_bdaddr[1] = btadr1; - pBtd->my_bdaddr[0] = btadr0; + pBtd->my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead + pBtd->my_bdaddr[4] = btadr4; + pBtd->my_bdaddr[3] = btadr3; + pBtd->my_bdaddr[2] = btadr2; + pBtd->my_bdaddr[1] = btadr1; + pBtd->my_bdaddr[0] = btadr0; - HIDBuffer[0] = 0x52;// HID BT Set_report (0x50) | Report Type (Output 0x02) - HIDBuffer[1] = 0x01;// Report ID + HIDBuffer[0] = 0x52; // HID BT Set_report (0x50) | Report Type (Output 0x02) + HIDBuffer[1] = 0x01; // Report ID - //Needed for PS3 Move Controller commands to work via bluetooth - HIDMoveBuffer[0] = 0xA2;// HID BT DATA_request (0xA0) | Report Type (Output 0x02) - HIDMoveBuffer[1] = 0x02;// Report ID + //Needed for PS3 Move Controller commands to work via bluetooth + HIDMoveBuffer[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) + HIDMoveBuffer[1] = 0x02; // Report ID - /* Set device cid for the control and intterrupt channelse - LSB */ - control_dcid[0] = 0x40;//0x0040 - control_dcid[1] = 0x00; - interrupt_dcid[0] = 0x41;//0x0041 - interrupt_dcid[1] = 0x00; + /* Set device cid for the control and intterrupt channelse - LSB */ + control_dcid[0] = 0x40; //0x0040 + control_dcid[1] = 0x00; + interrupt_dcid[0] = 0x41; //0x0041 + interrupt_dcid[1] = 0x00; - Reset(); + Reset(); } + bool PS3BT::getButtonPress(Button b) { - return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b])); + return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b])); } + bool PS3BT::getButtonClick(Button b) { - uint32_t button = pgm_read_dword(&BUTTONS[(uint8_t)b]); - bool click = (ButtonClickState & button); - ButtonClickState &= ~button; // clear "click" event - return click; + uint32_t button = pgm_read_dword(&BUTTONS[(uint8_t)b]); + bool click = (ButtonClickState & button); + ButtonClickState &= ~button; // clear "click" event + return click; } + uint8_t PS3BT::getAnalogButton(Button a) { - if (l2capinbuf == NULL) - return 0; - return (uint8_t)(l2capinbuf[pgm_read_byte(&ANALOGBUTTONS[(uint8_t)a])]); + if (l2capinbuf == NULL) + return 0; + return (uint8_t)(l2capinbuf[pgm_read_byte(&ANALOGBUTTONS[(uint8_t)a])]); } + uint8_t PS3BT::getAnalogHat(AnalogHat a) { - if (l2capinbuf == NULL) - return 0; - return (uint8_t)(l2capinbuf[(uint8_t)a+15]); + if (l2capinbuf == NULL) + return 0; + return (uint8_t)(l2capinbuf[(uint8_t)a + 15]); } + int16_t PS3BT::getSensor(Sensor a) { - if (l2capinbuf == NULL) - return 0; - if(PS3Connected) { - if (a == aX || a == aY || a == aZ || a == gZ) - return ((l2capinbuf[(uint16_t)a] << 8) | l2capinbuf[(uint16_t)a + 1]); - else - return 0; - } else if(PS3MoveConnected) { - if (a == mXmove || a == mYmove) // These are all 12-bits long - return (((l2capinbuf[(uint16_t)a] & 0x0F) << 8) | (l2capinbuf[(uint16_t)a + 1])); - else if (a == mZmove || a == tempMove) // The tempearature is also 12 bits long - return ((l2capinbuf[(uint16_t)a] << 4) | ((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4)); - else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove - return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8)); - } else - return 0; + if (l2capinbuf == NULL) + return 0; + if (PS3Connected) { + if (a == aX || a == aY || a == aZ || a == gZ) + return ((l2capinbuf[(uint16_t)a] << 8) | l2capinbuf[(uint16_t)a + 1]); + else + return 0; + } else if (PS3MoveConnected) { + if (a == mXmove || a == mYmove) // These are all 12-bits long + return (((l2capinbuf[(uint16_t)a] & 0x0F) << 8) | (l2capinbuf[(uint16_t)a + 1])); + else if (a == mZmove || a == tempMove) // The tempearature is also 12 bits long + return ((l2capinbuf[(uint16_t)a] << 4) | ((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4)); + else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove + return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8)); + } else + return 0; } + double PS3BT::getAngle(Angle a) { - double accXval; - double accYval; - double accZval; + double accXval; + double accYval; + double accZval; - if(PS3Connected) { - // Data for the Kionix KXPC4 used in the DualShock 3 - const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) - accXval = -((double)getSensor(aX)-zeroG); - accYval = -((double)getSensor(aY)-zeroG); - accZval = -((double)getSensor(aZ)-zeroG); - } else if(PS3MoveConnected) { - // It's a Kionix KXSC4 inside the Motion controller - const uint16_t zeroG = 0x8000; - accXval = -(int16_t)(getSensor(aXmove)-zeroG); - accYval = (int16_t)(getSensor(aYmove)-zeroG); - accZval = (int16_t)(getSensor(aZmove)-zeroG); - } + if (PS3Connected) { + // Data for the Kionix KXPC4 used in the DualShock 3 + const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) + accXval = -((double)getSensor(aX) - zeroG); + accYval = -((double)getSensor(aY) - zeroG); + accZval = -((double)getSensor(aZ) - zeroG); + } else if (PS3MoveConnected) { + // It's a Kionix KXSC4 inside the Motion controller + const uint16_t zeroG = 0x8000; + accXval = -(int16_t)(getSensor(aXmove) - zeroG); + accYval = (int16_t)(getSensor(aYmove) - zeroG); + accZval = (int16_t)(getSensor(aZmove) - zeroG); + } - // Convert to 360 degrees resolution - // atan2 outputs the value of -π to π (radians) - // We are then converting it to 0 to 2π and then to degrees - if (a == Pitch) { - double angle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG; - return angle; - } else { - double angle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG; - return angle; - } + // Convert to 360 degrees resolution + // atan2 outputs the value of -π to π (radians) + // We are then converting it to 0 to 2π and then to degrees + if (a == Pitch) { + double angle = (atan2(accYval, accZval) + PI) * RAD_TO_DEG; + return angle; + } else { + double angle = (atan2(accXval, accZval) + PI) * RAD_TO_DEG; + return angle; + } } + double PS3BT::get9DOFValues(Sensor a) { // Thanks to Manfred Piendl - if(!PS3MoveConnected) - return 0; - int16_t value = getSensor(a); - if (a == mXmove || a == mYmove || a == mZmove) { - if (value > 2047) - value -= 0x1000; - return (double)value/3.2; // unit: muT = 10^(-6) Tesla - } else if (a == aXmove || a == aYmove || a == aZmove) { - if (value < 0) - value += 0x8000; - else - value -= 0x8000; - return (double)value/442.0; // unit: m/(s^2) - } else if (a == gXmove || a == gYmove || a == gZmove) { - if (value < 0) - value += 0x8000; - else - value -= 0x8000; - if (a == gXmove) - return (double)value/11.6; // unit: deg/s - else if (a == gYmove) - return (double)value/11.2; // unit: deg/s - else // gZmove - return (double)value/9.6; // unit: deg/s - } else - return 0; + if (!PS3MoveConnected) + return 0; + int16_t value = getSensor(a); + if (a == mXmove || a == mYmove || a == mZmove) { + if (value > 2047) + value -= 0x1000; + return (double)value / 3.2; // unit: muT = 10^(-6) Tesla + } else if (a == aXmove || a == aYmove || a == aZmove) { + if (value < 0) + value += 0x8000; + else + value -= 0x8000; + return (double)value / 442.0; // unit: m/(s^2) + } else if (a == gXmove || a == gYmove || a == gZmove) { + if (value < 0) + value += 0x8000; + else + value -= 0x8000; + if (a == gXmove) + return (double)value / 11.6; // unit: deg/s + else if (a == gYmove) + return (double)value / 11.2; // unit: deg/s + else // gZmove + return (double)value / 9.6; // unit: deg/s + } else + return 0; } + String PS3BT::getTemperature() { - if(PS3MoveConnected) { - int16_t input = getSensor(tempMove); + if (PS3MoveConnected) { + int16_t input = getSensor(tempMove); - String output = String(input/100); - output += "."; - if(input%100 < 10) - output += "0"; - output += String(input%100); + String output = String(input / 100); + output += "."; + if (input % 100 < 10) + output += "0"; + output += String(input % 100); - return output; - } + return output; + } } + bool PS3BT::getStatus(Status c) { - if (l2capinbuf == NULL) + if (l2capinbuf == NULL) + return false; + if (l2capinbuf[(uint16_t)c >> 8] == ((uint8_t)c & 0xff)) + return true; return false; - if (l2capinbuf[(uint16_t)c >> 8] == ((uint8_t)c & 0xff)) - return true; - return false; } + String PS3BT::getStatusString() { - if (PS3Connected || PS3NavigationConnected) { - char statusOutput[100]; + if (PS3Connected || PS3NavigationConnected) { + char statusOutput[100]; - strcpy(statusOutput,"ConnectionStatus: "); + strcpy(statusOutput, "ConnectionStatus: "); - if (getStatus(Plugged)) strcat(statusOutput,"Plugged"); - else if (getStatus(Unplugged)) strcat(statusOutput,"Unplugged"); - else strcat(statusOutput,"Error"); + if (getStatus(Plugged)) strcat(statusOutput, "Plugged"); + else if (getStatus(Unplugged)) strcat(statusOutput, "Unplugged"); + else strcat(statusOutput, "Error"); - strcat(statusOutput," - PowerRating: "); - if (getStatus(Charging)) strcat(statusOutput,"Charging"); - else if (getStatus(NotCharging)) strcat(statusOutput,"Not Charging"); - else if (getStatus(Shutdown)) strcat(statusOutput,"Shutdown"); - else if (getStatus(Dying)) strcat(statusOutput,"Dying"); - else if (getStatus(Low)) strcat(statusOutput,"Low"); - else if (getStatus(High)) strcat(statusOutput,"High"); - else if (getStatus(Full)) strcat(statusOutput,"Full"); - else strcat(statusOutput,"Error"); + strcat(statusOutput, " - PowerRating: "); + if (getStatus(Charging)) strcat(statusOutput, "Charging"); + else if (getStatus(NotCharging)) strcat(statusOutput, "Not Charging"); + else if (getStatus(Shutdown)) strcat(statusOutput, "Shutdown"); + else if (getStatus(Dying)) strcat(statusOutput, "Dying"); + else if (getStatus(Low)) strcat(statusOutput, "Low"); + else if (getStatus(High)) strcat(statusOutput, "High"); + else if (getStatus(Full)) strcat(statusOutput, "Full"); + else strcat(statusOutput, "Error"); - strcat(statusOutput," - WirelessStatus: "); + strcat(statusOutput, " - WirelessStatus: "); - if (getStatus(CableRumble)) strcat(statusOutput,"Cable - Rumble is on"); - else if (getStatus(Cable)) strcat(statusOutput,"Cable - Rumble is off"); - else if (getStatus(BluetoothRumble)) strcat(statusOutput,"Bluetooth - Rumble is on"); - else if (getStatus(Bluetooth)) strcat(statusOutput,"Bluetooth - Rumble is off"); - else strcat(statusOutput,"Error"); + if (getStatus(CableRumble)) strcat(statusOutput, "Cable - Rumble is on"); + else if (getStatus(Cable)) strcat(statusOutput, "Cable - Rumble is off"); + else if (getStatus(BluetoothRumble)) strcat(statusOutput, "Bluetooth - Rumble is on"); + else if (getStatus(Bluetooth)) strcat(statusOutput, "Bluetooth - Rumble is off"); + else strcat(statusOutput, "Error"); - return statusOutput; + return statusOutput; - } - else if(PS3MoveConnected) { - char statusOutput[50]; + } else if (PS3MoveConnected) { + char statusOutput[50]; - strcpy(statusOutput,"PowerRating: "); + strcpy(statusOutput, "PowerRating: "); - if (getStatus(MoveCharging)) strcat(statusOutput,"Charging"); - else if (getStatus(MoveNotCharging)) strcat(statusOutput,"Not Charging"); - else if (getStatus(MoveShutdown)) strcat(statusOutput,"Shutdown"); - else if (getStatus(MoveDying)) strcat(statusOutput,"Dying"); - else if (getStatus(MoveLow)) strcat(statusOutput,"Low"); - else if (getStatus(MoveHigh)) strcat(statusOutput,"High"); - else if (getStatus(MoveFull)) strcat(statusOutput,"Full"); - else strcat(statusOutput,"Error"); + if (getStatus(MoveCharging)) strcat(statusOutput, "Charging"); + else if (getStatus(MoveNotCharging)) strcat(statusOutput, "Not Charging"); + else if (getStatus(MoveShutdown)) strcat(statusOutput, "Shutdown"); + else if (getStatus(MoveDying)) strcat(statusOutput, "Dying"); + else if (getStatus(MoveLow)) strcat(statusOutput, "Low"); + else if (getStatus(MoveHigh)) strcat(statusOutput, "High"); + else if (getStatus(MoveFull)) strcat(statusOutput, "Full"); + else strcat(statusOutput, "Error"); - return statusOutput; - } + return statusOutput; + } } -void PS3BT::Reset() { - PS3Connected = false; - PS3MoveConnected = false; - PS3NavigationConnected = false; - activeConnection = false; - l2cap_event_flag = 0; // Reset flags - l2cap_state = L2CAP_WAIT; - // Needed for PS3 Dualshock Controller commands to work via bluetooth - for (uint8_t i = 0; i < OUTPUT_REPORT_BUFFER_SIZE; i++) - HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]); // First two bytes reserved for report type and ID +void PS3BT::Reset() { + PS3Connected = false; + PS3MoveConnected = false; + PS3NavigationConnected = false; + activeConnection = false; + l2cap_event_flag = 0; // Reset flags + l2cap_state = L2CAP_WAIT; + + // Needed for PS3 Dualshock Controller commands to work via bluetooth + for (uint8_t i = 0; i < OUTPUT_REPORT_BUFFER_SIZE; i++) + HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]); // First two bytes reserved for report type and ID } void PS3BT::disconnect() { // Use this void to disconnect any of the controllers - //First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection - pBtd->l2cap_disconnection_request(hci_handle,0x0A, interrupt_scid, interrupt_dcid); - Reset(); - l2cap_state = L2CAP_INTERRUPT_DISCONNECT; + //First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection + pBtd->l2cap_disconnection_request(hci_handle, 0x0A, interrupt_scid, interrupt_dcid); + Reset(); + l2cap_state = L2CAP_INTERRUPT_DISCONNECT; } void PS3BT::ACLData(uint8_t* ACLData) { - if(!pBtd->l2capConnectionClaimed && !PS3Connected && !PS3MoveConnected && !PS3NavigationConnected && !activeConnection && !pBtd->connectToWii && !pBtd->incomingWii && !pBtd->pairWithWii) { - if (ACLData[8] == L2CAP_CMD_CONNECTION_REQUEST) { - if((ACLData[12] | (ACLData[13] << 8)) == HID_CTRL_PSM) { - pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service - activeConnection = true; - hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection - l2cap_state = L2CAP_WAIT; - for(uint8_t i = 0; i < 30; i++) - remote_name[i] = pBtd->remote_name[i]; // Store the remote name for the connection + if (!pBtd->l2capConnectionClaimed && !PS3Connected && !PS3MoveConnected && !PS3NavigationConnected && !activeConnection && !pBtd->connectToWii && !pBtd->incomingWii && !pBtd->pairWithWii) { + if (ACLData[8] == L2CAP_CMD_CONNECTION_REQUEST) { + if ((ACLData[12] | (ACLData[13] << 8)) == HID_CTRL_PSM) { + pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service + activeConnection = true; + hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection + l2cap_state = L2CAP_WAIT; + for (uint8_t i = 0; i < 30; i++) + remote_name[i] = pBtd->remote_name[i]; // Store the remote name for the connection #ifdef DEBUG - if(pBtd->hci_version < 3) { // Check the HCI Version of the Bluetooth dongle - Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: "), 0x80); - Serial.print(pBtd->hci_version); - Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR"), 0x80); - } + if (pBtd->hci_version < 3) { // Check the HCI Version of the Bluetooth dongle + Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: "), 0x80); + Serial.print(pBtd->hci_version); + Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR"), 0x80); + } #endif - } + } + } } - } - if (((ACLData[0] | (ACLData[1] << 8)) == (hci_handle | 0x2000))) { //acl_handle_ok - for(uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) - l2capinbuf[i] = ACLData[i]; - if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U - if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { + if (((ACLData[0] | (ACLData[1] << 8)) == (hci_handle | 0x2000))) { //acl_handle_ok + for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) + l2capinbuf[i] = ACLData[i]; + if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U + if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { #ifdef DEBUG - Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); - PrintHex(l2capinbuf[13], 0x80); - Serial.print(" "); - PrintHex(l2capinbuf[12], 0x80); - Serial.print(" Data: "); - PrintHex(l2capinbuf[17], 0x80); - Serial.print(" "); - PrintHex(l2capinbuf[16], 0x80); - Serial.print(" "); - PrintHex(l2capinbuf[15], 0x80); - Serial.print(" "); - PrintHex(l2capinbuf[14], 0x80); + Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); + PrintHex (l2capinbuf[13], 0x80); + Serial.print(" "); + PrintHex (l2capinbuf[12], 0x80); + Serial.print(" Data: "); + PrintHex (l2capinbuf[17], 0x80); + Serial.print(" "); + PrintHex (l2capinbuf[16], 0x80); + Serial.print(" "); + PrintHex (l2capinbuf[15], 0x80); + Serial.print(" "); + PrintHex (l2capinbuf[14], 0x80); #endif - } - else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { + } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { #ifdef EXTRADEBUG - Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); - PrintHex(l2capinbuf[13], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[12], 0x80); - Notify(PSTR(" SCID: "), 0x80); - PrintHex(l2capinbuf[15], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[14], 0x80); - Notify(PSTR(" Identifier: "), 0x80); - PrintHex(l2capinbuf[9], 0x80); + Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); + PrintHex (l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[12], 0x80); + Notify(PSTR(" SCID: "), 0x80); + PrintHex (l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[14], 0x80); + Notify(PSTR(" Identifier: "), 0x80); + PrintHex (l2capinbuf[9], 0x80); #endif - if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { - identifier = l2capinbuf[9]; - control_scid[0] = l2capinbuf[14]; - control_scid[1] = l2capinbuf[15]; - l2cap_event_flag |= L2CAP_FLAG_CONNECTION_CONTROL_REQUEST; - } - else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) { - identifier = l2capinbuf[9]; - interrupt_scid[0] = l2capinbuf[14]; - interrupt_scid[1] = l2capinbuf[15]; - l2cap_event_flag |= L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST; - } - } - else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) { - if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success - if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { - //Serial.print("\r\nHID Control Configuration Complete"); - l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_SUCCESS; - } - else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { - //Serial.print("\r\nHID Interrupt Configuration Complete"); - l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS; - } - } - } - else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) { - if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { - //Serial.print("\r\nHID Control Configuration Request"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_REQUEST; - } - else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { - //Serial.print("\r\nHID Interrupt Configuration Request"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_REQUEST; - } - } - else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { - if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { + if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { + identifier = l2capinbuf[9]; + control_scid[0] = l2capinbuf[14]; + control_scid[1] = l2capinbuf[15]; + l2cap_event_flag |= L2CAP_FLAG_CONNECTION_CONTROL_REQUEST; + } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) { + identifier = l2capinbuf[9]; + interrupt_scid[0] = l2capinbuf[14]; + interrupt_scid[1] = l2capinbuf[15]; + l2cap_event_flag |= L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST; + } + } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) { + if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success + if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { + //Serial.print("\r\nHID Control Configuration Complete"); + l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_SUCCESS; + } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { + //Serial.print("\r\nHID Interrupt Configuration Complete"); + l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS; + } + } + } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) { + if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { + //Serial.print("\r\nHID Control Configuration Request"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_REQUEST; + } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { + //Serial.print("\r\nHID Interrupt Configuration Request"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_REQUEST; + } + } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { + if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80); + Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80); #endif - identifier = l2capinbuf[9]; - pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid); - Reset(); - } - else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { + identifier = l2capinbuf[9]; + pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid); + Reset(); + } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80); + Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80); #endif - identifier = l2capinbuf[9]; - pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid); - Reset(); - } - } - else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) { - if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) { - //Serial.print("\r\nDisconnect Response: Control Channel"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE; - } - else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) { - //Serial.print("\r\nDisconnect Response: Interrupt Channel"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE; - } - } + identifier = l2capinbuf[9]; + pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid); + Reset(); + } + } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) { + if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) { + //Serial.print("\r\nDisconnect Response: Control Channel"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE; + } else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) { + //Serial.print("\r\nDisconnect Response: Interrupt Channel"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE; + } + } #ifdef EXTRADEBUG - else { - Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); - PrintHex(l2capinbuf[8], 0x80); - } + else { + Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); + PrintHex (l2capinbuf[8], 0x80); + } #endif - } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt - //Serial.print("\r\nL2CAP Interrupt"); - if(PS3Connected || PS3MoveConnected || PS3NavigationConnected) { - /* Read Report */ - if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT - if(PS3Connected || PS3NavigationConnected) - ButtonState = (uint32_t)(l2capinbuf[11] | ((uint16_t)l2capinbuf[12] << 8) | ((uint32_t)l2capinbuf[13] << 16)); - else if(PS3MoveConnected) - ButtonState = (uint32_t)(l2capinbuf[10] | ((uint16_t)l2capinbuf[11] << 8) | ((uint32_t)l2capinbuf[12] << 16)); + } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt + //Serial.print("\r\nL2CAP Interrupt"); + if (PS3Connected || PS3MoveConnected || PS3NavigationConnected) { + /* Read Report */ + if (l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT + if (PS3Connected || PS3NavigationConnected) + ButtonState = (uint32_t)(l2capinbuf[11] | ((uint16_t)l2capinbuf[12] << 8) | ((uint32_t)l2capinbuf[13] << 16)); + else if (PS3MoveConnected) + ButtonState = (uint32_t)(l2capinbuf[10] | ((uint16_t)l2capinbuf[11] << 8) | ((uint32_t)l2capinbuf[12] << 16)); - //Notify(PSTR("\r\nButtonState", 0x80); - //PrintHex(ButtonState, 0x80); + //Notify(PSTR("\r\nButtonState", 0x80); + //PrintHex(ButtonState, 0x80); - if(ButtonState != OldButtonState) { - ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable - OldButtonState = ButtonState; - } + if (ButtonState != OldButtonState) { + ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable + OldButtonState = ButtonState; + } #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers - for(uint8_t i = 10; i < 58;i++) { - PrintHex(l2capinbuf[i], 0x80); - Serial.print(" "); - } - Serial.println(); + for (uint8_t i = 10; i < 58; i++) { + PrintHex (l2capinbuf[i], 0x80); + Serial.print(" "); + } + Serial.println(); #endif + } + } } - } + L2CAP_task(); } - L2CAP_task(); - } } + void PS3BT::L2CAP_task() { - switch (l2cap_state) { - case L2CAP_WAIT: - if (l2cap_connection_request_control_flag) { + switch (l2cap_state) { + case L2CAP_WAIT: + if (l2cap_connection_request_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80); + Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80); #endif - pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, PENDING); - delay(1); - pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, SUCCESSFUL); - identifier++; - delay(1); - pBtd->l2cap_config_request(hci_handle,identifier, control_scid); - l2cap_state = L2CAP_CONTROL_REQUEST; - } - break; - case L2CAP_CONTROL_REQUEST: - if (l2cap_config_request_control_flag) { + pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, PENDING); + delay(1); + pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, SUCCESSFUL); + identifier++; + delay(1); + pBtd->l2cap_config_request(hci_handle, identifier, control_scid); + l2cap_state = L2CAP_CONTROL_REQUEST; + } + break; + case L2CAP_CONTROL_REQUEST: + if (l2cap_config_request_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Configuration Request"), 0x80); + Notify(PSTR("\r\nHID Control Configuration Request"), 0x80); #endif - pBtd->l2cap_config_response(hci_handle,identifier, control_scid); - l2cap_state = L2CAP_CONTROL_SUCCESS; - } - break; + pBtd->l2cap_config_response(hci_handle, identifier, control_scid); + l2cap_state = L2CAP_CONTROL_SUCCESS; + } + break; - case L2CAP_CONTROL_SUCCESS: - if (l2cap_config_success_control_flag) { + case L2CAP_CONTROL_SUCCESS: + if (l2cap_config_success_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80); + Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80); #endif - l2cap_state = L2CAP_INTERRUPT_SETUP; - } - break; - case L2CAP_INTERRUPT_SETUP: - if (l2cap_connection_request_interrupt_flag) { + l2cap_state = L2CAP_INTERRUPT_SETUP; + } + break; + case L2CAP_INTERRUPT_SETUP: + if (l2cap_connection_request_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80); + Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80); #endif - pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, PENDING); - delay(1); - pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL); - identifier++; - delay(1); - pBtd->l2cap_config_request(hci_handle,identifier, interrupt_scid); + pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, PENDING); + delay(1); + pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL); + identifier++; + delay(1); + pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid); - l2cap_state = L2CAP_INTERRUPT_REQUEST; - } - break; - case L2CAP_INTERRUPT_REQUEST: - if (l2cap_config_request_interrupt_flag) { + l2cap_state = L2CAP_INTERRUPT_REQUEST; + } + break; + case L2CAP_INTERRUPT_REQUEST: + if (l2cap_config_request_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80); + Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80); #endif - pBtd->l2cap_config_response(hci_handle,identifier, interrupt_scid); - l2cap_state = L2CAP_INTERRUPT_SUCCESS; - } - break; - case L2CAP_INTERRUPT_SUCCESS: - if (l2cap_config_success_interrupt_flag) { + pBtd->l2cap_config_response(hci_handle, identifier, interrupt_scid); + l2cap_state = L2CAP_INTERRUPT_SUCCESS; + } + break; + case L2CAP_INTERRUPT_SUCCESS: + if (l2cap_config_success_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Interrupt Successfully Configured"), 0x80); + Notify(PSTR("\r\nHID Interrupt Successfully Configured"), 0x80); #endif - if(remote_name[0] == 'M') { // First letter in Motion Controller ('M') - for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) // Reset l2cap in buffer as it sometimes read it as a button has been pressed - l2capinbuf[i] = 0; - ButtonState = 0; - OldButtonState = 0; + if (remote_name[0] == 'M') { // First letter in Motion Controller ('M') + for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) // Reset l2cap in buffer as it sometimes read it as a button has been pressed + l2capinbuf[i] = 0; + ButtonState = 0; + OldButtonState = 0; - l2cap_state = L2CAP_HID_PS3_LED; - } else - l2cap_state = L2CAP_HID_ENABLE_SIXAXIS; - timer = millis(); - } - break; + l2cap_state = L2CAP_HID_PS3_LED; + } else + l2cap_state = L2CAP_HID_ENABLE_SIXAXIS; + timer = millis(); + } + break; - /* These states are handled in Run() */ + /* These states are handled in Run() */ - case L2CAP_INTERRUPT_DISCONNECT: - if (l2cap_disconnect_response_interrupt_flag) { + case L2CAP_INTERRUPT_DISCONNECT: + if (l2cap_disconnect_response_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80); + Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80); #endif - identifier++; - pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid); - l2cap_state = L2CAP_CONTROL_DISCONNECT; - } - break; + identifier++; + pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid); + l2cap_state = L2CAP_CONTROL_DISCONNECT; + } + break; - case L2CAP_CONTROL_DISCONNECT: - if (l2cap_disconnect_response_control_flag) { + case L2CAP_CONTROL_DISCONNECT: + if (l2cap_disconnect_response_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected Control Channel"), 0x80); + Notify(PSTR("\r\nDisconnected Control Channel"), 0x80); #endif - pBtd->hci_disconnect(hci_handle); - hci_handle = -1; // Reset handle - l2cap_event_flag = 0; // Reset flags - l2cap_state = L2CAP_WAIT; - } - break; - } + pBtd->hci_disconnect(hci_handle); + hci_handle = -1; // Reset handle + l2cap_event_flag = 0; // Reset flags + l2cap_state = L2CAP_WAIT; + } + break; + } } + void PS3BT::Run() { - switch (l2cap_state) { - case L2CAP_HID_ENABLE_SIXAXIS: - if(millis() - timer > 1000) { // loop 1 second before sending the command - for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) // Reset l2cap in buffer as it sometimes read it as a button has been pressed - l2capinbuf[i] = 0; - ButtonState = 0; - OldButtonState = 0; + switch (l2cap_state) { + case L2CAP_HID_ENABLE_SIXAXIS: + if (millis() - timer > 1000) { // loop 1 second before sending the command + for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) // Reset l2cap in buffer as it sometimes read it as a button has been pressed + l2capinbuf[i] = 0; + ButtonState = 0; + OldButtonState = 0; - enable_sixaxis(); - for (uint8_t i = 15; i < 19; i++) - l2capinbuf[i] = 0x7F; // Set the analog joystick values to center position - l2cap_state = L2CAP_HID_PS3_LED; - timer = millis(); - } - break; + enable_sixaxis(); + for (uint8_t i = 15; i < 19; i++) + l2capinbuf[i] = 0x7F; // Set the analog joystick values to center position + l2cap_state = L2CAP_HID_PS3_LED; + timer = millis(); + } + break; - case L2CAP_HID_PS3_LED: - if(millis() - timer > 1000) { // loop 1 second before sending the command - if (remote_name[0] == 'P') { // First letter in PLAYSTATION(R)3 Controller ('P') - setLedOn(LED1); + case L2CAP_HID_PS3_LED: + if (millis() - timer > 1000) { // loop 1 second before sending the command + if (remote_name[0] == 'P') { // First letter in PLAYSTATION(R)3 Controller ('P') + setLedOn(LED1); #ifdef DEBUG - Notify(PSTR("\r\nDualshock 3 Controller Enabled\r\n"), 0x80); + Notify(PSTR("\r\nDualshock 3 Controller Enabled\r\n"), 0x80); #endif - PS3Connected = true; - } else if (remote_name[0] == 'N') { // First letter in Navigation Controller ('N') - setLedOn(LED1); // This just turns LED constantly on, on the Navigation controller + PS3Connected = true; + } else if (remote_name[0] == 'N') { // First letter in Navigation Controller ('N') + setLedOn(LED1); // This just turns LED constantly on, on the Navigation controller #ifdef DEBUG - Notify(PSTR("\r\nNavigation Controller Enabled\r\n"), 0x80); + Notify(PSTR("\r\nNavigation Controller Enabled\r\n"), 0x80); #endif - PS3NavigationConnected = true; - } else if(remote_name[0] == 'M') { // First letter in Motion Controller ('M') - moveSetBulb(Red); - timerBulbRumble = millis(); + PS3NavigationConnected = true; + } else if (remote_name[0] == 'M') { // First letter in Motion Controller ('M') + moveSetBulb(Red); + timerBulbRumble = millis(); #ifdef DEBUG - Notify(PSTR("\r\nMotion Controller Enabled\r\n"), 0x80); + Notify(PSTR("\r\nMotion Controller Enabled\r\n"), 0x80); #endif - PS3MoveConnected = true; - } - l2cap_state = L2CAP_DONE; - } - break; + PS3MoveConnected = true; + } + l2cap_state = L2CAP_DONE; + } + break; - case L2CAP_DONE: - if (PS3MoveConnected) { //The Bulb and rumble values, has to be send at aproximatly every 5th second for it to stay on - if (millis() - timerBulbRumble > 4000) { //Send at least every 4th second - HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);//The Bulb and rumble values, has to be written again and again, for it to stay turned on - timerBulbRumble = millis(); - } - } - break; - } + case L2CAP_DONE: + if (PS3MoveConnected) { //The Bulb and rumble values, has to be send at aproximatly every 5th second for it to stay on + if (millis() - timerBulbRumble > 4000) { //Send at least every 4th second + HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); //The Bulb and rumble values, has to be written again and again, for it to stay turned on + timerBulbRumble = millis(); + } + } + break; + } } /************************************************************/ @@ -562,96 +564,107 @@ void PS3BT::Run() { /************************************************************/ //Playstation Sixaxis Dualshock and Navigation Controller commands + void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) { - if (millis() - timerHID <= 250)// Check if is has been more than 250ms since last command - delay((uint32_t)(250 - (millis() - timerHID)));//There have to be a delay between commands - pBtd->L2CAP_Command(hci_handle,data,nbytes,control_scid[0],control_scid[1]); // Both the Navigation and Dualshock controller sends data via the control channel - timerHID = millis(); + if (millis() - timerHID <= 250)// Check if is has been more than 250ms since last command + delay((uint32_t)(250 - (millis() - timerHID))); //There have to be a delay between commands + pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]); // Both the Navigation and Dualshock controller sends data via the control channel + timerHID = millis(); } + void PS3BT::setAllOff() { - for (uint8_t i = 0; i < OUTPUT_REPORT_BUFFER_SIZE; i++) - HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]);//First two bytes reserved for report type and ID + for (uint8_t i = 0; i < OUTPUT_REPORT_BUFFER_SIZE; i++) + HIDBuffer[i + 2] = pgm_read_byte(&OUTPUT_REPORT_BUFFER[i]); //First two bytes reserved for report type and ID - HID_Command(HIDBuffer, HID_BUFFERSIZE); -} -void PS3BT::setRumbleOff() { - HIDBuffer[3] = 0x00; - HIDBuffer[4] = 0x00;//low mode off - HIDBuffer[5] = 0x00; - HIDBuffer[6] = 0x00;//high mode off - - HID_Command(HIDBuffer, HID_BUFFERSIZE); -} -void PS3BT::setRumbleOn(Rumble mode) { - /* Still not totally sure how it works, maybe something like this instead? - * 3 - duration_right - * 4 - power_right - * 5 - duration_left - * 6 - power_left - */ - if ((mode & 0x30) > 0) { - HIDBuffer[3] = 0xfe; - HIDBuffer[5] = 0xfe; - if (mode == RumbleHigh) { - HIDBuffer[4] = 0;//low mode off - HIDBuffer[6] = 0xff;//high mode on - } - else { - HIDBuffer[4] = 0xff;//low mode on - HIDBuffer[6] = 0;//high mode off - } HID_Command(HIDBuffer, HID_BUFFERSIZE); - } } -void PS3BT::setLedOff(LED a) { - HIDBuffer[11] &= ~((uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1)); - HID_Command(HIDBuffer, HID_BUFFERSIZE); -} -void PS3BT::setLedOn(LED a) { - HIDBuffer[11] |= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1); - HID_Command(HIDBuffer, HID_BUFFERSIZE); -} -void PS3BT::setLedToggle(LED a) { - HIDBuffer[11] ^= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1); - HID_Command(HIDBuffer, HID_BUFFERSIZE); -} -void PS3BT::enable_sixaxis() { //Command used to enable the Dualshock 3 and Navigation controller to send data via USB - uint8_t cmd_buf[6]; - cmd_buf[0] = 0x53;// HID BT Set_report (0x50) | Report Type (Feature 0x03) - cmd_buf[1] = 0xF4;// Report ID - cmd_buf[2] = 0x42;// Special PS3 Controller enable commands - cmd_buf[3] = 0x03; - cmd_buf[4] = 0x00; - cmd_buf[5] = 0x00; - HID_Command(cmd_buf, 6); +void PS3BT::setRumbleOff() { + HIDBuffer[3] = 0x00; + HIDBuffer[4] = 0x00; //low mode off + HIDBuffer[5] = 0x00; + HIDBuffer[6] = 0x00; //high mode off + + HID_Command(HIDBuffer, HID_BUFFERSIZE); +} + +void PS3BT::setRumbleOn(Rumble mode) { + /* Still not totally sure how it works, maybe something like this instead? + * 3 - duration_right + * 4 - power_right + * 5 - duration_left + * 6 - power_left + */ + if ((mode & 0x30) > 0) { + HIDBuffer[3] = 0xfe; + HIDBuffer[5] = 0xfe; + if (mode == RumbleHigh) { + HIDBuffer[4] = 0; //low mode off + HIDBuffer[6] = 0xff; //high mode on + } else { + HIDBuffer[4] = 0xff; //low mode on + HIDBuffer[6] = 0; //high mode off + } + HID_Command(HIDBuffer, HID_BUFFERSIZE); + } +} + +void PS3BT::setLedOff(LED a) { + HIDBuffer[11] &= ~((uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1)); + HID_Command(HIDBuffer, HID_BUFFERSIZE); +} + +void PS3BT::setLedOn(LED a) { + HIDBuffer[11] |= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1); + HID_Command(HIDBuffer, HID_BUFFERSIZE); +} + +void PS3BT::setLedToggle(LED a) { + HIDBuffer[11] ^= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1); + HID_Command(HIDBuffer, HID_BUFFERSIZE); +} + +void PS3BT::enable_sixaxis() { //Command used to enable the Dualshock 3 and Navigation controller to send data via USB + uint8_t cmd_buf[6]; + cmd_buf[0] = 0x53; // HID BT Set_report (0x50) | Report Type (Feature 0x03) + cmd_buf[1] = 0xF4; // Report ID + cmd_buf[2] = 0x42; // Special PS3 Controller enable commands + cmd_buf[3] = 0x03; + cmd_buf[4] = 0x00; + cmd_buf[5] = 0x00; + + HID_Command(cmd_buf, 6); } //Playstation Move Controller commands -void PS3BT::HIDMove_Command(uint8_t* data,uint8_t nbytes) { - if (millis() - timerHID <= 250)// Check if is has been less than 200ms since last command - delay((uint32_t)(250 - (millis() - timerHID)));//There have to be a delay between commands - pBtd->L2CAP_Command(hci_handle,data,nbytes,interrupt_scid[0],interrupt_scid[1]); // The Move controller sends it's data via the intterrupt channel - timerHID = millis(); -} -void PS3BT::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { //Use this to set the Color using RGB values - //set the Bulb's values into the write buffer - HIDMoveBuffer[3] = r; - HIDMoveBuffer[4] = g; - HIDMoveBuffer[5] = b; - HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); +void PS3BT::HIDMove_Command(uint8_t* data, uint8_t nbytes) { + if (millis() - timerHID <= 250)// Check if is has been less than 200ms since last command + delay((uint32_t)(250 - (millis() - timerHID))); //There have to be a delay between commands + pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // The Move controller sends it's data via the intterrupt channel + timerHID = millis(); } + +void PS3BT::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { //Use this to set the Color using RGB values + //set the Bulb's values into the write buffer + HIDMoveBuffer[3] = r; + HIDMoveBuffer[4] = g; + HIDMoveBuffer[5] = b; + + HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); +} + void PS3BT::moveSetBulb(Colors color) { //Use this to set the Color using the predefined colors in enum - moveSetBulb((uint8_t)(color >> 16),(uint8_t)(color >> 8),(uint8_t)(color)); + moveSetBulb((uint8_t)(color >> 16), (uint8_t)(color >> 8), (uint8_t)(color)); } + void PS3BT::moveSetRumble(uint8_t rumble) { #ifdef DEBUG - if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100) - Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80); + if (rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100) + Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80); #endif - //set the rumble value into the write buffer - HIDMoveBuffer[7] = rumble; + //set the rumble value into the write buffer + HIDMoveBuffer[7] = rumble; - HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); + HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); } \ No newline at end of file diff --git a/PS3BT.h b/PS3BT.h index 75bd4d6e..4b5ed90f 100644 --- a/PS3BT.h +++ b/PS3BT.h @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -60,187 +60,187 @@ /** * This BluetoothService class implements support for all the official PS3 Controllers: * Dualshock 3, Navigation or a Motion controller via Bluetooth. - * + * * Information about the protocol can be found at the wiki: https://github.com/felis/USB_Host_Shield_2.0/wiki/PS3-Information. */ class PS3BT : public BluetoothService { public: - /** - * Constructor for the PS3BT class. - * @param pBtd Pointer to BTD class instance. - * @param btadr5,btadr4,btadr3,btadr2,btadr1,btadr0 - * Pass your dongles Bluetooth address into the constructor, - * This will set BTD#my_bdaddr, so you don't have to plug in the dongle before pairing with your controller. - */ - PS3BT(BTD *pBtd, uint8_t btadr5=0, uint8_t btadr4=0, uint8_t btadr3=0, uint8_t btadr2=0, uint8_t btadr1=0, uint8_t btadr0=0); - - /** @name BluetoothService implementation */ - /** - * Used to pass acldata to the services. - * @param ACLData Incoming acldata. - */ - virtual void ACLData(uint8_t* ACLData); - /** Used to run part of the state maschine. */ - virtual void Run(); - /** Use this to reset the service. */ - virtual void Reset(); - /** Used this to disconnect any of the controllers. */ - virtual void disconnect(); - /**@}*/ - - /** @name PS3 Controller functions */ - /** - * getButtonPress(Button b) will return true as long as the button is held down. - * - * While getButtonClick(Button b) will only return it once. - * - * So you instance if you need to increase a variable once you would use getButtonClick(Button b), - * but if you need to drive a robot forward you would use getButtonPress(Button b). - */ - bool getButtonPress(Button b); - bool getButtonClick(Button b); - /**@}*/ - /** @name PS3 Controller functions */ - /** - * Used to get the analog value from button presses. - * @param a The ::Button to read. - * The supported buttons are: - * ::UP, ::RIGHT, ::DOWN, ::LEFT, ::L1, ::L2, ::R1, ::R2, - * ::TRIANGLE, ::CIRCLE, ::CROSS, ::SQUARE, and ::T. - * @return Analog value in the range of 0-255. - */ - uint8_t getAnalogButton(Button a); - /** - * Used to read the analog joystick. - * @param a ::LeftHatX, ::LeftHatY, ::RightHatX, and ::RightHatY. - * @return Return the analog value in the range of 0-255. - */ - uint8_t getAnalogHat(AnalogHat a); - /** - * Used to read the sensors inside the Dualshock 3 and Move controller. - * @param a - * The Dualshock 3 has a 3-axis accelerometer and a 1-axis gyro inside. - * The Move controller has a 3-axis accelerometer, a 3-axis gyro, a 3-axis magnetometer - * and a temperature sensor inside. - * @return Return the raw sensor value. - */ - int16_t getSensor(Sensor a); - /** - * Use this to get ::Pitch and ::Roll calculated using the accelerometer. - * @param a Either ::Pitch or ::Roll. - * @return Return the angle in the range of 0-360. - */ - double getAngle(Angle a); - /** - * Read the sensors inside the Move controller. - * @param a ::aXmove, ::aYmove, ::aZmove, ::gXmove, ::gYmove, ::gZmove, ::mXmove, ::mYmove, and ::mXmove. - * @return The value in SI units. - */ - double get9DOFValues(Sensor a); - /** - * Get the ::Status from the controller. - * @param c The ::Status you want to read. - * @return True if correct and false if not. - */ - bool getStatus(Status c); - /** - * Read all the available ::Status from the controller. - * @return One large string with all the information. - */ - String getStatusString(); - /** - * Read the temperature from the Move controller. - * @return The temperature in degrees celsius. - */ - String getTemperature(); - - /** Used to set all LEDs and ::Rumble off. */ - void setAllOff(); - /** Turn off ::Rumble. */ - void setRumbleOff(); - /** - * Turn on ::Rumble. - * @param mode Either ::RumbleHigh or ::RumbleLow. - */ - void setRumbleOn(Rumble mode); - /** - * Turn the specific ::LED off. - * @param a The ::LED to turn off. - */ - void setLedOff(LED a); - /** - * Turn the specific ::LED on. - * @param a The ::LED to turn on. - */ - void setLedOn(LED a); - /** - * Toggle the specific ::LED. - * @param a The ::LED to toggle. - */ - void setLedToggle(LED a); - - /** - * Use this to set the Color using RGB values. - * @param r,g,b RGB value. - */ - void moveSetBulb(uint8_t r, uint8_t g, uint8_t b); - /** - * Use this to set the color using the predefined colors in ::Colors. - * @param color The desired color. - */ - void moveSetBulb(Colors color); - /** - * Set the rumble value inside the Move controller. - * @param rumble The desired value in the range from 64-255. - */ - void moveSetRumble(uint8_t rumble); - /**@}*/ - - /** Variable used to indicate if the normal playstation controller is successfully connected. */ - bool PS3Connected; - /** Variable used to indicate if the move controller is successfully connected. */ - bool PS3MoveConnected; - /** Variable used to indicate if the navigation controller is successfully connected. */ - bool PS3NavigationConnected; - + /** + * Constructor for the PS3BT class. + * @param pBtd Pointer to BTD class instance. + * @param btadr5,btadr4,btadr3,btadr2,btadr1,btadr0 + * Pass your dongles Bluetooth address into the constructor, + * This will set BTD#my_bdaddr, so you don't have to plug in the dongle before pairing with your controller. + */ + PS3BT(BTD *pBtd, uint8_t btadr5 = 0, uint8_t btadr4 = 0, uint8_t btadr3 = 0, uint8_t btadr2 = 0, uint8_t btadr1 = 0, uint8_t btadr0 = 0); + + /** @name BluetoothService implementation */ + /** + * Used to pass acldata to the services. + * @param ACLData Incoming acldata. + */ + virtual void ACLData(uint8_t* ACLData); + /** Used to run part of the state maschine. */ + virtual void Run(); + /** Use this to reset the service. */ + virtual void Reset(); + /** Used this to disconnect any of the controllers. */ + virtual void disconnect(); + /**@}*/ + + /** @name PS3 Controller functions */ + /** + * getButtonPress(Button b) will return true as long as the button is held down. + * + * While getButtonClick(Button b) will only return it once. + * + * So you instance if you need to increase a variable once you would use getButtonClick(Button b), + * but if you need to drive a robot forward you would use getButtonPress(Button b). + */ + bool getButtonPress(Button b); + bool getButtonClick(Button b); + /**@}*/ + /** @name PS3 Controller functions */ + /** + * Used to get the analog value from button presses. + * @param a The ::Button to read. + * The supported buttons are: + * ::UP, ::RIGHT, ::DOWN, ::LEFT, ::L1, ::L2, ::R1, ::R2, + * ::TRIANGLE, ::CIRCLE, ::CROSS, ::SQUARE, and ::T. + * @return Analog value in the range of 0-255. + */ + uint8_t getAnalogButton(Button a); + /** + * Used to read the analog joystick. + * @param a ::LeftHatX, ::LeftHatY, ::RightHatX, and ::RightHatY. + * @return Return the analog value in the range of 0-255. + */ + uint8_t getAnalogHat(AnalogHat a); + /** + * Used to read the sensors inside the Dualshock 3 and Move controller. + * @param a + * The Dualshock 3 has a 3-axis accelerometer and a 1-axis gyro inside. + * The Move controller has a 3-axis accelerometer, a 3-axis gyro, a 3-axis magnetometer + * and a temperature sensor inside. + * @return Return the raw sensor value. + */ + int16_t getSensor(Sensor a); + /** + * Use this to get ::Pitch and ::Roll calculated using the accelerometer. + * @param a Either ::Pitch or ::Roll. + * @return Return the angle in the range of 0-360. + */ + double getAngle(Angle a); + /** + * Read the sensors inside the Move controller. + * @param a ::aXmove, ::aYmove, ::aZmove, ::gXmove, ::gYmove, ::gZmove, ::mXmove, ::mYmove, and ::mXmove. + * @return The value in SI units. + */ + double get9DOFValues(Sensor a); + /** + * Get the ::Status from the controller. + * @param c The ::Status you want to read. + * @return True if correct and false if not. + */ + bool getStatus(Status c); + /** + * Read all the available ::Status from the controller. + * @return One large string with all the information. + */ + String getStatusString(); + /** + * Read the temperature from the Move controller. + * @return The temperature in degrees celsius. + */ + String getTemperature(); + + /** Used to set all LEDs and ::Rumble off. */ + void setAllOff(); + /** Turn off ::Rumble. */ + void setRumbleOff(); + /** + * Turn on ::Rumble. + * @param mode Either ::RumbleHigh or ::RumbleLow. + */ + void setRumbleOn(Rumble mode); + /** + * Turn the specific ::LED off. + * @param a The ::LED to turn off. + */ + void setLedOff(LED a); + /** + * Turn the specific ::LED on. + * @param a The ::LED to turn on. + */ + void setLedOn(LED a); + /** + * Toggle the specific ::LED. + * @param a The ::LED to toggle. + */ + void setLedToggle(LED a); + + /** + * Use this to set the Color using RGB values. + * @param r,g,b RGB value. + */ + void moveSetBulb(uint8_t r, uint8_t g, uint8_t b); + /** + * Use this to set the color using the predefined colors in ::Colors. + * @param color The desired color. + */ + void moveSetBulb(Colors color); + /** + * Set the rumble value inside the Move controller. + * @param rumble The desired value in the range from 64-255. + */ + void moveSetRumble(uint8_t rumble); + /**@}*/ + + /** Variable used to indicate if the normal playstation controller is successfully connected. */ + bool PS3Connected; + /** Variable used to indicate if the move controller is successfully connected. */ + bool PS3MoveConnected; + /** Variable used to indicate if the navigation controller is successfully connected. */ + bool PS3NavigationConnected; + private: - /* mandatory members */ - BTD *pBtd; - - void L2CAP_task(); // L2CAP state machine - - /* Variables filled from HCI event management */ - int16_t hci_handle; - uint8_t remote_name[30]; // First 30 chars of remote name - bool activeConnection; // Used to indicate if it's already has established a connection - - /* variables used by high level L2CAP task */ - uint8_t l2cap_state; - uint16_t l2cap_event_flag; // L2CAP flags of received bluetooth events - - unsigned long timer; - - uint32_t ButtonState; - uint32_t OldButtonState; - uint32_t ButtonClickState; - - uint32_t timerHID; // Timer used see if there has to be a delay before a new HID command - uint32_t timerBulbRumble;// used to continuously set PS3 Move controller Bulb and rumble values - - uint8_t l2capinbuf[BULK_MAXPKTSIZE]; // General purpose buffer for L2CAP in data - uint8_t HIDBuffer[HID_BUFFERSIZE]; // Used to store HID commands - uint8_t HIDMoveBuffer[HID_BUFFERSIZE]; // Used to store HID commands for the Move controller - - /* L2CAP Channels */ - uint8_t control_scid[2]; // L2CAP source CID for HID_Control - uint8_t control_dcid[2]; // 0x0040 - uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt - uint8_t interrupt_dcid[2]; // 0x0041 - uint8_t identifier; // Identifier for connection - - /* HID Commands */ - void HID_Command(uint8_t* data, uint8_t nbytes); - void HIDMove_Command(uint8_t* data, uint8_t nbytes); - void enable_sixaxis(); // Command used to enable the Dualshock 3 and Navigation controller to send data via Bluetooth + /* mandatory members */ + BTD *pBtd; + + void L2CAP_task(); // L2CAP state machine + + /* Variables filled from HCI event management */ + int16_t hci_handle; + uint8_t remote_name[30]; // First 30 chars of remote name + bool activeConnection; // Used to indicate if it's already has established a connection + + /* variables used by high level L2CAP task */ + uint8_t l2cap_state; + uint16_t l2cap_event_flag; // L2CAP flags of received bluetooth events + + unsigned long timer; + + uint32_t ButtonState; + uint32_t OldButtonState; + uint32_t ButtonClickState; + + uint32_t timerHID; // Timer used see if there has to be a delay before a new HID command + uint32_t timerBulbRumble; // used to continuously set PS3 Move controller Bulb and rumble values + + uint8_t l2capinbuf[BULK_MAXPKTSIZE]; // General purpose buffer for L2CAP in data + uint8_t HIDBuffer[HID_BUFFERSIZE]; // Used to store HID commands + uint8_t HIDMoveBuffer[HID_BUFFERSIZE]; // Used to store HID commands for the Move controller + + /* L2CAP Channels */ + uint8_t control_scid[2]; // L2CAP source CID for HID_Control + uint8_t control_dcid[2]; // 0x0040 + uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt + uint8_t interrupt_dcid[2]; // 0x0041 + uint8_t identifier; // Identifier for connection + + /* HID Commands */ + void HID_Command(uint8_t* data, uint8_t nbytes); + void HIDMove_Command(uint8_t* data, uint8_t nbytes); + void enable_sixaxis(); // Command used to enable the Dualshock 3 and Navigation controller to send data via Bluetooth }; #endif \ No newline at end of file diff --git a/PS3Enums.h b/PS3Enums.h index 449f93b9..d42f7903 100644 --- a/PS3Enums.h +++ b/PS3Enums.h @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -22,17 +22,17 @@ /** Used to set the LEDs on the controllers */ const uint8_t LEDS[] PROGMEM = { - 0x01, // LED1 - 0x02, // LED2 - 0x04, // LED3 - 0x08, // LED4 - - 0x09, // LED5 - 0x0A, // LED6 - 0x0C, // LED7 - 0x0D, // LED8 - 0x0E, // LED9 - 0x0F // LED10 + 0x01, // LED1 + 0x02, // LED2 + 0x04, // LED3 + 0x08, // LED4 + + 0x09, // LED5 + 0x0A, // LED6 + 0x0C, // LED7 + 0x0D, // LED8 + 0x0E, // LED9 + 0x0F // LED10 }; /** @@ -41,29 +41,29 @@ const uint8_t LEDS[] PROGMEM = { * Note: that the location is shiftet 9 when it's connected via USB. */ const uint32_t BUTTONS[] PROGMEM = { - 0x10, // UP - 0x20, // RIGHT - 0x40, // DOWN - 0x80, // LEFT + 0x10, // UP + 0x20, // RIGHT + 0x40, // DOWN + 0x80, // LEFT - 0x01, // SELECT - 0x08, // START - 0x02, // L3 - 0x04, // R3 - - 0x0100, // L2 - 0x0200, // R2 - 0x0400, // L1 - 0x0800, // R1 + 0x01, // SELECT + 0x08, // START + 0x02, // L3 + 0x04, // R3 - 0x1000, // TRIANGLE - 0x2000, // CIRCLE - 0x4000, // CROSS - 0x8000, // SQUARE - - 0x010000, // PS - 0x080000, // MOVE - covers 12 bits - we only need to read the top 8 - 0x100000 // T - covers 12 bits - we only need to read the top 8 + 0x0100, // L2 + 0x0200, // R2 + 0x0400, // L1 + 0x0800, // R1 + + 0x1000, // TRIANGLE + 0x2000, // CIRCLE + 0x4000, // CROSS + 0x8000, // SQUARE + + 0x010000, // PS + 0x080000, // MOVE - covers 12 bits - we only need to read the top 8 + 0x100000 // T - covers 12 bits - we only need to read the top 8 }; /** @@ -72,122 +72,125 @@ const uint32_t BUTTONS[] PROGMEM = { * Note: that the location is shiftet 9 when it's connected via USB. */ const uint8_t ANALOGBUTTONS[] PROGMEM = { - 23, // UP_ANALOG - 24, // RIGHT_ANALOG - 25, // DOWN_ANALOG - 26, // LEFT_ANALOG - 0,0,0,0, // Skip SELECT, L3, R3 and START - - 27, // L2_ANALOG - 28, // R2_ANALOG - 29, // L1_ANALOG - 30, // R1_ANALOG - 31, // TRIANGLE_ANALOG - 32, // CIRCLE_ANALOG - 33, // CROSS_ANALOG - 34, // SQUARE_ANALOG - 0,0, // Skip PS and MOVE - - // Playstation Move Controller - 15 // T_ANALOG - Both at byte 14 (last reading) and byte 15 (current reading) + 23, // UP_ANALOG + 24, // RIGHT_ANALOG + 25, // DOWN_ANALOG + 26, // LEFT_ANALOG + 0, 0, 0, 0, // Skip SELECT, L3, R3 and START + + 27, // L2_ANALOG + 28, // R2_ANALOG + 29, // L1_ANALOG + 30, // R1_ANALOG + 31, // TRIANGLE_ANALOG + 32, // CIRCLE_ANALOG + 33, // CROSS_ANALOG + 34, // SQUARE_ANALOG + 0, 0, // Skip PS and MOVE + + // Playstation Move Controller + 15 // T_ANALOG - Both at byte 14 (last reading) and byte 15 (current reading) }; /** Used to set the colors of the move controller. */ enum Colors { - /** r = 255, g = 0, b = 0 */ - Red = 0xFF0000, - /** r = 0, g = 255, b = 0 */ - Green = 0xFF00, - /** r = 0, g = 0, b = 255 */ - Blue = 0xFF, - - /** r = 255, g = 235, b = 4 */ - Yellow = 0xFFEB04, - /** r = 0, g = 255, b = 255 */ - Lightblue = 0xFFFF, - /** r = 255, g = 0, b = 255 */ - Purble = 0xFF00FF, - - /** r = 255, g = 255, b = 255 */ - White = 0xFFFFFF, - /** r = 0, g = 0, b = 0 */ - Off = 0x00, + /** r = 255, g = 0, b = 0 */ + Red = 0xFF0000, + /** r = 0, g = 255, b = 0 */ + Green = 0xFF00, + /** r = 0, g = 0, b = 255 */ + Blue = 0xFF, + + /** r = 255, g = 235, b = 4 */ + Yellow = 0xFFEB04, + /** r = 0, g = 255, b = 255 */ + Lightblue = 0xFFFF, + /** r = 255, g = 0, b = 255 */ + Purble = 0xFF00FF, + + /** r = 255, g = 255, b = 255 */ + White = 0xFFFFFF, + /** r = 0, g = 0, b = 0 */ + Off = 0x00, }; /** * Sensors inside the Sixaxis Dualshock 3 and Move controller. - * + * * Note: that the location is shiftet 9 when it's connected via USB. */ enum Sensor { - /** Accelerometer x-axis */ - aX = 50, - /** Accelerometer y-axis */ - aY = 52, - /** Accelerometer z-axis */ - aZ = 54, - /** Gyro z-axis */ - gZ = 56, + /** Accelerometer x-axis */ + aX = 50, + /** Accelerometer y-axis */ + aY = 52, + /** Accelerometer z-axis */ + aZ = 54, + /** Gyro z-axis */ + gZ = 56, - /** Accelerometer x-axis */ - aXmove = 28, - /** Accelerometer z-axis */ - aZmove = 30, - /** Accelerometer y-axis */ - aYmove = 32, - - /** Gyro x-axis */ - gXmove = 40, - /** Gyro z-axis */ - gZmove = 42, - /** Gyro y-axis */ - gYmove = 44, - - /** Temperature sensor */ - tempMove = 46, - - /** Magnetometer x-axis */ - mXmove = 47, - /** Magnetometer z-axis */ - mZmove = 49, - /** Magnetometer y-axis */ - mYmove = 50, + /** Accelerometer x-axis */ + aXmove = 28, + /** Accelerometer z-axis */ + aZmove = 30, + /** Accelerometer y-axis */ + aYmove = 32, + + /** Gyro x-axis */ + gXmove = 40, + /** Gyro z-axis */ + gZmove = 42, + /** Gyro y-axis */ + gYmove = 44, + + /** Temperature sensor */ + tempMove = 46, + + /** Magnetometer x-axis */ + mXmove = 47, + /** Magnetometer z-axis */ + mZmove = 49, + /** Magnetometer y-axis */ + mYmove = 50, }; + /** Used to get the angle calculated using the accelerometer. */ enum Angle { - Pitch = 0x01, - Roll = 0x02, + Pitch = 0x01, + Roll = 0x02, }; + enum Status { - // Note that the location is shiftet 9 when it's connected via USB - // Byte location | bit location - Plugged = (38 << 8) | 0x02, - Unplugged = (38 << 8) | 0x03, - - Charging = (39 << 8) | 0xEE, - NotCharging = (39 << 8) | 0xF1, - Shutdown = (39 << 8) | 0x01, - Dying = (39 << 8) | 0x02, - Low = (39 << 8) | 0x03, - High = (39 << 8) | 0x04, - Full = (39 << 8) | 0x05, - - MoveCharging = (21 << 8) | 0xEE, - MoveNotCharging = (21 << 8) | 0xF1, - MoveShutdown = (21 << 8) | 0x01, - MoveDying = (21 << 8) | 0x02, - MoveLow = (21 << 8) | 0x03, - MoveHigh = (21 << 8) | 0x04, - MoveFull = (21 << 8) | 0x05, - - CableRumble = (40 << 8) | 0x10,//Opperating by USB and rumble is turned on - Cable = (40 << 8) | 0x12,//Opperating by USB and rumble is turned off - BluetoothRumble = (40 << 8) | 0x14,//Opperating by bluetooth and rumble is turned on - Bluetooth = (40 << 8) | 0x16,//Opperating by bluetooth and rumble is turned off + // Note that the location is shiftet 9 when it's connected via USB + // Byte location | bit location + Plugged = (38 << 8) | 0x02, + Unplugged = (38 << 8) | 0x03, + + Charging = (39 << 8) | 0xEE, + NotCharging = (39 << 8) | 0xF1, + Shutdown = (39 << 8) | 0x01, + Dying = (39 << 8) | 0x02, + Low = (39 << 8) | 0x03, + High = (39 << 8) | 0x04, + Full = (39 << 8) | 0x05, + + MoveCharging = (21 << 8) | 0xEE, + MoveNotCharging = (21 << 8) | 0xF1, + MoveShutdown = (21 << 8) | 0x01, + MoveDying = (21 << 8) | 0x02, + MoveLow = (21 << 8) | 0x03, + MoveHigh = (21 << 8) | 0x04, + MoveFull = (21 << 8) | 0x05, + + CableRumble = (40 << 8) | 0x10, //Opperating by USB and rumble is turned on + Cable = (40 << 8) | 0x12, //Opperating by USB and rumble is turned off + BluetoothRumble = (40 << 8) | 0x14, //Opperating by bluetooth and rumble is turned on + Bluetooth = (40 << 8) | 0x16, //Opperating by bluetooth and rumble is turned off }; -enum Rumble { - RumbleHigh = 0x10, - RumbleLow = 0x20, + +enum Rumble { + RumbleHigh = 0x10, + RumbleLow = 0x20, }; #endif \ No newline at end of file diff --git a/PS3USB.cpp b/PS3USB.cpp index 13d835db..a7ca846a 100644 --- a/PS3USB.cpp +++ b/PS3USB.cpp @@ -21,531 +21,547 @@ //#define PRINTREPORT // Uncomment to print the report send by the PS3 Controllers const uint8_t PS3_REPORT_BUFFER[] PROGMEM = { - 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 + 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 }; const uint8_t MOVE_REPORT_BUFFER[] PROGMEM = { - 0x02, 0x00, // Always 0x02, 0x00, - 0x00, 0x00, 0x00, // r, g, b, - 0x00, // Always 0x00, - 0x00 // Rumble + 0x02, 0x00, // Always 0x02, 0x00, + 0x00, 0x00, 0x00, // r, g, b, + 0x00, // Always 0x00, + 0x00 // Rumble }; -PS3USB::PS3USB(USB *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0): +PS3USB::PS3USB(USB *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0) : pUsb(p), // pointer to USB class instance - mandatory bAddress(0), // device address - mandatory bPollEnable(false) // don't start polling before dongle is connected { - for(uint8_t i=0; iRegisterDeviceClass(this); //set devConfig[] entry + if (pUsb) // register in USB subsystem + pUsb->RegisterDeviceClass(this); //set devConfig[] entry - my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead - my_bdaddr[4] = btadr4; - my_bdaddr[3] = btadr3; - my_bdaddr[2] = btadr2; - my_bdaddr[1] = btadr1; - my_bdaddr[0] = btadr0; + my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead + my_bdaddr[4] = btadr4; + my_bdaddr[3] = btadr3; + my_bdaddr[2] = btadr2; + my_bdaddr[1] = btadr1; + my_bdaddr[0] = btadr0; } uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) { - uint8_t buf[sizeof(USB_DEVICE_DESCRIPTOR)]; - uint8_t rcode; - UsbDevice *p = NULL; - EpInfo *oldep_ptr = NULL; - uint16_t PID; - uint16_t VID; + uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)]; + uint8_t rcode; + UsbDevice *p = NULL; + EpInfo *oldep_ptr = NULL; + uint16_t PID; + uint16_t VID; - // get memory address of USB device address pool - AddressPool &addrPool = pUsb->GetAddressPool(); + // get memory address of USB device address pool + AddressPool &addrPool = pUsb->GetAddressPool(); #ifdef EXTRADEBUG - Notify(PSTR("\r\nPS3USB Init"), 0x80); + Notify(PSTR("\r\nPS3USB Init"), 0x80); #endif - // check if address has already been assigned to an instance - if (bAddress) { + // check if address has already been assigned to an instance + if (bAddress) { #ifdef DEBUG - Notify(PSTR("\r\nAddress in use"), 0x80); + Notify(PSTR("\r\nAddress in use"), 0x80); #endif - return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; - } - - // Get pointer to pseudo device with address 0 assigned - p = addrPool.GetUsbDevicePtr(0); - - if (!p) { -#ifdef DEBUG - Notify(PSTR("\r\nAddress not found"), 0x80); -#endif - return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - } - - if (!p->epinfo) { -#ifdef DEBUG - Notify(PSTR("\r\nepinfo is null"), 0x80); -#endif - return USB_ERROR_EPINFO_IS_NULL; - } - - // Save old pointer to EP_RECORD of address 0 - oldep_ptr = p->epinfo; - - // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence - p->epinfo = epInfo; - - p->lowspeed = lowspeed; - - // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data - // Restore p->epinfo - p->epinfo = oldep_ptr; - - if(rcode) - goto FailGetDevDescr; - - VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; - PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; - - if(VID != PS3_VID || (PID != PS3_PID && PID != PS3NAVIGATION_PID && PID != PS3MOVE_PID)) - goto FailUnknownDevice; - - // Allocate new address according to device class - bAddress = addrPool.AllocAddress(parent, false, port); - - if (!bAddress) - return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; - - // Extract Max Packet Size from device descriptor - epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; - - // Assign new address to the device - rcode = pUsb->setAddr( 0, 0, bAddress ); - if (rcode) { - p->lowspeed = false; - addrPool.FreeAddress(bAddress); - bAddress = 0; -#ifdef DEBUG - Notify(PSTR("\r\nsetAddr: "), 0x80); -#endif - PrintHex(rcode, 0x80); - return rcode; - } -#ifdef EXTRADEBUG - Notify(PSTR("\r\nAddr: "), 0x80); - PrintHex(bAddress, 0x80); -#endif - p->lowspeed = false; - - //get pointer to assigned address record - p = addrPool.GetUsbDevicePtr(bAddress); - if (!p) - return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - - p->lowspeed = lowspeed; - - // Assign epInfo to epinfo pointer - only EP0 is known - rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); - if (rcode) - goto FailSetDevTblEntry; - - - /* The application will work in reduced host mode, so we can save program and data - memory space. After verifying the PID and VID we will use known values for the - configuration values for device, interface, endpoints and HID for the PS3 Controllers */ - - /* Initialize data structures for endpoints of device */ - epInfo[ PS3_OUTPUT_PIPE ].epAddr = 0x02; // PS3 output endpoint - epInfo[ PS3_OUTPUT_PIPE ].epAttribs = EP_INTERRUPT; - epInfo[ PS3_OUTPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ PS3_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ PS3_OUTPUT_PIPE ].bmSndToggle = bmSNDTOG0; - epInfo[ PS3_OUTPUT_PIPE ].bmRcvToggle = bmRCVTOG0; - epInfo[ PS3_INPUT_PIPE ].epAddr = 0x01; // PS3 report endpoint - epInfo[ PS3_INPUT_PIPE ].epAttribs = EP_INTERRUPT; - epInfo[ PS3_INPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ PS3_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ PS3_INPUT_PIPE ].bmSndToggle = bmSNDTOG0; - epInfo[ PS3_INPUT_PIPE ].bmRcvToggle = bmRCVTOG0; - - rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo); - if( rcode ) - goto FailSetDevTblEntry; - - delay(200);//Give time for address change - - rcode = pUsb->setConf(bAddress, epInfo[ PS3_CONTROL_PIPE ].epAddr, 1); - if( rcode ) - goto FailSetConf; - - if(PID == PS3_PID || PID == PS3NAVIGATION_PID) { - if(PID == PS3_PID) { -#ifdef DEBUG - Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80); -#endif - PS3Connected = true; - } else { // must be a navigation controller -#ifdef DEBUG - Notify(PSTR("\r\nNavigation Controller Connected"), 0x80); -#endif - PS3NavigationConnected = true; + return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; } - /* Set internal bluetooth address and request for data */ - setBdaddr(my_bdaddr); - enable_sixaxis(); - setLedOn(LED1); - // Needed for PS3 Dualshock and Navigation commands to work - for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) - writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); + // Get pointer to pseudo device with address 0 assigned + p = addrPool.GetUsbDevicePtr(0); - for (uint8_t i = 6; i < 10; i++) - readBuf[i] = 0x7F; // Set the analog joystick values to center position - } - else { // must be a Motion controller + if (!p) { #ifdef DEBUG - Notify(PSTR("\r\nMotion Controller Connected"), 0x80); + Notify(PSTR("\r\nAddress not found"), 0x80); #endif - PS3MoveConnected = true; - setMoveBdaddr(my_bdaddr); // Set internal bluetooth address - moveSetBulb(Red); + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + } - // Needed for Move commands to work - for (uint8_t i = 0; i < MOVE_REPORT_BUFFER_SIZE; i++) - writeBuf[i] = pgm_read_byte(&MOVE_REPORT_BUFFER[i]); - } + if (!p->epinfo) { +#ifdef DEBUG + Notify(PSTR("\r\nepinfo is null"), 0x80); +#endif + return USB_ERROR_EPINFO_IS_NULL; + } - bPollEnable = true; - Notify(PSTR("\r\n"), 0x80); - timer = millis(); - return 0; // successful configuration + // Save old pointer to EP_RECORD of address 0 + oldep_ptr = p->epinfo; - /* diagnostic messages */ + // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence + p->epinfo = epInfo; + + p->lowspeed = lowspeed; + + // Get device descriptor + rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*)buf); // Get device descriptor - addr, ep, nbytes, data + // Restore p->epinfo + p->epinfo = oldep_ptr; + + if (rcode) + goto FailGetDevDescr; + + VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; + PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; + + if (VID != PS3_VID || (PID != PS3_PID && PID != PS3NAVIGATION_PID && PID != PS3MOVE_PID)) + goto FailUnknownDevice; + + // Allocate new address according to device class + bAddress = addrPool.AllocAddress(parent, false, port); + + if (!bAddress) + return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; + + // Extract Max Packet Size from device descriptor + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; + + // Assign new address to the device + rcode = pUsb->setAddr(0, 0, bAddress); + if (rcode) { + p->lowspeed = false; + addrPool.FreeAddress(bAddress); + bAddress = 0; +#ifdef DEBUG + Notify(PSTR("\r\nsetAddr: "), 0x80); +#endif + PrintHex (rcode, 0x80); + return rcode; + } +#ifdef EXTRADEBUG + Notify(PSTR("\r\nAddr: "), 0x80); + PrintHex (bAddress, 0x80); +#endif + p->lowspeed = false; + + //get pointer to assigned address record + p = addrPool.GetUsbDevicePtr(bAddress); + if (!p) + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + + p->lowspeed = lowspeed; + + // Assign epInfo to epinfo pointer - only EP0 is known + rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); + if (rcode) + goto FailSetDevTblEntry; + + + /* The application will work in reduced host mode, so we can save program and data + memory space. After verifying the PID and VID we will use known values for the + configuration values for device, interface, endpoints and HID for the PS3 Controllers */ + + /* Initialize data structures for endpoints of device */ + epInfo[ PS3_OUTPUT_PIPE ].epAddr = 0x02; // PS3 output endpoint + epInfo[ PS3_OUTPUT_PIPE ].epAttribs = EP_INTERRUPT; + epInfo[ PS3_OUTPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ PS3_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ PS3_OUTPUT_PIPE ].bmSndToggle = bmSNDTOG0; + epInfo[ PS3_OUTPUT_PIPE ].bmRcvToggle = bmRCVTOG0; + epInfo[ PS3_INPUT_PIPE ].epAddr = 0x01; // PS3 report endpoint + epInfo[ PS3_INPUT_PIPE ].epAttribs = EP_INTERRUPT; + epInfo[ PS3_INPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ PS3_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ PS3_INPUT_PIPE ].bmSndToggle = bmSNDTOG0; + epInfo[ PS3_INPUT_PIPE ].bmRcvToggle = bmRCVTOG0; + + rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo); + if (rcode) + goto FailSetDevTblEntry; + + delay(200); //Give time for address change + + rcode = pUsb->setConf(bAddress, epInfo[ PS3_CONTROL_PIPE ].epAddr, 1); + if (rcode) + goto FailSetConf; + + if (PID == PS3_PID || PID == PS3NAVIGATION_PID) { + if (PID == PS3_PID) { +#ifdef DEBUG + Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80); +#endif + PS3Connected = true; + } else { // must be a navigation controller +#ifdef DEBUG + Notify(PSTR("\r\nNavigation Controller Connected"), 0x80); +#endif + PS3NavigationConnected = true; + } + /* Set internal bluetooth address and request for data */ + setBdaddr(my_bdaddr); + enable_sixaxis(); + setLedOn(LED1); + + // Needed for PS3 Dualshock and Navigation commands to work + for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) + writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); + + for (uint8_t i = 6; i < 10; i++) + readBuf[i] = 0x7F; // Set the analog joystick values to center position + } else { // must be a Motion controller +#ifdef DEBUG + Notify(PSTR("\r\nMotion Controller Connected"), 0x80); +#endif + PS3MoveConnected = true; + setMoveBdaddr(my_bdaddr); // Set internal bluetooth address + moveSetBulb(Red); + + // Needed for Move commands to work + for (uint8_t i = 0; i < MOVE_REPORT_BUFFER_SIZE; i++) + writeBuf[i] = pgm_read_byte(&MOVE_REPORT_BUFFER[i]); + } + + bPollEnable = true; + Notify(PSTR("\r\n"), 0x80); + timer = millis(); + return 0; // successful configuration + + /* diagnostic messages */ FailGetDevDescr: #ifdef DEBUG - Notify(PSTR("\r\ngetDevDescr:"), 0x80); + Notify(PSTR("\r\ngetDevDescr:"), 0x80); #endif - goto Fail; + goto Fail; FailSetDevTblEntry: #ifdef DEBUG - Notify(PSTR("\r\nsetDevTblEn:"), 0x80); + Notify(PSTR("\r\nsetDevTblEn:"), 0x80); #endif - goto Fail; + goto Fail; FailSetConf: #ifdef DEBUG - Notify(PSTR("\r\nsetConf:"), 0x80); + Notify(PSTR("\r\nsetConf:"), 0x80); #endif - goto Fail; + goto Fail; FailUnknownDevice: #ifdef DEBUG - Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); - PrintHex(VID, 0x80); - Notify(PSTR(" PID: "), 0x80); - PrintHex(PID, 0x80); + Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); + PrintHex (VID, 0x80); + Notify(PSTR(" PID: "), 0x80); + PrintHex (PID, 0x80); #endif - rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; - goto Fail; + rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; + goto Fail; Fail: #ifdef DEBUG - Notify(PSTR("\r\nPS3 Init Failed, error code: "), 0x80); - Serial.print(rcode,HEX); + Notify(PSTR("\r\nPS3 Init Failed, error code: "), 0x80); + Serial.print(rcode, HEX); #endif - Release(); - return rcode; + Release(); + return rcode; } /* Performs a cleanup after failed Init() attempt */ uint8_t PS3USB::Release() { - PS3Connected = false; - PS3MoveConnected = false; - PS3NavigationConnected = false; - pUsb->GetAddressPool().FreeAddress(bAddress); - bAddress = 0; - bPollEnable = false; - return 0; + PS3Connected = false; + PS3MoveConnected = false; + PS3NavigationConnected = false; + pUsb->GetAddressPool().FreeAddress(bAddress); + bAddress = 0; + bPollEnable = false; + return 0; } -uint8_t PS3USB::Poll() { - if (!bPollEnable) - return 0; - if(PS3Connected || PS3NavigationConnected) { - uint16_t BUFFER_SIZE = EP_MAXPKTSIZE; - pUsb->inTransfer(bAddress, epInfo[ PS3_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1 - if(millis() - timer > 100) { // Loop 100ms before processing data - readReport(); +uint8_t PS3USB::Poll() { + if (!bPollEnable) + return 0; + + if (PS3Connected || PS3NavigationConnected) { + uint16_t BUFFER_SIZE = EP_MAXPKTSIZE; + pUsb->inTransfer(bAddress, epInfo[ PS3_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1 + if (millis() - timer > 100) { // Loop 100ms before processing data + readReport(); #ifdef PRINTREPORT - printReport(); // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers + printReport(); // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers #endif + } + } else if (PS3MoveConnected) { // One can only set the color of the bulb, set the rumble, set and get the bluetooth address and calibrate the magnetometer via USB + if (millis() - timer > 4000) // Send at least every 4th second + { + Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); // The Bulb and rumble values, has to be written again and again, for it to stay turned on + timer = millis(); + } } - } - else if(PS3MoveConnected) { // One can only set the color of the bulb, set the rumble, set and get the bluetooth address and calibrate the magnetometer via USB - if (millis() - timer > 4000) // Send at least every 4th second - { - Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); // The Bulb and rumble values, has to be written again and again, for it to stay turned on - timer = millis(); - } - } - return 0; + return 0; } void PS3USB::readReport() { - if (readBuf == NULL) - return; + if (readBuf == NULL) + return; - ButtonState = (uint32_t)(readBuf[2] | ((uint16_t)readBuf[3] << 8) | ((uint32_t)readBuf[4] << 16)); + ButtonState = (uint32_t)(readBuf[2] | ((uint16_t)readBuf[3] << 8) | ((uint32_t)readBuf[4] << 16)); - //Notify(PSTR("\r\nButtonState", 0x80); - //PrintHex(ButtonState, 0x80); + //Notify(PSTR("\r\nButtonState", 0x80); + //PrintHex(ButtonState, 0x80); - if(ButtonState != OldButtonState) { - ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable - OldButtonState = ButtonState; - } + if (ButtonState != OldButtonState) { + ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable + OldButtonState = ButtonState; + } } void PS3USB::printReport() { //Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers #ifdef PRINTREPORT - if (readBuf == NULL) - return; - for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE;i++) { - PrintHex(readBuf[i], 0x80); - Serial.print(" "); - } - Serial.println(); + if (readBuf == NULL) + return; + for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) { + PrintHex (readBuf[i], 0x80); + Serial.print(" "); + } + Serial.println(); #endif } bool PS3USB::getButtonPress(Button b) { - return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b])); + return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b])); } + bool PS3USB::getButtonClick(Button b) { - uint32_t button = pgm_read_dword(&BUTTONS[(uint8_t)b]); - bool click = (ButtonClickState & button); - ButtonClickState &= ~button; // clear "click" event - return click; + uint32_t button = pgm_read_dword(&BUTTONS[(uint8_t)b]); + bool click = (ButtonClickState & button); + ButtonClickState &= ~button; // clear "click" event + return click; } + uint8_t PS3USB::getAnalogButton(Button a) { - if (readBuf == NULL) - return 0; - return (uint8_t)(readBuf[(pgm_read_byte(&ANALOGBUTTONS[(uint8_t)a]))-9]); + if (readBuf == NULL) + return 0; + return (uint8_t)(readBuf[(pgm_read_byte(&ANALOGBUTTONS[(uint8_t)a])) - 9]); } + uint8_t PS3USB::getAnalogHat(AnalogHat a) { - if (readBuf == NULL) - return 0; - return (uint8_t)(readBuf[((uint8_t)a+6)]); + if (readBuf == NULL) + return 0; + return (uint8_t)(readBuf[((uint8_t)a + 6)]); } + uint16_t PS3USB::getSensor(Sensor a) { - if (readBuf == NULL) - return 0; - return ((readBuf[((uint16_t)a)-9] << 8) | readBuf[((uint16_t)a + 1)-9]); + if (readBuf == NULL) + return 0; + return ((readBuf[((uint16_t)a) - 9] << 8) | readBuf[((uint16_t)a + 1) - 9]); } + double PS3USB::getAngle(Angle a) { - if(PS3Connected) { - double accXval; - double accYval; - double accZval; + if (PS3Connected) { + double accXval; + double accYval; + double accZval; - // Data for the Kionix KXPC4 used in the DualShock 3 - const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) - accXval = -((double)getSensor(aX)-zeroG); - accYval = -((double)getSensor(aY)-zeroG); - accZval = -((double)getSensor(aZ)-zeroG); + // Data for the Kionix KXPC4 used in the DualShock 3 + const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) + accXval = -((double)getSensor(aX) - zeroG); + accYval = -((double)getSensor(aY) - zeroG); + accZval = -((double)getSensor(aZ) - zeroG); - // Convert to 360 degrees resolution - // atan2 outputs the value of -π to π (radians) - // We are then converting it to 0 to 2π and then to degrees - if (a == Pitch) { - double angle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG; - return angle; - } else { - double angle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG; - return angle; - } - } else - return 0; + // Convert to 360 degrees resolution + // atan2 outputs the value of -π to π (radians) + // We are then converting it to 0 to 2π and then to degrees + if (a == Pitch) { + double angle = (atan2(accYval, accZval) + PI) * RAD_TO_DEG; + return angle; + } else { + double angle = (atan2(accXval, accZval) + PI) * RAD_TO_DEG; + return angle; + } + } else + return 0; } + bool PS3USB::getStatus(Status c) { - if (readBuf == NULL) + if (readBuf == NULL) + return false; + if (readBuf[((uint16_t)c >> 8) - 9] == ((uint8_t)c & 0xff)) + return true; return false; - if (readBuf[((uint16_t)c >> 8)-9] == ((uint8_t)c & 0xff)) - return true; - return false; } + String PS3USB::getStatusString() { - if (PS3Connected || PS3NavigationConnected) { - char statusOutput[100]; + if (PS3Connected || PS3NavigationConnected) { + char statusOutput[100]; - strcpy(statusOutput,"ConnectionStatus: "); + strcpy(statusOutput, "ConnectionStatus: "); - if (getStatus(Plugged)) strcat(statusOutput,"Plugged"); - else if (getStatus(Unplugged)) strcat(statusOutput,"Unplugged"); - else strcat(statusOutput,"Error"); + if (getStatus(Plugged)) strcat(statusOutput, "Plugged"); + else if (getStatus(Unplugged)) strcat(statusOutput, "Unplugged"); + else strcat(statusOutput, "Error"); - strcat(statusOutput," - PowerRating: "); + strcat(statusOutput, " - PowerRating: "); - if (getStatus(Charging)) strcat(statusOutput,"Charging"); - else if (getStatus(NotCharging)) strcat(statusOutput,"Not Charging"); - else if (getStatus(Shutdown)) strcat(statusOutput,"Shutdown"); - else if (getStatus(Dying)) strcat(statusOutput,"Dying"); - else if (getStatus(Low)) strcat(statusOutput,"Low"); - else if (getStatus(High)) strcat(statusOutput,"High"); - else if (getStatus(Full)) strcat(statusOutput,"Full"); - else strcat(statusOutput,"Error"); + if (getStatus(Charging)) strcat(statusOutput, "Charging"); + else if (getStatus(NotCharging)) strcat(statusOutput, "Not Charging"); + else if (getStatus(Shutdown)) strcat(statusOutput, "Shutdown"); + else if (getStatus(Dying)) strcat(statusOutput, "Dying"); + else if (getStatus(Low)) strcat(statusOutput, "Low"); + else if (getStatus(High)) strcat(statusOutput, "High"); + else if (getStatus(Full)) strcat(statusOutput, "Full"); + else strcat(statusOutput, "Error"); - strcat(statusOutput," - WirelessStatus: "); + strcat(statusOutput, " - WirelessStatus: "); - if (getStatus(CableRumble)) strcat(statusOutput,"Cable - Rumble is on"); - else if (getStatus(Cable)) strcat(statusOutput,"Cable - Rumble is off"); - else if (getStatus(BluetoothRumble)) strcat(statusOutput,"Bluetooth - Rumble is on"); - else if (getStatus(Bluetooth)) strcat(statusOutput,"Bluetooth - Rumble is off"); - else strcat(statusOutput,"Error"); + if (getStatus(CableRumble)) strcat(statusOutput, "Cable - Rumble is on"); + else if (getStatus(Cable)) strcat(statusOutput, "Cable - Rumble is off"); + else if (getStatus(BluetoothRumble)) strcat(statusOutput, "Bluetooth - Rumble is on"); + else if (getStatus(Bluetooth)) strcat(statusOutput, "Bluetooth - Rumble is off"); + else strcat(statusOutput, "Error"); - return statusOutput; - } + return statusOutput; + } } /* Playstation Sixaxis Dualshock and Navigation Controller commands */ void PS3USB::PS3_Command(uint8_t* data, uint16_t nbytes) { - //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x01), Report Type (Output 0x02), interface (0x00), datalength, datalength, data) - pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x01, 0x02, 0x00, nbytes, nbytes, data, NULL); + //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x01), Report Type (Output 0x02), interface (0x00), datalength, datalength, data) + pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x01, 0x02, 0x00, nbytes, nbytes, data, NULL); } + void PS3USB::setAllOff() { - for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) - writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); // Reset buffer + for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) + writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); // Reset buffer - PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); -} -void PS3USB::setRumbleOff() { - writeBuf[1] = 0x00; - writeBuf[2] = 0x00;//low mode off - writeBuf[3] = 0x00; - writeBuf[4] = 0x00;//high mode off - - PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); -} -void PS3USB::setRumbleOn(Rumble mode) { - /* Still not totally sure how it works, maybe something like this instead? - * 3 - duration_right - * 4 - power_right - * 5 - duration_left - * 6 - power_left - */ - if ((mode & 0x30) > 0) { - writeBuf[1] = 0xfe; - writeBuf[3] = 0xfe; - if (mode == RumbleHigh) { - writeBuf[2] = 0;//low mode off - writeBuf[4] = 0xff;//high mode on - } - else { - writeBuf[2] = 0xff;//low mode on - writeBuf[4] = 0;//high mode off - } PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); - } } + +void PS3USB::setRumbleOff() { + writeBuf[1] = 0x00; + writeBuf[2] = 0x00; //low mode off + writeBuf[3] = 0x00; + writeBuf[4] = 0x00; //high mode off + + PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); +} + +void PS3USB::setRumbleOn(Rumble mode) { + /* Still not totally sure how it works, maybe something like this instead? + * 3 - duration_right + * 4 - power_right + * 5 - duration_left + * 6 - power_left + */ + if ((mode & 0x30) > 0) { + writeBuf[1] = 0xfe; + writeBuf[3] = 0xfe; + if (mode == RumbleHigh) { + writeBuf[2] = 0; //low mode off + writeBuf[4] = 0xff; //high mode on + } else { + writeBuf[2] = 0xff; //low mode on + writeBuf[4] = 0; //high mode off + } + PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); + } +} + void PS3USB::setLedOff(LED a) { - writeBuf[9] &= ~((uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1)); - PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); + writeBuf[9] &= ~((uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1)); + PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); } + void PS3USB::setLedOn(LED a) { - writeBuf[9] |= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1); - PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); + writeBuf[9] |= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1); + PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); } + void PS3USB::setLedToggle(LED a) { - writeBuf[9] ^= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1); - PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); + writeBuf[9] ^= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1); + PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE); } + void PS3USB::setBdaddr(uint8_t* BDADDR) { - /* Set the internal bluetooth address */ - uint8_t buf[8]; - buf[0] = 0x01; - buf[1] = 0x00; - for (uint8_t i = 0; i < 6; i++) - buf[i+2] = BDADDR[5 - i];//Copy into buffer, has to be written reversed + /* Set the internal bluetooth address */ + uint8_t buf[8]; + buf[0] = 0x01; + buf[1] = 0x00; + for (uint8_t i = 0; i < 6; i++) + buf[i + 2] = BDADDR[5 - i]; //Copy into buffer, has to be written reversed - //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) - pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL); + //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) + pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL); #ifdef DEBUG - Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80); - for(int8_t i = 5; i > 0; i--) { - PrintHex(my_bdaddr[i], 0x80); - Serial.print(":"); - } - PrintHex(my_bdaddr[0], 0x80); + Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80); + for (int8_t i = 5; i > 0; i--) { + PrintHex (my_bdaddr[i], 0x80); + Serial.print(":"); + } + PrintHex (my_bdaddr[0], 0x80); #endif - return; + return; } -void PS3USB::enable_sixaxis() { //Command used to enable the Dualshock 3 and Navigation controller to send data via USB - uint8_t cmd_buf[4]; - cmd_buf[0] = 0x42;// Special PS3 Controller enable commands - cmd_buf[1] = 0x0c; - cmd_buf[2] = 0x00; - cmd_buf[3] = 0x00; - //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF4), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data) - pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF4, 0x03, 0x00, 4, 4, cmd_buf, NULL); +void PS3USB::enable_sixaxis() { //Command used to enable the Dualshock 3 and Navigation controller to send data via USB + uint8_t cmd_buf[4]; + cmd_buf[0] = 0x42; // Special PS3 Controller enable commands + cmd_buf[1] = 0x0c; + cmd_buf[2] = 0x00; + cmd_buf[3] = 0x00; + + //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF4), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data) + pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0xF4, 0x03, 0x00, 4, 4, cmd_buf, NULL); } /* Playstation Move Controller commands */ void PS3USB::Move_Command(uint8_t* data, uint16_t nbytes) { - pUsb->outTransfer(bAddress, epInfo[ PS3_OUTPUT_PIPE ].epAddr, nbytes, data); + pUsb->outTransfer(bAddress, epInfo[ PS3_OUTPUT_PIPE ].epAddr, nbytes, data); } void PS3USB::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { //Use this to set the Color using RGB values - // set the Bulb's values into the write buffer - writeBuf[2] = r; - writeBuf[3] = g; - writeBuf[4] = b; + // set the Bulb's values into the write buffer + writeBuf[2] = r; + writeBuf[3] = g; + writeBuf[4] = b; - Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); + Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); } + void PS3USB::moveSetBulb(Colors color) { //Use this to set the Color using the predefined colors in "enums.h" - moveSetBulb((uint8_t)(color >> 16),(uint8_t)(color >> 8),(uint8_t)(color)); + moveSetBulb((uint8_t)(color >> 16), (uint8_t)(color >> 8), (uint8_t)(color)); } + void PS3USB::moveSetRumble(uint8_t rumble) { #ifdef DEBUG - if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100) - Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80); + if (rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100) + Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80); #endif - //set the rumble value into the write buffer - writeBuf[6] = rumble; + //set the rumble value into the write buffer + writeBuf[6] = rumble; - Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); + Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); } + void PS3USB::setMoveBdaddr(uint8_t* BDADDR) { - /* Set the internal bluetooth address */ - uint8_t buf[11]; - buf[0] = 0x05; - buf[7] = 0x10; - buf[8] = 0x01; - buf[9] = 0x02; - buf[10] = 0x12; + /* Set the internal bluetooth address */ + uint8_t buf[11]; + buf[0] = 0x05; + buf[7] = 0x10; + buf[8] = 0x01; + buf[9] = 0x02; + buf[10] = 0x12; - for (uint8_t i = 0; i < 6; i++) - buf[i + 1] = BDADDR[i]; + for (uint8_t i = 0; i < 6; i++) + buf[i + 1] = BDADDR[i]; - //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x05), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data) - pUsb->ctrlReq(bAddress,epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00,11,11, buf, NULL); + //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x05), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data) + pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x05, 0x03, 0x00, 11, 11, buf, NULL); #ifdef DEBUG - Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80); - for(int8_t i = 5; i > 0; i--) { - PrintHex(my_bdaddr[i], 0x80); - Serial.print(":"); - } - PrintHex(my_bdaddr[0], 0x80); + Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80); + for (int8_t i = 5; i > 0; i--) { + PrintHex (my_bdaddr[i], 0x80); + Serial.print(":"); + } + PrintHex (my_bdaddr[0], 0x80); #endif - return; + return; } \ No newline at end of file diff --git a/PS3USB.h b/PS3USB.h index b313b05e..adaa47e1 100644 --- a/PS3USB.h +++ b/PS3USB.h @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -56,193 +56,199 @@ /** * This class implements support for all the official PS3 Controllers: * Dualshock 3, Navigation or a Motion controller via USB. - * + * * One can only set the color of the bulb, set the rumble, set and get the bluetooth address and calibrate the magnetometer via USB on the Move controller. - * + * * Information about the protocol can be found at the wiki: https://github.com/felis/USB_Host_Shield_2.0/wiki/PS3-Information. */ class PS3USB : public USBDeviceConfig { public: - /** - * Constructor for the PS3USB class. - * @param pUsb Pointer to USB class instance. - * @param btadr5,btadr4,btadr3,btadr2,btadr1,btadr0 - * Pass your dongles Bluetooth address into the constructor, - * so you are able to pair the controller with a Bluetooth dongle. - */ - PS3USB(USB *pUsb, uint8_t btadr5=0, uint8_t btadr4=0, uint8_t btadr3=0, uint8_t btadr2=0, uint8_t btadr1=0, uint8_t btadr0=0); - - /** @name USBDeviceConfig implementation */ - /** - * Initialize the PS3 Controller. - * @param parent Hub number. - * @param port Port number on the hub. - * @param lowspeed Speed of the device. - * @return 0 on success. - */ - virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed); - /** - * Release the USB device. - * @return 0 on success. - */ - virtual uint8_t Release(); - /** - * Poll the USB Input endpoins and run the state machines. - * @return 0 on success. - */ - virtual uint8_t Poll(); - /** - * Get the device address. - * @return The device address. - */ - virtual uint8_t GetAddress() { return bAddress; }; - /** - * Used to check if the controller has been initialized. - * @return True if it's ready. - */ - virtual bool isReady() { return bPollEnable; }; - /**@}*/ + /** + * Constructor for the PS3USB class. + * @param pUsb Pointer to USB class instance. + * @param btadr5,btadr4,btadr3,btadr2,btadr1,btadr0 + * Pass your dongles Bluetooth address into the constructor, + * so you are able to pair the controller with a Bluetooth dongle. + */ + PS3USB(USB *pUsb, uint8_t btadr5 = 0, uint8_t btadr4 = 0, uint8_t btadr3 = 0, uint8_t btadr2 = 0, uint8_t btadr1 = 0, uint8_t btadr0 = 0); - /** - * Used to set the Bluetooth address inside the Dualshock 3 and Navigation controller. - * @param BDADDR Your dongles Bluetooth address. - */ - void setBdaddr(uint8_t* BDADDR); - /** - * Used to set the Bluetooth address inside the Move controller. - * @param BDADDR Your dongles Bluetooth address. - */ - void setMoveBdaddr(uint8_t* BDADDR); + /** @name USBDeviceConfig implementation */ + /** + * Initialize the PS3 Controller. + * @param parent Hub number. + * @param port Port number on the hub. + * @param lowspeed Speed of the device. + * @return 0 on success. + */ + virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed); + /** + * Release the USB device. + * @return 0 on success. + */ + virtual uint8_t Release(); + /** + * Poll the USB Input endpoins and run the state machines. + * @return 0 on success. + */ + virtual uint8_t Poll(); - /** @name PS3 Controller functions */ - /** - * getButtonPress(Button b) will return true as long as the button is held down. - * - * While getButtonClick(Button b) will only return it once. - * - * So you instance if you need to increase a variable once you would use getButtonClick(Button b), - * but if you need to drive a robot forward you would use getButtonPress(Button b). - */ - bool getButtonPress(Button b); - bool getButtonClick(Button b); - /**@}*/ - /** @name PS3 Controller functions */ - /** - * Used to get the analog value from button presses. - * @param a The ::Button to read. - * The supported buttons are: - * ::UP, ::RIGHT, ::DOWN, ::LEFT, ::L1, ::L2, ::R1, ::R2, - * ::TRIANGLE, ::CIRCLE, ::CROSS, ::SQUARE, and ::T. - * @return Analog value in the range of 0-255. - */ - uint8_t getAnalogButton(Button a); - /** - * Used to read the analog joystick. - * @param a ::LeftHatX, ::LeftHatY, ::RightHatX, and ::RightHatY. - * @return Return the analog value in the range of 0-255. - */ - uint8_t getAnalogHat(AnalogHat a); - /** - * Used to read the sensors inside the Dualshock 3 controller. - * @param a - * The Dualshock 3 has a 3-axis accelerometer and a 1-axis gyro inside. - * @return Return the raw sensor value. - */ - uint16_t getSensor(Sensor a); - /** - * Use this to get ::Pitch and ::Roll calculated using the accelerometer. - * @param a Either ::Pitch or ::Roll. - * @return Return the angle in the range of 0-360. - */ - double getAngle(Angle a); - /** - * Get the ::Status from the controller. - * @param c The ::Status you want to read. - * @return True if correct and false if not. - */ - bool getStatus(Status c); - /** - * Read all the available ::Status from the controller. - * @return One large string with all the information. - */ - String getStatusString(); + /** + * Get the device address. + * @return The device address. + */ + virtual uint8_t GetAddress() { + return bAddress; + }; - /** Used to set all LEDs and ::Rumble off. */ - void setAllOff(); - /** Turn off ::Rumble. */ - void setRumbleOff(); - /** - * Turn on ::Rumble. - * @param mode Either ::RumbleHigh or ::RumbleLow. - */ - void setRumbleOn(Rumble mode); - /** - * Turn the specific ::LED off. - * @param a The ::LED to turn off. - */ - void setLedOff(LED a); - /** - * Turn the specific ::LED on. - * @param a The ::LED to turn on. - */ - void setLedOn(LED a); - /** - * Toggle the specific ::LED. - * @param a The ::LED to toggle. - */ - void setLedToggle(LED a); - - /** - * Use this to set the Color using RGB values. - * @param r,g,b RGB value. - */ - void moveSetBulb(uint8_t r, uint8_t g, uint8_t b); - /** - * Use this to set the color using the predefined colors in ::Colors. - * @param color The desired color. - */ - void moveSetBulb(Colors color); - /** - * Set the rumble value inside the Move controller. - * @param rumble The desired value in the range from 64-255. - */ - void moveSetRumble(uint8_t rumble); - /**@}*/ + /** + * Used to check if the controller has been initialized. + * @return True if it's ready. + */ + virtual bool isReady() { + return bPollEnable; + }; + /**@}*/ - /** Variable used to indicate if the normal playstation controller is successfully connected. */ - bool PS3Connected; - /** Variable used to indicate if the move controller is successfully connected. */ - bool PS3MoveConnected; - /** Variable used to indicate if the navigation controller is successfully connected. */ - bool PS3NavigationConnected; - -protected: - /** Pointer to USB class instance. */ - USB *pUsb; - /** Device address. */ - uint8_t bAddress; - /** Endpoint info structure. */ - EpInfo epInfo[PS3_MAX_ENDPOINTS]; - -private: - bool bPollEnable; - - uint32_t timer; // used to continuously set PS3 Move controller Bulb and rumble values + /** + * Used to set the Bluetooth address inside the Dualshock 3 and Navigation controller. + * @param BDADDR Your dongles Bluetooth address. + */ + void setBdaddr(uint8_t* BDADDR); + /** + * Used to set the Bluetooth address inside the Move controller. + * @param BDADDR Your dongles Bluetooth address. + */ + void setMoveBdaddr(uint8_t* BDADDR); - uint32_t ButtonState; - uint32_t OldButtonState; - uint32_t ButtonClickState; - - uint8_t my_bdaddr[6]; // Change to your dongles Bluetooth address in the constructor - uint8_t readBuf[EP_MAXPKTSIZE]; // General purpose buffer for input data - uint8_t writeBuf[EP_MAXPKTSIZE]; // General purpose buffer for output data - - void readReport(); // read incoming data - void printReport(); // print incoming date - Uncomment for debugging + /** @name PS3 Controller functions */ + /** + * getButtonPress(Button b) will return true as long as the button is held down. + * + * While getButtonClick(Button b) will only return it once. + * + * So you instance if you need to increase a variable once you would use getButtonClick(Button b), + * but if you need to drive a robot forward you would use getButtonPress(Button b). + */ + bool getButtonPress(Button b); + bool getButtonClick(Button b); + /**@}*/ + /** @name PS3 Controller functions */ + /** + * Used to get the analog value from button presses. + * @param a The ::Button to read. + * The supported buttons are: + * ::UP, ::RIGHT, ::DOWN, ::LEFT, ::L1, ::L2, ::R1, ::R2, + * ::TRIANGLE, ::CIRCLE, ::CROSS, ::SQUARE, and ::T. + * @return Analog value in the range of 0-255. + */ + uint8_t getAnalogButton(Button a); + /** + * Used to read the analog joystick. + * @param a ::LeftHatX, ::LeftHatY, ::RightHatX, and ::RightHatY. + * @return Return the analog value in the range of 0-255. + */ + uint8_t getAnalogHat(AnalogHat a); + /** + * Used to read the sensors inside the Dualshock 3 controller. + * @param a + * The Dualshock 3 has a 3-axis accelerometer and a 1-axis gyro inside. + * @return Return the raw sensor value. + */ + uint16_t getSensor(Sensor a); + /** + * Use this to get ::Pitch and ::Roll calculated using the accelerometer. + * @param a Either ::Pitch or ::Roll. + * @return Return the angle in the range of 0-360. + */ + double getAngle(Angle a); + /** + * Get the ::Status from the controller. + * @param c The ::Status you want to read. + * @return True if correct and false if not. + */ + bool getStatus(Status c); + /** + * Read all the available ::Status from the controller. + * @return One large string with all the information. + */ + String getStatusString(); - /* Private commands */ - void PS3_Command(uint8_t* data, uint16_t nbytes); - void enable_sixaxis(); // Command used to enable the Dualshock 3 and Navigation controller to send data via USB - void Move_Command(uint8_t* data, uint16_t nbytes); + /** Used to set all LEDs and ::Rumble off. */ + void setAllOff(); + /** Turn off ::Rumble. */ + void setRumbleOff(); + /** + * Turn on ::Rumble. + * @param mode Either ::RumbleHigh or ::RumbleLow. + */ + void setRumbleOn(Rumble mode); + /** + * Turn the specific ::LED off. + * @param a The ::LED to turn off. + */ + void setLedOff(LED a); + /** + * Turn the specific ::LED on. + * @param a The ::LED to turn on. + */ + void setLedOn(LED a); + /** + * Toggle the specific ::LED. + * @param a The ::LED to toggle. + */ + void setLedToggle(LED a); + + /** + * Use this to set the Color using RGB values. + * @param r,g,b RGB value. + */ + void moveSetBulb(uint8_t r, uint8_t g, uint8_t b); + /** + * Use this to set the color using the predefined colors in ::Colors. + * @param color The desired color. + */ + void moveSetBulb(Colors color); + /** + * Set the rumble value inside the Move controller. + * @param rumble The desired value in the range from 64-255. + */ + void moveSetRumble(uint8_t rumble); + /**@}*/ + + /** Variable used to indicate if the normal playstation controller is successfully connected. */ + bool PS3Connected; + /** Variable used to indicate if the move controller is successfully connected. */ + bool PS3MoveConnected; + /** Variable used to indicate if the navigation controller is successfully connected. */ + bool PS3NavigationConnected; + +protected: + /** Pointer to USB class instance. */ + USB *pUsb; + /** Device address. */ + uint8_t bAddress; + /** Endpoint info structure. */ + EpInfo epInfo[PS3_MAX_ENDPOINTS]; + +private: + bool bPollEnable; + + uint32_t timer; // used to continuously set PS3 Move controller Bulb and rumble values + + uint32_t ButtonState; + uint32_t OldButtonState; + uint32_t ButtonClickState; + + uint8_t my_bdaddr[6]; // Change to your dongles Bluetooth address in the constructor + uint8_t readBuf[EP_MAXPKTSIZE]; // General purpose buffer for input data + uint8_t writeBuf[EP_MAXPKTSIZE]; // General purpose buffer for output data + + void readReport(); // read incoming data + void printReport(); // print incoming date - Uncomment for debugging + + /* Private commands */ + void PS3_Command(uint8_t* data, uint16_t nbytes); + void enable_sixaxis(); // Command used to enable the Dualshock 3 and Navigation controller to send data via USB + void Move_Command(uint8_t* data, uint16_t nbytes); }; #endif diff --git a/SPP.cpp b/SPP.cpp index 538d2193..8455d974 100644 --- a/SPP.cpp +++ b/SPP.cpp @@ -23,889 +23,914 @@ /* * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. */ -const uint8_t rfcomm_crc_table[256] PROGMEM = { /* reversed, 8-bit, poly=0x07 */ - 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, - 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, - 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, - 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, - 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, - 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, - 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, - 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, - 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, - 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, - 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, - 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, - 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, - 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, - 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, - 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF +const uint8_t rfcomm_crc_table[256] PROGMEM = {/* reversed, 8-bit, poly=0x07 */ + 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, + 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, + 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, + 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, + 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, + 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, + 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, + 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, + 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, + 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, + 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, + 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, + 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, + 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, + 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, + 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF }; -SPP::SPP(BTD *p, const char* name, const char* pin): +SPP::SPP(BTD *p, const char* name, const char* pin) : pBtd(p) // Pointer to BTD class instance - mandatory { - if (pBtd) - pBtd->registerServiceClass(this); // Register it as a Bluetooth service + if (pBtd) + pBtd->registerServiceClass(this); // Register it as a Bluetooth service - pBtd->btdName = name; - pBtd->btdPin = pin; + pBtd->btdName = name; + pBtd->btdPin = pin; - /* Set device cid for the SDP and RFCOMM channelse */ - sdp_dcid[0] = 0x50; // 0x0050 - sdp_dcid[1] = 0x00; - rfcomm_dcid[0] = 0x51; // 0x0051 - rfcomm_dcid[1] = 0x00; + /* Set device cid for the SDP and RFCOMM channelse */ + sdp_dcid[0] = 0x50; // 0x0050 + sdp_dcid[1] = 0x00; + rfcomm_dcid[0] = 0x51; // 0x0051 + rfcomm_dcid[1] = 0x00; - Reset(); + Reset(); } + void SPP::Reset() { - connected = false; - RFCOMMConnected = false; - SDPConnected = false; - l2cap_sdp_state = L2CAP_SDP_WAIT; - l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; - l2cap_event_flag = 0; + connected = false; + RFCOMMConnected = false; + SDPConnected = false; + l2cap_sdp_state = L2CAP_SDP_WAIT; + l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; + l2cap_event_flag = 0; } -void SPP::disconnect(){ - connected = false; - // First the two L2CAP channels has to be disconencted and then the HCI connection - if(RFCOMMConnected) - pBtd->l2cap_disconnection_request(hci_handle,0x0A, rfcomm_scid, rfcomm_dcid); - if(RFCOMMConnected && SDPConnected) - delay(1); // Add delay between commands - if(SDPConnected) - pBtd->l2cap_disconnection_request(hci_handle,0x0B, sdp_scid, sdp_dcid); - l2cap_sdp_state = L2CAP_DISCONNECT_RESPONSE; + +void SPP::disconnect() { + connected = false; + // First the two L2CAP channels has to be disconencted and then the HCI connection + if (RFCOMMConnected) + pBtd->l2cap_disconnection_request(hci_handle, 0x0A, rfcomm_scid, rfcomm_dcid); + if (RFCOMMConnected && SDPConnected) + delay(1); // Add delay between commands + if (SDPConnected) + pBtd->l2cap_disconnection_request(hci_handle, 0x0B, sdp_scid, sdp_dcid); + l2cap_sdp_state = L2CAP_DISCONNECT_RESPONSE; } + void SPP::ACLData(uint8_t* l2capinbuf) { - if(!connected) { - if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { - if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM && !pBtd->sdpConnectionClaimed) { - pBtd->sdpConnectionClaimed = true; - hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection - l2cap_sdp_state = L2CAP_SDP_WAIT; // Reset state - } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM && !pBtd->rfcommConnectionClaimed) { - pBtd->rfcommConnectionClaimed = true; - hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection - l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; // Reset state - } + if (!connected) { + if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { + if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM && !pBtd->sdpConnectionClaimed) { + pBtd->sdpConnectionClaimed = true; + hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection + l2cap_sdp_state = L2CAP_SDP_WAIT; // Reset state + } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM && !pBtd->rfcommConnectionClaimed) { + pBtd->rfcommConnectionClaimed = true; + hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection + l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; // Reset state + } + } } - } - if (((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000))) { // acl_handle_ok - if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U - if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { + if (((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000))) { // acl_handle_ok + if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U + if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { #ifdef DEBUG - Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); - PrintHex(l2capinbuf[13], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[12], 0x80); - Notify(PSTR(" Data: "), 0x80); - PrintHex(l2capinbuf[17], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[16], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[15], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[14], 0x80); + Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); + PrintHex (l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[12], 0x80); + Notify(PSTR(" Data: "), 0x80); + PrintHex (l2capinbuf[17], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[16], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[14], 0x80); #endif - } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { + } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { #ifdef EXTRADEBUG - Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); - PrintHex(l2capinbuf[13], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[12], 0x80); - Notify(PSTR(" SCID: "), 0x80); - PrintHex(l2capinbuf[15], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[14], 0x80); - Notify(PSTR(" Identifier: "), 0x80); - PrintHex(l2capinbuf[9], 0x80); + Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); + PrintHex (l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[12], 0x80); + Notify(PSTR(" SCID: "), 0x80); + PrintHex (l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[14], 0x80); + Notify(PSTR(" Identifier: "), 0x80); + PrintHex (l2capinbuf[9], 0x80); #endif - if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM) { // It doesn't matter if it receives another reqeust, since it waits for the channel to disconnect in the L2CAP_SDP_DONE state, and the l2cap_event_flag will be cleared if so - identifier = l2capinbuf[9]; - sdp_scid[0] = l2capinbuf[14]; - sdp_scid[1] = l2capinbuf[15]; - l2cap_event_flag |= L2CAP_FLAG_CONNECTION_SDP_REQUEST; - } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM) { // ----- || ----- - identifier = l2capinbuf[9]; - rfcomm_scid[0] = l2capinbuf[14]; - rfcomm_scid[1] = l2capinbuf[15]; - l2cap_event_flag |= L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST; - } - } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) { - if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success - if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { - //Serial.print("\r\nSDP Configuration Complete"); - l2cap_event_flag |= L2CAP_FLAG_CONFIG_SDP_SUCCESS; - } - else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { - //Serial.print("\r\nRFCOMM Configuration Complete"); - l2cap_event_flag |= L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS; - } - } - } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) { - if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { - //Serial.print("\r\nSDP Configuration Request"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_CONFIG_SDP_REQUEST; - } - else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { - //Serial.print("\r\nRFCOMM Configuration Request"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_CONFIG_RFCOMM_REQUEST; - } - } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { - if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { - //Notify(PSTR("\r\nDisconnect Request: SDP Channel"), 0x80); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_SDP_REQUEST; - } else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { - //Notify(PSTR("\r\nDisconnect Request: RFCOMM Channel"), 0x80); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST; - } - } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) { - if (l2capinbuf[12] == sdp_scid[0] && l2capinbuf[13] == sdp_scid[1]) { - //Serial.print("\r\nDisconnect Response: SDP Channel"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RESPONSE; - } else if (l2capinbuf[12] == rfcomm_scid[0] && l2capinbuf[13] == rfcomm_scid[1]) { - //Serial.print("\r\nDisconnect Response: RFCOMM Channel"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RESPONSE; - } - } else if (l2capinbuf[8] == L2CAP_CMD_INFORMATION_REQUEST) { + if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM) { // It doesn't matter if it receives another reqeust, since it waits for the channel to disconnect in the L2CAP_SDP_DONE state, and the l2cap_event_flag will be cleared if so + identifier = l2capinbuf[9]; + sdp_scid[0] = l2capinbuf[14]; + sdp_scid[1] = l2capinbuf[15]; + l2cap_event_flag |= L2CAP_FLAG_CONNECTION_SDP_REQUEST; + } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM) { // ----- || ----- + identifier = l2capinbuf[9]; + rfcomm_scid[0] = l2capinbuf[14]; + rfcomm_scid[1] = l2capinbuf[15]; + l2cap_event_flag |= L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST; + } + } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) { + if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success + if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { + //Serial.print("\r\nSDP Configuration Complete"); + l2cap_event_flag |= L2CAP_FLAG_CONFIG_SDP_SUCCESS; + } else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { + //Serial.print("\r\nRFCOMM Configuration Complete"); + l2cap_event_flag |= L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS; + } + } + } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) { + if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { + //Serial.print("\r\nSDP Configuration Request"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_CONFIG_SDP_REQUEST; + } else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { + //Serial.print("\r\nRFCOMM Configuration Request"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_CONFIG_RFCOMM_REQUEST; + } + } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { + if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { + //Notify(PSTR("\r\nDisconnect Request: SDP Channel"), 0x80); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_SDP_REQUEST; + } else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { + //Notify(PSTR("\r\nDisconnect Request: RFCOMM Channel"), 0x80); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST; + } + } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) { + if (l2capinbuf[12] == sdp_scid[0] && l2capinbuf[13] == sdp_scid[1]) { + //Serial.print("\r\nDisconnect Response: SDP Channel"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RESPONSE; + } else if (l2capinbuf[12] == rfcomm_scid[0] && l2capinbuf[13] == rfcomm_scid[1]) { + //Serial.print("\r\nDisconnect Response: RFCOMM Channel"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RESPONSE; + } + } else if (l2capinbuf[8] == L2CAP_CMD_INFORMATION_REQUEST) { #ifdef DEBUG - Notify(PSTR("\r\nInformation request"), 0x80); + Notify(PSTR("\r\nInformation request"), 0x80); #endif - identifier = l2capinbuf[9]; - pBtd->l2cap_information_response(hci_handle,identifier,l2capinbuf[12],l2capinbuf[13]); - } + identifier = l2capinbuf[9]; + pBtd->l2cap_information_response(hci_handle, identifier, l2capinbuf[12], l2capinbuf[13]); + } #ifdef EXTRADEBUG - else { - Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); - PrintHex(l2capinbuf[8], 0x80); - } + else { + Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); + PrintHex (l2capinbuf[8], 0x80); + } #endif - } else if (l2capinbuf[6] == sdp_dcid[0] && l2capinbuf[7] == sdp_dcid[1]) { // SDP - if(l2capinbuf[8] == SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU) { - /* - Serial.print("\r\nUUID: 0x"); - Serial.print(l2capinbuf[16],HEX); - Serial.print(" "); - Serial.print(l2capinbuf[17],HEX); - */ - if ((l2capinbuf[16] << 8 | l2capinbuf[17]) == SERIALPORT_UUID) { - if(firstMessage) { - serialPortResponse1(l2capinbuf[9],l2capinbuf[10]); - firstMessage = false; - } else { - serialPortResponse2(l2capinbuf[9],l2capinbuf[10]); // Serialport continuation state - firstMessage = true; - } - } else if ((l2capinbuf[16] << 8 | l2capinbuf[17]) == L2CAP_UUID) { - if(firstMessage) { - l2capResponse1(l2capinbuf[9],l2capinbuf[10]); - firstMessage = false; - } else { - l2capResponse2(l2capinbuf[9],l2capinbuf[10]); // L2CAP continuation state - firstMessage = true; - } - } else - serviceNotSupported(l2capinbuf[9],l2capinbuf[10]); // The service is not supported - } - } else if (l2capinbuf[6] == rfcomm_dcid[0] && l2capinbuf[7] == rfcomm_dcid[1]) { // RFCOMM - rfcommChannel = l2capinbuf[8] & 0xF8; - rfcommDirection = l2capinbuf[8] & 0x04; - rfcommCommandResponse = l2capinbuf[8] & 0x02; - rfcommChannelType = l2capinbuf[9] & 0xEF; - rfcommPfBit = l2capinbuf[9] & 0x10; + } else if (l2capinbuf[6] == sdp_dcid[0] && l2capinbuf[7] == sdp_dcid[1]) { // SDP + if (l2capinbuf[8] == SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU) { + /* + Serial.print("\r\nUUID: 0x"); + Serial.print(l2capinbuf[16],HEX); + Serial.print(" "); + Serial.print(l2capinbuf[17],HEX); + */ + if ((l2capinbuf[16] << 8 | l2capinbuf[17]) == SERIALPORT_UUID) { + if (firstMessage) { + serialPortResponse1(l2capinbuf[9], l2capinbuf[10]); + firstMessage = false; + } else { + serialPortResponse2(l2capinbuf[9], l2capinbuf[10]); // Serialport continuation state + firstMessage = true; + } + } else if ((l2capinbuf[16] << 8 | l2capinbuf[17]) == L2CAP_UUID) { + if (firstMessage) { + l2capResponse1(l2capinbuf[9], l2capinbuf[10]); + firstMessage = false; + } else { + l2capResponse2(l2capinbuf[9], l2capinbuf[10]); // L2CAP continuation state + firstMessage = true; + } + } else + serviceNotSupported(l2capinbuf[9], l2capinbuf[10]); // The service is not supported + } + } else if (l2capinbuf[6] == rfcomm_dcid[0] && l2capinbuf[7] == rfcomm_dcid[1]) { // RFCOMM + rfcommChannel = l2capinbuf[8] & 0xF8; + rfcommDirection = l2capinbuf[8] & 0x04; + rfcommCommandResponse = l2capinbuf[8] & 0x02; + rfcommChannelType = l2capinbuf[9] & 0xEF; + rfcommPfBit = l2capinbuf[9] & 0x10; - if(rfcommChannel>>3 != 0x00) - rfcommChannelConnection = rfcommChannel; + if (rfcommChannel >> 3 != 0x00) + rfcommChannelConnection = rfcommChannel; #ifdef EXTRADEBUG - Notify(PSTR("\r\nRFCOMM Channel: "), 0x80); - Serial.print(rfcommChannel>>3,HEX); - Notify(PSTR(" Direction: "), 0x80); - Serial.print(rfcommDirection>>2,HEX); - Notify(PSTR(" CommandResponse: "), 0x80); - Serial.print(rfcommCommandResponse>>1,HEX); - Notify(PSTR(" ChannelType: "), 0x80); - Serial.print(rfcommChannelType,HEX); - Notify(PSTR(" PF_BIT: "), 0x80); - Serial.print(rfcommPfBit,HEX); + Notify(PSTR("\r\nRFCOMM Channel: "), 0x80); + Serial.print(rfcommChannel >> 3, HEX); + Notify(PSTR(" Direction: "), 0x80); + Serial.print(rfcommDirection >> 2, HEX); + Notify(PSTR(" CommandResponse: "), 0x80); + Serial.print(rfcommCommandResponse >> 1, HEX); + Notify(PSTR(" ChannelType: "), 0x80); + Serial.print(rfcommChannelType, HEX); + Notify(PSTR(" PF_BIT: "), 0x80); + Serial.print(rfcommPfBit, HEX); #endif - if (rfcommChannelType == RFCOMM_DISC) { + if (rfcommChannelType == RFCOMM_DISC) { #ifdef DEBUG - Notify(PSTR("\r\nReceived Disconnect RFCOMM Command on channel: "), 0x80); - Serial.print(rfcommChannel>>3,HEX); + Notify(PSTR("\r\nReceived Disconnect RFCOMM Command on channel: "), 0x80); + Serial.print(rfcommChannel >> 3, HEX); #endif - connected = false; - sendRfcomm(rfcommChannel,rfcommDirection,rfcommCommandResponse,RFCOMM_UA,rfcommPfBit,rfcommbuf,0x00); // UA Command - } - if(connected) { - /* Read the incoming message */ - if(rfcommChannelType == RFCOMM_UIH && rfcommChannel == rfcommChannelConnection) { - uint8_t length = l2capinbuf[10] >> 1; // Get length - uint8_t offset = l2capinbuf[4]-length-4; // See if there is credit - if(rfcommAvailable + length <= sizeof(rfcommDataBuffer)) { // Don't add data to buffer if it would be full - for(uint8_t i = 0; i < length; i++) - rfcommDataBuffer[rfcommAvailable+i] = l2capinbuf[11+i+offset]; - rfcommAvailable += length; - } + connected = false; + sendRfcomm(rfcommChannel, rfcommDirection, rfcommCommandResponse, RFCOMM_UA, rfcommPfBit, rfcommbuf, 0x00); // UA Command + } + if (connected) { + /* Read the incoming message */ + if (rfcommChannelType == RFCOMM_UIH && rfcommChannel == rfcommChannelConnection) { + uint8_t length = l2capinbuf[10] >> 1; // Get length + uint8_t offset = l2capinbuf[4] - length - 4; // See if there is credit + if (rfcommAvailable + length <= sizeof (rfcommDataBuffer)) { // Don't add data to buffer if it would be full + for (uint8_t i = 0; i < length; i++) + rfcommDataBuffer[rfcommAvailable + i] = l2capinbuf[11 + i + offset]; + rfcommAvailable += length; + } #ifdef EXTRADEBUG - Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80); - Serial.print(rfcommAvailable); - if (offset) { - Notify(PSTR(" - Credit: 0x"), 0x80); - Serial.print(l2capinbuf[11],HEX); - } + Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80); + Serial.print(rfcommAvailable); + if (offset) { + Notify(PSTR(" - Credit: 0x"), 0x80); + Serial.print(l2capinbuf[11], HEX); + } #endif #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send to the Arduino via Bluetooth - for(uint8_t i = 0; i < length; i++) - Serial.write(l2capinbuf[i+11+offset]); + for (uint8_t i = 0; i < length; i++) + Serial.write(l2capinbuf[i + 11 + offset]); #endif - } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command + } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command #ifdef DEBUG - Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80); + Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80); #endif - rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command - rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 - rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1 - rfcommbuf[3] = l2capinbuf[14]; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM - rfcommbuf[4] = l2capinbuf[15]; // Priority - rfcommbuf[5] = l2capinbuf[16]; // Timer - rfcommbuf[6] = l2capinbuf[17]; // Max Fram Size LSB - rfcommbuf[7] = l2capinbuf[18]; // Max Fram Size MSB - rfcommbuf[8] = l2capinbuf[19]; // MaxRatransm. - rfcommbuf[9] = l2capinbuf[20]; // Number of Frames - sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); // UIH Remote Port Negotiation Response - } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command + rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command + rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1 + rfcommbuf[3] = l2capinbuf[14]; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM + rfcommbuf[4] = l2capinbuf[15]; // Priority + rfcommbuf[5] = l2capinbuf[16]; // Timer + rfcommbuf[6] = l2capinbuf[17]; // Max Fram Size LSB + rfcommbuf[7] = l2capinbuf[18]; // Max Fram Size MSB + rfcommbuf[8] = l2capinbuf[19]; // MaxRatransm. + rfcommbuf[9] = l2capinbuf[20]; // Number of Frames + sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x0A); // UIH Remote Port Negotiation Response + } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command #ifdef DEBUG - Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80); + Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80); #endif - rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response - rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 - rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) - rfcommbuf[3] = l2capinbuf[14]; - sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x04); - } - } else { - if(rfcommChannelType == RFCOMM_SABM) { // SABM Command - this is sent twice: once for channel 0 and then for the channel to establish + rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response + rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) + rfcommbuf[3] = l2capinbuf[14]; + sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x04); + } + } else { + if (rfcommChannelType == RFCOMM_SABM) { // SABM Command - this is sent twice: once for channel 0 and then for the channel to establish #ifdef DEBUG - Notify(PSTR("\r\nReceived SABM Command"), 0x80); + Notify(PSTR("\r\nReceived SABM Command"), 0x80); #endif - sendRfcomm(rfcommChannel,rfcommDirection,rfcommCommandResponse,RFCOMM_UA,rfcommPfBit,rfcommbuf,0x00); // UA Command - } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_PN_CMD) { // UIH Parameter Negotiation Command + sendRfcomm(rfcommChannel, rfcommDirection, rfcommCommandResponse, RFCOMM_UA, rfcommPfBit, rfcommbuf, 0x00); // UA Command + } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_PN_CMD) { // UIH Parameter Negotiation Command #ifdef DEBUG - Notify(PSTR("\r\nReceived UIH Parameter Negotiation Command"), 0x80); + Notify(PSTR("\r\nReceived UIH Parameter Negotiation Command"), 0x80); #endif - rfcommbuf[0] = BT_RFCOMM_PN_RSP; // UIH Parameter Negotiation Response - rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 - rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1 - rfcommbuf[3] = 0xE0; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM - rfcommbuf[4] = 0x00; // Priority - rfcommbuf[5] = 0x00; // Timer - rfcommbuf[6] = BULK_MAXPKTSIZE-14; // Max Fram Size LSB - set to the size of received data (50) - rfcommbuf[7] = 0x00; // Max Fram Size MSB - rfcommbuf[8] = 0x00; // MaxRatransm. - rfcommbuf[9] = 0x00; // Number of Frames - sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); - } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command + rfcommbuf[0] = BT_RFCOMM_PN_RSP; // UIH Parameter Negotiation Response + rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1 + rfcommbuf[3] = 0xE0; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM + rfcommbuf[4] = 0x00; // Priority + rfcommbuf[5] = 0x00; // Timer + rfcommbuf[6] = BULK_MAXPKTSIZE - 14; // Max Fram Size LSB - set to the size of received data (50) + rfcommbuf[7] = 0x00; // Max Fram Size MSB + rfcommbuf[8] = 0x00; // MaxRatransm. + rfcommbuf[9] = 0x00; // Number of Frames + sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x0A); + } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command #ifdef DEBUG - Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80); + Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80); #endif - rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response - rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 - rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) - rfcommbuf[3] = l2capinbuf[14]; - sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x04); + rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response + rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) + rfcommbuf[3] = l2capinbuf[14]; + sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x04); - delay(1); + delay(1); #ifdef DEBUG - Notify(PSTR("\r\nSend UIH Modem Status Command"), 0x80); + Notify(PSTR("\r\nSend UIH Modem Status Command"), 0x80); #endif - rfcommbuf[0] = BT_RFCOMM_MSC_CMD; // UIH Modem Status Command - rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 - rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) - rfcommbuf[3] = 0x8D; // Can receive frames (YES), Ready to Communicate (YES), Ready to Receive (YES), Incomig Call (NO), Data is Value (YES) + rfcommbuf[0] = BT_RFCOMM_MSC_CMD; // UIH Modem Status Command + rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) + rfcommbuf[3] = 0x8D; // Can receive frames (YES), Ready to Communicate (YES), Ready to Receive (YES), Incomig Call (NO), Data is Value (YES) - sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x04); - } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_RSP) { // UIH Modem Status Response - if(!creditSent) { + sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x04); + } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_RSP) { // UIH Modem Status Response + if (!creditSent) { #ifdef DEBUG - Notify(PSTR("\r\nSend UIH Command with credit"), 0x80); + Notify(PSTR("\r\nSend UIH Command with credit"), 0x80); #endif - sendRfcommCredit(rfcommChannelConnection,rfcommDirection,0,RFCOMM_UIH,0x10,sizeof(rfcommDataBuffer)); // Send credit - creditSent = true; - timer = millis(); - waitForLastCommand = true; - } - } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[10] == 0x01) { // UIH Command with credit + sendRfcommCredit(rfcommChannelConnection, rfcommDirection, 0, RFCOMM_UIH, 0x10, sizeof (rfcommDataBuffer)); // Send credit + creditSent = true; + timer = millis(); + waitForLastCommand = true; + } + } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[10] == 0x01) { // UIH Command with credit #ifdef DEBUG - Notify(PSTR("\r\nReceived UIH Command with credit"), 0x80); + Notify(PSTR("\r\nReceived UIH Command with credit"), 0x80); #endif - } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command + } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command #ifdef DEBUG - Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80); + Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80); #endif - rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command - rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 - rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1 - rfcommbuf[3] = l2capinbuf[14]; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM - rfcommbuf[4] = l2capinbuf[15]; // Priority - rfcommbuf[5] = l2capinbuf[16]; // Timer - rfcommbuf[6] = l2capinbuf[17]; // Max Fram Size LSB - rfcommbuf[7] = l2capinbuf[18]; // Max Fram Size MSB - rfcommbuf[8] = l2capinbuf[19]; // MaxRatransm. - rfcommbuf[9] = l2capinbuf[20]; // Number of Frames - sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); // UIH Remote Port Negotiation Response + rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command + rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1 + rfcommbuf[3] = l2capinbuf[14]; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM + rfcommbuf[4] = l2capinbuf[15]; // Priority + rfcommbuf[5] = l2capinbuf[16]; // Timer + rfcommbuf[6] = l2capinbuf[17]; // Max Fram Size LSB + rfcommbuf[7] = l2capinbuf[18]; // Max Fram Size MSB + rfcommbuf[8] = l2capinbuf[19]; // MaxRatransm. + rfcommbuf[9] = l2capinbuf[20]; // Number of Frames + sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x0A); // UIH Remote Port Negotiation Response #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Connection is now established\r\n"), 0x80); + Notify(PSTR("\r\nRFCOMM Connection is now established\r\n"), 0x80); #endif - waitForLastCommand = false; - creditSent = false; - connected = true; // The RFCOMM channel is now established + waitForLastCommand = false; + creditSent = false; + connected = true; // The RFCOMM channel is now established + } +#ifdef DEBUG + else if (rfcommChannelType != RFCOMM_DISC) { + Notify(PSTR("\r\nUnsupported RFCOMM Data - ChannelType: "), 0x80); + PrintHex (rfcommChannelType, 0x80); + Notify(PSTR(" Command: "), 0x80); + PrintHex (l2capinbuf[11], 0x80); + } +#endif + } } -#ifdef DEBUG - else if(rfcommChannelType != RFCOMM_DISC) { - Notify(PSTR("\r\nUnsupported RFCOMM Data - ChannelType: "), 0x80); - PrintHex(rfcommChannelType, 0x80); - Notify(PSTR(" Command: "), 0x80); - PrintHex(l2capinbuf[11], 0x80); - } -#endif - } - } #ifdef EXTRADEBUG - else { - Notify(PSTR("\r\nUnsupported L2CAP Data - Channel ID: "), 0x80); - PrintHex(l2capinbuf[7], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[6], 0x80); + else { + Notify(PSTR("\r\nUnsupported L2CAP Data - Channel ID: "), 0x80); + PrintHex (l2capinbuf[7], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[6], 0x80); + } +#endif + SDP_task(); + RFCOMM_task(); } -#endif - SDP_task(); - RFCOMM_task(); - } } + void SPP::Run() { - if(waitForLastCommand && (millis() - timer) > 100) { // We will only wait 100ms and see if the UIH Remote Port Negotiation Command is send, as some deviced don't send it + if (waitForLastCommand && (millis() - timer) > 100) { // We will only wait 100ms and see if the UIH Remote Port Negotiation Command is send, as some deviced don't send it #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n"), 0x80); + Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n"), 0x80); #endif - creditSent = false; - waitForLastCommand = false; - connected = true; // The RFCOMM channel is now established - } + creditSent = false; + waitForLastCommand = false; + connected = true; // The RFCOMM channel is now established + } } + void SPP::SDP_task() { - switch (l2cap_sdp_state) - { - case L2CAP_SDP_WAIT: - if (l2cap_connection_request_sdp_flag) { - l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_SDP_REQUEST; // Clear flag + switch (l2cap_sdp_state) { + case L2CAP_SDP_WAIT: + if (l2cap_connection_request_sdp_flag) { + l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_SDP_REQUEST; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nSDP Incoming Connection Request"), 0x80); + Notify(PSTR("\r\nSDP Incoming Connection Request"), 0x80); #endif - pBtd->l2cap_connection_response(hci_handle,identifier, sdp_dcid, sdp_scid, PENDING); - delay(1); - pBtd->l2cap_connection_response(hci_handle,identifier, sdp_dcid, sdp_scid, SUCCESSFUL); - identifier++; - delay(1); - pBtd->l2cap_config_request(hci_handle,identifier, sdp_scid); - l2cap_sdp_state = L2CAP_SDP_REQUEST; - } - break; - case L2CAP_SDP_REQUEST: - if (l2cap_config_request_sdp_flag) { - l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_REQUEST; // Clear flag + pBtd->l2cap_connection_response(hci_handle, identifier, sdp_dcid, sdp_scid, PENDING); + delay(1); + pBtd->l2cap_connection_response(hci_handle, identifier, sdp_dcid, sdp_scid, SUCCESSFUL); + identifier++; + delay(1); + pBtd->l2cap_config_request(hci_handle, identifier, sdp_scid); + l2cap_sdp_state = L2CAP_SDP_REQUEST; + } + break; + case L2CAP_SDP_REQUEST: + if (l2cap_config_request_sdp_flag) { + l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_REQUEST; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nSDP Configuration Request"), 0x80); + Notify(PSTR("\r\nSDP Configuration Request"), 0x80); #endif - pBtd->l2cap_config_response(hci_handle,identifier, sdp_scid); - l2cap_sdp_state = L2CAP_SDP_SUCCESS; - } - break; - case L2CAP_SDP_SUCCESS: - if (l2cap_config_success_sdp_flag) { - l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_SUCCESS; // Clear flag + pBtd->l2cap_config_response(hci_handle, identifier, sdp_scid); + l2cap_sdp_state = L2CAP_SDP_SUCCESS; + } + break; + case L2CAP_SDP_SUCCESS: + if (l2cap_config_success_sdp_flag) { + l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_SUCCESS; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nSDP Successfully Configured"), 0x80); + Notify(PSTR("\r\nSDP Successfully Configured"), 0x80); #endif - firstMessage = true; // Reset bool - SDPConnected = true; - l2cap_sdp_state = L2CAP_SDP_DONE; - } - break; - case L2CAP_SDP_DONE: - if(l2cap_disconnect_request_sdp_flag) { - l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_SDP_REQUEST; // Clear flag - SDPConnected = false; + firstMessage = true; // Reset bool + SDPConnected = true; + l2cap_sdp_state = L2CAP_SDP_DONE; + } + break; + case L2CAP_SDP_DONE: + if (l2cap_disconnect_request_sdp_flag) { + l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_SDP_REQUEST; // Clear flag + SDPConnected = false; #ifdef DEBUG - Notify(PSTR("\r\nDisconnected SDP Channel"), 0x80); + Notify(PSTR("\r\nDisconnected SDP Channel"), 0x80); #endif - pBtd->l2cap_disconnection_response(hci_handle,identifier,sdp_dcid,sdp_scid); - l2cap_sdp_state = L2CAP_SDP_WAIT; - } else if(l2cap_connection_request_sdp_flag) - l2cap_rfcomm_state = L2CAP_SDP_WAIT; - break; - case L2CAP_DISCONNECT_RESPONSE: // This is for both disconnection response from the RFCOMM and SDP channel if they were connected - if (l2cap_disconnect_response_flag) { + pBtd->l2cap_disconnection_response(hci_handle, identifier, sdp_dcid, sdp_scid); + l2cap_sdp_state = L2CAP_SDP_WAIT; + } else if (l2cap_connection_request_sdp_flag) + l2cap_rfcomm_state = L2CAP_SDP_WAIT; + break; + case L2CAP_DISCONNECT_RESPONSE: // This is for both disconnection response from the RFCOMM and SDP channel if they were connected + if (l2cap_disconnect_response_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected L2CAP Connection"), 0x80); + Notify(PSTR("\r\nDisconnected L2CAP Connection"), 0x80); #endif - RFCOMMConnected = false; - SDPConnected = false; - pBtd->hci_disconnect(hci_handle); - hci_handle = -1; // Reset handle - l2cap_event_flag = 0; // Reset flags - l2cap_sdp_state = L2CAP_SDP_WAIT; - l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; - } - break; - } + RFCOMMConnected = false; + SDPConnected = false; + pBtd->hci_disconnect(hci_handle); + hci_handle = -1; // Reset handle + l2cap_event_flag = 0; // Reset flags + l2cap_sdp_state = L2CAP_SDP_WAIT; + l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; + } + break; + } } -void SPP::RFCOMM_task() -{ - switch (l2cap_rfcomm_state) - { - case L2CAP_RFCOMM_WAIT: - if(l2cap_connection_request_rfcomm_flag) { - l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST; // Clear flag + +void SPP::RFCOMM_task() { + switch (l2cap_rfcomm_state) { + case L2CAP_RFCOMM_WAIT: + if (l2cap_connection_request_rfcomm_flag) { + l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Incoming Connection Request"), 0x80); + Notify(PSTR("\r\nRFCOMM Incoming Connection Request"), 0x80); #endif - pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, PENDING); - delay(1); - pBtd->l2cap_connection_response(hci_handle,identifier, rfcomm_dcid, rfcomm_scid, SUCCESSFUL); - identifier++; - delay(1); - pBtd->l2cap_config_request(hci_handle,identifier, rfcomm_scid); - l2cap_rfcomm_state = L2CAP_RFCOMM_REQUEST; - } - break; - case L2CAP_RFCOMM_REQUEST: - if (l2cap_config_request_rfcomm_flag) { - l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_REQUEST; // Clear flag + pBtd->l2cap_connection_response(hci_handle, identifier, rfcomm_dcid, rfcomm_scid, PENDING); + delay(1); + pBtd->l2cap_connection_response(hci_handle, identifier, rfcomm_dcid, rfcomm_scid, SUCCESSFUL); + identifier++; + delay(1); + pBtd->l2cap_config_request(hci_handle, identifier, rfcomm_scid); + l2cap_rfcomm_state = L2CAP_RFCOMM_REQUEST; + } + break; + case L2CAP_RFCOMM_REQUEST: + if (l2cap_config_request_rfcomm_flag) { + l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_REQUEST; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Configuration Request"), 0x80); + Notify(PSTR("\r\nRFCOMM Configuration Request"), 0x80); #endif - pBtd->l2cap_config_response(hci_handle,identifier, rfcomm_scid); - l2cap_rfcomm_state = L2CAP_RFCOMM_SUCCESS; - } - break; - case L2CAP_RFCOMM_SUCCESS: - if (l2cap_config_success_rfcomm_flag) { - l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS; // Clear flag + pBtd->l2cap_config_response(hci_handle, identifier, rfcomm_scid); + l2cap_rfcomm_state = L2CAP_RFCOMM_SUCCESS; + } + break; + case L2CAP_RFCOMM_SUCCESS: + if (l2cap_config_success_rfcomm_flag) { + l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS; // Clear flag #ifdef DEBUG - Notify(PSTR("\r\nRFCOMM Successfully Configured"), 0x80); + Notify(PSTR("\r\nRFCOMM Successfully Configured"), 0x80); #endif - rfcommAvailable = 0; // Reset number of bytes available - bytesRead = 0; // Reset number of bytes received - RFCOMMConnected = true; - l2cap_rfcomm_state = L2CAP_RFCOMM_DONE; - } - break; - case L2CAP_RFCOMM_DONE: - if(l2cap_disconnect_request_rfcomm_flag) { - l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST; // Clear flag - RFCOMMConnected = false; - connected = false; + rfcommAvailable = 0; // Reset number of bytes available + bytesRead = 0; // Reset number of bytes received + RFCOMMConnected = true; + l2cap_rfcomm_state = L2CAP_RFCOMM_DONE; + } + break; + case L2CAP_RFCOMM_DONE: + if (l2cap_disconnect_request_rfcomm_flag) { + l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST; // Clear flag + RFCOMMConnected = false; + connected = false; #ifdef DEBUG - Notify(PSTR("\r\nDisconnected RFCOMM Channel"), 0x80); + Notify(PSTR("\r\nDisconnected RFCOMM Channel"), 0x80); #endif - pBtd->l2cap_disconnection_response(hci_handle,identifier,rfcomm_dcid,rfcomm_scid); - l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; - } else if(l2cap_connection_request_rfcomm_flag) - l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; - break; - } + pBtd->l2cap_disconnection_response(hci_handle, identifier, rfcomm_dcid, rfcomm_scid); + l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; + } else if (l2cap_connection_request_rfcomm_flag) + l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; + break; + } } /************************************************************/ /* SDP Commands */ + /************************************************************/ void SPP::SDP_Command(uint8_t* data, uint8_t nbytes) { // See page 223 in the Bluetooth specs - pBtd->L2CAP_Command(hci_handle,data,nbytes,sdp_scid[0],sdp_scid[1]); + pBtd->L2CAP_Command(hci_handle, data, nbytes, sdp_scid[0], sdp_scid[1]); } + void SPP::serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLow) { // See page 235 in the Bluetooth specs - l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; - l2capoutbuf[1] = transactionIDHigh; - l2capoutbuf[2] = transactionIDLow; - l2capoutbuf[3] = 0x00; // Parameter Length - l2capoutbuf[4] = 0x05; // Parameter Length - l2capoutbuf[5] = 0x00; // AttributeListsByteCount - l2capoutbuf[6] = 0x02; // AttributeListsByteCount + l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; + l2capoutbuf[1] = transactionIDHigh; + l2capoutbuf[2] = transactionIDLow; + l2capoutbuf[3] = 0x00; // Parameter Length + l2capoutbuf[4] = 0x05; // Parameter Length + l2capoutbuf[5] = 0x00; // AttributeListsByteCount + l2capoutbuf[6] = 0x02; // AttributeListsByteCount - /* Attribute ID/Value Sequence: */ - l2capoutbuf[7] = 0x35; - l2capoutbuf[8] = 0x00; - l2capoutbuf[9] = 0x00; + /* Attribute ID/Value Sequence: */ + l2capoutbuf[7] = 0x35; + l2capoutbuf[8] = 0x00; + l2capoutbuf[9] = 0x00; - SDP_Command(l2capoutbuf,10); + SDP_Command(l2capoutbuf, 10); } + void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) { - l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; - l2capoutbuf[1] = transactionIDHigh; - l2capoutbuf[2] = transactionIDLow; - l2capoutbuf[3] = 0x00; // Parameter Length - l2capoutbuf[4] = 0x2B; // Parameter Length - l2capoutbuf[5] = 0x00; // AttributeListsByteCount - l2capoutbuf[6] = 0x26; // AttributeListsByteCount + l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; + l2capoutbuf[1] = transactionIDHigh; + l2capoutbuf[2] = transactionIDLow; + l2capoutbuf[3] = 0x00; // Parameter Length + l2capoutbuf[4] = 0x2B; // Parameter Length + l2capoutbuf[5] = 0x00; // AttributeListsByteCount + l2capoutbuf[6] = 0x26; // AttributeListsByteCount - /* Attribute ID/Value Sequence: */ - l2capoutbuf[7] = 0x36; - l2capoutbuf[8] = 0x00; - l2capoutbuf[9] = 0x3C; - l2capoutbuf[10] = 0x36; - l2capoutbuf[11] = 0x00; + /* Attribute ID/Value Sequence: */ + l2capoutbuf[7] = 0x36; + l2capoutbuf[8] = 0x00; + l2capoutbuf[9] = 0x3C; + l2capoutbuf[10] = 0x36; + l2capoutbuf[11] = 0x00; - l2capoutbuf[12] = 0x39; - l2capoutbuf[13] = 0x09; - l2capoutbuf[14] = 0x00; - l2capoutbuf[15] = 0x00; - l2capoutbuf[16] = 0x0A; - l2capoutbuf[17] = 0x00; - l2capoutbuf[18] = 0x01; - l2capoutbuf[19] = 0x00; - l2capoutbuf[20] = 0x06; - l2capoutbuf[21] = 0x09; - l2capoutbuf[22] = 0x00; - l2capoutbuf[23] = 0x01; - l2capoutbuf[24] = 0x35; - l2capoutbuf[25] = 0x03; - l2capoutbuf[26] = 0x19; - l2capoutbuf[27] = 0x11; + l2capoutbuf[12] = 0x39; + l2capoutbuf[13] = 0x09; + l2capoutbuf[14] = 0x00; + l2capoutbuf[15] = 0x00; + l2capoutbuf[16] = 0x0A; + l2capoutbuf[17] = 0x00; + l2capoutbuf[18] = 0x01; + l2capoutbuf[19] = 0x00; + l2capoutbuf[20] = 0x06; + l2capoutbuf[21] = 0x09; + l2capoutbuf[22] = 0x00; + l2capoutbuf[23] = 0x01; + l2capoutbuf[24] = 0x35; + l2capoutbuf[25] = 0x03; + l2capoutbuf[26] = 0x19; + l2capoutbuf[27] = 0x11; - l2capoutbuf[28] = 0x01; - l2capoutbuf[29] = 0x09; - l2capoutbuf[30] = 0x00; - l2capoutbuf[31] = 0x04; - l2capoutbuf[32] = 0x35; - l2capoutbuf[33] = 0x0C; - l2capoutbuf[34] = 0x35; - l2capoutbuf[35] = 0x03; - l2capoutbuf[36] = 0x19; - l2capoutbuf[37] = 0x01; - l2capoutbuf[38] = 0x00; - l2capoutbuf[39] = 0x35; - l2capoutbuf[40] = 0x05; - l2capoutbuf[41] = 0x19; - l2capoutbuf[42] = 0x00; - l2capoutbuf[43] = 0x03; + l2capoutbuf[28] = 0x01; + l2capoutbuf[29] = 0x09; + l2capoutbuf[30] = 0x00; + l2capoutbuf[31] = 0x04; + l2capoutbuf[32] = 0x35; + l2capoutbuf[33] = 0x0C; + l2capoutbuf[34] = 0x35; + l2capoutbuf[35] = 0x03; + l2capoutbuf[36] = 0x19; + l2capoutbuf[37] = 0x01; + l2capoutbuf[38] = 0x00; + l2capoutbuf[39] = 0x35; + l2capoutbuf[40] = 0x05; + l2capoutbuf[41] = 0x19; + l2capoutbuf[42] = 0x00; + l2capoutbuf[43] = 0x03; - l2capoutbuf[44] = 0x08; - l2capoutbuf[45] = 0x02; // Two extra bytes - l2capoutbuf[46] = 0x00; // 25 (0x19) more bytes to come - l2capoutbuf[47] = 0x19; + l2capoutbuf[44] = 0x08; + l2capoutbuf[45] = 0x02; // Two extra bytes + l2capoutbuf[46] = 0x00; // 25 (0x19) more bytes to come + l2capoutbuf[47] = 0x19; - SDP_Command(l2capoutbuf,48); + SDP_Command(l2capoutbuf, 48); } + void SPP::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) { - l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; - l2capoutbuf[1] = transactionIDHigh; - l2capoutbuf[2] = transactionIDLow; - l2capoutbuf[3] = 0x00; // Parameter Length - l2capoutbuf[4] = 0x1C; // Parameter Length - l2capoutbuf[5] = 0x00; // AttributeListsByteCount - l2capoutbuf[6] = 0x19; // AttributeListsByteCount + l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; + l2capoutbuf[1] = transactionIDHigh; + l2capoutbuf[2] = transactionIDLow; + l2capoutbuf[3] = 0x00; // Parameter Length + l2capoutbuf[4] = 0x1C; // Parameter Length + l2capoutbuf[5] = 0x00; // AttributeListsByteCount + l2capoutbuf[6] = 0x19; // AttributeListsByteCount - /* Attribute ID/Value Sequence: */ - l2capoutbuf[7] = 0x01; - l2capoutbuf[8] = 0x09; - l2capoutbuf[9] = 0x00; - l2capoutbuf[10] = 0x06; - l2capoutbuf[11] = 0x35; + /* Attribute ID/Value Sequence: */ + l2capoutbuf[7] = 0x01; + l2capoutbuf[8] = 0x09; + l2capoutbuf[9] = 0x00; + l2capoutbuf[10] = 0x06; + l2capoutbuf[11] = 0x35; - l2capoutbuf[12] = 0x09; - l2capoutbuf[13] = 0x09; - l2capoutbuf[14] = 0x65; - l2capoutbuf[15] = 0x6E; - l2capoutbuf[16] = 0x09; - l2capoutbuf[17] = 0x00; - l2capoutbuf[18] = 0x6A; - l2capoutbuf[19] = 0x09; - l2capoutbuf[20] = 0x01; - l2capoutbuf[21] = 0x00; - l2capoutbuf[22] = 0x09; - l2capoutbuf[23] = 0x01; - l2capoutbuf[24] = 0x00; - l2capoutbuf[25] = 0x25; + l2capoutbuf[12] = 0x09; + l2capoutbuf[13] = 0x09; + l2capoutbuf[14] = 0x65; + l2capoutbuf[15] = 0x6E; + l2capoutbuf[16] = 0x09; + l2capoutbuf[17] = 0x00; + l2capoutbuf[18] = 0x6A; + l2capoutbuf[19] = 0x09; + l2capoutbuf[20] = 0x01; + l2capoutbuf[21] = 0x00; + l2capoutbuf[22] = 0x09; + l2capoutbuf[23] = 0x01; + l2capoutbuf[24] = 0x00; + l2capoutbuf[25] = 0x25; - l2capoutbuf[26] = 0x05; // Name length - l2capoutbuf[27] = 'T'; - l2capoutbuf[28] = 'K'; - l2capoutbuf[29] = 'J'; - l2capoutbuf[30] = 'S'; - l2capoutbuf[31] = 'P'; - l2capoutbuf[32] = 0x00; // No more data + l2capoutbuf[26] = 0x05; // Name length + l2capoutbuf[27] = 'T'; + l2capoutbuf[28] = 'K'; + l2capoutbuf[29] = 'J'; + l2capoutbuf[30] = 'S'; + l2capoutbuf[31] = 'P'; + l2capoutbuf[32] = 0x00; // No more data - SDP_Command(l2capoutbuf,33); + SDP_Command(l2capoutbuf, 33); } + void SPP::l2capResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) { - serialPortResponse1(transactionIDHigh,transactionIDLow); // These has to send all the supported functions, since it only supports virtual serialport it just sends the message again + serialPortResponse1(transactionIDHigh, transactionIDLow); // These has to send all the supported functions, since it only supports virtual serialport it just sends the message again } + void SPP::l2capResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) { - serialPortResponse2(transactionIDHigh,transactionIDLow); // Same data as serialPortResponse2 + serialPortResponse2(transactionIDHigh, transactionIDLow); // Same data as serialPortResponse2 } /************************************************************/ /* RFCOMM Commands */ + /************************************************************/ void SPP::RFCOMM_Command(uint8_t* data, uint8_t nbytes) { - pBtd->L2CAP_Command(hci_handle,data,nbytes,rfcomm_scid[0],rfcomm_scid[1]); + pBtd->L2CAP_Command(hci_handle, data, nbytes, rfcomm_scid[0], rfcomm_scid[1]); } void SPP::sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t* data, uint8_t length) { - l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address - l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control - l2capoutbuf[2] = length << 1 | 0x01; // Length and format (allways 0x01 bytes format) - uint8_t i = 0; - for(; i < length; i++) - l2capoutbuf[i+3] = data[i]; - l2capoutbuf[i+3] = calcFcs(l2capoutbuf); + l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address + l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control + l2capoutbuf[2] = length << 1 | 0x01; // Length and format (allways 0x01 bytes format) + uint8_t i = 0; + for (; i < length; i++) + l2capoutbuf[i + 3] = data[i]; + l2capoutbuf[i + 3] = calcFcs(l2capoutbuf); #ifdef EXTRADEBUG - Notify(PSTR(" - RFCOMM Data: "), 0x80); - for(i = 0; i < length+4; i++) { - Serial.print(l2capoutbuf[i],HEX); - Notify(PSTR(" "), 0x80); - } + Notify(PSTR(" - RFCOMM Data: "), 0x80); + for (i = 0; i < length + 4; i++) { + Serial.print(l2capoutbuf[i], HEX); + Notify(PSTR(" "), 0x80); + } #endif - RFCOMM_Command(l2capoutbuf,length+4); + RFCOMM_Command(l2capoutbuf, length + 4); } void SPP::sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t credit) { - l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address - l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control - l2capoutbuf[2] = 0x01; // Length = 0 - l2capoutbuf[3] = credit; // Credit - l2capoutbuf[4] = calcFcs(l2capoutbuf); + l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address + l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control + l2capoutbuf[2] = 0x01; // Length = 0 + l2capoutbuf[3] = credit; // Credit + l2capoutbuf[4] = calcFcs(l2capoutbuf); #ifdef EXTRADEBUG - Notify(PSTR(" - RFCOMM Credit Data: "), 0x80); - for(uint8_t i = 0; i < 5; i++) { - Serial.print(l2capoutbuf[i],HEX); - Notify(PSTR(" "), 0x80); - } + Notify(PSTR(" - RFCOMM Credit Data: "), 0x80); + for (uint8_t i = 0; i < 5; i++) { + Serial.print(l2capoutbuf[i], HEX); + Notify(PSTR(" "), 0x80); + } #endif - RFCOMM_Command(l2capoutbuf,5); + RFCOMM_Command(l2capoutbuf, 5); } /* CRC on 2 bytes */ uint8_t SPP::__crc(uint8_t* data) { - return(pgm_read_byte(&rfcomm_crc_table[pgm_read_byte(&rfcomm_crc_table[0xff ^ data[0]]) ^ data[1]])); + return (pgm_read_byte(&rfcomm_crc_table[pgm_read_byte(&rfcomm_crc_table[0xff ^ data[0]]) ^ data[1]])); } /* Calculate FCS - we never actually check if the host sends correct FCS to the Arduino */ uint8_t SPP::calcFcs(uint8_t *data) { - if((data[1] & 0xEF) == RFCOMM_UIH) - return (0xff - __crc(data)); // FCS on 2 bytes - else - return (0xff - pgm_read_byte(&rfcomm_crc_table[__crc(data) ^ data[2]])); // FCS on 3 bytes + if ((data[1] & 0xEF) == RFCOMM_UIH) + return (0xff - __crc(data)); // FCS on 2 bytes + else + return (0xff - pgm_read_byte(&rfcomm_crc_table[__crc(data) ^ data[2]])); // FCS on 3 bytes } /* Serial commands */ void SPP::print(const String &str) { - if(!connected) - return; - uint8_t length = str.length(); - if(length > (sizeof(l2capoutbuf)-4)) - length = sizeof(l2capoutbuf)-4; - l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress;; // RFCOMM Address - l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control - l2capoutbuf[2] = length << 1 | 1; // Length - uint8_t i = 0; - for(; i < length; i++) - l2capoutbuf[i+3] = str[i]; - l2capoutbuf[i+3] = calcFcs(l2capoutbuf); + if (!connected) + return; + uint8_t length = str.length(); + if (length > (sizeof (l2capoutbuf) - 4)) + length = sizeof (l2capoutbuf) - 4; + l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress; + ; // RFCOMM Address + l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control + l2capoutbuf[2] = length << 1 | 1; // Length + uint8_t i = 0; + for (; i < length; i++) + l2capoutbuf[i + 3] = str[i]; + l2capoutbuf[i + 3] = calcFcs(l2capoutbuf); - RFCOMM_Command(l2capoutbuf,length+4); + RFCOMM_Command(l2capoutbuf, length + 4); } + void SPP::print(const char* str) { - if(!connected) - return; - uint8_t length = strlen(str); - if(length > (sizeof(l2capoutbuf)-4)) - length = sizeof(l2capoutbuf)-4; - l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress;; // RFCOMM Address - l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control - l2capoutbuf[2] = length << 1 | 1; // Length - uint8_t i = 0; - for(; i < length; i++) - l2capoutbuf[i+3] = str[i]; - l2capoutbuf[i+3] = calcFcs(l2capoutbuf); + if (!connected) + return; + uint8_t length = strlen(str); + if (length > (sizeof (l2capoutbuf) - 4)) + length = sizeof (l2capoutbuf) - 4; + l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress; + ; // RFCOMM Address + l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control + l2capoutbuf[2] = length << 1 | 1; // Length + uint8_t i = 0; + for (; i < length; i++) + l2capoutbuf[i + 3] = str[i]; + l2capoutbuf[i + 3] = calcFcs(l2capoutbuf); - RFCOMM_Command(l2capoutbuf,length+4); + RFCOMM_Command(l2capoutbuf, length + 4); } + void SPP::print(uint8_t* array, uint8_t length) { - if(!connected) - return; - if(length > (sizeof(l2capoutbuf)-4)) - length = sizeof(l2capoutbuf)-4; - l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress;; // RFCOMM Address - l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control - l2capoutbuf[2] = length << 1 | 1; // Length - uint8_t i = 0; - for(; i < length; i++) - l2capoutbuf[i+3] = array[i]; - l2capoutbuf[i+3] = calcFcs(l2capoutbuf); + if (!connected) + return; + if (length > (sizeof (l2capoutbuf) - 4)) + length = sizeof (l2capoutbuf) - 4; + l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress; + ; // RFCOMM Address + l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control + l2capoutbuf[2] = length << 1 | 1; // Length + uint8_t i = 0; + for (; i < length; i++) + l2capoutbuf[i + 3] = array[i]; + l2capoutbuf[i + 3] = calcFcs(l2capoutbuf); - RFCOMM_Command(l2capoutbuf,length+4); + RFCOMM_Command(l2capoutbuf, length + 4); } + void SPP::println(const String &str) { - String output = str + "\r\n"; - print(output); + String output = str + "\r\n"; + print(output); } + void SPP::println(const char* str) { - char output[strlen(str)+3]; - strcpy(output,str); - strcat(output,"\r\n"); - print(output); + char output[strlen(str) + 3]; + strcpy(output, str); + strcat(output, "\r\n"); + print(output); } + void SPP::println(uint8_t data) { - uint8_t buf[3] = {data, '\r', '\n'}; - print(buf,3); + uint8_t buf[3] = {data, '\r', '\n'}; + print(buf, 3); } + void SPP::println(uint8_t* array, uint8_t length) { - uint8_t buf[length+2]; - memcpy(buf,array,length); - buf[length] = '\r'; - buf[length+1] = '\n'; - print(buf,length+2); + uint8_t buf[length + 2]; + memcpy(buf, array, length); + buf[length] = '\r'; + buf[length + 1] = '\n'; + print(buf, length + 2); } + void SPP::printFlashString(const __FlashStringHelper *ifsh, bool newline) { - const char PROGMEM *p = (const char PROGMEM *)ifsh; - uint8_t size = 0; - while (1) { // Calculate the size of the string - uint8_t c = pgm_read_byte(p+size); - if (c == 0) - break; - size++; - } - uint8_t buf[size+2]; // Add two extra in case it needs to print a newline and carriage return + const char PROGMEM *p = (const char PROGMEM *)ifsh; + uint8_t size = 0; + while (1) { // Calculate the size of the string + uint8_t c = pgm_read_byte(p + size); + if (c == 0) + break; + size++; + } + uint8_t buf[size + 2]; // Add two extra in case it needs to print a newline and carriage return - for(uint8_t i = 0; i < size; i++) - buf[i] = pgm_read_byte(p++); + for (uint8_t i = 0; i < size; i++) + buf[i] = pgm_read_byte(p++); - if(newline) { - buf[size] = '\r'; - buf[size+1] = '\n'; - print(buf,size+2); - } else - print(buf,size); + if (newline) { + buf[size] = '\r'; + buf[size + 1] = '\n'; + print(buf, size + 2); + } else + print(buf, size); } + void SPP::println(void) { - uint8_t buf[2] = {'\r','\n'}; - print(buf,2); + uint8_t buf[2] = {'\r', '\n'}; + print(buf, 2); } /* These must be used to print numbers */ void SPP::printNumber(uint32_t n) { - char output[11]; - intToString(n,output); - print(output); + char output[11]; + intToString(n, output); + print(output); } + void SPP::printNumberln(uint32_t n) { - char output[13]; - intToString(n,output); - strcat(output,"\r\n"); - print(output); + char output[13]; + intToString(n, output); + strcat(output, "\r\n"); + print(output); } + void SPP::printNumber(int32_t n) { - char output[12]; - intToString(n,output); - print(output); + char output[12]; + intToString(n, output); + print(output); } + void SPP::printNumberln(int32_t n) { - char output[14]; - intToString(n,output); - strcat(output,"\r\n"); - print(output); + char output[14]; + intToString(n, output); + strcat(output, "\r\n"); + print(output); } + void SPP::intToString(int32_t input, char* output) { - if(input < 0) { - char buf[11]; - intToString((uint32_t)(input*-1),buf); - strcpy(output,"-"); - strcat(output,buf); - } else - intToString((uint32_t)input,output); + if (input < 0) { + char buf[11]; + intToString((uint32_t)(input*-1), buf); + strcpy(output, "-"); + strcat(output, buf); + } else + intToString((uint32_t)input, output); } + void SPP::intToString(uint32_t input, char* output) { - uint32_t temp = input; - uint8_t digits = 0; - while(temp) { - temp /= 10; - digits++; - } - if(digits == 0) - strcpy(output,"0"); - else { - for(uint8_t i = 1; i <= digits; i++) { - output[digits-i] = input%10 + '0'; // Get number and convert to ASCII Character - input /= 10; + uint32_t temp = input; + uint8_t digits = 0; + while (temp) { + temp /= 10; + digits++; + } + if (digits == 0) + strcpy(output, "0"); + else { + for (uint8_t i = 1; i <= digits; i++) { + output[digits - i] = input % 10 + '0'; // Get number and convert to ASCII Character + input /= 10; + } + output[digits] = '\0'; // Add null character } - output[digits] = '\0'; // Add null character - } } void SPP::printNumber(double n, uint8_t digits) { - char output[13+digits]; - doubleToString(n,output,digits); - print(output); + char output[13 + digits]; + doubleToString(n, output, digits); + print(output); } + void SPP::printNumberln(double n, uint8_t digits) { - char output[15+digits]; - doubleToString(n,output,digits); - strcat(output,"\r\n"); - print(output); + char output[15 + digits]; + doubleToString(n, output, digits); + strcat(output, "\r\n"); + print(output); } + void SPP::doubleToString(double input, char* output, uint8_t digits) { - char buffer[13+digits]; - if(input < 0) { - strcpy(output,"-"); - input = -input; - } - else - strcpy(output,""); + char buffer[13 + digits]; + if (input < 0) { + strcpy(output, "-"); + input = -input; + } else + strcpy(output, ""); - // Round correctly - double rounding = 0.5; - for (uint8_t i=0; i (sizeof(rfcommDataBuffer)-5)) { // We will send the command just before it runs out of credit - bytesRead = 0; - sendRfcommCredit(rfcommChannelConnection,rfcommDirection,0,RFCOMM_UIH,0x10,sizeof(rfcommDataBuffer)); // Send more credit + if (rfcommAvailable == 0) // Don't read if there is nothing in the buffer + return 0; + uint8_t output = rfcommDataBuffer[0]; + for (uint8_t i = 1; i < rfcommAvailable; i++) + rfcommDataBuffer[i - 1] = rfcommDataBuffer[i]; // Shift the buffer one left + rfcommAvailable--; + bytesRead++; + if (bytesRead > (sizeof (rfcommDataBuffer) - 5)) { // We will send the command just before it runs out of credit + bytesRead = 0; + sendRfcommCredit(rfcommChannelConnection, rfcommDirection, 0, RFCOMM_UIH, 0x10, sizeof (rfcommDataBuffer)); // Send more credit #ifdef EXTRADEBUG - Notify(PSTR("\r\nSent "), 0x80); - Serial.print(sizeof(rfcommDataBuffer)); - Notify(PSTR(" more credit"), 0x80); + Notify(PSTR("\r\nSent "), 0x80); + Serial.print(sizeof (rfcommDataBuffer)); + Notify(PSTR(" more credit"), 0x80); #endif - } - return output; + } + return output; } \ No newline at end of file diff --git a/SPP.h b/SPP.h index 070add5b..61a5a4c2 100644 --- a/SPP.h +++ b/SPP.h @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -70,7 +70,7 @@ #define extendAddress 0x01 // Allways 1 -// Multiplexer message types +// Multiplexer message types #define BT_RFCOMM_PN_CMD 0x83 #define BT_RFCOMM_PN_RSP 0x81 #define BT_RFCOMM_MSC_CMD 0xE3 @@ -87,276 +87,310 @@ #define BT_RFCOMM_RLS_CMD 0x53 #define BT_RFCOMM_RLS_RSP 0x51 #define BT_RFCOMM_NSC_RSP 0x11 -*/ + */ /** This BluetoothService class implements the Serial Port Protocol (SPP). */ class SPP : public BluetoothService { public: - /** - * Constructor for the SPP class. - * @param p Pointer to BTD class instance. - * @param name Set the name to BTD#btdName. If argument is omitted, then "Arduino" will be used. - * @param pin Write the pin to BTD#btdPin. If argument is omitted, then "1234" will be used. - */ - SPP(BTD *p, const char* name = "Arduino", const char* pin = "1234"); + /** + * Constructor for the SPP class. + * @param p Pointer to BTD class instance. + * @param name Set the name to BTD#btdName. If argument is omitted, then "Arduino" will be used. + * @param pin Write the pin to BTD#btdPin. If argument is omitted, then "1234" will be used. + */ + SPP(BTD *p, const char* name = "Arduino", const char* pin = "1234"); - /** @name BluetoothService implementation */ - /** - * Used to pass acldata to the services. - * @param ACLData Incoming acldata. - */ - virtual void ACLData(uint8_t* ACLData); - /** Used to establish the connection automatically. */ - virtual void Run(); - /** Use this to reset the service. */ - virtual void Reset(); - /** Used this to disconnect the virtual serial port. */ - virtual void disconnect(); - /**@}*/ - - /** Variable used to indicate if the connection is established. */ - bool connected; - - /** @name Serial port profile (SPP) Print functions */ - /** - * Used to send Arduino String data type. - * @param str String to send. - */ - void print(const String &str); - /** - * Same as print(const String &str), but will include newline and carriage return. - * @param str String to send. - */ - void println(const String &str); - - /** - * Used to send standard strings. - * @param str String to send. - */ - void print(const char* str); - /** - * Same as print(const char* str), but will include newline and carriage return. - * @param str String to send. - */ - void println(const char* str); + /** @name BluetoothService implementation */ + /** + * Used to pass acldata to the services. + * @param ACLData Incoming acldata. + */ + virtual void ACLData(uint8_t* ACLData); + /** Used to establish the connection automatically. */ + virtual void Run(); + /** Use this to reset the service. */ + virtual void Reset(); + /** Used this to disconnect the virtual serial port. */ + virtual void disconnect(); + /**@}*/ - /** - * Used to send single bytes. - * @param data Data to send. - */ - void print(uint8_t data) { print(&data,1); }; - /** - * Same as print(uint8_t data), but will include newline and carriage return. - * @param data Data to send. - */ - void println(uint8_t data); + /** Variable used to indicate if the connection is established. */ + bool connected; - /** - * Used to send arrays. - * @param array Array to send. - * @param length Number of bytes to send. - */ - void print(uint8_t* array, uint8_t length); - /** - * Same as print(uint8_t* array, uint8_t length), but will include newline and carriage return. - * @param array Array to send. - * @param length Number of bytes to send. - */ - void println(uint8_t* array, uint8_t length); + /** @name Serial port profile (SPP) Print functions */ + /** + * Used to send Arduino String data type. + * @param str String to send. + */ + void print(const String &str); + /** + * Same as print(const String &str), but will include newline and carriage return. + * @param str String to send. + */ + void println(const String &str); - /** - * Used to print strings stored in flash. - * Use "SerialBT.print(F("String"));" to print a string stored in flash. - * @param ifsh String to send - see: http://playground.arduino.cc/Learning/Memory. - */ - void print(const __FlashStringHelper *ifsh) { printFlashString(ifsh,false); }; - /** - * Same as print(const __FlashStringHelper *ifsh), but will include newline and carriage return. - * @param ifsh String to send - see: http://playground.arduino.cc/Learning/Memory. - */ - void println(const __FlashStringHelper *ifsh) { printFlashString(ifsh,true); }; - /** - * Helper function to print a string stored in flash. - * @param ifsh String stored in flash you want to print. - * @param newline Set this to true to include newline and carriage return. - */ - void printFlashString(const __FlashStringHelper *ifsh, bool newline); + /** + * Used to send standard strings. + * @param str String to send. + */ + void print(const char* str); + /** + * Same as print(const char* str), but will include newline and carriage return. + * @param str String to send. + */ + void println(const char* str); + + /** + * Used to send single bytes. + * @param data Data to send. + */ + void print(uint8_t data) { + print(&data, 1); + }; + /** + * Same as print(uint8_t data), but will include newline and carriage return. + * @param data Data to send. + */ + void println(uint8_t data); + + /** + * Used to send arrays. + * @param array Array to send. + * @param length Number of bytes to send. + */ + void print(uint8_t* array, uint8_t length); + /** + * Same as print(uint8_t* array, uint8_t length), but will include newline and carriage return. + * @param array Array to send. + * @param length Number of bytes to send. + */ + void println(uint8_t* array, uint8_t length); + + /** + * Used to print strings stored in flash. + * Use "SerialBT.print(F("String"));" to print a string stored in flash. + * @param ifsh String to send - see: http://playground.arduino.cc/Learning/Memory. + */ + void print(const __FlashStringHelper *ifsh) { + printFlashString(ifsh, false); + }; + + /** + * Same as print(const __FlashStringHelper *ifsh), but will include newline and carriage return. + * @param ifsh String to send - see: http://playground.arduino.cc/Learning/Memory. + */ + void println(const __FlashStringHelper *ifsh) { + printFlashString(ifsh, true); + }; + /** + * Helper function to print a string stored in flash. + * @param ifsh String stored in flash you want to print. + * @param newline Set this to true to include newline and carriage return. + */ + void printFlashString(const __FlashStringHelper *ifsh, bool newline); - /** Use this to print newline and carriage return. */ - void println(void); + /** Use this to print newline and carriage return. */ + void println(void); - /** - * Used to print unsigned integers. - * @param n Unsigned integer to send. - */ - void printNumber(uint8_t n) { printNumber((uint32_t)n); }; - /** - * Same as printNumber(uint8_t n), but will include newline and carriage return. - * @param n Unsigned integer to send. - */ - void printNumberln(uint8_t n) { printNumberln((uint32_t)n); }; - /** - * Used to print signed integers. - * @param n Signed integer to send. - */ - void printNumber(int8_t n) { printNumber((int32_t)n); }; - /** - * Same as printNumber(int8_t n), but will include newline and carriage return. - * @param n Signed integer to send. - */ - void printNumberln(int8_t n) { printNumberln((int32_t)n); }; + /** + * Used to print unsigned integers. + * @param n Unsigned integer to send. + */ + void printNumber(uint8_t n) { + printNumber((uint32_t) n); + }; - /** - * Used to print unsigned integers. - * @param n Unsigned integer to send. - */ - void printNumber(uint16_t n) { printNumber((uint32_t)n); }; - /** - * Same as printNumber(uint16_t n), but will include newline and carriage return. - * @param n Unsigned integer to send. - */ - void printNumberln(uint16_t n) { printNumberln((uint32_t)n); }; - /** - * Used to print signed integers. - * @param n Signed integer to send. - */ - void printNumber(int16_t n) { printNumber((int32_t)n); }; - /** - * Same as printNumber(int16_t n), but will include newline and carriage return. - * @param n Signed integer to send. - */ - void printNumberln(int16_t n) { printNumberln((int32_t)n); }; + /** + * Same as printNumber(uint8_t n), but will include newline and carriage return. + * @param n Unsigned integer to send. + */ + void printNumberln(uint8_t n) { + printNumberln((uint32_t) n); + }; - /** - * Used to print unsigned integers. - * @param n Unsigned integer to send. - */ - void printNumber(uint32_t n); - /** - * Same as printNumber(uint32_t n), but will include newline and carriage return. - * @param n Unsigned integer to send. - */ - void printNumberln(uint32_t n); - - /** - * Used to print signed integers. - * @param n Signed integer to send. - */ - void printNumber(int32_t n); - /** - * Same as printNumber(int32_t n), but will include newline and carriage return. - * @param n Signed integer to send. - */ - void printNumberln(int32_t n); + /** + * Used to print signed integers. + * @param n Signed integer to send. + */ + void printNumber(int8_t n) { + printNumber((int32_t) n); + }; - /** - * Helper function to convert from an unsigned integer to a string. - * @param input Unsigned integer to convert. - * @param output Output buffer. - */ - void intToString(int32_t input, char* output); - /** - * Helper function to convert from a signed integer to a string. - * @param input Signed integer to convert. - * @param output Output buffer. - */ - void intToString(uint32_t input, char* output); + /** + * Same as printNumber(int8_t n), but will include newline and carriage return. + * @param n Signed integer to send. + */ + void printNumberln(int8_t n) { + printNumberln((int32_t) n); + }; + + /** + * Used to print unsigned integers. + * @param n Unsigned integer to send. + */ + void printNumber(uint16_t n) { + printNumber((uint32_t) n); + }; + + /** + * Same as printNumber(uint16_t n), but will include newline and carriage return. + * @param n Unsigned integer to send. + */ + void printNumberln(uint16_t n) { + printNumberln((uint32_t) n); + }; + + /** + * Used to print signed integers. + * @param n Signed integer to send. + */ + void printNumber(int16_t n) { + printNumber((int32_t) n); + }; + + /** + * Same as printNumber(int16_t n), but will include newline and carriage return. + * @param n Signed integer to send. + */ + void printNumberln(int16_t n) { + printNumberln((int32_t) n); + }; + + /** + * Used to print unsigned integers. + * @param n Unsigned integer to send. + */ + void printNumber(uint32_t n); + /** + * Same as printNumber(uint32_t n), but will include newline and carriage return. + * @param n Unsigned integer to send. + */ + void printNumberln(uint32_t n); + + /** + * Used to print signed integers. + * @param n Signed integer to send. + */ + void printNumber(int32_t n); + /** + * Same as printNumber(int32_t n), but will include newline and carriage return. + * @param n Signed integer to send. + */ + void printNumberln(int32_t n); + + /** + * Helper function to convert from an unsigned integer to a string. + * @param input Unsigned integer to convert. + * @param output Output buffer. + */ + void intToString(int32_t input, char* output); + /** + * Helper function to convert from a signed integer to a string. + * @param input Signed integer to convert. + * @param output Output buffer. + */ + void intToString(uint32_t input, char* output); + + /** + * Used to print floating-point numbers. + * @param n Floating-point number to print. + * @param digits Number of digits to send. If argument is omitted, then 2 digits will be used. + */ + void printNumber(double n, uint8_t digits = 2); + /** + * Same as printNumber(double n, uint8_t digits), but will include newline and carriage return. + * @param n Floating-point number to print. + * @param digits Number of digits to send. If argument is omitted, then 2 digits will be used. + */ + void printNumberln(double n, uint8_t digits = 2); + /** + * Helper function to convert from a double to a string. + * @param input Floating-point number to convert. + * @param output Output buffer. + * @param digits Number of digits to convert. If argument is omitted, then 2 digits will be used. + */ + void doubleToString(double input, char* output, uint8_t digits = 2); + + /** + * Get number of bytes waiting to be read. + * @return Return the number of bytes ready to be read. + */ + uint8_t available() { + return rfcommAvailable; + }; + /** + * Used to read the buffer. + * @return Return the byte. Will return 0 if no byte is available. + */ + uint8_t read(); + + /** Discard all the bytes in the buffer. */ + void flush() { + rfcommAvailable = 0; + }; + /**@}*/ - /** - * Used to print floating-point numbers. - * @param n Floating-point number to print. - * @param digits Number of digits to send. If argument is omitted, then 2 digits will be used. - */ - void printNumber(double n, uint8_t digits = 2); - /** - * Same as printNumber(double n, uint8_t digits), but will include newline and carriage return. - * @param n Floating-point number to print. - * @param digits Number of digits to send. If argument is omitted, then 2 digits will be used. - */ - void printNumberln(double n, uint8_t digits = 2); - /** - * Helper function to convert from a double to a string. - * @param input Floating-point number to convert. - * @param output Output buffer. - * @param digits Number of digits to convert. If argument is omitted, then 2 digits will be used. - */ - void doubleToString(double input, char* output, uint8_t digits = 2); - - /** - * Get number of bytes waiting to be read. - * @return Return the number of bytes ready to be read. - */ - uint8_t available() { return rfcommAvailable; }; - /** - * Used to read the buffer. - * @return Return the byte. Will return 0 if no byte is available. - */ - uint8_t read(); - /** Discard all the bytes in the buffer. */ - void flush() { rfcommAvailable = 0; }; - /**@}*/ - private: - /* Bluetooth dongle library pointer */ - BTD *pBtd; - - /* Set true when a channel is created */ - bool SDPConnected; - bool RFCOMMConnected; - - uint16_t hci_handle; // The HCI Handle for the connection - - /* Variables used by L2CAP state maschines */ - uint8_t l2cap_sdp_state; - uint8_t l2cap_rfcomm_state; - uint16_t l2cap_event_flag; // l2cap flags of received bluetooth events - - uint8_t l2capoutbuf[BULK_MAXPKTSIZE]; // General purpose buffer for l2cap out data - uint8_t rfcommbuf[10]; // Buffer for RFCOMM Commands - - /* L2CAP Channels */ - uint8_t sdp_scid[2]; // L2CAP source CID for SDP - uint8_t sdp_dcid[2]; // 0x0050 - uint8_t rfcomm_scid[2]; // L2CAP source CID for RFCOMM - uint8_t rfcomm_dcid[2]; // 0x0051 - uint8_t identifier; // Identifier for command + /* Bluetooth dongle library pointer */ + BTD *pBtd; - /* RFCOMM Variables */ - uint8_t rfcommChannel; - uint8_t rfcommChannelConnection; // This is the channel the SPP chanel will be running at - uint8_t rfcommDirection; - uint8_t rfcommCommandResponse; - uint8_t rfcommChannelType; - uint8_t rfcommPfBit; - - unsigned long timer; - bool waitForLastCommand; - bool creditSent; - - uint8_t rfcommDataBuffer[100]; // Create a 100 sized buffer for incoming data - uint8_t rfcommAvailable; - - bool firstMessage; // Used to see if it's the first SDP request received - uint8_t bytesRead; // Counter to see when it's time to send more credit - - /* State machines */ - void SDP_task(); // SDP state machine - void RFCOMM_task(); // RFCOMM state machine - - /* SDP Commands */ - void SDP_Command(uint8_t* data, uint8_t nbytes); - void serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLow); - void serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow); - void serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow); - void l2capResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow); - void l2capResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow); - - /* RFCOMM Commands */ - void RFCOMM_Command(uint8_t* data, uint8_t nbytes); - void sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t* data, uint8_t length); - void sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t credit); - uint8_t calcFcs(uint8_t *data); - uint8_t __crc(uint8_t* data); + /* Set true when a channel is created */ + bool SDPConnected; + bool RFCOMMConnected; + + uint16_t hci_handle; // The HCI Handle for the connection + + /* Variables used by L2CAP state maschines */ + uint8_t l2cap_sdp_state; + uint8_t l2cap_rfcomm_state; + uint16_t l2cap_event_flag; // l2cap flags of received bluetooth events + + uint8_t l2capoutbuf[BULK_MAXPKTSIZE]; // General purpose buffer for l2cap out data + uint8_t rfcommbuf[10]; // Buffer for RFCOMM Commands + + /* L2CAP Channels */ + uint8_t sdp_scid[2]; // L2CAP source CID for SDP + uint8_t sdp_dcid[2]; // 0x0050 + uint8_t rfcomm_scid[2]; // L2CAP source CID for RFCOMM + uint8_t rfcomm_dcid[2]; // 0x0051 + uint8_t identifier; // Identifier for command + + /* RFCOMM Variables */ + uint8_t rfcommChannel; + uint8_t rfcommChannelConnection; // This is the channel the SPP chanel will be running at + uint8_t rfcommDirection; + uint8_t rfcommCommandResponse; + uint8_t rfcommChannelType; + uint8_t rfcommPfBit; + + unsigned long timer; + bool waitForLastCommand; + bool creditSent; + + uint8_t rfcommDataBuffer[100]; // Create a 100 sized buffer for incoming data + uint8_t rfcommAvailable; + + bool firstMessage; // Used to see if it's the first SDP request received + uint8_t bytesRead; // Counter to see when it's time to send more credit + + /* State machines */ + void SDP_task(); // SDP state machine + void RFCOMM_task(); // RFCOMM state machine + + /* SDP Commands */ + void SDP_Command(uint8_t* data, uint8_t nbytes); + void serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLow); + void serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow); + void serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow); + void l2capResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow); + void l2capResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow); + + /* RFCOMM Commands */ + void RFCOMM_Command(uint8_t* data, uint8_t nbytes); + void sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t* data, uint8_t length); + void sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t credit); + uint8_t calcFcs(uint8_t *data); + uint8_t __crc(uint8_t* data); }; #endif \ No newline at end of file diff --git a/Usb.cpp b/Usb.cpp index 3a6ea5db..ec273f43 100644 --- a/Usb.cpp +++ b/Usb.cpp @@ -176,7 +176,7 @@ uint8_t USB::ctrlReq(uint8_t addr, uint8_t ep, uint8_t bmReqType, uint8_t bReque // Invoke callback function if inTransfer completed successfuly and callback function pointer is specified if (!rcode && p) - ((USBReadParser*) p)->Parse(read, dataptr, total - left); + ((USBReadParser*)p)->Parse(read, dataptr, total - left); left -= read; @@ -236,14 +236,14 @@ uint8_t USB::InTransfer(EpInfo *pep, uint16_t nak_limit, uint16_t *nbytesptr, ui /* the only case when absense of RCVDAVIRQ makes sense is when toggle error occured. Need to add handling for that */ if ((regRd(rHIRQ) & bmRCVDAVIRQ) == 0) { //printf("Problem! NO RCVDAVIRQ!\r\n"); - rcode = 0xf0; //receive error + rcode = 0xf0; //receive error break; } pktsize = regRd(rRCVBC); //number of received bytes //printf("Got %i bytes ", pktsize); assert(pktsize <= nbytes); - int16_t mem_left = (int16_t) nbytes - *((int16_t*) nbytesptr); + int16_t mem_left = (int16_t)nbytes - *((int16_t*)nbytesptr); if (mem_left < 0) mem_left = 0; @@ -591,7 +591,7 @@ uint8_t USB::getConfDescr(uint8_t addr, uint8_t ep, uint8_t conf, USBReadParser if (ret) return ret; - uint16_t total = ((USB_CONFIGURATION_DESCRIPTOR*) buf)->wTotalLength; + uint16_t total = ((USB_CONFIGURATION_DESCRIPTOR*)buf)->wTotalLength; //USBTRACE2("\r\ntotal conf.size:", total); diff --git a/Usb.h b/Usb.h index 15bc20f2..c27796d2 100644 --- a/Usb.h +++ b/Usb.h @@ -147,7 +147,7 @@ typedef struct { uint8_t recipient : 5; // Recipient of the request uint8_t type : 2; // Type of request uint8_t direction : 1; // Direction of data X-fer - }__attribute__((packed)); + } __attribute__((packed)); } ReqType_u; uint8_t bRequest; // 1 Request @@ -157,7 +157,7 @@ typedef struct { struct { uint8_t wValueLo; uint8_t wValueHi; - }__attribute__((packed)); + } __attribute__((packed)); } wVal_u; uint16_t wIndex; // 4 Depends on bRequest uint16_t wLength; // 6 Depends on bRequest @@ -190,12 +190,12 @@ public: }; AddressPool& GetAddressPool() { - return (AddressPool&) addrPool; + return(AddressPool&) addrPool; }; uint8_t RegisterDeviceClass(USBDeviceConfig *pdev) { - for (uint8_t i = 0; i < USB_NUMDEVICES; i++) { - if (!devConfig[i]) { + for(uint8_t i = 0; i < USB_NUMDEVICES; i++) { + if(!devConfig[i]) { devConfig[i] = pdev; return 0; } @@ -250,27 +250,27 @@ private: //get device descriptor inline uint8_t USB::getDevDescr(uint8_t addr, uint8_t ep, uint16_t nbytes, uint8_t* dataptr) { - return ( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, 0x00, USB_DESCRIPTOR_DEVICE, 0x0000, nbytes, dataptr)); + return( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, 0x00, USB_DESCRIPTOR_DEVICE, 0x0000, nbytes, dataptr)); } //get configuration descriptor inline uint8_t USB::getConfDescr(uint8_t addr, uint8_t ep, uint16_t nbytes, uint8_t conf, uint8_t* dataptr) { - return ( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, conf, USB_DESCRIPTOR_CONFIGURATION, 0x0000, nbytes, dataptr)); + return( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, conf, USB_DESCRIPTOR_CONFIGURATION, 0x0000, nbytes, dataptr)); } //get string descriptor inline uint8_t USB::getStrDescr(uint8_t addr, uint8_t ep, uint16_t nuint8_ts, uint8_t index, uint16_t langid, uint8_t* dataptr) { - return ( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, index, USB_DESCRIPTOR_STRING, langid, nuint8_ts, dataptr)); + return( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, index, USB_DESCRIPTOR_STRING, langid, nuint8_ts, dataptr)); } //set address inline uint8_t USB::setAddr(uint8_t oldaddr, uint8_t ep, uint8_t newaddr) { - return ( ctrlReq(oldaddr, ep, bmREQ_SET, USB_REQUEST_SET_ADDRESS, newaddr, 0x00, 0x0000, 0x0000, NULL)); + return( ctrlReq(oldaddr, ep, bmREQ_SET, USB_REQUEST_SET_ADDRESS, newaddr, 0x00, 0x0000, 0x0000, NULL)); } //set configuration inline uint8_t USB::setConf(uint8_t addr, uint8_t ep, uint8_t conf_value) { - return ( ctrlReq(addr, ep, bmREQ_SET, USB_REQUEST_SET_CONFIGURATION, conf_value, 0x00, 0x0000, 0x0000, NULL)); + return( ctrlReq(addr, ep, bmREQ_SET, USB_REQUEST_SET_CONFIGURATION, conf_value, 0x00, 0x0000, 0x0000, NULL)); } #endif // defined(USB_METHODS_INLINE) diff --git a/Wii.cpp b/Wii.cpp index c215bd54..a96e5a58 100755 --- a/Wii.cpp +++ b/Wii.cpp @@ -25,1030 +25,1037 @@ //#define PRINTREPORT // Uncomment to print the report send by the Wii controllers const uint8_t LEDS[] PROGMEM = { - 0x10, // LED1 - 0x20, // LED2 - 0x40, // LED3 - 0x80, // LED4 + 0x10, // LED1 + 0x20, // LED2 + 0x40, // LED3 + 0x80, // LED4 - 0x90, // LED5 - 0xA0, // LED6 - 0xC0, // LED7 - 0xD0, // LED8 - 0xE0, // LED9 - 0xF0 // LED10 - }; + 0x90, // LED5 + 0xA0, // LED6 + 0xC0, // LED7 + 0xD0, // LED8 + 0xE0, // LED9 + 0xF0 // LED10 +}; const uint32_t BUTTONS[] PROGMEM = { - 0x00008, // UP - 0x00002, // RIGHT - 0x00004, // DOWN - 0x00001, // LEFT + 0x00008, // UP + 0x00002, // RIGHT + 0x00004, // DOWN + 0x00001, // LEFT - 0, // Skip - 0x00010, // PLUS - 0x00100, // TWO - 0x00200, // ONE + 0, // Skip + 0x00010, // PLUS + 0x00100, // TWO + 0x00200, // ONE - 0x01000, // MINUS - 0x08000, // HOME - 0x10000, // Z - 0x20000, // C + 0x01000, // MINUS + 0x08000, // HOME + 0x10000, // Z + 0x20000, // C - 0x00400, // B - 0x00800 // A + 0x00400, // B + 0x00800 // A }; const uint32_t PROCONTROLLERBUTTONS[] PROGMEM = { - 0x00100, // UP - 0x00080, // RIGHT - 0x00040, // DOWN - 0x00200, // LEFT + 0x00100, // UP + 0x00080, // RIGHT + 0x00040, // DOWN + 0x00200, // LEFT - 0, // Skip - 0x00004, // PLUS - 0x20000, // L3 - 0x10000, // R3 + 0, // Skip + 0x00004, // PLUS + 0x20000, // L3 + 0x10000, // R3 - 0x00010, // MINUS - 0x00008, // HOME - 0,0, // Skip + 0x00010, // MINUS + 0x00008, // HOME + 0, 0, // Skip - 0x04000, // B - 0x01000, // A - 0x00800, // X - 0x02000, // Y + 0x04000, // B + 0x01000, // A + 0x00800, // X + 0x02000, // Y - 0x00020, // L - 0x00002, // R - 0x08000, // ZL - 0x00400 // ZR + 0x00020, // L + 0x00002, // R + 0x08000, // ZL + 0x00400 // ZR }; -WII::WII(BTD *p, bool pair): +WII::WII(BTD *p, bool pair) : pBtd(p) // pointer to USB class instance - mandatory { - if (pBtd) - pBtd->registerServiceClass(this); // Register it as a Bluetooth service + if (pBtd) + pBtd->registerServiceClass(this); // Register it as a Bluetooth service - pBtd->pairWithWii = pair; + pBtd->pairWithWii = pair; - HIDBuffer[0] = 0xA2;// HID BT DATA_request (0xA0) | Report Type (Output 0x02) + HIDBuffer[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) - /* Set device cid for the control and intterrupt channelse - LSB */ - control_dcid[0] = 0x60;//0x0060 - control_dcid[1] = 0x00; - interrupt_dcid[0] = 0x61;//0x0061 - interrupt_dcid[1] = 0x00; + /* Set device cid for the control and intterrupt channelse - LSB */ + control_dcid[0] = 0x60; //0x0060 + control_dcid[1] = 0x00; + interrupt_dcid[0] = 0x61; //0x0061 + interrupt_dcid[1] = 0x00; - Reset(); + Reset(); } + void WII::Reset() { - wiimoteConnected = false; - nunchuckConnected = false; - motionPlusConnected = false; - activateNunchuck = false; - motionValuesReset = false; - activeConnection = false; - pBtd->motionPlusInside = false; - pBtd->wiiUProController = false; - wiiUProControllerConnected = false; - l2cap_event_flag = 0; // Reset flags - l2cap_state = L2CAP_WAIT; + wiimoteConnected = false; + nunchuckConnected = false; + motionPlusConnected = false; + activateNunchuck = false; + motionValuesReset = false; + activeConnection = false; + pBtd->motionPlusInside = false; + pBtd->wiiUProController = false; + wiiUProControllerConnected = false; + l2cap_event_flag = 0; // Reset flags + l2cap_state = L2CAP_WAIT; } void WII::disconnect() { // Use this void to disconnect any of the controllers - if(motionPlusConnected && !pBtd->motionPlusInside) { // Disable the Motion Plus extension + if (motionPlusConnected && !pBtd->motionPlusInside) { // Disable the Motion Plus extension #ifdef DEBUG - Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80); + Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80); #endif - initExtension1(); // This will disable the Motion Plus extension - } - //First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection - pBtd->l2cap_disconnection_request(hci_handle,0x0A, interrupt_scid, interrupt_dcid); - Reset(); - l2cap_state = L2CAP_INTERRUPT_DISCONNECT; + initExtension1(); // This will disable the Motion Plus extension + } + //First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection + pBtd->l2cap_disconnection_request(hci_handle, 0x0A, interrupt_scid, interrupt_dcid); + Reset(); + l2cap_state = L2CAP_INTERRUPT_DISCONNECT; } void WII::ACLData(uint8_t* l2capinbuf) { - if(!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) { - if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { - if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { - pBtd->incomingWii = false; - pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service - activeConnection = true; - hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection - l2cap_state = L2CAP_WAIT; - } + if (!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) { + if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { + if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { + pBtd->incomingWii = false; + pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service + activeConnection = true; + hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection + l2cap_state = L2CAP_WAIT; + } + } } - } - if ((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000)) { // acl_handle_ok or it's a new connection - if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U - if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { + if ((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000)) { // acl_handle_ok or it's a new connection + if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U + if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { #ifdef DEBUG - Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); - PrintHex(l2capinbuf[13], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[12], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[17], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[16], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[15], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[14], 0x80); + Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80); + PrintHex (l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[12], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[17], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[16], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[14], 0x80); #endif - } - else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) { - if (((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success - if (l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) { // Success - //Serial.print("\r\nHID Control Connection Complete"); - identifier = l2capinbuf[9]; - control_scid[0] = l2capinbuf[12]; - control_scid[1] = l2capinbuf[13]; - l2cap_event_flag |= L2CAP_FLAG_CONTROL_CONNECTED; - } - else if (l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) { - //Serial.print("\r\nHID Interrupt Connection Complete"); - identifier = l2capinbuf[9]; - interrupt_scid[0] = l2capinbuf[12]; - interrupt_scid[1] = l2capinbuf[13]; - l2cap_event_flag |= L2CAP_FLAG_INTERRUPT_CONNECTED; - } - } - } - else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { -#ifdef EXTRADEBUG - Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); - PrintHex(l2capinbuf[13], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[12], 0x80); - Notify(PSTR(" SCID: "), 0x80); - PrintHex(l2capinbuf[15], 0x80); - Notify(PSTR(" "), 0x80); - PrintHex(l2capinbuf[14], 0x80); - Notify(PSTR(" Identifier: "), 0x80); - PrintHex(l2capinbuf[9], 0x80); -#endif - if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { - identifier = l2capinbuf[9]; - control_scid[0] = l2capinbuf[14]; - control_scid[1] = l2capinbuf[15]; - l2cap_event_flag |= L2CAP_FLAG_CONNECTION_CONTROL_REQUEST; - } - else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) { - identifier = l2capinbuf[9]; - interrupt_scid[0] = l2capinbuf[14]; - interrupt_scid[1] = l2capinbuf[15]; - l2cap_event_flag |= L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST; - } - } - else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) { - if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success - if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { - //Serial.print("\r\nHID Control Configuration Complete"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_SUCCESS; - } - else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { - //Serial.print("\r\nHID Interrupt Configuration Complete"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS; - } - } - } - else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) { - if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { - //Serial.print("\r\nHID Control Configuration Request"); - pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid); - } - else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { - //Serial.print("\r\nHID Interrupt Configuration Request"); - pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid); - } - } - else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { - if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { -#ifdef DEBUG - Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80); -#endif - identifier = l2capinbuf[9]; - pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid); - Reset(); - } - else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { -#ifdef DEBUG - Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80); -#endif - identifier = l2capinbuf[9]; - pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid); - Reset(); - } - } - else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) { - if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) { - //Serial.print("\r\nDisconnect Response: Control Channel"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE; - } - else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) { - //Serial.print("\r\nDisconnect Response: Interrupt Channel"); - identifier = l2capinbuf[9]; - l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE; - } - } -#ifdef EXTRADEBUG - else { - identifier = l2capinbuf[9]; - Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); - PrintHex(l2capinbuf[8], 0x80); - } -#endif - } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt - //Serial.print("\r\nL2CAP Interrupt"); - if(wiimoteConnected) { - if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT - if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons - if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes - ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); - else if(wiiUProControllerConnected) - ButtonState = (uint32_t)(((~l2capinbuf[23]) & 0xFE) | ((uint16_t)(~l2capinbuf[24]) << 8) | ((uint32_t)((~l2capinbuf[25]) & 0x03) << 16)); - else if(motionPlusConnected) { - if(l2capinbuf[20] & 0x02) // Only update the wiimote buttons, since the extension bytes are from the Motion Plus - ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000))); - else if (nunchuckConnected) // Update if it's a report from the Nunchuck - ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x0C) << 14)); - //else if(classicControllerConnected) // Update if it's a report from the Classic Controller - } - else if(nunchuckConnected) // The Nunchuck is directly connected - ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x03) << 16)); - //else if(classicControllerConnected) // The Classic Controller is directly connected - else if(!unknownExtensionConnected) - ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); -#ifdef PRINTREPORT - Notify(PSTR("ButtonState: "), 0x80); - PrintHex(ButtonState, 0x80); - Notify(PSTR("\r\n"), 0x80); -#endif - if(ButtonState != OldButtonState) { - ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable - OldButtonState = ButtonState; - } - } - if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer - accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500; - accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500; - accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500; - wiimotePitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; - wiimoteRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; - } - switch (l2capinbuf[9]) { - case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV - wiiState = l2capinbuf[12]; // (0x01: Battery is nearly empty), (0x02: An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4) - batteryLevel = l2capinbuf[15]; // Update battery level - if(l2capinbuf[12] & 0x01) { -#ifdef DEBUG - Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80); -#endif - } - if(l2capinbuf[12] & 0x02) { // Check if a extension is connected -#ifdef DEBUG - if(!unknownExtensionConnected) - Notify(PSTR("\r\nExtension connected"), 0x80); -#endif - unknownExtensionConnected = true; -#ifdef WIICAMERA - if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera -#endif - setReportMode(false,0x35); // Also read the extension - } - else { -#ifdef DEBUG - Notify(PSTR("\r\nExtension disconnected"), 0x80); -#endif - if(motionPlusConnected) { -#ifdef DEBUG - Notify(PSTR(" - from Motion Plus"), 0x80); -#endif - l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED; - if(!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false - nunchuckConnected = false; - //else if(classicControllerConnected) - } - else if(nunchuckConnected) { -#ifdef DEBUG - Notify(PSTR(" - Nunchuck"), 0x80); -#endif - nunchuckConnected = false; // It must be the Nunchuck controller then - l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED; - setLedStatus(); - setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer - } else { - setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer - } - } - break; - case 0x21: // Read Memory Data - if((l2capinbuf[12] & 0x0F) == 0) { // No error - // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers - if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { -#ifdef DEBUG - Notify(PSTR("\r\nNunchuck connected"), 0x80); -#endif - l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED; - } else if(l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) { -#ifdef DEBUG - Notify(PSTR("\r\nMotion Plus connected"), 0x80); -#endif - l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED; - } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) { -#ifdef DEBUG - Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80); -#endif - motionPlusConnected = true; - } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) { -#ifdef DEBUG - Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80); -#endif - activateNunchuck = false; - motionPlusConnected = true; - nunchuckConnected = true; - } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) { -#ifdef DEBUG - Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80); - Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80); -#endif - stateCounter = 300; // Skip the rest in "L2CAP_CHECK_MOTION_PLUS_STATE" - } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) { -#ifdef DEBUG - Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80); -#endif - wiiUProControllerConnected = true; - } -#ifdef DEBUG - else { - Notify(PSTR("\r\nUnknown Device: "), 0x80); - PrintHex(l2capinbuf[13], 0x80); - PrintHex(l2capinbuf[14], 0x80); - Notify(PSTR("\r\nData: "), 0x80); - for(uint8_t i = 0; i < ((l2capinbuf[12] >> 4)+1); i++) { // bit 4-7 is the length-1 - PrintHex(l2capinbuf[15+i], 0x80); - Notify(PSTR(" "), 0x80); - } - } -#endif - } -#ifdef EXTRADEBUG - else { - Notify(PSTR("\r\nReport Error: "), 0x80); - PrintHex(l2capinbuf[13], 0x80); - PrintHex(l2capinbuf[14], 0x80); - } -#endif - break; - case 0x22: // Acknowledge output report, return function result -#ifdef DEBUG - if(l2capinbuf[13] != 0x00) { // Check if there is an error - Notify(PSTR("\r\nCommand failed: "), 0x80); - PrintHex(l2capinbuf[12], 0x80); - } -#endif - break; - case 0x30: // Core buttons - (a1) 30 BB BB - break; - case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA - pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected - roll = wiimoteRoll; - break; - case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE - break; - case 0x33: // Core Buttons with Accelerometer and 12 IR bytes - (a1) 33 BB BB AA AA AA II II II II II II II II II II II II - pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus data available - roll = wiimoteRoll; -#ifdef WIICAMERA - // Read the IR data - IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position - IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position - IR_object_s1 = (l2capinbuf[17] & 0x0F); // size value, 0-15 - - IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4)); - IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2)); - IR_object_s2 = (l2capinbuf[20] & 0x0F); - - IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4)); - IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2)); - IR_object_s3 = (l2capinbuf[23] & 0x0F); - - IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4)); - IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2)); - IR_object_s4 = (l2capinbuf[26] & 0x0F); -#endif - break; - case 0x34: // Core Buttons with 19 Extension bytes - (a1) 34 BB BB EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE - break; - /* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */ - case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes - // (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II - // corresponds to output report mode 0x3e - - /**** for reading in full mode: DOES NOT WORK YET ****/ - /* When it works it will also have intensity and bounding box data */ - /* - IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4)); - IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2)); - IR_object_s1 = (l2capinbuf[15] & 0x0F); - */ - break; - case 0x3F: - /* - IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4)); - IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2)); - IR_object_s1 = (l2capinbuf[15] & 0x0F); - */ - break; - case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes - // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE - if(motionPlusConnected) { - if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension - if(motionValuesReset) { // We will only use the values when the gyro value has been set - gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6))-gyroYawZero); - gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6))-gyroRollZero); - gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6))-gyroPitchZero); - - yawGyroSpeed = (double)gyroYawRaw/((double)gyroYawZero/yawGyroScale); - rollGyroSpeed = -(double)gyroRollRaw/((double)gyroRollZero/rollGyroScale); // We invert these values so they will fit the acc values - pitchGyroSpeed = (double)gyroPitchRaw/((double)gyroPitchZero/pitchGyroScale); - - /* The onboard gyro has two ranges for slow and fast mode */ - if(!(l2capinbuf[18] & 0x02)) // Check if fast more is used - yawGyroSpeed *= 4.545; - if(!(l2capinbuf[18] & 0x01)) // Check if fast more is used - pitchGyroSpeed *= 4.545; - if(!(l2capinbuf[19] & 0x02)) // Check if fast more is used - rollGyroSpeed *= 4.545; - - pitch = (0.93*(pitch+(pitchGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiimotePitch); // Use a complimentary filter to calculate the angle - roll = (0.93*(roll+(rollGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiimoteRoll); - - gyroYaw += (yawGyroSpeed*((double)(micros()-timer)/1000000)); - gyroRoll += (rollGyroSpeed*((double)(micros()-timer)/1000000)); - gyroPitch += (pitchGyroSpeed*((double)(micros()-timer)/1000000)); - timer = micros(); - /* - // Uncomment these lines to tune the gyro scale variabels - Serial.print("\r\ngyroYaw: "); - Serial.print(gyroYaw); - Serial.print("\tgyroRoll: "); - Serial.print(gyroRoll); - Serial.print("\tgyroPitch: "); - Serial.print(gyroPitch); - */ - /* - Serial.print("\twiimoteRoll: "); - Serial.print(wiimoteRoll); - Serial.print("\twiimotePitch: "); - Serial.print(wiimotePitch); - */ - } else { - if((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values -#ifdef DEBUG - Notify(PSTR("\r\nThe gyro values has been reset"), 0x80); -#endif - gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)); - gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)); - gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)); - - rollGyroScale = 500; // You might need to adjust these - pitchGyroScale = 400; - yawGyroScale = 415; - - gyroYaw = 0; - gyroRoll = 0; - gyroPitch = 0; - - motionValuesReset = true; - timer = micros(); + } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) { + if (((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success + if (l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) { // Success + //Serial.print("\r\nHID Control Connection Complete"); + identifier = l2capinbuf[9]; + control_scid[0] = l2capinbuf[12]; + control_scid[1] = l2capinbuf[13]; + l2cap_event_flag |= L2CAP_FLAG_CONTROL_CONNECTED; + } else if (l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) { + //Serial.print("\r\nHID Interrupt Connection Complete"); + identifier = l2capinbuf[9]; + interrupt_scid[0] = l2capinbuf[12]; + interrupt_scid[1] = l2capinbuf[13]; + l2cap_event_flag |= L2CAP_FLAG_INTERRUPT_CONNECTED; } - } - } else { - if(nunchuckConnected) { - hatValues[HatX] = l2capinbuf[15]; - hatValues[HatY] = l2capinbuf[16]; - accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3))-416; - accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4))-416; - accZ = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5))-416; - nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; - nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; - } - //else if(classicControllerConnected) { } } - if(l2capinbuf[19] & 0x01) { - if(!extensionConnected) { - extensionConnected = true; - unknownExtensionConnected = true; -#ifdef DEBUG - Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80); + } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { +#ifdef EXTRADEBUG + Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80); + PrintHex (l2capinbuf[13], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[12], 0x80); + Notify(PSTR(" SCID: "), 0x80); + PrintHex (l2capinbuf[15], 0x80); + Notify(PSTR(" "), 0x80); + PrintHex (l2capinbuf[14], 0x80); + Notify(PSTR(" Identifier: "), 0x80); + PrintHex (l2capinbuf[9], 0x80); #endif - } + if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) { + identifier = l2capinbuf[9]; + control_scid[0] = l2capinbuf[14]; + control_scid[1] = l2capinbuf[15]; + l2cap_event_flag |= L2CAP_FLAG_CONNECTION_CONTROL_REQUEST; + } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) { + identifier = l2capinbuf[9]; + interrupt_scid[0] = l2capinbuf[14]; + interrupt_scid[1] = l2capinbuf[15]; + l2cap_event_flag |= L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST; } - else { - if(extensionConnected && !unknownExtensionConnected) { - extensionConnected = false; - unknownExtensionConnected = true; + } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) { + if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success + if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { + //Serial.print("\r\nHID Control Configuration Complete"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_SUCCESS; + } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { + //Serial.print("\r\nHID Interrupt Configuration Complete"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS; + } + } + } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) { + if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { + //Serial.print("\r\nHID Control Configuration Request"); + pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid); + } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { + //Serial.print("\r\nHID Interrupt Configuration Request"); + pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid); + } + } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { + if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) { #ifdef DEBUG - Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80); + Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80); #endif - nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent - } + identifier = l2capinbuf[9]; + pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid); + Reset(); + } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) { +#ifdef DEBUG + Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80); +#endif + identifier = l2capinbuf[9]; + pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid); + Reset(); } + } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) { + if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) { + //Serial.print("\r\nDisconnect Response: Control Channel"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE; + } else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) { + //Serial.print("\r\nDisconnect Response: Interrupt Channel"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE; + } + } +#ifdef EXTRADEBUG + else { + identifier = l2capinbuf[9]; + Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80); + PrintHex (l2capinbuf[8], 0x80); + } +#endif + } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt + //Serial.print("\r\nL2CAP Interrupt"); + if (wiimoteConnected) { + if (l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT + if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons + if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes + ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); + else if (wiiUProControllerConnected) + ButtonState = (uint32_t)(((~l2capinbuf[23]) & 0xFE) | ((uint16_t)(~l2capinbuf[24]) << 8) | ((uint32_t)((~l2capinbuf[25]) & 0x03) << 16)); + else if (motionPlusConnected) { + if (l2capinbuf[20] & 0x02) // Only update the wiimote buttons, since the extension bytes are from the Motion Plus + ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000))); + else if (nunchuckConnected) // Update if it's a report from the Nunchuck + ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x0C) << 14)); + //else if(classicControllerConnected) // Update if it's a report from the Classic Controller + } else if (nunchuckConnected) // The Nunchuck is directly connected + ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x03) << 16)); + //else if(classicControllerConnected) // The Classic Controller is directly connected + else if (!unknownExtensionConnected) + ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); +#ifdef PRINTREPORT + Notify(PSTR("ButtonState: "), 0x80); + PrintHex (ButtonState, 0x80); + Notify(PSTR("\r\n"), 0x80); +#endif + if (ButtonState != OldButtonState) { + ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable + OldButtonState = ButtonState; + } + } + if (l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer + accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5)) - 500; + accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500; + accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500; + wiimotePitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG; + wiimoteRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG; + } + switch (l2capinbuf[9]) { + case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV + wiiState = l2capinbuf[12]; // (0x01: Battery is nearly empty), (0x02: An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4) + batteryLevel = l2capinbuf[15]; // Update battery level + if (l2capinbuf[12] & 0x01) { +#ifdef DEBUG + Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80); +#endif + } + if (l2capinbuf[12] & 0x02) { // Check if a extension is connected +#ifdef DEBUG + if (!unknownExtensionConnected) + Notify(PSTR("\r\nExtension connected"), 0x80); +#endif + unknownExtensionConnected = true; +#ifdef WIICAMERA + if (!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera +#endif + setReportMode(false, 0x35); // Also read the extension + } else { +#ifdef DEBUG + Notify(PSTR("\r\nExtension disconnected"), 0x80); +#endif + if (motionPlusConnected) { +#ifdef DEBUG + Notify(PSTR(" - from Motion Plus"), 0x80); +#endif + l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED; + if (!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false + nunchuckConnected = false; + //else if(classicControllerConnected) + } else if (nunchuckConnected) { +#ifdef DEBUG + Notify(PSTR(" - Nunchuck"), 0x80); +#endif + nunchuckConnected = false; // It must be the Nunchuck controller then + l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED; + setLedStatus(); + setReportMode(false, 0x31); // If there is no extension connected we will read the button and accelerometer + } else { + setReportMode(false, 0x31); // If there is no extension connected we will read the button and accelerometer + } + } + break; + case 0x21: // Read Memory Data + if ((l2capinbuf[12] & 0x0F) == 0) { // No error + // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers + if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { +#ifdef DEBUG + Notify(PSTR("\r\nNunchuck connected"), 0x80); +#endif + l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED; + } else if (l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) { +#ifdef DEBUG + Notify(PSTR("\r\nMotion Plus connected"), 0x80); +#endif + l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED; + } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) { +#ifdef DEBUG + Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80); +#endif + motionPlusConnected = true; + } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) { +#ifdef DEBUG + Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80); +#endif + activateNunchuck = false; + motionPlusConnected = true; + nunchuckConnected = true; + } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) { +#ifdef DEBUG + Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80); + Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80); +#endif + stateCounter = 300; // Skip the rest in "L2CAP_CHECK_MOTION_PLUS_STATE" + } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) { +#ifdef DEBUG + Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80); +#endif + wiiUProControllerConnected = true; + } +#ifdef DEBUG + else { + Notify(PSTR("\r\nUnknown Device: "), 0x80); + PrintHex (l2capinbuf[13], 0x80); + PrintHex (l2capinbuf[14], 0x80); + Notify(PSTR("\r\nData: "), 0x80); + for (uint8_t i = 0; i < ((l2capinbuf[12] >> 4) + 1); i++) { // bit 4-7 is the length-1 + PrintHex (l2capinbuf[15 + i], 0x80); + Notify(PSTR(" "), 0x80); + } + } +#endif + } +#ifdef EXTRADEBUG + else { + Notify(PSTR("\r\nReport Error: "), 0x80); + PrintHex (l2capinbuf[13], 0x80); + PrintHex (l2capinbuf[14], 0x80); + } +#endif + break; + case 0x22: // Acknowledge output report, return function result +#ifdef DEBUG + if (l2capinbuf[13] != 0x00) { // Check if there is an error + Notify(PSTR("\r\nCommand failed: "), 0x80); + PrintHex (l2capinbuf[12], 0x80); + } +#endif + break; + case 0x30: // Core buttons - (a1) 30 BB BB + break; + case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA + pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected + roll = wiimoteRoll; + break; + case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE + break; + case 0x33: // Core Buttons with Accelerometer and 12 IR bytes - (a1) 33 BB BB AA AA AA II II II II II II II II II II II II + pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus data available + roll = wiimoteRoll; +#ifdef WIICAMERA + // Read the IR data + IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position + IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position + IR_object_s1 = (l2capinbuf[17] & 0x0F); // size value, 0-15 - } else if(nunchuckConnected) { - hatValues[HatX] = l2capinbuf[15]; - hatValues[HatY] = l2capinbuf[16]; - accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2))-416; - accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4))-416; - accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6))-416; - nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; - nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; + IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4)); + IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2)); + IR_object_s2 = (l2capinbuf[20] & 0x0F); - pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected - roll = wiimoteRoll; - } else if(wiiUProControllerConnected) { - hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8); - hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8); - hatValues[LeftHatY] = (l2capinbuf[19] | l2capinbuf[20] << 8); - hatValues[RightHatY] = (l2capinbuf[21] | l2capinbuf[22] << 8); - } - break; -#ifdef DEBUG - default: - Notify(PSTR("\r\nUnknown Report type: "), 0x80); - Serial.print(l2capinbuf[9],HEX); - break; + IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4)); + IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2)); + IR_object_s3 = (l2capinbuf[23] & 0x0F); + + IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4)); + IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2)); + IR_object_s4 = (l2capinbuf[26] & 0x0F); #endif - } + break; + case 0x34: // Core Buttons with 19 Extension bytes - (a1) 34 BB BB EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE + break; + /* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */ + case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes + // (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II + // corresponds to output report mode 0x3e + + /**** for reading in full mode: DOES NOT WORK YET ****/ + /* When it works it will also have intensity and bounding box data */ + /* + IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4)); + IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2)); + IR_object_s1 = (l2capinbuf[15] & 0x0F); + */ + break; + case 0x3F: + /* + IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4)); + IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2)); + IR_object_s1 = (l2capinbuf[15] & 0x0F); + */ + break; + case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes + // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE + if (motionPlusConnected) { + if (l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension + if (motionValuesReset) { // We will only use the values when the gyro value has been set + gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)) - gyroYawZero); + gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero); + gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero); + + yawGyroSpeed = (double)gyroYawRaw / ((double)gyroYawZero / yawGyroScale); + rollGyroSpeed = -(double)gyroRollRaw / ((double)gyroRollZero / rollGyroScale); // We invert these values so they will fit the acc values + pitchGyroSpeed = (double)gyroPitchRaw / ((double)gyroPitchZero / pitchGyroScale); + + /* The onboard gyro has two ranges for slow and fast mode */ + if (!(l2capinbuf[18] & 0x02)) // Check if fast more is used + yawGyroSpeed *= 4.545; + if (!(l2capinbuf[18] & 0x01)) // Check if fast more is used + pitchGyroSpeed *= 4.545; + if (!(l2capinbuf[19] & 0x02)) // Check if fast more is used + rollGyroSpeed *= 4.545; + + pitch = (0.93 * (pitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimotePitch); // Use a complimentary filter to calculate the angle + roll = (0.93 * (roll + (rollGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * wiimoteRoll); + + gyroYaw += (yawGyroSpeed * ((double)(micros() - timer) / 1000000)); + gyroRoll += (rollGyroSpeed * ((double)(micros() - timer) / 1000000)); + gyroPitch += (pitchGyroSpeed * ((double)(micros() - timer) / 1000000)); + timer = micros(); + /* + // Uncomment these lines to tune the gyro scale variabels + Serial.print("\r\ngyroYaw: "); + Serial.print(gyroYaw); + Serial.print("\tgyroRoll: "); + Serial.print(gyroRoll); + Serial.print("\tgyroPitch: "); + Serial.print(gyroPitch); + */ + /* + Serial.print("\twiimoteRoll: "); + Serial.print(wiimoteRoll); + Serial.print("\twiimotePitch: "); + Serial.print(wiimotePitch); + */ + } else { + if ((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values +#ifdef DEBUG + Notify(PSTR("\r\nThe gyro values has been reset"), 0x80); +#endif + gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)); + gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)); + gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)); + + rollGyroScale = 500; // You might need to adjust these + pitchGyroScale = 400; + yawGyroScale = 415; + + gyroYaw = 0; + gyroRoll = 0; + gyroPitch = 0; + + motionValuesReset = true; + timer = micros(); + } + } + } else { + if (nunchuckConnected) { + hatValues[HatX] = l2capinbuf[15]; + hatValues[HatY] = l2capinbuf[16]; + accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416; + accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4)) - 416; + accZ = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5)) - 416; + nunchuckPitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG; + nunchuckRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG; + } + //else if(classicControllerConnected) { } + } + if (l2capinbuf[19] & 0x01) { + if (!extensionConnected) { + extensionConnected = true; + unknownExtensionConnected = true; +#ifdef DEBUG + Notify(PSTR("\r\nExtension connected to Motion Plus"), 0x80); +#endif + } + } else { + if (extensionConnected && !unknownExtensionConnected) { + extensionConnected = false; + unknownExtensionConnected = true; +#ifdef DEBUG + Notify(PSTR("\r\nExtension disconnected from Motion Plus"), 0x80); +#endif + nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent + } + } + + } else if (nunchuckConnected) { + hatValues[HatX] = l2capinbuf[15]; + hatValues[HatY] = l2capinbuf[16]; + accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416; + accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416; + accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416; + nunchuckPitch = (atan2(accY, accZ) + PI) * RAD_TO_DEG; + nunchuckRoll = (atan2(accX, accZ) + PI) * RAD_TO_DEG; + + pitch = wiimotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected + roll = wiimoteRoll; + } else if (wiiUProControllerConnected) { + hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8); + hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8); + hatValues[LeftHatY] = (l2capinbuf[19] | l2capinbuf[20] << 8); + hatValues[RightHatY] = (l2capinbuf[21] | l2capinbuf[22] << 8); + } + break; +#ifdef DEBUG + default: + Notify(PSTR("\r\nUnknown Report type: "), 0x80); + Serial.print(l2capinbuf[9], HEX); + break; +#endif + } + } + } } - } + L2CAP_task(); } - L2CAP_task(); - } } + void WII::L2CAP_task() { - switch (l2cap_state) { - /* These states are used if the Wiimote is the host */ - case L2CAP_CONTROL_SUCCESS: - if (l2cap_config_success_control_flag) { + switch (l2cap_state) { + /* These states are used if the Wiimote is the host */ + case L2CAP_CONTROL_SUCCESS: + if (l2cap_config_success_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80); + Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80); #endif - l2cap_state = L2CAP_INTERRUPT_SETUP; - } - break; + l2cap_state = L2CAP_INTERRUPT_SETUP; + } + break; - case L2CAP_INTERRUPT_SETUP: - if (l2cap_connection_request_interrupt_flag) { + case L2CAP_INTERRUPT_SETUP: + if (l2cap_connection_request_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80); + Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80); #endif - pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, PENDING); - delay(1); - pBtd->l2cap_connection_response(hci_handle,identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL); - identifier++; - delay(1); - pBtd->l2cap_config_request(hci_handle,identifier, interrupt_scid); + pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, PENDING); + delay(1); + pBtd->l2cap_connection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid, SUCCESSFUL); + identifier++; + delay(1); + pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid); - l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST; - } - break; + l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST; + } + break; - /* These states are used if the Arduino is the host */ - case L2CAP_CONTROL_CONNECT_REQUEST: - if (l2cap_connected_control_flag) { + /* These states are used if the Arduino is the host */ + case L2CAP_CONTROL_CONNECT_REQUEST: + if (l2cap_connected_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nSend HID Control Config Request"), 0x80); + Notify(PSTR("\r\nSend HID Control Config Request"), 0x80); #endif - identifier++; - pBtd->l2cap_config_request(hci_handle, identifier, control_scid); - l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST; - } - break; + identifier++; + pBtd->l2cap_config_request(hci_handle, identifier, control_scid); + l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST; + } + break; - case L2CAP_CONTROL_CONFIG_REQUEST: - if(l2cap_config_success_control_flag) { + case L2CAP_CONTROL_CONFIG_REQUEST: + if (l2cap_config_success_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80); + Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80); #endif - identifier++; - pBtd->l2cap_connection_request(hci_handle,identifier,interrupt_dcid,HID_INTR_PSM); - l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST; - } - break; + identifier++; + pBtd->l2cap_connection_request(hci_handle, identifier, interrupt_dcid, HID_INTR_PSM); + l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST; + } + break; - case L2CAP_INTERRUPT_CONNECT_REQUEST: - if(l2cap_connected_interrupt_flag) { + case L2CAP_INTERRUPT_CONNECT_REQUEST: + if (l2cap_connected_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80); + Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80); #endif - identifier++; - pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid); - l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST; - } - break; + identifier++; + pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid); + l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST; + } + break; - case L2CAP_INTERRUPT_CONFIG_REQUEST: - if(l2cap_config_success_interrupt_flag) { // Now the HID channels is established + case L2CAP_INTERRUPT_CONFIG_REQUEST: + if (l2cap_config_success_interrupt_flag) { // Now the HID channels is established #ifdef DEBUG - Notify(PSTR("\r\nHID Channels Established"), 0x80); + Notify(PSTR("\r\nHID Channels Established"), 0x80); #endif - pBtd->connectToWii = false; - pBtd->pairWithWii = false; - wiimoteConnected = true; - stateCounter = 0; - l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE; - } - break; + pBtd->connectToWii = false; + pBtd->pairWithWii = false; + wiimoteConnected = true; + stateCounter = 0; + l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE; + } + break; - /* The next states are in run() */ + /* The next states are in run() */ - case L2CAP_INTERRUPT_DISCONNECT: - if (l2cap_disconnect_response_interrupt_flag) { + case L2CAP_INTERRUPT_DISCONNECT: + if (l2cap_disconnect_response_interrupt_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80); + Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80); #endif - identifier++; - pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid); - l2cap_state = L2CAP_CONTROL_DISCONNECT; - } - break; + identifier++; + pBtd->l2cap_disconnection_request(hci_handle, identifier, control_scid, control_dcid); + l2cap_state = L2CAP_CONTROL_DISCONNECT; + } + break; - case L2CAP_CONTROL_DISCONNECT: - if (l2cap_disconnect_response_control_flag) { + case L2CAP_CONTROL_DISCONNECT: + if (l2cap_disconnect_response_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nDisconnected Control Channel"), 0x80); + Notify(PSTR("\r\nDisconnected Control Channel"), 0x80); #endif - pBtd->hci_disconnect(hci_handle); - hci_handle = -1; // Reset handle - l2cap_event_flag = 0; // Reset flags - l2cap_state = L2CAP_WAIT; - } - break; - } + pBtd->hci_disconnect(hci_handle); + hci_handle = -1; // Reset handle + l2cap_event_flag = 0; // Reset flags + l2cap_state = L2CAP_WAIT; + } + break; + } } + void WII::Run() { - switch (l2cap_state) { - case L2CAP_WAIT: - if(pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) { - pBtd->l2capConnectionClaimed = true; - activeConnection = true; + switch (l2cap_state) { + case L2CAP_WAIT: + if (pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) { + pBtd->l2capConnectionClaimed = true; + activeConnection = true; #ifdef DEBUG - Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80); + Notify(PSTR("\r\nSend HID Control Connection Request"), 0x80); #endif - hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection - l2cap_event_flag = 0; // Reset flags - identifier = 0; - pBtd->l2cap_connection_request(hci_handle,identifier,control_dcid,HID_CTRL_PSM); - l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST; - } else if (l2cap_connection_request_control_flag) { + hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection + l2cap_event_flag = 0; // Reset flags + identifier = 0; + pBtd->l2cap_connection_request(hci_handle, identifier, control_dcid, HID_CTRL_PSM); + l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST; + } else if (l2cap_connection_request_control_flag) { #ifdef DEBUG - Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80); + Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80); #endif - pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, PENDING); - delay(1); - pBtd->l2cap_connection_response(hci_handle,identifier, control_dcid, control_scid, SUCCESSFUL); - identifier++; - delay(1); - pBtd->l2cap_config_request(hci_handle,identifier, control_scid); - l2cap_state = L2CAP_CONTROL_SUCCESS; - } - break; + pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, PENDING); + delay(1); + pBtd->l2cap_connection_response(hci_handle, identifier, control_dcid, control_scid, SUCCESSFUL); + identifier++; + delay(1); + pBtd->l2cap_config_request(hci_handle, identifier, control_scid); + l2cap_state = L2CAP_CONTROL_SUCCESS; + } + break; - case L2CAP_CHECK_MOTION_PLUS_STATE: + case L2CAP_CHECK_MOTION_PLUS_STATE: #ifdef DEBUG - if(stateCounter == 0) // Only print onnce - Notify(PSTR("\r\nChecking if a Motion Plus is connected"), 0x80); + if (stateCounter == 0) // Only print onnce + Notify(PSTR("\r\nChecking if a Motion Plus is connected"), 0x80); #endif - stateCounter++; - if(stateCounter%200 == 0) - checkMotionPresent(); // Check if there is a motion plus connected - if(motion_plus_connected_flag) { - stateCounter = 0; - l2cap_state = L2CAP_INIT_MOTION_PLUS_STATE; - timer = micros(); + stateCounter++; + if (stateCounter % 200 == 0) + checkMotionPresent(); // Check if there is a motion plus connected + if (motion_plus_connected_flag) { + stateCounter = 0; + l2cap_state = L2CAP_INIT_MOTION_PLUS_STATE; + timer = micros(); - if(unknownExtensionConnected) { + if (unknownExtensionConnected) { #ifdef DEBUG - Notify(PSTR("\r\nA extension is also connected"), 0x80); + Notify(PSTR("\r\nA extension is also connected"), 0x80); #endif - activateNunchuck = true; // For we will just set this to true as this the only extension supported so far - } + activateNunchuck = true; // For we will just set this to true as this the only extension supported so far + } - } - else if(stateCounter == 601) { // We will try three times to check for the motion plus + } else if (stateCounter == 601) { // We will try three times to check for the motion plus #ifdef DEBUG - Notify(PSTR("\r\nNo Motion Plus was detected"), 0x80); + Notify(PSTR("\r\nNo Motion Plus was detected"), 0x80); #endif - stateCounter = 0; - l2cap_state = L2CAP_CHECK_EXTENSION_STATE; - } - break; + stateCounter = 0; + l2cap_state = L2CAP_CHECK_EXTENSION_STATE; + } + break; - case L2CAP_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port + case L2CAP_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port #ifdef DEBUG - if(stateCounter == 0) // Only print onnce - Notify(PSTR("\r\nChecking if there is any extension connected"), 0x80); + if (stateCounter == 0) // Only print onnce + Notify(PSTR("\r\nChecking if there is any extension connected"), 0x80); #endif - stateCounter++; // We use this counter as there has to be a short delay between the commands - if(stateCounter == 1) - statusRequest(); // See if a new device has connected - if(stateCounter == 100) { - if(unknownExtensionConnected) // Check if there is a extension is connected to the port - initExtension1(); - else - stateCounter = 399; - } else if(stateCounter == 200) - initExtension2(); - else if(stateCounter == 300) { - readExtensionType(); - unknownExtensionConnected = false; - } else if(stateCounter == 400) { - stateCounter = 0; - l2cap_state = L2CAP_LED_STATE; - } - break; + stateCounter++; // We use this counter as there has to be a short delay between the commands + if (stateCounter == 1) + statusRequest(); // See if a new device has connected + if (stateCounter == 100) { + if (unknownExtensionConnected) // Check if there is a extension is connected to the port + initExtension1(); + else + stateCounter = 399; + } else if (stateCounter == 200) + initExtension2(); + else if (stateCounter == 300) { + readExtensionType(); + unknownExtensionConnected = false; + } else if (stateCounter == 400) { + stateCounter = 0; + l2cap_state = L2CAP_LED_STATE; + } + break; - case L2CAP_INIT_MOTION_PLUS_STATE: - stateCounter++; - if(stateCounter == 1) - initMotionPlus(); - else if(stateCounter == 100) - activateMotionPlus(); - else if(stateCounter == 200) - readExtensionType(); // Check if it has been activated - else if(stateCounter == 300) { - stateCounter = 0; - unknownExtensionConnected = false; // The motion plus will send a status report when it's activated, we will set this to false so it doesn't reinitialize the Motion Plus - l2cap_state = L2CAP_LED_STATE; - } - break; + case L2CAP_INIT_MOTION_PLUS_STATE: + stateCounter++; + if (stateCounter == 1) + initMotionPlus(); + else if (stateCounter == 100) + activateMotionPlus(); + else if (stateCounter == 200) + readExtensionType(); // Check if it has been activated + else if (stateCounter == 300) { + stateCounter = 0; + unknownExtensionConnected = false; // The motion plus will send a status report when it's activated, we will set this to false so it doesn't reinitialize the Motion Plus + l2cap_state = L2CAP_LED_STATE; + } + break; - case L2CAP_LED_STATE: - if(nunchuck_connected_flag) - nunchuckConnected = true; - setLedStatus(); - l2cap_state = L2CAP_DONE; - break; + case L2CAP_LED_STATE: + if (nunchuck_connected_flag) + nunchuckConnected = true; + setLedStatus(); + l2cap_state = L2CAP_DONE; + break; - case L2CAP_DONE: - if(unknownExtensionConnected) { + case L2CAP_DONE: + if (unknownExtensionConnected) { #ifdef DEBUG - if(stateCounter == 0) // Only print once - Notify(PSTR("\r\nChecking extension port"), 0x80); + if (stateCounter == 0) // Only print once + Notify(PSTR("\r\nChecking extension port"), 0x80); #endif - stateCounter++; // We will use this counter as there has to be a short delay between the commands - if(stateCounter == 50) - statusRequest(); - else if(stateCounter == 100) - initExtension1(); - else if(stateCounter == 150) - if((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected)) - initExtension2(); - else - stateCounter = 299; // There is no extension connected - else if(stateCounter == 200) - readExtensionType(); - else if(stateCounter == 250) { - if(nunchuck_connected_flag) { + stateCounter++; // We will use this counter as there has to be a short delay between the commands + if (stateCounter == 50) + statusRequest(); + else if (stateCounter == 100) + initExtension1(); + else if (stateCounter == 150) + if ((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected)) + initExtension2(); + else + stateCounter = 299; // There is no extension connected + else if (stateCounter == 200) + readExtensionType(); + else if (stateCounter == 250) { + if (nunchuck_connected_flag) { #ifdef DEBUG - Notify(PSTR("\r\nNunchuck was reconnected"), 0x80); + Notify(PSTR("\r\nNunchuck was reconnected"), 0x80); #endif - activateNunchuck = true; - nunchuckConnected = true; - } - if(!motionPlusConnected) - stateCounter = 449; - } - else if (stateCounter == 300) { - if(motionPlusConnected) { + activateNunchuck = true; + nunchuckConnected = true; + } + if (!motionPlusConnected) + stateCounter = 449; + } else if (stateCounter == 300) { + if (motionPlusConnected) { #ifdef DEBUG - Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80); + Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80); #endif - initMotionPlus(); - } else - stateCounter = 449; - } - else if(stateCounter == 350) - activateMotionPlus(); - else if(stateCounter == 400) - readExtensionType(); // Check if it has been activated - else if(stateCounter == 450) { - setLedStatus(); - stateCounter = 0; - unknownExtensionConnected = false; - } - } else - stateCounter = 0; - break; - } + initMotionPlus(); + } else + stateCounter = 449; + } else if (stateCounter == 350) + activateMotionPlus(); + else if (stateCounter == 400) + readExtensionType(); // Check if it has been activated + else if (stateCounter == 450) { + setLedStatus(); + stateCounter = 0; + unknownExtensionConnected = false; + } + } else + stateCounter = 0; + break; + } } /************************************************************/ /* HID Commands */ + /************************************************************/ void WII::HID_Command(uint8_t* data, uint8_t nbytes) { - if(pBtd->motionPlusInside) - pBtd->L2CAP_Command(hci_handle,data,nbytes,interrupt_scid[0],interrupt_scid[1]); // It's the new wiimote with the Motion Plus Inside - else - pBtd->L2CAP_Command(hci_handle,data,nbytes,control_scid[0],control_scid[1]); + if (pBtd->motionPlusInside) + pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // It's the new wiimote with the Motion Plus Inside + else + pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]); } -void WII::setAllOff() { - HIDBuffer[1] = 0x11; - HIDBuffer[2] = 0x00; - HID_Command(HIDBuffer, 3); -} -void WII::setRumbleOff() { - HIDBuffer[1] = 0x11; - HIDBuffer[2] &= ~0x01; // Bit 0 control the rumble - HID_Command(HIDBuffer, 3); -} -void WII::setRumbleOn() { - HIDBuffer[1] = 0x11; - HIDBuffer[2] |= 0x01; // Bit 0 control the rumble - HID_Command(HIDBuffer, 3); -} -void WII::setRumbleToggle() { - HIDBuffer[1] = 0x11; - HIDBuffer[2] ^= 0x01; // Bit 0 control the rumble - HID_Command(HIDBuffer, 3); -} -void WII::setLedOff(LED a) { - HIDBuffer[1] = 0x11; - HIDBuffer[2] &= ~(pgm_read_byte(&LEDS[(uint8_t)a])); - HID_Command(HIDBuffer, 3); -} -void WII::setLedOn(LED a) { - HIDBuffer[1] = 0x11; - HIDBuffer[2] |= pgm_read_byte(&LEDS[(uint8_t)a]); - HID_Command(HIDBuffer, 3); -} -void WII::setLedToggle(LED a) { - HIDBuffer[1] = 0x11; - HIDBuffer[2] ^= pgm_read_byte(&LEDS[(uint8_t)a]); - HID_Command(HIDBuffer, 3); -} -void WII::setLedStatus() { - HIDBuffer[1] = 0x11; - HIDBuffer[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit - if(wiimoteConnected) - HIDBuffer[2] |= 0x10; // If it's connected LED1 will light up - if(motionPlusConnected) - HIDBuffer[2] |= 0x20; // If it's connected LED2 will light up - if(nunchuckConnected) - HIDBuffer[2] |= 0x40; // If it's connected LED3 will light up - HID_Command(HIDBuffer, 3); +void WII::setAllOff() { + HIDBuffer[1] = 0x11; + HIDBuffer[2] = 0x00; + HID_Command(HIDBuffer, 3); } + +void WII::setRumbleOff() { + HIDBuffer[1] = 0x11; + HIDBuffer[2] &= ~0x01; // Bit 0 control the rumble + HID_Command(HIDBuffer, 3); +} + +void WII::setRumbleOn() { + HIDBuffer[1] = 0x11; + HIDBuffer[2] |= 0x01; // Bit 0 control the rumble + HID_Command(HIDBuffer, 3); +} + +void WII::setRumbleToggle() { + HIDBuffer[1] = 0x11; + HIDBuffer[2] ^= 0x01; // Bit 0 control the rumble + HID_Command(HIDBuffer, 3); +} + +void WII::setLedOff(LED a) { + HIDBuffer[1] = 0x11; + HIDBuffer[2] &= ~(pgm_read_byte(&LEDS[(uint8_t)a])); + HID_Command(HIDBuffer, 3); +} + +void WII::setLedOn(LED a) { + HIDBuffer[1] = 0x11; + HIDBuffer[2] |= pgm_read_byte(&LEDS[(uint8_t)a]); + HID_Command(HIDBuffer, 3); +} + +void WII::setLedToggle(LED a) { + HIDBuffer[1] = 0x11; + HIDBuffer[2] ^= pgm_read_byte(&LEDS[(uint8_t)a]); + HID_Command(HIDBuffer, 3); +} + +void WII::setLedStatus() { + HIDBuffer[1] = 0x11; + HIDBuffer[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit + if (wiimoteConnected) + HIDBuffer[2] |= 0x10; // If it's connected LED1 will light up + if (motionPlusConnected) + HIDBuffer[2] |= 0x20; // If it's connected LED2 will light up + if (nunchuckConnected) + HIDBuffer[2] |= 0x40; // If it's connected LED3 will light up + + HID_Command(HIDBuffer, 3); +} + void WII::setReportMode(bool continuous, uint8_t mode) { - uint8_t cmd_buf[4]; - cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) - cmd_buf[1] = 0x12; - if(continuous) - cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit - else - cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Keep the rumble bit - cmd_buf[3] = mode; - HID_Command(cmd_buf, 4); + uint8_t cmd_buf[4]; + cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) + cmd_buf[1] = 0x12; + if (continuous) + cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit + else + cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Keep the rumble bit + cmd_buf[3] = mode; + HID_Command(cmd_buf, 4); } + void WII::statusRequest() { - uint8_t cmd_buf[3]; - cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) - cmd_buf[1] = 0x15; - cmd_buf[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit - HID_Command(cmd_buf, 3); + uint8_t cmd_buf[3]; + cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) + cmd_buf[1] = 0x15; + cmd_buf[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit + HID_Command(cmd_buf, 3); } /************************************************************/ /* Memmory Commands */ + /************************************************************/ void WII::writeData(uint32_t offset, uint8_t size, uint8_t* data) { - uint8_t cmd_buf[23]; - cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) - cmd_buf[1] = 0x16; // Write data - cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Write to memory, clear bit 2 to write to EEPROM - cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16); - cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8); - cmd_buf[5] = (uint8_t)(offset & 0xFF); - cmd_buf[6] = size; - uint8_t i = 0; - for(; i < size; i++) - cmd_buf[7+i] = data[i]; - for(; i < 16; i++) // Set the rest to zero - cmd_buf[7+i] = 0x00; - HID_Command(cmd_buf,23); + uint8_t cmd_buf[23]; + cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) + cmd_buf[1] = 0x16; // Write data + cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Write to memory, clear bit 2 to write to EEPROM + cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16); + cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8); + cmd_buf[5] = (uint8_t)(offset & 0xFF); + cmd_buf[6] = size; + uint8_t i = 0; + for (; i < size; i++) + cmd_buf[7 + i] = data[i]; + for (; i < 16; i++) // Set the rest to zero + cmd_buf[7 + i] = 0x00; + HID_Command(cmd_buf, 23); } -void WII::initExtension1() { - uint8_t buf[1]; - buf[0] = 0x55; - writeData(0xA400F0,1,buf); -} -void WII::initExtension2() { - uint8_t buf[1]; - buf[0] = 0x00; - writeData(0xA400FB,1,buf); -} -void WII::initMotionPlus() { - uint8_t buf[1]; - buf[0] = 0x55; - writeData(0xA600F0,1,buf); -} -void WII::activateMotionPlus() { - uint8_t buf[1]; - if(pBtd->wiiUProController) { -#ifdef DEBUG - Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80); -#endif - buf[0] = 0x00; // It seems like you can send anything but 0x04, 0x05, and 0x07 - } else if(activateNunchuck) { -#ifdef DEBUG - Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"), 0x80); -#endif - buf[0] = 0x05; // Activate nunchuck pass-through mode - } - //else if(classicControllerConnected && extensionConnected) - //buf[0] = 0x07; - else { -#ifdef DEBUG - Notify(PSTR("\r\nActivating Motion Plus in normal mode"), 0x80); -#endif - buf[0] = 0x04; // Don't use any extension - } - writeData(0xA600FE,1,buf); -} -void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) { - uint8_t cmd_buf[8]; - cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) - cmd_buf[1] = 0x17; // Read data - if(EEPROM) - cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Read from EEPROM - else - cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory - cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16); - cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8); - cmd_buf[5] = (uint8_t)(offset & 0xFF); - cmd_buf[6] = (uint8_t)((size & 0xFF00) >> 8); - cmd_buf[7] = (uint8_t)(size & 0xFF); - HID_Command(cmd_buf,8); +void WII::initExtension1() { + uint8_t buf[1]; + buf[0] = 0x55; + writeData(0xA400F0, 1, buf); } + +void WII::initExtension2() { + uint8_t buf[1]; + buf[0] = 0x00; + writeData(0xA400FB, 1, buf); +} + +void WII::initMotionPlus() { + uint8_t buf[1]; + buf[0] = 0x55; + writeData(0xA600F0, 1, buf); +} + +void WII::activateMotionPlus() { + uint8_t buf[1]; + if (pBtd->wiiUProController) { +#ifdef DEBUG + Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80); +#endif + buf[0] = 0x00; // It seems like you can send anything but 0x04, 0x05, and 0x07 + } else if (activateNunchuck) { +#ifdef DEBUG + Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"), 0x80); +#endif + buf[0] = 0x05; // Activate nunchuck pass-through mode + } //else if(classicControllerConnected && extensionConnected) + //buf[0] = 0x07; + else { +#ifdef DEBUG + Notify(PSTR("\r\nActivating Motion Plus in normal mode"), 0x80); +#endif + buf[0] = 0x04; // Don't use any extension + } + writeData(0xA600FE, 1, buf); +} + +void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) { + uint8_t cmd_buf[8]; + cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) + cmd_buf[1] = 0x17; // Read data + if (EEPROM) + cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Read from EEPROM + else + cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory + cmd_buf[3] = (uint8_t)((offset & 0xFF0000) >> 16); + cmd_buf[4] = (uint8_t)((offset & 0xFF00) >> 8); + cmd_buf[5] = (uint8_t)(offset & 0xFF); + cmd_buf[6] = (uint8_t)((size & 0xFF00) >> 8); + cmd_buf[7] = (uint8_t)(size & 0xFF); + + HID_Command(cmd_buf, 8); +} + void WII::readExtensionType() { - readData(0xA400FA,6,false); + readData(0xA400FA, 6, false); } + void WII::readCalData() { - readData(0x0016,8,true); + readData(0x0016, 8, true); } + void WII::checkMotionPresent() { - readData(0xA600FA,6,false); + readData(0xA600FA, 6, false); } /************************************************************/ /* WII Commands */ + /************************************************************/ bool WII::getButtonPress(Button b) { // Return true when a button is pressed - if(wiiUProControllerConnected) - return (ButtonState & pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b])); - else - return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b])); + if (wiiUProControllerConnected) + return (ButtonState & pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b])); + else + return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b])); } + bool WII::getButtonClick(Button b) { // Only return true when a button is clicked - uint32_t button; - if(wiiUProControllerConnected) - button = pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b]); - else - button = pgm_read_dword(&BUTTONS[(uint8_t)b]); - bool click = (ButtonClickState & button); - ButtonClickState &= ~button; // clear "click" event - return click; + uint32_t button; + if (wiiUProControllerConnected) + button = pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b]); + else + button = pgm_read_dword(&BUTTONS[(uint8_t)b]); + bool click = (ButtonClickState & button); + ButtonClickState &= ~button; // clear "click" event + return click; } + uint8_t WII::getAnalogHat(Hat a) { - if(!nunchuckConnected) - return 127; // Return center position - else { - uint8_t output = hatValues[(uint8_t)a]; - if(output == 0xFF || output == 0x00) // The joystick will only read 255 or 0 when the cable is unplugged or initializing, so we will just return the center position - return 127; - else - return output; - } + if (!nunchuckConnected) + return 127; // Return center position + else { + uint8_t output = hatValues[(uint8_t)a]; + if (output == 0xFF || output == 0x00) // The joystick will only read 255 or 0 when the cable is unplugged or initializing, so we will just return the center position + return 127; + else + return output; + } } + uint16_t WII::getAnalogHat(AnalogHat a) { - if(!wiiUProControllerConnected) - return 2000; - else { - uint16_t output = hatValues[(uint8_t)a]; - if(output == 0x00) // The joystick will only read 0 when it is first initializing, so we will just return the center position - return 2000; - else - return output; - } + if (!wiiUProControllerConnected) + return 2000; + else { + uint16_t output = hatValues[(uint8_t)a]; + if (output == 0x00) // The joystick will only read 0 when it is first initializing, so we will just return the center position + return 2000; + else + return output; + } } /************************************************************/ /* The following functions are for the IR camera */ @@ -1056,7 +1063,7 @@ uint16_t WII::getAnalogHat(AnalogHat a) { #ifdef WIICAMERA -void WII::IRinitialize(){ // Turns on and initialises the IR camera +void WII::IRinitialize() { // Turns on and initialises the IR camera enableIRCamera1(); #ifdef DEBUG @@ -1092,7 +1099,7 @@ void WII::IRinitialize(){ // Turns on and initialises the IR camera setWiiModeNumber(mode_num); // Change input for whatever mode you want i.e. 0x01, 0x03, or 0x05 #ifdef DEBUG Notify(PSTR("\r\nSet Wii Mode Number To 0x"), 0x80); - PrintHex(mode_num, 0x80); + PrintHex (mode_num, 0x80); #endif delay(80); @@ -1115,51 +1122,51 @@ void WII::IRinitialize(){ // Turns on and initialises the IR camera #endif } -void WII::enableIRCamera1(){ - uint8_t cmd_buf[3]; - cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) - cmd_buf[1] = 0x13; // Output report 13 - cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 - HID_Command(cmd_buf, 3); +void WII::enableIRCamera1() { + uint8_t cmd_buf[3]; + cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) + cmd_buf[1] = 0x13; // Output report 13 + cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 + HID_Command(cmd_buf, 3); } -void WII::enableIRCamera2(){ - uint8_t cmd_buf[3]; - cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) - cmd_buf[1] = 0x1A; // Output report 1A - cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 - HID_Command(cmd_buf, 3); +void WII::enableIRCamera2() { + uint8_t cmd_buf[3]; + cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02) + cmd_buf[1] = 0x1A; // Output report 1A + cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 + HID_Command(cmd_buf, 3); } -void WII::writeSensitivityBlock1(){ - uint8_t buf[9]; - buf[0] = 0x00; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = 0x00; - buf[5] = 0x00; - buf[6] = 0x90; - buf[7] = 0x00; - buf[8] = 0x41; +void WII::writeSensitivityBlock1() { + uint8_t buf[9]; + buf[0] = 0x00; + buf[1] = 0x00; + buf[2] = 0x00; + buf[3] = 0x00; + buf[4] = 0x00; + buf[5] = 0x00; + buf[6] = 0x90; + buf[7] = 0x00; + buf[8] = 0x41; - writeData(0xB00000, 9, buf); + writeData(0xB00000, 9, buf); } -void WII::writeSensitivityBlock2(){ - uint8_t buf[2]; - buf[0] = 0x40; - buf[1] = 0x00; +void WII::writeSensitivityBlock2() { + uint8_t buf[2]; + buf[0] = 0x40; + buf[1] = 0x00; - writeData(0xB0001A, 2, buf); + writeData(0xB0001A, 2, buf); } -void WII::write0x08Value(){ - uint8_t cmd = 0x08; - writeData(0xb00030, 1, &cmd); +void WII::write0x08Value() { + uint8_t cmd = 0x08; + writeData(0xb00030, 1, &cmd); } -void WII::setWiiModeNumber(uint8_t mode_number){ // mode_number in hex i.e. 0x03 for extended mode - writeData(0xb00033,1,&mode_number); +void WII::setWiiModeNumber(uint8_t mode_number) { // mode_number in hex i.e. 0x03 for extended mode + writeData(0xb00033, 1, &mode_number); } #endif diff --git a/Wii.h b/Wii.h index d6540704..6ffe6803 100755 --- a/Wii.h +++ b/Wii.h @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -39,7 +39,7 @@ #define L2CAP_CONTROL_CONNECT_REQUEST 3 #define L2CAP_CONTROL_CONFIG_REQUEST 4 #define L2CAP_INTERRUPT_CONNECT_REQUEST 5 - + #define L2CAP_INTERRUPT_CONFIG_REQUEST 6 #define L2CAP_CHECK_MOTION_PLUS_STATE 7 @@ -82,10 +82,10 @@ /** Enum used to read the joystick on the Nunchuck. */ enum Hat { - /** Read the x-axis on the Nunchuck joystick. */ - HatX = 0, - /** Read the y-axis on the Nunchuck joystick. */ - HatY = 1, + /** Read the x-axis on the Nunchuck joystick. */ + HatX = 0, + /** Read the y-axis on the Nunchuck joystick. */ + HatY = 1, }; /** @@ -95,347 +95,394 @@ enum Hat { */ class WII : public BluetoothService { public: - /** - * Constructor for the WII class. - * @param p Pointer to BTD class instance. - * @param pair Set this to true in order to pair with the Wiimote. If the argument is omitted then it won't pair with it. - * One can use ::PAIR to set it to true. - */ - WII(BTD *p, bool pair=false); + /** + * Constructor for the WII class. + * @param p Pointer to BTD class instance. + * @param pair Set this to true in order to pair with the Wiimote. If the argument is omitted then it won't pair with it. + * One can use ::PAIR to set it to true. + */ + WII(BTD *p, bool pair = false); - /** @name BluetoothService implementation */ - /** - * Used to pass acldata to the services. - * @param ACLData Incoming acldata. - */ - virtual void ACLData(uint8_t* ACLData); - /** Used to run part of the state maschine. */ - virtual void Run(); - /** Use this to reset the service. */ - virtual void Reset(); - /** Used this to disconnect any of the controllers. */ - virtual void disconnect(); - /**@}*/ + /** @name BluetoothService implementation */ + /** + * Used to pass acldata to the services. + * @param ACLData Incoming acldata. + */ + virtual void ACLData(uint8_t* ACLData); + /** Used to run part of the state maschine. */ + virtual void Run(); + /** Use this to reset the service. */ + virtual void Reset(); + /** Used this to disconnect any of the controllers. */ + virtual void disconnect(); + /**@}*/ - /** @name Wii Controller functions */ - /** - * getButtonPress(Button b) will return true as long as the button is held down. - * - * While getButtonClick(Button b) will only return it once. - * - * So you instance if you need to increase a variable once you would use getButtonClick(Button b), - * but if you need to drive a robot forward you would use getButtonPress(Button b). - */ - bool getButtonPress(Button b); - bool getButtonClick(Button b); - /**@}*/ + /** @name Wii Controller functions */ + /** + * getButtonPress(Button b) will return true as long as the button is held down. + * + * While getButtonClick(Button b) will only return it once. + * + * So you instance if you need to increase a variable once you would use getButtonClick(Button b), + * but if you need to drive a robot forward you would use getButtonPress(Button b). + */ + bool getButtonPress(Button b); + bool getButtonClick(Button b); + /**@}*/ - /** @name Wii Controller functions */ - /** - * Used to read the joystick of the Nunchuck. - * @param a Either ::HatX or ::HatY. - * @return Return the analog value in the range from approximately 25-230. - */ - uint8_t getAnalogHat(Hat a); - /** - * Used to read the joystick of the Wii U Pro Controller. - * @param a Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY. - * @return Return the analog value in the range from approximately 800-3200. - */ - uint16_t getAnalogHat(AnalogHat a); + /** @name Wii Controller functions */ + /** + * Used to read the joystick of the Nunchuck. + * @param a Either ::HatX or ::HatY. + * @return Return the analog value in the range from approximately 25-230. + */ + uint8_t getAnalogHat(Hat a); + /** + * Used to read the joystick of the Wii U Pro Controller. + * @param a Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY. + * @return Return the analog value in the range from approximately 800-3200. + */ + uint16_t getAnalogHat(AnalogHat a); - /** - * Pitch calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected. - * @return Pitch in the range from 0-360. - */ - double getPitch() { return pitch; }; - /** - * Roll calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected. - * @return Roll in the range from 0-360. - */ - double getRoll() { return roll; }; - /** - * This is the yaw calculated by the gyro. - * - * NOTE: This angle will drift a lot and is only available if the Motion Plus extension is connected. - * @return The angle calculated using the gyro. - */ - double getYaw() { return gyroYaw; }; - - /** Used to set all LEDs and rumble off. */ - void setAllOff(); - /** Turn off rumble. */ - void setRumbleOff(); - /** Turn on rumble. */ - void setRumbleOn(); - /** Toggle rumble. */ - void setRumbleToggle(); - /** - * Turn the specific ::LED off. - * @param a The ::LED to turn off. - */ - void setLedOff(LED a); - /** - * Turn the specific ::LED on. - * @param a The ::LED to turn on. - */ - void setLedOn(LED a); - /** - * Toggle the specific ::LED. - * @param a The ::LED to toggle. - */ - void setLedToggle(LED a); - /** - * This will set the LEDs, so the user can see which connections are active. - * - * The first ::LED indicate that the Wiimote is connected, - * - * the second ::LED indicate indicate that a Motion Plus is also connected - * - * the third ::LED will indicate that a Nunchuck controller is also connected. - */ - void setLedStatus(); + /** + * Pitch calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected. + * @return Pitch in the range from 0-360. + */ + double getPitch() { + return pitch; + }; - /** - * Return the battery level of the Wiimote. - * @return The battery level in the range 0-255. - */ - uint8_t getBatteryLevel() { return batteryLevel; }; - /** - * Return the Wiimote state. - * @return See: http://wiibrew.org/wiki/Wiimote#0x20:_Status. - */ - uint8_t getWiiState() { return wiiState; }; - /**@}*/ + /** + * Roll calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected. + * @return Roll in the range from 0-360. + */ + double getRoll() { + return roll; + }; - /**@{*/ - /** Variable used to indicate if a Wiimote is connected. */ - bool wiimoteConnected; - /** Variable used to indicate if a Nunchuck controller is connected. */ - bool nunchuckConnected; - /** Variable used to indicate if a Nunchuck controller is connected. */ - bool motionPlusConnected; - /** Variable used to indicate if a Wii U Pro controller is connected. */ - bool wiiUProControllerConnected; - /**@}*/ - - /* IMU Data, might be usefull if you need to do something more advanced than just calculating the angle */ - - /**@{*/ - /** Pitch and roll calculated from the accelerometer inside the Wiimote. */ - double wiimotePitch; - double wiimoteRoll; - /**@}*/ + /** + * This is the yaw calculated by the gyro. + * + * NOTE: This angle will drift a lot and is only available if the Motion Plus extension is connected. + * @return The angle calculated using the gyro. + */ + double getYaw() { + return gyroYaw; + }; - /**@{*/ - /** Pitch and roll calculated from the accelerometer inside the Nunchuck. */ - double nunchuckPitch; - double nunchuckRoll; - /**@}*/ + /** Used to set all LEDs and rumble off. */ + void setAllOff(); + /** Turn off rumble. */ + void setRumbleOff(); + /** Turn on rumble. */ + void setRumbleOn(); + /** Toggle rumble. */ + void setRumbleToggle(); + /** + * Turn the specific ::LED off. + * @param a The ::LED to turn off. + */ + void setLedOff(LED a); + /** + * Turn the specific ::LED on. + * @param a The ::LED to turn on. + */ + void setLedOn(LED a); + /** + * Toggle the specific ::LED. + * @param a The ::LED to toggle. + */ + void setLedToggle(LED a); + /** + * This will set the LEDs, so the user can see which connections are active. + * + * The first ::LED indicate that the Wiimote is connected, + * + * the second ::LED indicate indicate that a Motion Plus is also connected + * + * the third ::LED will indicate that a Nunchuck controller is also connected. + */ + void setLedStatus(); - /**@{*/ - /** Accelerometer values used to calculate pitch and roll. */ - int16_t accX; - int16_t accY; - int16_t accZ; - /**@}*/ + /** + * Return the battery level of the Wiimote. + * @return The battery level in the range 0-255. + */ + uint8_t getBatteryLevel() { + return batteryLevel; + }; - /* Variables for the gyro inside the Motion Plus */ - /** This is the pitch calculated by the gyro - use this to tune WII#pitchGyroScale. */ - double gyroPitch; - /** This is the roll calculated by the gyro - use this to tune WII#rollGyroScale. */ - double gyroRoll; - /** This is the yaw calculated by the gyro - use this to tune WII#yawGyroScale. */ - double gyroYaw; + /** + * Return the Wiimote state. + * @return See: http://wiibrew.org/wiki/Wiimote#0x20:_Status. + */ + uint8_t getWiiState() { + return wiiState; + }; + /**@}*/ - /**@{*/ - /** The speed in deg/s from the gyro. */ - double pitchGyroSpeed; - double rollGyroSpeed; - double yawGyroSpeed; - /**@}*/ - - /**@{*/ - /** You might need to fine-tune these values. */ - uint16_t pitchGyroScale; - uint16_t rollGyroScale; - uint16_t yawGyroScale; - /**@}*/ - - /**@{*/ - /** Raw value read directly from the Motion Plus. */ - int16_t gyroYawRaw; - int16_t gyroRollRaw; - int16_t gyroPitchRaw; - /**@}*/ - - /**@{*/ - /** These values are set when the controller is first initialized. */ - int16_t gyroYawZero; - int16_t gyroRollZero; - int16_t gyroPitchZero; - /**@}*/ + /**@{*/ + /** Variable used to indicate if a Wiimote is connected. */ + bool wiimoteConnected; + /** Variable used to indicate if a Nunchuck controller is connected. */ + bool nunchuckConnected; + /** Variable used to indicate if a Nunchuck controller is connected. */ + bool motionPlusConnected; + /** Variable used to indicate if a Wii U Pro controller is connected. */ + bool wiiUProControllerConnected; + /**@}*/ + + /* IMU Data, might be usefull if you need to do something more advanced than just calculating the angle */ + + /**@{*/ + /** Pitch and roll calculated from the accelerometer inside the Wiimote. */ + double wiimotePitch; + double wiimoteRoll; + /**@}*/ + + /**@{*/ + /** Pitch and roll calculated from the accelerometer inside the Nunchuck. */ + double nunchuckPitch; + double nunchuckRoll; + /**@}*/ + + /**@{*/ + /** Accelerometer values used to calculate pitch and roll. */ + int16_t accX; + int16_t accY; + int16_t accZ; + /**@}*/ + + /* Variables for the gyro inside the Motion Plus */ + /** This is the pitch calculated by the gyro - use this to tune WII#pitchGyroScale. */ + double gyroPitch; + /** This is the roll calculated by the gyro - use this to tune WII#rollGyroScale. */ + double gyroRoll; + /** This is the yaw calculated by the gyro - use this to tune WII#yawGyroScale. */ + double gyroYaw; + + /**@{*/ + /** The speed in deg/s from the gyro. */ + double pitchGyroSpeed; + double rollGyroSpeed; + double yawGyroSpeed; + /**@}*/ + + /**@{*/ + /** You might need to fine-tune these values. */ + uint16_t pitchGyroScale; + uint16_t rollGyroScale; + uint16_t yawGyroScale; + /**@}*/ + + /**@{*/ + /** Raw value read directly from the Motion Plus. */ + int16_t gyroYawRaw; + int16_t gyroRollRaw; + int16_t gyroPitchRaw; + /**@}*/ + + /**@{*/ + /** These values are set when the controller is first initialized. */ + int16_t gyroYawZero; + int16_t gyroRollZero; + int16_t gyroPitchZero; + /**@}*/ #ifdef WIICAMERA - /** @name Wiimote IR camera functions - * You will have to uncomment #WIICAMERA in Wii.h to use the IR camera. - */ - /** Initialises the camera as per the steps from: http://wiibrew.org/wiki/Wiimote#IR_Camera */ - void IRinitialize(); + /** @name Wiimote IR camera functions + * You will have to uncomment #WIICAMERA in Wii.h to use the IR camera. + */ + /** Initialises the camera as per the steps from: http://wiibrew.org/wiki/Wiimote#IR_Camera */ + void IRinitialize(); - /** - * IR object 1 x-position read from the Wii IR camera. - * @return The x-position of the object in the range 0-1023. - */ - uint16_t getIRx1() { return IR_object_x1; }; - /** - * IR object 1 y-position read from the Wii IR camera. - * @return The y-position of the object in the range 0-767. - */ - uint16_t getIRy1() { return IR_object_y1; }; - /** - * IR object 1 size read from the Wii IR camera. - * @return The size of the object in the range 0-15. - */ - uint8_t getIRs1() { return IR_object_s1; }; + /** + * IR object 1 x-position read from the Wii IR camera. + * @return The x-position of the object in the range 0-1023. + */ + uint16_t getIRx1() { + return IR_object_x1; + }; - /** - * IR object 2 x-position read from the Wii IR camera. - * @return The x-position of the object in the range 0-1023. - */ - uint16_t getIRx2() { return IR_object_x2; }; - /** - * IR object 2 y-position read from the Wii IR camera. - * @return The y-position of the object in the range 0-767. - */ - uint16_t getIRy2() { return IR_object_y2; }; - /** - * IR object 2 size read from the Wii IR camera. - * @return The size of the object in the range 0-15. - */ - uint8_t getIRs2() { return IR_object_s2; }; + /** + * IR object 1 y-position read from the Wii IR camera. + * @return The y-position of the object in the range 0-767. + */ + uint16_t getIRy1() { + return IR_object_y1; + }; - /** - * IR object 3 x-position read from the Wii IR camera. - * @return The x-position of the object in the range 0-1023. - */ - uint16_t getIRx3() { return IR_object_x3; }; - /** - * IR object 3 y-position read from the Wii IR camera. - * @return The y-position of the object in the range 0-767. - */ - uint16_t getIRy3() { return IR_object_y3; }; - /** - * IR object 3 size read from the Wii IR camera. - * @return The size of the object in the range 0-15. - */ - uint8_t getIRs3() { return IR_object_s3; }; + /** + * IR object 1 size read from the Wii IR camera. + * @return The size of the object in the range 0-15. + */ + uint8_t getIRs1() { + return IR_object_s1; + }; - /** - * IR object 4 x-position read from the Wii IR camera. - * @return The x-position of the object in the range 0-1023. - */ - uint16_t getIRx4() { return IR_object_x4; }; - /** - * IR object 4 y-position read from the Wii IR camera. - * @return The y-position of the object in the range 0-767. - */ - uint16_t getIRy4() { return IR_object_y4; }; - /** - * IR object 4 size read from the Wii IR camera. - * @return The size of the object in the range 0-15. - */ - uint8_t getIRs4() { return IR_object_s4; }; + /** + * IR object 2 x-position read from the Wii IR camera. + * @return The x-position of the object in the range 0-1023. + */ + uint16_t getIRx2() { + return IR_object_x2; + }; - /** - * Use this to check if the camera is enabled or not. - * If not call WII#IRinitialize to initialize the IR camera. - * @return True if it's enabled, false if not. - */ - bool isIRCameraEnabled() { return (wiiState & 0x08); }; - /**@}*/ + /** + * IR object 2 y-position read from the Wii IR camera. + * @return The y-position of the object in the range 0-767. + */ + uint16_t getIRy2() { + return IR_object_y2; + }; + + /** + * IR object 2 size read from the Wii IR camera. + * @return The size of the object in the range 0-15. + */ + uint8_t getIRs2() { + return IR_object_s2; + }; + + /** + * IR object 3 x-position read from the Wii IR camera. + * @return The x-position of the object in the range 0-1023. + */ + uint16_t getIRx3() { + return IR_object_x3; + }; + + /** + * IR object 3 y-position read from the Wii IR camera. + * @return The y-position of the object in the range 0-767. + */ + uint16_t getIRy3() { + return IR_object_y3; + }; + + /** + * IR object 3 size read from the Wii IR camera. + * @return The size of the object in the range 0-15. + */ + uint8_t getIRs3() { + return IR_object_s3; + }; + + /** + * IR object 4 x-position read from the Wii IR camera. + * @return The x-position of the object in the range 0-1023. + */ + uint16_t getIRx4() { + return IR_object_x4; + }; + + /** + * IR object 4 y-position read from the Wii IR camera. + * @return The y-position of the object in the range 0-767. + */ + uint16_t getIRy4() { + return IR_object_y4; + }; + + /** + * IR object 4 size read from the Wii IR camera. + * @return The size of the object in the range 0-15. + */ + uint8_t getIRs4() { + return IR_object_s4; + }; + + /** + * Use this to check if the camera is enabled or not. + * If not call WII#IRinitialize to initialize the IR camera. + * @return True if it's enabled, false if not. + */ + bool isIRCameraEnabled() { + return(wiiState & 0x08); + }; + /**@}*/ #endif - + private: - /* Mandatory members */ - BTD *pBtd; - - void L2CAP_task(); // L2CAP state machine - - /* Variables filled from HCI event management */ - uint16_t hci_handle; - bool activeConnection; // Used to indicate if it's already has established a connection - - /* variables used by high level L2CAP task */ - uint8_t l2cap_state; - uint16_t l2cap_event_flag;// l2cap flags of received bluetooth events - - uint32_t ButtonState; - uint32_t OldButtonState; - uint32_t ButtonClickState; - uint16_t hatValues[4]; - - uint8_t HIDBuffer[3];// Used to store HID commands - - uint16_t stateCounter; - bool unknownExtensionConnected; - bool extensionConnected; - - /* L2CAP Channels */ - uint8_t control_scid[2]; // L2CAP source CID for HID_Control - uint8_t control_dcid[2]; // 0x0060 - uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt - uint8_t interrupt_dcid[2]; // 0x0061 - uint8_t identifier; // Identifier for connection - - /* HID Commands */ - void HID_Command(uint8_t* data, uint8_t nbytes); - void setReportMode(bool continuous, uint8_t mode); - void statusRequest(); - - void writeData(uint32_t offset, uint8_t size, uint8_t* data); - void initExtension1(); - void initExtension2(); - - void readData(uint32_t offset, uint16_t size, bool EEPROM); - void readExtensionType(); - void readCalData(); - - void checkMotionPresent(); // Used to see if a Motion Plus is connected to the Wiimote - void initMotionPlus(); - void activateMotionPlus(); + /* Mandatory members */ + BTD *pBtd; - double pitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected - double roll; // Fusioned angle using a complimentary filter if the Motion Plus is connected - - bool activateNunchuck; - bool motionValuesReset; // This bool is true when the gyro values has been reset - unsigned long timer; + void L2CAP_task(); // L2CAP state machine + + /* Variables filled from HCI event management */ + uint16_t hci_handle; + bool activeConnection; // Used to indicate if it's already has established a connection + + /* variables used by high level L2CAP task */ + uint8_t l2cap_state; + uint16_t l2cap_event_flag; // l2cap flags of received bluetooth events + + uint32_t ButtonState; + uint32_t OldButtonState; + uint32_t ButtonClickState; + uint16_t hatValues[4]; + + uint8_t HIDBuffer[3]; // Used to store HID commands + + uint16_t stateCounter; + bool unknownExtensionConnected; + bool extensionConnected; + + /* L2CAP Channels */ + uint8_t control_scid[2]; // L2CAP source CID for HID_Control + uint8_t control_dcid[2]; // 0x0060 + uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt + uint8_t interrupt_dcid[2]; // 0x0061 + uint8_t identifier; // Identifier for connection + + /* HID Commands */ + void HID_Command(uint8_t* data, uint8_t nbytes); + void setReportMode(bool continuous, uint8_t mode); + void statusRequest(); + + void writeData(uint32_t offset, uint8_t size, uint8_t* data); + void initExtension1(); + void initExtension2(); + + void readData(uint32_t offset, uint16_t size, bool EEPROM); + void readExtensionType(); + void readCalData(); + + void checkMotionPresent(); // Used to see if a Motion Plus is connected to the Wiimote + void initMotionPlus(); + void activateMotionPlus(); + + double pitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected + double roll; // Fusioned angle using a complimentary filter if the Motion Plus is connected + + bool activateNunchuck; + bool motionValuesReset; // This bool is true when the gyro values has been reset + unsigned long timer; + + uint8_t wiiState; // Stores the value in l2capinbuf[12] - (0x01: Battery is nearly empty), (0x02: An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4) + uint8_t batteryLevel; - uint8_t wiiState; // Stores the value in l2capinbuf[12] - (0x01: Battery is nearly empty), (0x02: An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4) - uint8_t batteryLevel; - #ifdef WIICAMERA - /* Private function and variables for the readings from teh IR Camera */ - void enableIRCamera1(); // Sets bit 2 of output report 13 - void enableIRCamera2(); // Sets bit 2 of output report 1A - void writeSensitivityBlock1(); - void writeSensitivityBlock2(); - void write0x08Value(); - void setWiiModeNumber(uint8_t mode_number); - - uint16_t IR_object_x1; // IR x position 10 bits - uint16_t IR_object_y1; // IR y position 10 bits - uint8_t IR_object_s1; // IR size value - uint16_t IR_object_x2; - uint16_t IR_object_y2; - uint8_t IR_object_s2; - uint16_t IR_object_x3; // IR x position 10 bits - uint16_t IR_object_y3; // IR y position 10 bits - uint8_t IR_object_s3; // IR size value - uint16_t IR_object_x4; - uint16_t IR_object_y4; - uint8_t IR_object_s4; + /* Private function and variables for the readings from teh IR Camera */ + void enableIRCamera1(); // Sets bit 2 of output report 13 + void enableIRCamera2(); // Sets bit 2 of output report 1A + void writeSensitivityBlock1(); + void writeSensitivityBlock2(); + void write0x08Value(); + void setWiiModeNumber(uint8_t mode_number); + + uint16_t IR_object_x1; // IR x position 10 bits + uint16_t IR_object_y1; // IR y position 10 bits + uint8_t IR_object_s1; // IR size value + uint16_t IR_object_x2; + uint16_t IR_object_y2; + uint8_t IR_object_s2; + uint16_t IR_object_x3; // IR x position 10 bits + uint16_t IR_object_y3; // IR y position 10 bits + uint8_t IR_object_s3; // IR size value + uint16_t IR_object_x4; + uint16_t IR_object_y4; + uint8_t IR_object_s4; #endif }; #endif \ No newline at end of file diff --git a/XBOXRECV.cpp b/XBOXRECV.cpp index ab0df9ef..72f34b50 100644 --- a/XBOXRECV.cpp +++ b/XBOXRECV.cpp @@ -22,395 +22,411 @@ //#define EXTRADEBUG // Uncomment to get even more debugging data //#define PRINTREPORT // Uncomment to print the report send by the Xbox 360 Controller -XBOXRECV::XBOXRECV(USB *p): +XBOXRECV::XBOXRECV(USB *p) : pUsb(p), // pointer to USB class instance - mandatory bAddress(0), // device address - mandatory bPollEnable(false) { // don't start polling before dongle is connected - for(uint8_t i=0; iRegisterDeviceClass(this); //set devConfig[] entry + if (pUsb) // register in USB subsystem + pUsb->RegisterDeviceClass(this); //set devConfig[] entry } uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) { - uint8_t buf[sizeof(USB_DEVICE_DESCRIPTOR)]; - uint8_t rcode; - UsbDevice *p = NULL; - EpInfo *oldep_ptr = NULL; - uint16_t PID; - uint16_t VID; + uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)]; + uint8_t rcode; + UsbDevice *p = NULL; + EpInfo *oldep_ptr = NULL; + uint16_t PID; + uint16_t VID; - // get memory address of USB device address pool - AddressPool &addrPool = pUsb->GetAddressPool(); + // get memory address of USB device address pool + AddressPool &addrPool = pUsb->GetAddressPool(); #ifdef EXTRADEBUG - Notify(PSTR("\r\nXBOXRECV Init"), 0x80); + Notify(PSTR("\r\nXBOXRECV Init"), 0x80); #endif - // check if address has already been assigned to an instance - if (bAddress) { + // check if address has already been assigned to an instance + if (bAddress) { #ifdef DEBUG - Notify(PSTR("\r\nAddress in use"), 0x80); + Notify(PSTR("\r\nAddress in use"), 0x80); #endif - return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; - } + return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; + } - // Get pointer to pseudo device with address 0 assigned - p = addrPool.GetUsbDevicePtr(0); + // Get pointer to pseudo device with address 0 assigned + p = addrPool.GetUsbDevicePtr(0); - if (!p) { + if (!p) { #ifdef DEBUG - Notify(PSTR("\r\nAddress not found"), 0x80); + Notify(PSTR("\r\nAddress not found"), 0x80); #endif - return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - } + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + } - if (!p->epinfo) { + if (!p->epinfo) { #ifdef DEBUG - Notify(PSTR("\r\nepinfo is null"), 0x80); + Notify(PSTR("\r\nepinfo is null"), 0x80); #endif - return USB_ERROR_EPINFO_IS_NULL; - } + return USB_ERROR_EPINFO_IS_NULL; + } - // Save old pointer to EP_RECORD of address 0 - oldep_ptr = p->epinfo; + // Save old pointer to EP_RECORD of address 0 + oldep_ptr = p->epinfo; - // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence - p->epinfo = epInfo; + // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence + p->epinfo = epInfo; - p->lowspeed = lowspeed; + p->lowspeed = lowspeed; - // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data - // Restore p->epinfo - p->epinfo = oldep_ptr; + // Get device descriptor + rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*)buf); // Get device descriptor - addr, ep, nbytes, data + // Restore p->epinfo + p->epinfo = oldep_ptr; - if(rcode) - goto FailGetDevDescr; + if (rcode) + goto FailGetDevDescr; - VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; - PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; + VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; + PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; - if(VID != XBOX_VID && VID != MADCATZ_VID) // We just check if it's a xbox receiver using the Vendor ID - goto FailUnknownDevice; - else if(PID != XBOX_WIRELESS_RECEIVER_PID && PID != XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) { + if (VID != XBOX_VID && VID != MADCATZ_VID) // We just check if it's a xbox receiver using the Vendor ID + goto FailUnknownDevice; + else if (PID != XBOX_WIRELESS_RECEIVER_PID && PID != XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) { #ifdef DEBUG - Notify(PSTR("\r\nYou'll need a wireless receiver for this libary to work"), 0x80); + Notify(PSTR("\r\nYou'll need a wireless receiver for this libary to work"), 0x80); #endif - goto FailUnknownDevice; - } + goto FailUnknownDevice; + } - // Allocate new address according to device class - bAddress = addrPool.AllocAddress(parent, false, port); + // Allocate new address according to device class + bAddress = addrPool.AllocAddress(parent, false, port); - if (!bAddress) - return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; + if (!bAddress) + return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; - // Extract Max Packet Size from device descriptor - epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; + // Extract Max Packet Size from device descriptor + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; - // Assign new address to the device - rcode = pUsb->setAddr( 0, 0, bAddress ); - if (rcode) { + // Assign new address to the device + rcode = pUsb->setAddr(0, 0, bAddress); + if (rcode) { + p->lowspeed = false; + addrPool.FreeAddress(bAddress); + bAddress = 0; +#ifdef DEBUG + Notify(PSTR("\r\nsetAddr: "), 0x80); +#endif + PrintHex (rcode, 0x80); + return rcode; + } +#ifdef EXTRADEBUG + Notify(PSTR("\r\nAddr: "), 0x80); + PrintHex (bAddress, 0x80); +#endif p->lowspeed = false; - addrPool.FreeAddress(bAddress); - bAddress = 0; -#ifdef DEBUG - Notify(PSTR("\r\nsetAddr: "), 0x80); -#endif - PrintHex(rcode, 0x80); - return rcode; - } -#ifdef EXTRADEBUG - Notify(PSTR("\r\nAddr: "), 0x80); - PrintHex(bAddress, 0x80); -#endif - p->lowspeed = false; - //get pointer to assigned address record - p = addrPool.GetUsbDevicePtr(bAddress); - if (!p) - return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + //get pointer to assigned address record + p = addrPool.GetUsbDevicePtr(bAddress); + if (!p) + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - p->lowspeed = lowspeed; + p->lowspeed = lowspeed; - // Assign epInfo to epinfo pointer - only EP0 is known - rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); - if (rcode) - goto FailSetDevTblEntry; + // Assign epInfo to epinfo pointer - only EP0 is known + rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); + if (rcode) + goto FailSetDevTblEntry; - /* The application will work in reduced host mode, so we can save program and data - memory space. After verifying the VID we will use known values for the - configuration values for device, interface, endpoints and HID for the XBOX360 Wireless receiver */ + /* The application will work in reduced host mode, so we can save program and data + memory space. After verifying the VID we will use known values for the + configuration values for device, interface, endpoints and HID for the XBOX360 Wireless receiver */ - /* Initialize data structures for endpoints of device */ - epInfo[ XBOX_INPUT_PIPE_1 ].epAddr = 0x01; // XBOX 360 report endpoint - poll interval 1ms - epInfo[ XBOX_INPUT_PIPE_1 ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_INPUT_PIPE_1 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_INPUT_PIPE_1 ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_INPUT_PIPE_1 ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_INPUT_PIPE_1 ].bmRcvToggle = bmRCVTOG0; - epInfo[ XBOX_OUTPUT_PIPE_1 ].epAddr = 0x01; // XBOX 360 output endpoint - poll interval 8ms - epInfo[ XBOX_OUTPUT_PIPE_1 ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_OUTPUT_PIPE_1 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_OUTPUT_PIPE_1 ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_OUTPUT_PIPE_1 ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_OUTPUT_PIPE_1 ].bmRcvToggle = bmRCVTOG0; + /* Initialize data structures for endpoints of device */ + epInfo[ XBOX_INPUT_PIPE_1 ].epAddr = 0x01; // XBOX 360 report endpoint - poll interval 1ms + epInfo[ XBOX_INPUT_PIPE_1 ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_INPUT_PIPE_1 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_INPUT_PIPE_1 ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_INPUT_PIPE_1 ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_INPUT_PIPE_1 ].bmRcvToggle = bmRCVTOG0; + epInfo[ XBOX_OUTPUT_PIPE_1 ].epAddr = 0x01; // XBOX 360 output endpoint - poll interval 8ms + epInfo[ XBOX_OUTPUT_PIPE_1 ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_OUTPUT_PIPE_1 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_OUTPUT_PIPE_1 ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_OUTPUT_PIPE_1 ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_OUTPUT_PIPE_1 ].bmRcvToggle = bmRCVTOG0; - epInfo[ XBOX_INPUT_PIPE_2 ].epAddr = 0x03; // XBOX 360 report endpoint - poll interval 1ms - epInfo[ XBOX_INPUT_PIPE_2 ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_INPUT_PIPE_2 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_INPUT_PIPE_2 ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_INPUT_PIPE_2 ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_INPUT_PIPE_2 ].bmRcvToggle = bmRCVTOG0; - epInfo[ XBOX_OUTPUT_PIPE_2 ].epAddr = 0x03; // XBOX 360 output endpoint - poll interval 8ms - epInfo[ XBOX_OUTPUT_PIPE_2 ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_OUTPUT_PIPE_2 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_OUTPUT_PIPE_2 ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_OUTPUT_PIPE_2 ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_OUTPUT_PIPE_2 ].bmRcvToggle = bmRCVTOG0; + epInfo[ XBOX_INPUT_PIPE_2 ].epAddr = 0x03; // XBOX 360 report endpoint - poll interval 1ms + epInfo[ XBOX_INPUT_PIPE_2 ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_INPUT_PIPE_2 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_INPUT_PIPE_2 ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_INPUT_PIPE_2 ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_INPUT_PIPE_2 ].bmRcvToggle = bmRCVTOG0; + epInfo[ XBOX_OUTPUT_PIPE_2 ].epAddr = 0x03; // XBOX 360 output endpoint - poll interval 8ms + epInfo[ XBOX_OUTPUT_PIPE_2 ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_OUTPUT_PIPE_2 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_OUTPUT_PIPE_2 ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_OUTPUT_PIPE_2 ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_OUTPUT_PIPE_2 ].bmRcvToggle = bmRCVTOG0; - epInfo[ XBOX_INPUT_PIPE_3 ].epAddr = 0x05; // XBOX 360 report endpoint - poll interval 1ms - epInfo[ XBOX_INPUT_PIPE_3 ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_INPUT_PIPE_3 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_INPUT_PIPE_3 ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_INPUT_PIPE_3 ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_INPUT_PIPE_3 ].bmRcvToggle = bmRCVTOG0; - epInfo[ XBOX_OUTPUT_PIPE_3 ].epAddr = 0x05; // XBOX 360 output endpoint - poll interval 8ms - epInfo[ XBOX_OUTPUT_PIPE_3 ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_OUTPUT_PIPE_3 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_OUTPUT_PIPE_3 ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_OUTPUT_PIPE_3 ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_OUTPUT_PIPE_3 ].bmRcvToggle = bmRCVTOG0; + epInfo[ XBOX_INPUT_PIPE_3 ].epAddr = 0x05; // XBOX 360 report endpoint - poll interval 1ms + epInfo[ XBOX_INPUT_PIPE_3 ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_INPUT_PIPE_3 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_INPUT_PIPE_3 ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_INPUT_PIPE_3 ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_INPUT_PIPE_3 ].bmRcvToggle = bmRCVTOG0; + epInfo[ XBOX_OUTPUT_PIPE_3 ].epAddr = 0x05; // XBOX 360 output endpoint - poll interval 8ms + epInfo[ XBOX_OUTPUT_PIPE_3 ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_OUTPUT_PIPE_3 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_OUTPUT_PIPE_3 ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_OUTPUT_PIPE_3 ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_OUTPUT_PIPE_3 ].bmRcvToggle = bmRCVTOG0; - epInfo[ XBOX_INPUT_PIPE_4 ].epAddr = 0x07; // XBOX 360 report endpoint - poll interval 1ms - epInfo[ XBOX_INPUT_PIPE_4 ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_INPUT_PIPE_4 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_INPUT_PIPE_4 ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_INPUT_PIPE_4 ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_INPUT_PIPE_4 ].bmRcvToggle = bmRCVTOG0; - epInfo[ XBOX_OUTPUT_PIPE_4 ].epAddr = 0x07; // XBOX 360 output endpoint - poll interval 8ms - epInfo[ XBOX_OUTPUT_PIPE_4 ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_OUTPUT_PIPE_4 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_OUTPUT_PIPE_4 ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_OUTPUT_PIPE_4 ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_OUTPUT_PIPE_4 ].bmRcvToggle = bmRCVTOG0; + epInfo[ XBOX_INPUT_PIPE_4 ].epAddr = 0x07; // XBOX 360 report endpoint - poll interval 1ms + epInfo[ XBOX_INPUT_PIPE_4 ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_INPUT_PIPE_4 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_INPUT_PIPE_4 ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_INPUT_PIPE_4 ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_INPUT_PIPE_4 ].bmRcvToggle = bmRCVTOG0; + epInfo[ XBOX_OUTPUT_PIPE_4 ].epAddr = 0x07; // XBOX 360 output endpoint - poll interval 8ms + epInfo[ XBOX_OUTPUT_PIPE_4 ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_OUTPUT_PIPE_4 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_OUTPUT_PIPE_4 ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_OUTPUT_PIPE_4 ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_OUTPUT_PIPE_4 ].bmRcvToggle = bmRCVTOG0; - rcode = pUsb->setEpInfoEntry(bAddress, 9, epInfo); - if( rcode ) - goto FailSetDevTblEntry; + rcode = pUsb->setEpInfoEntry(bAddress, 9, epInfo); + if (rcode) + goto FailSetDevTblEntry; - delay(200);//Give time for address change + delay(200); //Give time for address change - rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1); - if( rcode ) - goto FailSetConf; + rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1); + if (rcode) + goto FailSetConf; #ifdef DEBUG - Notify(PSTR("\r\nXbox Wireless Receiver Connected\r\n"), 0x80); + Notify(PSTR("\r\nXbox Wireless Receiver Connected\r\n"), 0x80); #endif - XboxReceiverConnected = true; - bPollEnable = true; - return 0; // successful configuration + XboxReceiverConnected = true; + bPollEnable = true; + return 0; // successful configuration - /* diagnostic messages */ + /* diagnostic messages */ FailGetDevDescr: #ifdef DEBUG - Notify(PSTR("\r\ngetDevDescr:"), 0x80); + Notify(PSTR("\r\ngetDevDescr:"), 0x80); #endif - goto Fail; + goto Fail; FailSetDevTblEntry: #ifdef DEBUG - Notify(PSTR("\r\nsetDevTblEn:"), 0x80); + Notify(PSTR("\r\nsetDevTblEn:"), 0x80); #endif - goto Fail; + goto Fail; FailSetConf: #ifdef DEBUG - Notify(PSTR("\r\nsetConf:"), 0x80); + Notify(PSTR("\r\nsetConf:"), 0x80); #endif - goto Fail; + goto Fail; FailUnknownDevice: #ifdef DEBUG - Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); - PrintHex(VID, 0x80); - Notify(PSTR(" PID: "), 0x80); - PrintHex(PID, 0x80); + Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); + PrintHex (VID, 0x80); + Notify(PSTR(" PID: "), 0x80); + PrintHex (PID, 0x80); #endif - rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; - goto Fail; + rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; + goto Fail; Fail: #ifdef DEBUG - Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80); - Serial.print(rcode,HEX); + Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80); + Serial.print(rcode, HEX); #endif - Release(); - return rcode; + Release(); + return rcode; } /* Performs a cleanup after failed Init() attempt */ uint8_t XBOXRECV::Release() { - XboxReceiverConnected = false; - for(uint8_t i=0;i<4;i++) - Xbox360Connected[i] = 0x00; - pUsb->GetAddressPool().FreeAddress(bAddress); - bAddress = 0; - bPollEnable = false; - return 0; + XboxReceiverConnected = false; + for (uint8_t i = 0; i < 4; i++) + Xbox360Connected[i] = 0x00; + pUsb->GetAddressPool().FreeAddress(bAddress); + bAddress = 0; + bPollEnable = false; + return 0; } + uint8_t XBOXRECV::Poll() { - if (!bPollEnable) - return 0; - if(!timer || ((millis() - timer) > 3000)) { // Run checkStatus every 3 seconds - timer = millis(); - checkStatus(); - } - uint8_t inputPipe; - uint16_t bufferSize; - for(uint8_t i=0;i<4;i++) { - switch (i) { - case 0: inputPipe = XBOX_INPUT_PIPE_1; break; - case 1: inputPipe = XBOX_INPUT_PIPE_2; break; - case 2: inputPipe = XBOX_INPUT_PIPE_3; break; - case 3: inputPipe = XBOX_INPUT_PIPE_4; break; + if (!bPollEnable) + return 0; + if (!timer || ((millis() - timer) > 3000)) { // Run checkStatus every 3 seconds + timer = millis(); + checkStatus(); } - bufferSize = EP_MAXPKTSIZE; // This is the maximum number of bytes we want to receive - pUsb->inTransfer(bAddress, epInfo[ inputPipe ].epAddr, &bufferSize, readBuf); - if(bufferSize > 0) { // The number of received bytes + uint8_t inputPipe; + uint16_t bufferSize; + for (uint8_t i = 0; i < 4; i++) { + switch (i) { + case 0: inputPipe = XBOX_INPUT_PIPE_1; + break; + case 1: inputPipe = XBOX_INPUT_PIPE_2; + break; + case 2: inputPipe = XBOX_INPUT_PIPE_3; + break; + case 3: inputPipe = XBOX_INPUT_PIPE_4; + break; + } + bufferSize = EP_MAXPKTSIZE; // This is the maximum number of bytes we want to receive + pUsb->inTransfer(bAddress, epInfo[ inputPipe ].epAddr, &bufferSize, readBuf); + if (bufferSize > 0) { // The number of received bytes #ifdef EXTRADEBUG - Notify(PSTR("Bytes Received: "), 0x80); - Serial.print(bufferSize); - Notify(PSTR("\r\n"), 0x80); + Notify(PSTR("Bytes Received: "), 0x80); + Serial.print(bufferSize); + Notify(PSTR("\r\n"), 0x80); #endif - readReport(i); + readReport(i); #ifdef PRINTREPORT - printReport(i,bufferSize); // Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller + printReport(i, bufferSize); // Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller #endif + } } - } - return 0; + return 0; } void XBOXRECV::readReport(uint8_t controller) { - if (readBuf == NULL) - return; - // This report is send when a controller is connected and disconnected - if(readBuf[0] == 0x08 && readBuf[1] != Xbox360Connected[controller]) { - Xbox360Connected[controller] = readBuf[1]; + if (readBuf == NULL) + return; + // This report is send when a controller is connected and disconnected + if (readBuf[0] == 0x08 && readBuf[1] != Xbox360Connected[controller]) { + Xbox360Connected[controller] = readBuf[1]; #ifdef DEBUG - Notify(PSTR("Controller "), 0x80); - Serial.print(controller); + Notify(PSTR("Controller "), 0x80); + Serial.print(controller); #endif - if(Xbox360Connected[controller]) { + if (Xbox360Connected[controller]) { #ifdef DEBUG - char* str = 0; - switch(readBuf[1]) { - case 0x80: str = PSTR(" as controller\r\n"); break; - case 0x40: str = PSTR(" as headset\r\n"); break; - case 0xC0: str = PSTR(" as controller+headset\r\n"); break; - } - Notify(PSTR(": connected"), 0x80); - Notify(str, 0x80); + char* str = 0; + switch (readBuf[1]) { + case 0x80: str = PSTR(" as controller\r\n"); + break; + case 0x40: str = PSTR(" as headset\r\n"); + break; + case 0xC0: str = PSTR(" as controller+headset\r\n"); + break; + } + Notify(PSTR(": connected"), 0x80); + Notify(str, 0x80); #endif - LED led; - switch (controller) { - case 0: led = LED1; break; - case 1: led = LED2; break; - case 2: led = LED3; break; - case 3: led = LED4; break; - } - setLedOn(controller,led); + LED led; + switch (controller) { + case 0: led = LED1; + break; + case 1: led = LED2; + break; + case 2: led = LED3; + break; + case 3: led = LED4; + break; + } + setLedOn(controller, led); + } +#ifdef DEBUG + else + Notify(PSTR(": disconnected\r\n"), 0x80); +#endif + return; } -#ifdef DEBUG - else - Notify(PSTR(": disconnected\r\n"), 0x80); -#endif - return; - } - // Controller status report - if(readBuf[1] == 0x00 && readBuf[3] & 0x13 && readBuf[4] >= 0x22) { - controllerStatus[controller] = ((uint16_t)readBuf[3] << 8) | readBuf[4]; - return; - } - if(readBuf[1] != 0x01) // Check if it's the correct report - the receiver also sends different status reports - return; + // Controller status report + if (readBuf[1] == 0x00 && readBuf[3] & 0x13 && readBuf[4] >= 0x22) { + controllerStatus[controller] = ((uint16_t)readBuf[3] << 8) | readBuf[4]; + return; + } + if (readBuf[1] != 0x01) // Check if it's the correct report - the receiver also sends different status reports + return; - // A controller must be connected if it's sending data - if(!Xbox360Connected[controller]) - Xbox360Connected[controller] |= 0x80; + // A controller must be connected if it's sending data + if (!Xbox360Connected[controller]) + Xbox360Connected[controller] |= 0x80; - ButtonState[controller] = (uint32_t)(readBuf[9] | ((uint16_t)readBuf[8] << 8) | ((uint32_t)readBuf[7] << 16) | ((uint32_t)readBuf[6] << 24)); + ButtonState[controller] = (uint32_t)(readBuf[9] | ((uint16_t)readBuf[8] << 8) | ((uint32_t)readBuf[7] << 16) | ((uint32_t)readBuf[6] << 24)); - hatValue[controller][LeftHatX] = (int16_t)(((uint16_t)readBuf[11] << 8) | readBuf[10]); - hatValue[controller][LeftHatY] = (int16_t)(((uint16_t)readBuf[13] << 8) | readBuf[12]); - hatValue[controller][RightHatX] = (int16_t)(((uint16_t)readBuf[15] << 8) | readBuf[14]); - hatValue[controller][RightHatY] = (int16_t)(((uint16_t)readBuf[17] << 8) | readBuf[16]); + hatValue[controller][LeftHatX] = (int16_t)(((uint16_t)readBuf[11] << 8) | readBuf[10]); + hatValue[controller][LeftHatY] = (int16_t)(((uint16_t)readBuf[13] << 8) | readBuf[12]); + hatValue[controller][RightHatX] = (int16_t)(((uint16_t)readBuf[15] << 8) | readBuf[14]); + hatValue[controller][RightHatY] = (int16_t)(((uint16_t)readBuf[17] << 8) | readBuf[16]); - //Notify(PSTR("\r\nButtonState: "), 0x80); - //PrintHex(ButtonState[controller], 0x80); + //Notify(PSTR("\r\nButtonState: "), 0x80); + //PrintHex(ButtonState[controller], 0x80); - if(ButtonState[controller] != OldButtonState[controller]) { - buttonStateChanged[controller] = true; - ButtonClickState[controller] = (ButtonState[controller] >> 16) & ((~OldButtonState[controller]) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2 - if(((uint8_t)OldButtonState[controller]) == 0 && ((uint8_t)ButtonState[controller]) != 0) // The L2 and R2 buttons are special as they are analog buttons - R2Clicked[controller] = true; - if((uint8_t)(OldButtonState[controller] >> 8) == 0 && (uint8_t)(ButtonState[controller] >> 8) != 0) - L2Clicked[controller] = true; - OldButtonState[controller] = ButtonState[controller]; - } + if (ButtonState[controller] != OldButtonState[controller]) { + buttonStateChanged[controller] = true; + ButtonClickState[controller] = (ButtonState[controller] >> 16) & ((~OldButtonState[controller]) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2 + if (((uint8_t)OldButtonState[controller]) == 0 && ((uint8_t)ButtonState[controller]) != 0) // The L2 and R2 buttons are special as they are analog buttons + R2Clicked[controller] = true; + if ((uint8_t)(OldButtonState[controller] >> 8) == 0 && (uint8_t)(ButtonState[controller] >> 8) != 0) + L2Clicked[controller] = true; + OldButtonState[controller] = ButtonState[controller]; + } } void XBOXRECV::printReport(uint8_t controller, uint8_t nBytes) { //Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller #ifdef PRINTREPORT - if (readBuf == NULL) - return; - Notify(PSTR("Controller "), 0x80); - Serial.print(controller); - Notify(PSTR(": "), 0x80); - for(uint8_t i = 0; i < nBytes;i++) { - PrintHex(readBuf[i], 0x80); - Serial.print(" "); - } - Serial.println(); + if (readBuf == NULL) + return; + Notify(PSTR("Controller "), 0x80); + Serial.print(controller); + Notify(PSTR(": "), 0x80); + for (uint8_t i = 0; i < nBytes; i++) { + PrintHex (readBuf[i], 0x80); + Serial.print(" "); + } + Serial.println(); #endif } + uint8_t XBOXRECV::getButtonPress(uint8_t controller, Button b) { - if(b == L2) // These are analog buttons - return (uint8_t)(ButtonState[controller] >> 8); - else if(b == R2) - return (uint8_t)ButtonState[controller]; - return (ButtonState[controller] & ((uint32_t)pgm_read_word(&XBOXBUTTONS[(uint8_t)b]) << 16)); + if (b == L2) // These are analog buttons + return (uint8_t)(ButtonState[controller] >> 8); + else if (b == R2) + return (uint8_t)ButtonState[controller]; + return (ButtonState[controller] & ((uint32_t)pgm_read_word(&XBOXBUTTONS[(uint8_t)b]) << 16)); } + bool XBOXRECV::getButtonClick(uint8_t controller, Button b) { - if(b == L2) { - if(L2Clicked[controller]) { - L2Clicked[controller] = false; - return true; + if (b == L2) { + if (L2Clicked[controller]) { + L2Clicked[controller] = false; + return true; + } + return false; + } else if (b == R2) { + if (R2Clicked[controller]) { + R2Clicked[controller] = false; + return true; + } + return false; } - return false; - } - else if(b == R2) { - if(R2Clicked[controller]) { - R2Clicked[controller] = false; - return true; - } - return false; - } - uint16_t button = pgm_read_word(&XBOXBUTTONS[(uint8_t)b]); - bool click = (ButtonClickState[controller] & button); - ButtonClickState[controller] &= ~button; // clear "click" event - return click; + uint16_t button = pgm_read_word(&XBOXBUTTONS[(uint8_t)b]); + bool click = (ButtonClickState[controller] & button); + ButtonClickState[controller] &= ~button; // clear "click" event + return click; } + int16_t XBOXRECV::getAnalogHat(uint8_t controller, AnalogHat a) { - return hatValue[controller][a]; + return hatValue[controller][a]; } + bool XBOXRECV::buttonChanged(uint8_t controller) { - bool state = buttonStateChanged[controller]; - buttonStateChanged[controller] = false; - return state; + bool state = buttonStateChanged[controller]; + buttonStateChanged[controller] = false; + return state; } + /* ControllerStatus Breakdown ControllerStatus[controller] & 0x0001 // 0 @@ -429,81 +445,90 @@ ControllerStatus Breakdown ControllerStatus[controller] & 0x2000 // 0 ControllerStatus[controller] & 0x4000 // 0 ControllerStatus[controller] & 0x8000 // 0 -*/ + */ uint8_t XBOXRECV::getBatteryLevel(uint8_t controller) { - uint8_t batteryLevel = ((controllerStatus[controller] & 0x00C0) >> 6) * 33; - if(batteryLevel == 99) - batteryLevel = 100; - return batteryLevel; + uint8_t batteryLevel = ((controllerStatus[controller] & 0x00C0) >> 6) * 33; + if (batteryLevel == 99) + batteryLevel = 100; + return batteryLevel; } void XBOXRECV::XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes) { - uint8_t rcode; - uint8_t outputPipe; - switch (controller) { - case 0: outputPipe = XBOX_OUTPUT_PIPE_1; break; - case 1: outputPipe = XBOX_OUTPUT_PIPE_2; break; - case 2: outputPipe = XBOX_OUTPUT_PIPE_3; break; - case 3: outputPipe = XBOX_OUTPUT_PIPE_4; break; - } - rcode = pUsb->outTransfer(bAddress, epInfo[ outputPipe ].epAddr, nbytes, data); + uint8_t rcode; + uint8_t outputPipe; + switch (controller) { + case 0: outputPipe = XBOX_OUTPUT_PIPE_1; + break; + case 1: outputPipe = XBOX_OUTPUT_PIPE_2; + break; + case 2: outputPipe = XBOX_OUTPUT_PIPE_3; + break; + case 3: outputPipe = XBOX_OUTPUT_PIPE_4; + break; + } + rcode = pUsb->outTransfer(bAddress, epInfo[ outputPipe ].epAddr, nbytes, data); #ifdef EXTRADEBUG - if(rcode) - Notify(PSTR("Error sending Xbox message\r\n"), 0x80); + if (rcode) + Notify(PSTR("Error sending Xbox message\r\n"), 0x80); #endif } -void XBOXRECV::setLedRaw(uint8_t controller, uint8_t value) { - writeBuf[0] = 0x00; - writeBuf[1] = 0x00; - writeBuf[2] = 0x08; - writeBuf[3] = value | 0x40; - XboxCommand(controller, writeBuf, 4); +void XBOXRECV::setLedRaw(uint8_t controller, uint8_t value) { + writeBuf[0] = 0x00; + writeBuf[1] = 0x00; + writeBuf[2] = 0x08; + writeBuf[3] = value | 0x40; + + XboxCommand(controller, writeBuf, 4); } + void XBOXRECV::setLedOn(uint8_t controller, LED led) { - if(led != ALL) // All LEDs can't be on a the same time - setLedRaw(controller,(pgm_read_byte(&XBOXLEDS[(uint8_t)led]))+4); + if (led != ALL) // All LEDs can't be on a the same time + setLedRaw(controller, (pgm_read_byte(&XBOXLEDS[(uint8_t)led])) + 4); } + void XBOXRECV::setLedBlink(uint8_t controller, LED led) { - setLedRaw(controller,pgm_read_byte(&XBOXLEDS[(uint8_t)led])); + setLedRaw(controller, pgm_read_byte(&XBOXLEDS[(uint8_t)led])); } + void XBOXRECV::setLedMode(uint8_t controller, LEDMode ledMode) { // This function is used to do some speciel LED stuff the controller supports - setLedRaw(controller,(uint8_t)ledMode); + setLedRaw(controller, (uint8_t)ledMode); } + /* PC runs this at interval of approx 2 seconds Thanks to BusHound from Perisoft.net for the Windows USB Analysis output Found by timstamp.co.uk -*/ + */ void XBOXRECV::checkStatus() { - if(!bPollEnable) - return; - // Get controller info - writeBuf[0] = 0x08; - writeBuf[1] = 0x00; - writeBuf[2] = 0x0f; - writeBuf[3] = 0xc0; - for(uint8_t i=0; i<4; i++) { - XboxCommand(i, writeBuf, 4); - } - // Get battery status - writeBuf[0] = 0x00; - writeBuf[1] = 0x00; - writeBuf[2] = 0x00; - writeBuf[3] = 0x40; - for(uint8_t i=0; i<4; i++) { - if(Xbox360Connected[i]) - XboxCommand(i, writeBuf, 4); - } + if (!bPollEnable) + return; + // Get controller info + writeBuf[0] = 0x08; + writeBuf[1] = 0x00; + writeBuf[2] = 0x0f; + writeBuf[3] = 0xc0; + for (uint8_t i = 0; i < 4; i++) { + XboxCommand(i, writeBuf, 4); + } + // Get battery status + writeBuf[0] = 0x00; + writeBuf[1] = 0x00; + writeBuf[2] = 0x00; + writeBuf[3] = 0x40; + for (uint8_t i = 0; i < 4; i++) { + if (Xbox360Connected[i]) + XboxCommand(i, writeBuf, 4); + } } void XBOXRECV::setRumbleOn(uint8_t controller, uint8_t lValue, uint8_t rValue) { - writeBuf[0] = 0x00; - writeBuf[1] = 0x01; - writeBuf[2] = 0x0f; - writeBuf[3] = 0xc0; - writeBuf[4] = 0x00; - writeBuf[5] = lValue; // big weight - writeBuf[6] = rValue; // small weight + writeBuf[0] = 0x00; + writeBuf[1] = 0x01; + writeBuf[2] = 0x0f; + writeBuf[3] = 0xc0; + writeBuf[4] = 0x00; + writeBuf[5] = lValue; // big weight + writeBuf[6] = rValue; // small weight - XboxCommand(controller, writeBuf, 7); + XboxCommand(controller, writeBuf, 7); } diff --git a/XBOXRECV.h b/XBOXRECV.h index 6c7ed570..1235f201 100644 --- a/XBOXRECV.h +++ b/XBOXRECV.h @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -27,7 +27,7 @@ #endif #include "Usb.h" -#include "xboxEnums.h" +#include "xboxEnums.h" /* Data Xbox 360 taken from descriptors */ #define EP_MAXPKTSIZE 32 // max size for data via USB @@ -62,167 +62,183 @@ */ class XBOXRECV : public USBDeviceConfig { public: - /** - * Constructor for the XBOXRECV class. - * @param pUsb Pointer to USB class instance. - */ - XBOXRECV(USB *pUsb); + /** + * Constructor for the XBOXRECV class. + * @param pUsb Pointer to USB class instance. + */ + XBOXRECV(USB *pUsb); - /** @name USBDeviceConfig implementation */ - /** - * Initialize the Xbox wireless receiver. - * @param parent Hub number. - * @param port Port number on the hub. - * @param lowspeed Speed of the device. - * @return 0 on success. - */ - virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed); - /** - * Release the USB device. - * @return 0 on success. - */ - virtual uint8_t Release(); - /** - * Poll the USB Input endpoins and run the state machines. - * @return 0 on success. - */ - virtual uint8_t Poll(); - /** - * Get the device address. - * @return The device address. - */ - virtual uint8_t GetAddress() { return bAddress; }; - /** - * Used to check if the controller has been initialized. - * @return True if it's ready. - */ - virtual bool isReady() { return bPollEnable; }; - /**@}*/ + /** @name USBDeviceConfig implementation */ + /** + * Initialize the Xbox wireless receiver. + * @param parent Hub number. + * @param port Port number on the hub. + * @param lowspeed Speed of the device. + * @return 0 on success. + */ + virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed); + /** + * Release the USB device. + * @return 0 on success. + */ + virtual uint8_t Release(); + /** + * Poll the USB Input endpoins and run the state machines. + * @return 0 on success. + */ + virtual uint8_t Poll(); - /** @name Xbox Controller functions */ - /** - * getButtonPress(uint8_t controller, Button b) will return true as long as the button is held down. - * - * While getButtonClick(uint8_t controller, Button b) will only return it once. - * - * So you instance if you need to increase a variable once you would use getButtonClick(uint8_t controller, Button b), - * but if you need to drive a robot forward you would use getButtonPress(uint8_t controller, Button b). - * @param controller The controller to read from. - * @param b ::Button to read. - * @return getButtonClick(uint8_t controller, Button b) will return a bool, but getButtonPress(uint8_t controller, Button b) - * will return a byte if reading ::L2 or ::R2. - */ - uint8_t getButtonPress(uint8_t controller, Button b); - bool getButtonClick(uint8_t controller, Button b); - /**@}*/ + /** + * Get the device address. + * @return The device address. + */ + virtual uint8_t GetAddress() { + return bAddress; + }; - /** @name Xbox Controller functions */ - /** - * Return the analog value from the joysticks on the controller. - * @param controller The controller to read from. - * @param a Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY. - * @return Returns a signed 16-bit integer. - */ - int16_t getAnalogHat(uint8_t controller, AnalogHat a); - /** - * Turn rumble off and all the LEDs on the specific controller. - * @param controller The controller to write to. - */ - void setAllOff(uint8_t controller) { setRumbleOn(controller,0,0); setLedOff(controller); }; - /** - * Turn rumble off the specific controller. - * @param controller The controller to write to. - */ - void setRumbleOff(uint8_t controller) { setRumbleOn(controller,0,0); }; - /** - * Turn rumble on. - * @param controller The controller to write to. - * @param lValue Left motor (big weight) inside the controller. - * @param rValue Right motor (small weight) inside the controller. - */ - void setRumbleOn(uint8_t controller, uint8_t lValue, uint8_t rValue); - /** - * Set LED value. Without using the ::LED or ::LEDMode enum. - * @param controller The controller to write to. - * @param value See: - * setLedOff(uint8_t controller), setLedOn(uint8_t controller, LED l), - * setLedBlink(uint8_t controller, LED l), and setLedMode(uint8_t controller, LEDMode lm). - */ - void setLedRaw(uint8_t controller, uint8_t value); - /** - * Turn all LEDs off the specific controller. - * @param controller The controller to write to. - */ - void setLedOff(uint8_t controller) { setLedRaw(controller,0); }; - /** - * Turn on a LED by using the ::LED enum. - * @param controller The controller to write to. - * @param l ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller. - */ - void setLedOn(uint8_t controller, LED l); - /** - * Turn on a LED by using the ::LED enum. - * @param controller The controller to write to. - * @param l ::ALL, ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller. - */ - void setLedBlink(uint8_t controller, LED l); - /** - * Used to set special LED modes supported by the Xbox controller. - * @param controller The controller to write to. - * @param lm See ::LEDMode. - */ - void setLedMode(uint8_t controller, LEDMode lm); - /** - * Used to get the battery level from the controller. - * @param controller The controller to read from. - * @return Returns the battery level in percentage in 33% steps. - */ - uint8_t getBatteryLevel(uint8_t controller); - /** - * Used to check if a button has changed. - * @param controller The controller to read from. - * @return True if a button has changed. - */ - bool buttonChanged(uint8_t controller); - /**@}*/ + /** + * Used to check if the controller has been initialized. + * @return True if it's ready. + */ + virtual bool isReady() { + return bPollEnable; + }; + /**@}*/ - /** True if a wireless receiver is connected. */ - bool XboxReceiverConnected; - /** Variable used to indicate if the XBOX 360 controller is successfully connected. */ - uint8_t Xbox360Connected[4]; - -protected: - /** Pointer to USB class instance. */ - USB *pUsb; - /** Device address. */ - uint8_t bAddress; - /** Endpoint info structure. */ - EpInfo epInfo[XBOX_MAX_ENDPOINTS]; - -private: - bool bPollEnable; + /** @name Xbox Controller functions */ + /** + * getButtonPress(uint8_t controller, Button b) will return true as long as the button is held down. + * + * While getButtonClick(uint8_t controller, Button b) will only return it once. + * + * So you instance if you need to increase a variable once you would use getButtonClick(uint8_t controller, Button b), + * but if you need to drive a robot forward you would use getButtonPress(uint8_t controller, Button b). + * @param controller The controller to read from. + * @param b ::Button to read. + * @return getButtonClick(uint8_t controller, Button b) will return a bool, but getButtonPress(uint8_t controller, Button b) + * will return a byte if reading ::L2 or ::R2. + */ + uint8_t getButtonPress(uint8_t controller, Button b); + bool getButtonClick(uint8_t controller, Button b); + /**@}*/ - /* Variables to store the buttons */ - uint32_t ButtonState[4]; - uint32_t OldButtonState[4]; - uint16_t ButtonClickState[4]; - int16_t hatValue[4][4]; - uint16_t controllerStatus[4]; - bool buttonStateChanged[4]; // True if a button has changed - - bool L2Clicked[4]; // These buttons are analog, so we use we use these bools to check if they where clicked or not - bool R2Clicked[4]; + /** @name Xbox Controller functions */ + /** + * Return the analog value from the joysticks on the controller. + * @param controller The controller to read from. + * @param a Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY. + * @return Returns a signed 16-bit integer. + */ + int16_t getAnalogHat(uint8_t controller, AnalogHat a); - unsigned long timer; // Timing for checkStatus() signals - - uint8_t readBuf[EP_MAXPKTSIZE]; // General purpose buffer for input data - uint8_t writeBuf[EP_MAXPKTSIZE]; // General purpose buffer for output data - - void readReport(uint8_t controller); // read incoming data - void printReport(uint8_t controller, uint8_t nBytes); // print incoming date - Uncomment for debugging + /** + * Turn rumble off and all the LEDs on the specific controller. + * @param controller The controller to write to. + */ + void setAllOff(uint8_t controller) { + setRumbleOn(controller, 0, 0); + setLedOff(controller); + }; - /* Private commands */ - void XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes); - void checkStatus(); + /** + * Turn rumble off the specific controller. + * @param controller The controller to write to. + */ + void setRumbleOff(uint8_t controller) { + setRumbleOn(controller, 0, 0); + }; + /** + * Turn rumble on. + * @param controller The controller to write to. + * @param lValue Left motor (big weight) inside the controller. + * @param rValue Right motor (small weight) inside the controller. + */ + void setRumbleOn(uint8_t controller, uint8_t lValue, uint8_t rValue); + /** + * Set LED value. Without using the ::LED or ::LEDMode enum. + * @param controller The controller to write to. + * @param value See: + * setLedOff(uint8_t controller), setLedOn(uint8_t controller, LED l), + * setLedBlink(uint8_t controller, LED l), and setLedMode(uint8_t controller, LEDMode lm). + */ + void setLedRaw(uint8_t controller, uint8_t value); + + /** + * Turn all LEDs off the specific controller. + * @param controller The controller to write to. + */ + void setLedOff(uint8_t controller) { + setLedRaw(controller, 0); + }; + /** + * Turn on a LED by using the ::LED enum. + * @param controller The controller to write to. + * @param l ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller. + */ + void setLedOn(uint8_t controller, LED l); + /** + * Turn on a LED by using the ::LED enum. + * @param controller The controller to write to. + * @param l ::ALL, ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller. + */ + void setLedBlink(uint8_t controller, LED l); + /** + * Used to set special LED modes supported by the Xbox controller. + * @param controller The controller to write to. + * @param lm See ::LEDMode. + */ + void setLedMode(uint8_t controller, LEDMode lm); + /** + * Used to get the battery level from the controller. + * @param controller The controller to read from. + * @return Returns the battery level in percentage in 33% steps. + */ + uint8_t getBatteryLevel(uint8_t controller); + /** + * Used to check if a button has changed. + * @param controller The controller to read from. + * @return True if a button has changed. + */ + bool buttonChanged(uint8_t controller); + /**@}*/ + + /** True if a wireless receiver is connected. */ + bool XboxReceiverConnected; + /** Variable used to indicate if the XBOX 360 controller is successfully connected. */ + uint8_t Xbox360Connected[4]; + +protected: + /** Pointer to USB class instance. */ + USB *pUsb; + /** Device address. */ + uint8_t bAddress; + /** Endpoint info structure. */ + EpInfo epInfo[XBOX_MAX_ENDPOINTS]; + +private: + bool bPollEnable; + + /* Variables to store the buttons */ + uint32_t ButtonState[4]; + uint32_t OldButtonState[4]; + uint16_t ButtonClickState[4]; + int16_t hatValue[4][4]; + uint16_t controllerStatus[4]; + bool buttonStateChanged[4]; // True if a button has changed + + bool L2Clicked[4]; // These buttons are analog, so we use we use these bools to check if they where clicked or not + bool R2Clicked[4]; + + unsigned long timer; // Timing for checkStatus() signals + + uint8_t readBuf[EP_MAXPKTSIZE]; // General purpose buffer for input data + uint8_t writeBuf[EP_MAXPKTSIZE]; // General purpose buffer for output data + + void readReport(uint8_t controller); // read incoming data + void printReport(uint8_t controller, uint8_t nBytes); // print incoming date - Uncomment for debugging + + /* Private commands */ + void XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes); + void checkStatus(); }; #endif \ No newline at end of file diff --git a/XBOXUSB.cpp b/XBOXUSB.cpp index 93047227..89fdffd2 100644 --- a/XBOXUSB.cpp +++ b/XBOXUSB.cpp @@ -20,323 +20,329 @@ //#define EXTRADEBUG // Uncomment to get even more debugging data //#define PRINTREPORT // Uncomment to print the report send by the Xbox 360 Controller -XBOXUSB::XBOXUSB(USB *p): +XBOXUSB::XBOXUSB(USB *p) : pUsb(p), // pointer to USB class instance - mandatory bAddress(0), // device address - mandatory bPollEnable(false) { // don't start polling before dongle is connected - for(uint8_t i=0; iRegisterDeviceClass(this); //set devConfig[] entry + if (pUsb) // register in USB subsystem + pUsb->RegisterDeviceClass(this); //set devConfig[] entry } uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) { - uint8_t buf[sizeof(USB_DEVICE_DESCRIPTOR)]; - uint8_t rcode; - UsbDevice *p = NULL; - EpInfo *oldep_ptr = NULL; - uint16_t PID; - uint16_t VID; + uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)]; + uint8_t rcode; + UsbDevice *p = NULL; + EpInfo *oldep_ptr = NULL; + uint16_t PID; + uint16_t VID; - // get memory address of USB device address pool - AddressPool &addrPool = pUsb->GetAddressPool(); + // get memory address of USB device address pool + AddressPool &addrPool = pUsb->GetAddressPool(); #ifdef EXTRADEBUG - Notify(PSTR("\r\nXBOXUSB Init"), 0x80); + Notify(PSTR("\r\nXBOXUSB Init"), 0x80); #endif - // check if address has already been assigned to an instance - if (bAddress) { + // check if address has already been assigned to an instance + if (bAddress) { #ifdef DEBUG - Notify(PSTR("\r\nAddress in use"), 0x80); + Notify(PSTR("\r\nAddress in use"), 0x80); #endif - return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; - } + return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; + } - // Get pointer to pseudo device with address 0 assigned - p = addrPool.GetUsbDevicePtr(0); + // Get pointer to pseudo device with address 0 assigned + p = addrPool.GetUsbDevicePtr(0); - if (!p) { + if (!p) { #ifdef DEBUG - Notify(PSTR("\r\nAddress not found"), 0x80); + Notify(PSTR("\r\nAddress not found"), 0x80); #endif - return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - } + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + } - if (!p->epinfo) { + if (!p->epinfo) { #ifdef DEBUG - Notify(PSTR("\r\nepinfo is null"), 0x80); + Notify(PSTR("\r\nepinfo is null"), 0x80); #endif - return USB_ERROR_EPINFO_IS_NULL; - } + return USB_ERROR_EPINFO_IS_NULL; + } - // Save old pointer to EP_RECORD of address 0 - oldep_ptr = p->epinfo; + // Save old pointer to EP_RECORD of address 0 + oldep_ptr = p->epinfo; - // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence - p->epinfo = epInfo; + // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence + p->epinfo = epInfo; - p->lowspeed = lowspeed; + p->lowspeed = lowspeed; - // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data - // Restore p->epinfo - p->epinfo = oldep_ptr; + // Get device descriptor + rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*)buf); // Get device descriptor - addr, ep, nbytes, data + // Restore p->epinfo + p->epinfo = oldep_ptr; - if(rcode) - goto FailGetDevDescr; + if (rcode) + goto FailGetDevDescr; - VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; - PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; + VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor; + PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct; - if(VID != XBOX_VID && VID != MADCATZ_VID && VID != JOYTECH_VID) // We just check if it's a xbox controller using the Vendor ID - goto FailUnknownDevice; - if(PID == XBOX_WIRELESS_PID) { + if (VID != XBOX_VID && VID != MADCATZ_VID && VID != JOYTECH_VID) // We just check if it's a xbox controller using the Vendor ID + goto FailUnknownDevice; + if (PID == XBOX_WIRELESS_PID) { #ifdef DEBUG - Notify(PSTR("\r\nYou have plugged in a wireless Xbox 360 controller - it doesn't support USB communication"), 0x80); + Notify(PSTR("\r\nYou have plugged in a wireless Xbox 360 controller - it doesn't support USB communication"), 0x80); #endif - goto FailUnknownDevice; - } - else if(PID == XBOX_WIRELESS_RECEIVER_PID || PID == XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) { + goto FailUnknownDevice; + } else if (PID == XBOX_WIRELESS_RECEIVER_PID || PID == XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) { #ifdef DEBUG - Notify(PSTR("\r\nThis library only supports Xbox 360 controllers via USB"), 0x80); + Notify(PSTR("\r\nThis library only supports Xbox 360 controllers via USB"), 0x80); #endif - goto FailUnknownDevice; - } + goto FailUnknownDevice; + } - // Allocate new address according to device class - bAddress = addrPool.AllocAddress(parent, false, port); + // Allocate new address according to device class + bAddress = addrPool.AllocAddress(parent, false, port); - if (!bAddress) - return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; + if (!bAddress) + return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; - // Extract Max Packet Size from device descriptor - epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; + // Extract Max Packet Size from device descriptor + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; - // Assign new address to the device - rcode = pUsb->setAddr( 0, 0, bAddress ); - if (rcode) { + // Assign new address to the device + rcode = pUsb->setAddr(0, 0, bAddress); + if (rcode) { + p->lowspeed = false; + addrPool.FreeAddress(bAddress); + bAddress = 0; +#ifdef DEBUG + Notify(PSTR("\r\nsetAddr: "), 0x80); +#endif + PrintHex (rcode, 0x80); + return rcode; + } +#ifdef EXTRADEBUG + Notify(PSTR("\r\nAddr: "), 0x80); + PrintHex (bAddress, 0x80); +#endif p->lowspeed = false; - addrPool.FreeAddress(bAddress); - bAddress = 0; -#ifdef DEBUG - Notify(PSTR("\r\nsetAddr: "), 0x80); -#endif - PrintHex(rcode, 0x80); - return rcode; - } -#ifdef EXTRADEBUG - Notify(PSTR("\r\nAddr: "), 0x80); - PrintHex(bAddress, 0x80); -#endif - p->lowspeed = false; - //get pointer to assigned address record - p = addrPool.GetUsbDevicePtr(bAddress); - if (!p) - return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + //get pointer to assigned address record + p = addrPool.GetUsbDevicePtr(bAddress); + if (!p) + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - p->lowspeed = lowspeed; + p->lowspeed = lowspeed; - // Assign epInfo to epinfo pointer - only EP0 is known - rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); - if (rcode) - goto FailSetDevTblEntry; + // Assign epInfo to epinfo pointer - only EP0 is known + rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); + if (rcode) + goto FailSetDevTblEntry; - /* The application will work in reduced host mode, so we can save program and data - memory space. After verifying the VID we will use known values for the - configuration values for device, interface, endpoints and HID for the XBOX360 Controllers */ + /* The application will work in reduced host mode, so we can save program and data + memory space. After verifying the VID we will use known values for the + configuration values for device, interface, endpoints and HID for the XBOX360 Controllers */ - /* Initialize data structures for endpoints of device */ - epInfo[ XBOX_INPUT_PIPE ].epAddr = 0x01; // XBOX 360 report endpoint - epInfo[ XBOX_INPUT_PIPE ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_INPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_INPUT_PIPE ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_INPUT_PIPE ].bmRcvToggle = bmRCVTOG0; - epInfo[ XBOX_OUTPUT_PIPE ].epAddr = 0x02; // XBOX 360 output endpoint - epInfo[ XBOX_OUTPUT_PIPE ].epAttribs = EP_INTERRUPT; - epInfo[ XBOX_OUTPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints - epInfo[ XBOX_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; - epInfo[ XBOX_OUTPUT_PIPE ].bmSndToggle = bmSNDTOG0; - epInfo[ XBOX_OUTPUT_PIPE ].bmRcvToggle = bmRCVTOG0; + /* Initialize data structures for endpoints of device */ + epInfo[ XBOX_INPUT_PIPE ].epAddr = 0x01; // XBOX 360 report endpoint + epInfo[ XBOX_INPUT_PIPE ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_INPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_INPUT_PIPE ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_INPUT_PIPE ].bmRcvToggle = bmRCVTOG0; + epInfo[ XBOX_OUTPUT_PIPE ].epAddr = 0x02; // XBOX 360 output endpoint + epInfo[ XBOX_OUTPUT_PIPE ].epAttribs = EP_INTERRUPT; + epInfo[ XBOX_OUTPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints + epInfo[ XBOX_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE; + epInfo[ XBOX_OUTPUT_PIPE ].bmSndToggle = bmSNDTOG0; + epInfo[ XBOX_OUTPUT_PIPE ].bmRcvToggle = bmRCVTOG0; - rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo); - if( rcode ) - goto FailSetDevTblEntry; + rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo); + if (rcode) + goto FailSetDevTblEntry; - delay(200);//Give time for address change + delay(200); //Give time for address change - rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1); - if( rcode ) - goto FailSetConf; + rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1); + if (rcode) + goto FailSetConf; #ifdef DEBUG - Notify(PSTR("\r\nXbox 360 Controller Connected\r\n"), 0x80); + Notify(PSTR("\r\nXbox 360 Controller Connected\r\n"), 0x80); #endif - setLedOn(LED1); - Xbox360Connected = true; - bPollEnable = true; - return 0; // successful configuration + setLedOn(LED1); + Xbox360Connected = true; + bPollEnable = true; + return 0; // successful configuration - /* diagnostic messages */ + /* diagnostic messages */ FailGetDevDescr: #ifdef DEBUG - Notify(PSTR("\r\ngetDevDescr:"), 0x80); + Notify(PSTR("\r\ngetDevDescr:"), 0x80); #endif - goto Fail; + goto Fail; FailSetDevTblEntry: #ifdef DEBUG - Notify(PSTR("\r\nsetDevTblEn:"), 0x80); + Notify(PSTR("\r\nsetDevTblEn:"), 0x80); #endif - goto Fail; + goto Fail; FailSetConf: #ifdef DEBUG - Notify(PSTR("\r\nsetConf:"), 0x80); + Notify(PSTR("\r\nsetConf:"), 0x80); #endif - goto Fail; + goto Fail; FailUnknownDevice: #ifdef DEBUG - Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); - PrintHex(VID, 0x80); - Notify(PSTR(" PID: "), 0x80); - PrintHex(PID, 0x80); + Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80); + PrintHex (VID, 0x80); + Notify(PSTR(" PID: "), 0x80); + PrintHex (PID, 0x80); #endif - rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; - goto Fail; + rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; + goto Fail; Fail: #ifdef DEBUG - Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80); - Serial.print(rcode,HEX); + Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80); + Serial.print(rcode, HEX); #endif - Release(); - return rcode; + Release(); + return rcode; } /* Performs a cleanup after failed Init() attempt */ uint8_t XBOXUSB::Release() { - Xbox360Connected = false; - pUsb->GetAddressPool().FreeAddress(bAddress); - bAddress = 0; - bPollEnable = false; - return 0; + Xbox360Connected = false; + pUsb->GetAddressPool().FreeAddress(bAddress); + bAddress = 0; + bPollEnable = false; + return 0; } + uint8_t XBOXUSB::Poll() { - if (!bPollEnable) - return 0; - uint16_t BUFFER_SIZE = EP_MAXPKTSIZE; - pUsb->inTransfer(bAddress, epInfo[ XBOX_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1 - readReport(); + if (!bPollEnable) + return 0; + uint16_t BUFFER_SIZE = EP_MAXPKTSIZE; + pUsb->inTransfer(bAddress, epInfo[ XBOX_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1 + readReport(); #ifdef PRINTREPORT - printReport(); // Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller + printReport(); // Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller #endif - return 0; + return 0; } void XBOXUSB::readReport() { - if (readBuf == NULL) - return; - if(readBuf[0] != 0x00 || readBuf[1] != 0x14) { // Check if it's the correct report - the controller also sends different status reports - return; - } + if (readBuf == NULL) + return; + if (readBuf[0] != 0x00 || readBuf[1] != 0x14) { // Check if it's the correct report - the controller also sends different status reports + return; + } - ButtonState = (uint32_t)(readBuf[5] | ((uint16_t)readBuf[4] << 8) | ((uint32_t)readBuf[3] << 16) | ((uint32_t)readBuf[2] << 24)); + ButtonState = (uint32_t)(readBuf[5] | ((uint16_t)readBuf[4] << 8) | ((uint32_t)readBuf[3] << 16) | ((uint32_t)readBuf[2] << 24)); - hatValue[LeftHatX] = (int16_t)(((uint16_t)readBuf[7] << 8) | readBuf[6]); - hatValue[LeftHatY] = (int16_t)(((uint16_t)readBuf[9] << 8) | readBuf[8]); - hatValue[RightHatX] = (int16_t)(((uint16_t)readBuf[11] << 8) | readBuf[10]); - hatValue[RightHatY] = (int16_t)(((uint16_t)readBuf[13] << 8) | readBuf[12]); + hatValue[LeftHatX] = (int16_t)(((uint16_t)readBuf[7] << 8) | readBuf[6]); + hatValue[LeftHatY] = (int16_t)(((uint16_t)readBuf[9] << 8) | readBuf[8]); + hatValue[RightHatX] = (int16_t)(((uint16_t)readBuf[11] << 8) | readBuf[10]); + hatValue[RightHatY] = (int16_t)(((uint16_t)readBuf[13] << 8) | readBuf[12]); - //Notify(PSTR("\r\nButtonState"), 0x80); - //PrintHex(ButtonState, 0x80); + //Notify(PSTR("\r\nButtonState"), 0x80); + //PrintHex(ButtonState, 0x80); - if(ButtonState != OldButtonState) { - ButtonClickState = (ButtonState >> 16) & ((~OldButtonState) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2 - if(((uint8_t)OldButtonState) == 0 && ((uint8_t)ButtonState) != 0) // The L2 and R2 buttons are special as they are analog buttons - R2Clicked = true; - if((uint8_t)(OldButtonState >> 8) == 0 && (uint8_t)(ButtonState >> 8) != 0) - L2Clicked = true; - OldButtonState = ButtonState; - } + if (ButtonState != OldButtonState) { + ButtonClickState = (ButtonState >> 16) & ((~OldButtonState) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2 + if (((uint8_t)OldButtonState) == 0 && ((uint8_t)ButtonState) != 0) // The L2 and R2 buttons are special as they are analog buttons + R2Clicked = true; + if ((uint8_t)(OldButtonState >> 8) == 0 && (uint8_t)(ButtonState >> 8) != 0) + L2Clicked = true; + OldButtonState = ButtonState; + } } void XBOXUSB::printReport() { //Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller #ifdef PRINTREPORT - if (readBuf == NULL) - return; - for(uint8_t i = 0; i < XBOX_REPORT_BUFFER_SIZE;i++) { - PrintHex(readBuf[i], 0x80); - Serial.print(" "); - } - Serial.println(); + if (readBuf == NULL) + return; + for (uint8_t i = 0; i < XBOX_REPORT_BUFFER_SIZE; i++) { + PrintHex (readBuf[i], 0x80); + Serial.print(" "); + } + Serial.println(); #endif } uint8_t XBOXUSB::getButtonPress(Button b) { - if(b == L2) // These are analog buttons - return (uint8_t)(ButtonState >> 8); - else if(b == R2) - return (uint8_t)ButtonState; - return (ButtonState & ((uint32_t)pgm_read_word(&XBOXBUTTONS[(uint8_t)b]) << 16)); + if (b == L2) // These are analog buttons + return (uint8_t)(ButtonState >> 8); + else if (b == R2) + return (uint8_t)ButtonState; + return (ButtonState & ((uint32_t)pgm_read_word(&XBOXBUTTONS[(uint8_t)b]) << 16)); } + bool XBOXUSB::getButtonClick(Button b) { - if(b == L2) { - if(L2Clicked) { - L2Clicked = false; - return true; + if (b == L2) { + if (L2Clicked) { + L2Clicked = false; + return true; + } + return false; + } else if (b == R2) { + if (R2Clicked) { + R2Clicked = false; + return true; + } + return false; } - return false; - } - else if(b == R2) { - if(R2Clicked) { - R2Clicked = false; - return true; - } - return false; - } - uint16_t button = pgm_read_word(&XBOXBUTTONS[(uint8_t)b]); - bool click = (ButtonClickState & button); - ButtonClickState &= ~button; // clear "click" event - return click; + uint16_t button = pgm_read_word(&XBOXBUTTONS[(uint8_t)b]); + bool click = (ButtonClickState & button); + ButtonClickState &= ~button; // clear "click" event + return click; } + int16_t XBOXUSB::getAnalogHat(AnalogHat a) { - return hatValue[a]; + return hatValue[a]; } /* Xbox Controller commands */ void XBOXUSB::XboxCommand(uint8_t* data, uint16_t nbytes) { - //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x00), Report Type (Output 0x02), interface (0x00), datalength, datalength, data) - pUsb->ctrlReq(bAddress,epInfo[XBOX_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x00, 0x02, 0x00, nbytes, nbytes, data, NULL); + //bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x00), Report Type (Output 0x02), interface (0x00), datalength, datalength, data) + pUsb->ctrlReq(bAddress, epInfo[XBOX_CONTROL_PIPE].epAddr, bmREQ_HID_OUT, HID_REQUEST_SET_REPORT, 0x00, 0x02, 0x00, nbytes, nbytes, data, NULL); } + void XBOXUSB::setLedRaw(uint8_t value) { - writeBuf[0] = 0x01; - writeBuf[1] = 0x03; - writeBuf[2] = value; + writeBuf[0] = 0x01; + writeBuf[1] = 0x03; + writeBuf[2] = value; - XboxCommand(writeBuf, 3); + XboxCommand(writeBuf, 3); } + void XBOXUSB::setLedOn(LED led) { - if(led != ALL) // All LEDs can't be on a the same time - setLedRaw((pgm_read_byte(&XBOXLEDS[(uint8_t)led]))+4); + if (led != ALL) // All LEDs can't be on a the same time + setLedRaw((pgm_read_byte(&XBOXLEDS[(uint8_t)led])) + 4); } -void XBOXUSB::setLedBlink(LED led) { - setLedRaw(pgm_read_byte(&XBOXLEDS[(uint8_t)led])); -} -void XBOXUSB::setLedMode(LEDMode ledMode) { // This function is used to do some speciel LED stuff the controller supports - setLedRaw((uint8_t)ledMode); -} -void XBOXUSB::setRumbleOn(uint8_t lValue, uint8_t rValue) { - writeBuf[0] = 0x00; - writeBuf[1] = 0x08; - writeBuf[2] = 0x00; - writeBuf[3] = lValue; // big weight - writeBuf[4] = rValue; // small weight - writeBuf[5] = 0x00; - writeBuf[6] = 0x00; - writeBuf[7] = 0x00; - XboxCommand(writeBuf, 8); +void XBOXUSB::setLedBlink(LED led) { + setLedRaw(pgm_read_byte(&XBOXLEDS[(uint8_t)led])); +} + +void XBOXUSB::setLedMode(LEDMode ledMode) { // This function is used to do some speciel LED stuff the controller supports + setLedRaw((uint8_t)ledMode); +} + +void XBOXUSB::setRumbleOn(uint8_t lValue, uint8_t rValue) { + writeBuf[0] = 0x00; + writeBuf[1] = 0x08; + writeBuf[2] = 0x00; + writeBuf[3] = lValue; // big weight + writeBuf[4] = rValue; // small weight + writeBuf[5] = 0x00; + writeBuf[6] = 0x00; + writeBuf[7] = 0x00; + + XboxCommand(writeBuf, 8); } diff --git a/XBOXUSB.h b/XBOXUSB.h index ada5a48a..5f0621d3 100644 --- a/XBOXUSB.h +++ b/XBOXUSB.h @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -25,7 +25,7 @@ #endif #include "Usb.h" -#include "xboxEnums.h" +#include "xboxEnums.h" /* Data Xbox 360 taken from descriptors */ #define EP_MAXPKTSIZE 32 // max size for data via USB @@ -58,135 +58,150 @@ /** This class implements support for a Xbox wired controller via USB. */ class XBOXUSB : public USBDeviceConfig { public: - /** - * Constructor for the XBOXUSB class. - * @param pUsb Pointer to USB class instance. - */ - XBOXUSB(USB *pUsb); - - /** @name USBDeviceConfig implementation */ - /** - * Initialize the Xbox Controller. - * @param parent Hub number. - * @param port Port number on the hub. - * @param lowspeed Speed of the device. - * @return 0 on success. - */ - virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed); - /** - * Release the USB device. - * @return 0 on success. - */ - virtual uint8_t Release(); - /** - * Poll the USB Input endpoins and run the state machines. - * @return 0 on success. - */ - virtual uint8_t Poll(); - /** - * Get the device address. - * @return The device address. - */ - virtual uint8_t GetAddress() { return bAddress; }; - /** - * Used to check if the controller has been initialized. - * @return True if it's ready. - */ - virtual bool isReady() { return bPollEnable; }; - /**@}*/ - - /** @name Xbox Controller functions */ - /** - * getButtonPress(Button b) will return true as long as the button is held down. - * - * While getButtonClick(Button b) will only return it once. - * - * So you instance if you need to increase a variable once you would use getButtonClick(Button b), - * but if you need to drive a robot forward you would use getButtonPress(Button b). - * @param b ::Button to read. - * @return getButtonClick(Button b) will return a bool, but getButtonPress(Button b) - * will return a byte if reading ::L2 or ::R2. - */ - uint8_t getButtonPress(Button b); - bool getButtonClick(Button b); - /**@}*/ + /** + * Constructor for the XBOXUSB class. + * @param pUsb Pointer to USB class instance. + */ + XBOXUSB(USB *pUsb); - /** @name Xbox Controller functions */ - /** - * Return the analog value from the joysticks on the controller. - * @param a Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY. - * @return Returns a signed 16-bit integer. - */ - int16_t getAnalogHat(AnalogHat a); - - /** Turn rumble off and all the LEDs on the controller. */ - void setAllOff() { setRumbleOn(0,0); setLedRaw(0); }; - /** Turn rumble off the controller. */ - void setRumbleOff() { setRumbleOn(0,0); }; - /** - * Turn rumble on. - * @param lValue Left motor (big weight) inside the controller. - * @param rValue Right motor (small weight) inside the controller. - */ - void setRumbleOn(uint8_t lValue, uint8_t rValue); - /** - * Set LED value. Without using the ::LED or ::LEDMode enum. - * @param value See: - * setLedOff(), setLedOn(LED l), - * setLedBlink(LED l), and setLedMode(LEDMode lm). - */ - void setLedRaw(uint8_t value); - /** Turn all LEDs off the controller. */ - void setLedOff() { setLedRaw(0); }; - /** - * Turn on a LED by using the ::LED enum. - * @param l ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller. - */ - void setLedOn(LED l); - /** - * Turn on a LED by using the ::LED enum. - * @param l ::ALL, ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller. - */ - void setLedBlink(LED l); - /** - * Used to set special LED modes supported by the Xbox controller. - * @param controller The controller to write to. - * @param lm See ::LEDMode. - */ - void setLedMode(LEDMode lm); - /**@}*/ - - /** True if a Xbox 360 controller is connected. */ - bool Xbox360Connected; - -protected: - /** Pointer to USB class instance. */ - USB *pUsb; - /** Device address. */ - uint8_t bAddress; - /** Endpoint info structure. */ - EpInfo epInfo[XBOX_MAX_ENDPOINTS]; - -private: - bool bPollEnable; + /** @name USBDeviceConfig implementation */ + /** + * Initialize the Xbox Controller. + * @param parent Hub number. + * @param port Port number on the hub. + * @param lowspeed Speed of the device. + * @return 0 on success. + */ + virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed); + /** + * Release the USB device. + * @return 0 on success. + */ + virtual uint8_t Release(); + /** + * Poll the USB Input endpoins and run the state machines. + * @return 0 on success. + */ + virtual uint8_t Poll(); - /* Variables to store the buttons */ - uint32_t ButtonState; - uint32_t OldButtonState; - uint16_t ButtonClickState; - int16_t hatValue[4]; - uint16_t controllerStatus; - - bool L2Clicked; // These buttons are analog, so we use we use these bools to check if they where clicked or not - bool R2Clicked; - - uint8_t readBuf[EP_MAXPKTSIZE]; // General purpose buffer for input data - uint8_t writeBuf[EP_MAXPKTSIZE]; // General purpose buffer for output data - - void readReport(); // read incoming data - void printReport(); // print incoming date - Uncomment for debugging - - /* Private commands */ - void XboxCommand(uint8_t* data, uint16_t nbytes); + /** + * Get the device address. + * @return The device address. + */ + virtual uint8_t GetAddress() { + return bAddress; + }; + + /** + * Used to check if the controller has been initialized. + * @return True if it's ready. + */ + virtual bool isReady() { + return bPollEnable; + }; + /**@}*/ + + /** @name Xbox Controller functions */ + /** + * getButtonPress(Button b) will return true as long as the button is held down. + * + * While getButtonClick(Button b) will only return it once. + * + * So you instance if you need to increase a variable once you would use getButtonClick(Button b), + * but if you need to drive a robot forward you would use getButtonPress(Button b). + * @param b ::Button to read. + * @return getButtonClick(Button b) will return a bool, but getButtonPress(Button b) + * will return a byte if reading ::L2 or ::R2. + */ + uint8_t getButtonPress(Button b); + bool getButtonClick(Button b); + /**@}*/ + + /** @name Xbox Controller functions */ + /** + * Return the analog value from the joysticks on the controller. + * @param a Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY. + * @return Returns a signed 16-bit integer. + */ + int16_t getAnalogHat(AnalogHat a); + + /** Turn rumble off and all the LEDs on the controller. */ + void setAllOff() { + setRumbleOn(0, 0); + setLedRaw(0); + }; + + /** Turn rumble off the controller. */ + void setRumbleOff() { + setRumbleOn(0, 0); + }; + /** + * Turn rumble on. + * @param lValue Left motor (big weight) inside the controller. + * @param rValue Right motor (small weight) inside the controller. + */ + void setRumbleOn(uint8_t lValue, uint8_t rValue); + /** + * Set LED value. Without using the ::LED or ::LEDMode enum. + * @param value See: + * setLedOff(), setLedOn(LED l), + * setLedBlink(LED l), and setLedMode(LEDMode lm). + */ + void setLedRaw(uint8_t value); + + /** Turn all LEDs off the controller. */ + void setLedOff() { + setLedRaw(0); + }; + /** + * Turn on a LED by using the ::LED enum. + * @param l ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller. + */ + void setLedOn(LED l); + /** + * Turn on a LED by using the ::LED enum. + * @param l ::ALL, ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller. + */ + void setLedBlink(LED l); + /** + * Used to set special LED modes supported by the Xbox controller. + * @param controller The controller to write to. + * @param lm See ::LEDMode. + */ + void setLedMode(LEDMode lm); + /**@}*/ + + /** True if a Xbox 360 controller is connected. */ + bool Xbox360Connected; + +protected: + /** Pointer to USB class instance. */ + USB *pUsb; + /** Device address. */ + uint8_t bAddress; + /** Endpoint info structure. */ + EpInfo epInfo[XBOX_MAX_ENDPOINTS]; + +private: + bool bPollEnable; + + /* Variables to store the buttons */ + uint32_t ButtonState; + uint32_t OldButtonState; + uint16_t ButtonClickState; + int16_t hatValue[4]; + uint16_t controllerStatus; + + bool L2Clicked; // These buttons are analog, so we use we use these bools to check if they where clicked or not + bool R2Clicked; + + uint8_t readBuf[EP_MAXPKTSIZE]; // General purpose buffer for input data + uint8_t writeBuf[EP_MAXPKTSIZE]; // General purpose buffer for output data + + void readReport(); // read incoming data + void printReport(); // print incoming date - Uncomment for debugging + + /* Private commands */ + void XboxCommand(uint8_t* data, uint16_t nbytes); }; #endif diff --git a/address.h b/address.h index 2b9de918..b035aac6 100644 --- a/address.h +++ b/address.h @@ -40,9 +40,9 @@ struct EpInfo { uint8_t bmSndToggle : 1; // Send toggle, when zero bmSNDTOG0, bmSNDTOG1 otherwise uint8_t bmRcvToggle : 1; // Send toggle, when zero bmRCVTOG0, bmRCVTOG1 otherwise uint8_t bmNakPower : 6; // Binary order for NAK_LIMIT value - }__attribute__((packed)); + } __attribute__((packed)); }; -}__attribute__((packed)); +} __attribute__((packed)); // 7 6 5 4 3 2 1 0 // --------------------------------- @@ -63,10 +63,10 @@ struct UsbDeviceAddress { uint8_t bmParent : 3; // parent hub address uint8_t bmHub : 1; // hub flag uint8_t bmReserved : 1; // reserved, must be zerro - }__attribute__((packed)); + } __attribute__((packed)); uint8_t devAddress; }; -}__attribute__((packed)); +} __attribute__((packed)); #define bmUSB_DEV_ADDR_ADDRESS 0x07 #define bmUSB_DEV_ADDR_PARENT 0x38 @@ -78,7 +78,7 @@ struct UsbDevice { uint8_t epcount; // number of endpoints bool lowspeed; // indicates if a device is the low speed one // uint8_t devclass; // device class -}__attribute__((packed)); +} __attribute__((packed)); class AddressPool { public: @@ -112,8 +112,8 @@ class AddressPoolImpl : public AddressPool { // Returns thePool index for a given address uint8_t FindAddressIndex(uint8_t address = 0) { - for (uint8_t i = 1; i < MAX_DEVICES_ALLOWED; i++) { - if (thePool[i].address == address) + for(uint8_t i = 1; i < MAX_DEVICES_ALLOWED; i++) { + if(thePool[i].address == address) return i; } return 0; @@ -121,8 +121,8 @@ class AddressPoolImpl : public AddressPool { // Returns thePool child index for a given parent uint8_t FindChildIndex(UsbDeviceAddress addr, uint8_t start = 1) { - for (uint8_t i = (start < 1 || start >= MAX_DEVICES_ALLOWED) ? 1 : start; i < MAX_DEVICES_ALLOWED; i++) { - if (((UsbDeviceAddress*) & thePool[i].address)->bmParent == addr.bmAddress) + for(uint8_t i = (start < 1 || start >= MAX_DEVICES_ALLOWED) ? 1 : start; i < MAX_DEVICES_ALLOWED; i++) { + if(((UsbDeviceAddress*) & thePool[i].address)->bmParent == addr.bmAddress) return i; } return 0; @@ -131,16 +131,16 @@ class AddressPoolImpl : public AddressPool { void FreeAddressByIndex(uint8_t index) { // Zerro field is reserved and should not be affected - if (index == 0) + if(index == 0) return; // If a hub was switched off all port addresses should be freed - if (((UsbDeviceAddress*) & thePool[index].address)->bmHub == 1) { - for (uint8_t i = 1; (i = FindChildIndex(*((UsbDeviceAddress*) & thePool[index].address), i));) + if(((UsbDeviceAddress*) & thePool[index].address)->bmHub == 1) { + for(uint8_t i = 1; (i = FindChildIndex(*((UsbDeviceAddress*) & thePool[index].address), i));) FreeAddressByIndex(i); // If the hub had the last allocated address, hubCounter should be decremented - if (hubCounter == ((UsbDeviceAddress*) & thePool[index].address)->bmAddress) + if(hubCounter == ((UsbDeviceAddress*) & thePool[index].address)->bmAddress) hubCounter--; } InitEntry(index); @@ -148,7 +148,7 @@ class AddressPoolImpl : public AddressPool { // Initializes the whole address pool at once void InitAllAddresses() { - for (uint8_t i = 1; i < MAX_DEVICES_ALLOWED; i++) + for(uint8_t i = 1; i < MAX_DEVICES_ALLOWED; i++) InitEntry(i); hubCounter = 0; @@ -172,22 +172,22 @@ public: // Returns a pointer to a specified address entry virtual UsbDevice* GetUsbDevicePtr(uint8_t addr) { - if (!addr) + if(!addr) return thePool; uint8_t index = FindAddressIndex(addr); - return (!index) ? NULL : thePool + index; + return(!index) ? NULL : thePool + index; }; // Performs an operation specified by pfunc for each addressed device void ForEachUsbDevice(UsbDeviceHandleFunc pfunc) { - if (!pfunc) + if(!pfunc) return; - for (uint8_t i = 1; i < MAX_DEVICES_ALLOWED; i++) - if (thePool[i].address) + for(uint8_t i = 1; i < MAX_DEVICES_ALLOWED; i++) + if(thePool[i].address) pfunc(thePool + i); }; // Allocates new address @@ -196,20 +196,20 @@ public: /* if (parent != 0 && port == 0) Serial.println("PRT:0"); */ - if (parent > 127 || port > 7) + if(parent > 127 || port > 7) return 0; - if (is_hub && hubCounter == 7) + if(is_hub && hubCounter == 7) return 0; // finds first empty address entry starting from one uint8_t index = FindAddressIndex(0); - if (!index) // if empty entry is not found + if(!index) // if empty entry is not found return 0; - if (parent == 0) { - if (is_hub) { + if(parent == 0) { + if(is_hub) { thePool[index].address = 0x41; hubCounter++; } else @@ -222,7 +222,7 @@ public: addr.bmParent = ((UsbDeviceAddress*) & parent)->bmAddress; - if (is_hub) { + if(is_hub) { addr.bmHub = 1; addr.bmAddress = ++hubCounter; } else { @@ -244,7 +244,7 @@ public: virtual void FreeAddress(uint8_t addr) { // if the root hub is disconnected all the addresses should be initialized - if (addr == 0x41) { + if(addr == 0x41) { InitAllAddresses(); return; } diff --git a/adk.cpp b/adk.cpp index 95bf7161..0ea19de0 100644 --- a/adk.cpp +++ b/adk.cpp @@ -41,8 +41,7 @@ pUsb(p), //pointer to USB class instance - mandatory bAddress(0), //device address - mandatory bConfNum(0), //configuration number bNumEP(1), //if config descriptor needs to be parsed -ready(false) - { +ready(false) { // initialize endpoint data structures for (uint8_t i = 0; i < ADK_MAX_ENDPOINTS; i++) { epInfo[i].epAddr = 0; @@ -101,7 +100,7 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*) buf); + rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*)buf); // Restore p->epinfo p->epinfo = oldep_ptr; @@ -114,7 +113,7 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) { bAddress = addrPool.AllocAddress(parent, false, port); // Extract Max Packet Size from device descriptor - epInfo[0].maxPktSize = (uint8_t) ((USB_DEVICE_DESCRIPTOR*) buf)->bMaxPacketSize0; + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; // Assign new address to the device rcode = pUsb->setAddr(0, 0, bAddress); @@ -145,11 +144,11 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) { } //check if ADK device is already in accessory mode; if yes, configure and exit - if (((USB_DEVICE_DESCRIPTOR*) buf)->idVendor == ADK_VID && - (((USB_DEVICE_DESCRIPTOR*) buf)->idProduct == ADK_PID || ((USB_DEVICE_DESCRIPTOR*) buf)->idProduct == ADB_PID)) { + if (((USB_DEVICE_DESCRIPTOR*)buf)->idVendor == ADK_VID && + (((USB_DEVICE_DESCRIPTOR*)buf)->idProduct == ADK_PID || ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct == ADB_PID)) { USBTRACE("\r\nAcc.mode device detected"); /* go through configurations, find first bulk-IN, bulk-OUT EP, fill epInfo and quit */ - num_of_conf = ((USB_DEVICE_DESCRIPTOR*) buf)->bNumConfigurations; + num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; //USBTRACE2("\r\nNC:",num_of_conf); @@ -286,7 +285,7 @@ void ADK::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto // Fill in the endpoint info structure epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); - epInfo[index].maxPktSize = (uint8_t) pep->wMaxPacketSize; + epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; bNumEP++; diff --git a/adk.h b/adk.h index 7f8bf606..4ea0ed63 100644 --- a/adk.h +++ b/adk.h @@ -133,17 +133,17 @@ public: /* returns 2 bytes in *adkproto */ inline uint8_t ADK::getProto(uint8_t* adkproto) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_GET, ADK_GETPROTO, 0, 0, 0, 2, 2, adkproto, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_GET, ADK_GETPROTO, 0, 0, 0, 2, 2, adkproto, NULL)); } /* send ADK string */ inline uint8_t ADK::sendStr(uint8_t index, const char* str) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_SEND, ADK_SENDSTR, 0, 0, index, strlen(str) + 1, strlen(str) + 1, (uint8_t*) str, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_SEND, ADK_SENDSTR, 0, 0, index, strlen(str) + 1, strlen(str) + 1, (uint8_t*) str, NULL)); } /* switch to accessory mode */ inline uint8_t ADK::switchAcc(void) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_SEND, ADK_ACCSTART, 0, 0, 0, 0, 0, NULL, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_SEND, ADK_ACCSTART, 0, 0, 0, 0, 0, NULL, NULL)); } #endif // _ADK_H_ \ No newline at end of file diff --git a/avrpins.h b/avrpins.h index 765a5530..833f9f6b 100644 --- a/avrpins.h +++ b/avrpins.h @@ -178,13 +178,13 @@ public: } static void Set(uint8_t val) { - if (val) + if(val) Set(); else Clear(); } static void SetDir(uint8_t val) { - if (val) + if(val) SetDirWrite(); else SetDirRead(); } @@ -210,12 +210,12 @@ public: } static void WaiteForSet() { - while (IsSet() == 0) { + while(IsSet() == 0) { } } static void WaiteForClear() { - while (IsSet()) { + while(IsSet()) { } } }; //class TPin... @@ -411,7 +411,7 @@ class Tp_Tc { public: static void SetDir(uint8_t val) { - if (val) + if(val) SetDirWrite(); else SetDirRead(); } diff --git a/cdcacm.cpp b/cdcacm.cpp index 9c535a6e..07e1e76a 100644 --- a/cdcacm.cpp +++ b/cdcacm.cpp @@ -79,7 +79,7 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, constBufSize, (uint8_t*) buf); + rcode = pUsb->getDevDescr(0, 0, constBufSize, (uint8_t*)buf); // Restore p->epinfo p->epinfo = oldep_ptr; @@ -94,7 +94,7 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) { return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; // Extract Max Packet Size from the device descriptor - epInfo[0].maxPktSize = (uint8_t) ((USB_DEVICE_DESCRIPTOR*) buf)->bMaxPacketSize0; + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; // Assign new address to the device rcode = pUsb->setAddr(0, 0, bAddress); @@ -118,7 +118,7 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; - num_of_conf = ((USB_DEVICE_DESCRIPTOR*) buf)->bNumConfigurations; + num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; // Assign epInfo to epinfo pointer rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); @@ -226,7 +226,7 @@ void ACM::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto // Fill in the endpoint info structure epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); - epInfo[index].maxPktSize = (uint8_t) pep->wMaxPacketSize; + epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; epInfo[index].epAttribs = 0; bNumEP++; @@ -304,11 +304,11 @@ uint8_t ACM::ClearCommFeature(uint16_t fid) { } uint8_t ACM::SetLineCoding(const LINE_CODING *dataptr) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_CDCOUT, CDC_SET_LINE_CODING, 0x00, 0x00, bControlIface, sizeof (LINE_CODING), sizeof (LINE_CODING), (uint8_t*) dataptr, NULL)); + return ( pUsb->ctrlReq(bAddress, 0, bmREQ_CDCOUT, CDC_SET_LINE_CODING, 0x00, 0x00, bControlIface, sizeof (LINE_CODING), sizeof (LINE_CODING), (uint8_t*)dataptr, NULL)); } uint8_t ACM::GetLineCoding(LINE_CODING *dataptr) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_CDCIN, CDC_GET_LINE_CODING, 0x00, 0x00, bControlIface, sizeof (LINE_CODING), sizeof (LINE_CODING), (uint8_t*) dataptr, NULL)); + return ( pUsb->ctrlReq(bAddress, 0, bmREQ_CDCIN, CDC_GET_LINE_CODING, 0x00, 0x00, bControlIface, sizeof (LINE_CODING), sizeof (LINE_CODING), (uint8_t*)dataptr, NULL)); } uint8_t ACM::SetControlLineState(uint8_t state) { diff --git a/cdcftdi.cpp b/cdcftdi.cpp index 2ebca7ab..65f26cb5 100644 --- a/cdcftdi.cpp +++ b/cdcftdi.cpp @@ -78,7 +78,7 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*) buf); + rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*)buf); // Restore p->epinfo p->epinfo = oldep_ptr; @@ -86,11 +86,11 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) { if (rcode) goto FailGetDevDescr; - if (((USB_DEVICE_DESCRIPTOR*) buf)->idVendor != FTDI_VID || ((USB_DEVICE_DESCRIPTOR*) buf)->idProduct != FTDI_PID) + if (((USB_DEVICE_DESCRIPTOR*)buf)->idVendor != FTDI_VID || ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct != FTDI_PID) return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; // Save type of FTDI chip - wFTDIType = ((USB_DEVICE_DESCRIPTOR*) buf)->bcdDevice; + wFTDIType = ((USB_DEVICE_DESCRIPTOR*)buf)->bcdDevice; // Allocate new address according to device class bAddress = addrPool.AllocAddress(parent, false, port); @@ -99,7 +99,7 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) { return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; // Extract Max Packet Size from the device descriptor - epInfo[0].maxPktSize = (uint8_t) ((USB_DEVICE_DESCRIPTOR*) buf)->bMaxPacketSize0; + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; // Assign new address to the device rcode = pUsb->setAddr(0, 0, bAddress); @@ -123,7 +123,7 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; - num_of_conf = ((USB_DEVICE_DESCRIPTOR*) buf)->bNumConfigurations; + num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; // Assign epInfo to epinfo pointer rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); @@ -224,7 +224,7 @@ void FTDI::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t prot // Fill in the endpoint info structure epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); - epInfo[index].maxPktSize = (uint8_t) pep->wMaxPacketSize; + epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; epInfo[index].epAttribs = 0; bNumEP++; diff --git a/cdcprolific.cpp b/cdcprolific.cpp index f0317730..139a627d 100644 --- a/cdcprolific.cpp +++ b/cdcprolific.cpp @@ -57,7 +57,7 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*) buf); + rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*)buf); // Restore p->epinfo p->epinfo = oldep_ptr; @@ -65,11 +65,11 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) { if (rcode) goto FailGetDevDescr; - if (((USB_DEVICE_DESCRIPTOR*) buf)->idVendor != PL_VID && ((USB_DEVICE_DESCRIPTOR*) buf)->idProduct != PL_PID) + if (((USB_DEVICE_DESCRIPTOR*)buf)->idVendor != PL_VID && ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct != PL_PID) return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; // Save type of PL chip - wPLType = ((USB_DEVICE_DESCRIPTOR*) buf)->bcdDevice; + wPLType = ((USB_DEVICE_DESCRIPTOR*)buf)->bcdDevice; // Allocate new address according to device class bAddress = addrPool.AllocAddress(parent, false, port); @@ -78,7 +78,7 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) { return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; // Extract Max Packet Size from the device descriptor - epInfo[0].maxPktSize = (uint8_t) ((USB_DEVICE_DESCRIPTOR*) buf)->bMaxPacketSize0; + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; // Assign new address to the device rcode = pUsb->setAddr(0, 0, bAddress); @@ -102,7 +102,7 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; - num_of_conf = ((USB_DEVICE_DESCRIPTOR*) buf)->bNumConfigurations; + num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; // Assign epInfo to epinfo pointer rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); diff --git a/confdescparser.h b/confdescparser.h index 9642145f..c446b05e 100644 --- a/confdescparser.h +++ b/confdescparser.h @@ -85,8 +85,8 @@ void ConfigDescParser::Parse(const uin uint16_t cntdn = (uint16_t) len; uint8_t *p = (uint8_t*) pbuf; - while (cntdn) - if (!ParseDescriptor(&p, &cntdn)) + while(cntdn) + if(!ParseDescriptor(&p, &cntdn)) return; } @@ -94,13 +94,13 @@ void ConfigDescParser::Parse(const uin compare masks for them. When the match is found, calls EndpointXtract passing buffer containing endpoint descriptor */ template bool ConfigDescParser::ParseDescriptor(uint8_t **pp, uint16_t *pcntdn) { - switch (stateParseDescr) { + switch(stateParseDescr) { case 0: theBuffer.valueSize = 2; valParser.Initialize(&theBuffer); stateParseDescr = 1; case 1: - if (!valParser.Parse(pp, pcntdn)) + if(!valParser.Parse(pp, pcntdn)) return false; dscrLen = *((uint8_t*) theBuffer.pValue); dscrType = *((uint8_t*) theBuffer.pValue + 1); @@ -114,14 +114,14 @@ bool ConfigDescParser::ParseDescriptor theBuffer.pValue = varBuffer + 2; stateParseDescr = 3; case 3: - switch (dscrType) { + switch(dscrType) { case USB_DESCRIPTOR_INTERFACE: isGoodInterface = false; case USB_DESCRIPTOR_CONFIGURATION: - theBuffer.valueSize = sizeof (USB_CONFIGURATION_DESCRIPTOR) - 2; + theBuffer.valueSize = sizeof(USB_CONFIGURATION_DESCRIPTOR) - 2; break; case USB_DESCRIPTOR_ENDPOINT: - theBuffer.valueSize = sizeof (USB_ENDPOINT_DESCRIPTOR) - 2; + theBuffer.valueSize = sizeof(USB_ENDPOINT_DESCRIPTOR) - 2; break; case HID_DESCRIPTOR_HID: theBuffer.valueSize = dscrLen - 2; @@ -130,20 +130,20 @@ bool ConfigDescParser::ParseDescriptor valParser.Initialize(&theBuffer); stateParseDescr = 4; case 4: - switch (dscrType) { + switch(dscrType) { case USB_DESCRIPTOR_CONFIGURATION: - if (!valParser.Parse(pp, pcntdn)) + if(!valParser.Parse(pp, pcntdn)) return false; confValue = ((USB_CONFIGURATION_DESCRIPTOR*) varBuffer)->bConfigurationValue; break; case USB_DESCRIPTOR_INTERFACE: - if (!valParser.Parse(pp, pcntdn)) + if(!valParser.Parse(pp, pcntdn)) return false; - if ((MASK & CP_MASK_COMPARE_CLASS) && ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceClass != CLASS_ID) + if((MASK & CP_MASK_COMPARE_CLASS) && ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceClass != CLASS_ID) break; - if ((MASK & CP_MASK_COMPARE_SUBCLASS) && ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceSubClass != SUBCLASS_ID) + if((MASK & CP_MASK_COMPARE_SUBCLASS) && ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceSubClass != SUBCLASS_ID) break; - if ((MASK & CP_MASK_COMPARE_PROTOCOL) && ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceProtocol != PROTOCOL_ID) + if((MASK & CP_MASK_COMPARE_PROTOCOL) && ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceProtocol != PROTOCOL_ID) break; isGoodInterface = true; @@ -152,10 +152,10 @@ bool ConfigDescParser::ParseDescriptor protoValue = ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceProtocol; break; case USB_DESCRIPTOR_ENDPOINT: - if (!valParser.Parse(pp, pcntdn)) + if(!valParser.Parse(pp, pcntdn)) return false; - if (isGoodInterface) - if (theXtractor) + if(isGoodInterface) + if(theXtractor) theXtractor->EndpointXtract(confValue, ifaceNumber, ifaceAltSet, protoValue, (USB_ENDPOINT_DESCRIPTOR*) varBuffer); break; //case HID_DESCRIPTOR_HID: @@ -164,7 +164,7 @@ bool ConfigDescParser::ParseDescriptor // PrintHidDescriptor((const USB_HID_DESCRIPTOR*)varBuffer); // break; default: - if (!theSkipper.Skip(pp, pcntdn, dscrLen - 2)) + if(!theSkipper.Skip(pp, pcntdn, dscrLen - 2)) return false; } theBuffer.pValue = varBuffer; @@ -197,7 +197,7 @@ void ConfigDescParser::PrintHidDescrip //Notify(PSTR("\r\nwDescriptorLength:\t")); //PrintHex(pDesc->wDescriptorLength); - for (uint8_t i = 0; i < pDesc->bNumDescriptors; i++) { + for(uint8_t i = 0; i < pDesc->bNumDescriptors; i++) { HID_CLASS_DESCRIPTOR_LEN_AND_TYPE *pLT = (HID_CLASS_DESCRIPTOR_LEN_AND_TYPE*)&(pDesc->bDescrType); Notify(PSTR("\r\nbDescrType:\t\t"), 0x80); diff --git a/controllerEnums.h b/controllerEnums.h index 3137e730..5e35a268 100644 --- a/controllerEnums.h +++ b/controllerEnums.h @@ -1,15 +1,15 @@ /* Copyright (C) 2013 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -21,96 +21,98 @@ /* This header file is used to store different enums for the controllers, This is necessary so all the different libraries can be used at once -*/ + */ /** Enum used to turn on the LEDs on the different controllers. */ enum LED { - LED1 = 0, - LED2 = 1, - LED3 = 2, - LED4 = 3, - - LED5 = 4, - LED6 = 5, - LED7 = 6, - LED8 = 7, - LED9 = 8, - LED10 = 9, - /** Used to blink all LEDs on the Xbox controller */ - ALL = 4, + LED1 = 0, + LED2 = 1, + LED3 = 2, + LED4 = 3, + + LED5 = 4, + LED6 = 5, + LED7 = 6, + LED8 = 7, + LED9 = 8, + LED10 = 9, + /** Used to blink all LEDs on the Xbox controller */ + ALL = 4, }; + /** This enum is used to read all the different buttons on the different controllers */ enum Button { - /**@{*/ - /** These buttons are available on all the the controllers */ - UP = 0, - RIGHT = 1, - DOWN = 2, - LEFT = 3, - /**@}*/ + /**@{*/ + /** These buttons are available on all the the controllers */ + UP = 0, + RIGHT = 1, + DOWN = 2, + LEFT = 3, + /**@}*/ - /**@{*/ - /** Wii buttons */ - PLUS = 5, - TWO = 6, - ONE = 7, - MINUS = 8, - HOME = 9, - Z = 10, - C = 11, - B = 12, - A = 13, - /**@}*/ + /**@{*/ + /** Wii buttons */ + PLUS = 5, + TWO = 6, + ONE = 7, + MINUS = 8, + HOME = 9, + Z = 10, + C = 11, + B = 12, + A = 13, + /**@}*/ - /**@{*/ - /** These are only available on the Wii U Pro Controller */ - L = 16, - R = 17, - ZL = 18, - ZR = 19, - /**@}*/ + /**@{*/ + /** These are only available on the Wii U Pro Controller */ + L = 16, + R = 17, + ZL = 18, + ZR = 19, + /**@}*/ - /**@{*/ - /** PS3 controllers buttons */ - SELECT = 4, - START = 5, - L3 = 6, - R3 = 7, - - L2 = 8, - R2 = 9, - L1 = 10, - R1 = 11, - TRIANGLE = 12, - CIRCLE = 13, - CROSS = 14, - SQUARE = 15, - - PS = 16, - - MOVE = 17, // Covers 12 bits - we only need to read the top 8 - T = 18, // Covers 12 bits - we only need to read the top 8 - /**@}*/ + /**@{*/ + /** PS3 controllers buttons */ + SELECT = 4, + START = 5, + L3 = 6, + R3 = 7, - /**@{*/ - /** Xbox buttons */ - BACK = 4, - X = 14, - Y = 15, - XBOX = 16, - SYNC = 17, - /**@}*/ + L2 = 8, + R2 = 9, + L1 = 10, + R1 = 11, + TRIANGLE = 12, + CIRCLE = 13, + CROSS = 14, + SQUARE = 15, + + PS = 16, + + MOVE = 17, // Covers 12 bits - we only need to read the top 8 + T = 18, // Covers 12 bits - we only need to read the top 8 + /**@}*/ + + /**@{*/ + /** Xbox buttons */ + BACK = 4, + X = 14, + Y = 15, + XBOX = 16, + SYNC = 17, + /**@}*/ }; + /** Joysticks on the PS3 and Xbox controllers. */ -enum AnalogHat { - /** Left joystick x-axis */ - LeftHatX = 0, - /** Left joystick y-axis */ - LeftHatY = 1, - /** Right joystick x-axis */ - RightHatX = 2, - /** Right joystick y-axis */ - RightHatY = 3, +enum AnalogHat { + /** Left joystick x-axis */ + LeftHatX = 0, + /** Left joystick y-axis */ + LeftHatY = 1, + /** Right joystick x-axis */ + RightHatX = 2, + /** Right joystick y-axis */ + RightHatY = 3, }; #endif \ No newline at end of file diff --git a/hexdump.h b/hexdump.h index e70317a3..7328d6e5 100644 --- a/hexdump.h +++ b/hexdump.h @@ -41,15 +41,15 @@ public: template void HexDumper::Parse(const LEN_TYPE len, const uint8_t *pbuf, const OFFSET_TYPE &offset) { - for (LEN_TYPE j = 0; j < len; j++, byteCount++, byteTotal++) { - if (!byteCount) { + for(LEN_TYPE j = 0; j < len; j++, byteCount++, byteTotal++) { + if(!byteCount) { SerialPrintHex (byteTotal); Serial.print(": "); } SerialPrintHex (pbuf[j]); Serial.print(" "); - if (byteCount == 15) { + if(byteCount == 15) { Serial.println(""); byteCount = 0xFF; } diff --git a/hid.cpp b/hid.cpp index 7d61c8fb..9d8fcd0f 100644 --- a/hid.cpp +++ b/hid.cpp @@ -7,7 +7,7 @@ uint8_t HID::GetReportDescr(uint8_t ep, USBReadParser *parser) { uint8_t buf[constBufLen]; uint8_t rcode = pUsb->ctrlReq(bAddress, ep, bmREQ_HIDREPORT, USB_REQUEST_GET_DESCRIPTOR, 0x00, - HID_DESCRIPTOR_REPORT, 0x0000, 128, constBufLen, buf, (USBReadParser*) parser); + HID_DESCRIPTOR_REPORT, 0x0000, 128, constBufLen, buf, (USBReadParser*)parser); //return ((rcode != hrSTALL) ? rcode : 0); return rcode; diff --git a/hidboot.cpp b/hidboot.cpp index 2168ecc7..984680eb 100644 --- a/hidboot.cpp +++ b/hidboot.cpp @@ -17,7 +17,7 @@ e-mail : support@circuitsathome.com #include "hidboot.h" void MouseReportParser::Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf) { - MOUSEINFO *pmi = (MOUSEINFO*) buf; + MOUSEINFO *pmi = (MOUSEINFO*)buf; if (prevState.mouseInfo.bmLeftButton == 0 && pmi->bmLeftButton == 1) OnLeftButtonDown(pmi); @@ -112,20 +112,20 @@ uint8_t KeyboardReportParser::OemToAscii(uint8_t mod, uint8_t key) { // Lower case letters else return (key - 4 + 'a'); - } // Numbers + }// Numbers else if (key > 0x1d && key < 0x27) { if (shift) - return ((uint8_t) pgm_read_byte(&numKeys[key - 0x1e])); + return ((uint8_t)pgm_read_byte(&numKeys[key - 0x1e])); else return (key - 0x1e + '1'); - } // Keypad Numbers + }// Keypad Numbers else if (key > 0x58 && key < 0x62) { if (kbdLockingKeys.kbdLeds.bmNumLock == 1) return (key - 0x59 + '1'); } else if (key > 0x2c && key < 0x39) - return ((shift) ? (uint8_t) pgm_read_byte(&symKeysUp[key - 0x2d]) : (uint8_t) pgm_read_byte(&symKeysLo[key - 0x2d])); + return ((shift) ? (uint8_t)pgm_read_byte(&symKeysUp[key - 0x2d]) : (uint8_t)pgm_read_byte(&symKeysLo[key - 0x2d])); else if (key > 0x53 && key < 0x59) - return (uint8_t) pgm_read_byte(&padKeys[key - 0x54]); + return (uint8_t)pgm_read_byte(&padKeys[key - 0x54]); else { switch (key) { case KEY_SPACE: return (0x20); diff --git a/hidboot.h b/hidboot.h index 71e813d4..581ee566 100644 --- a/hidboot.h +++ b/hidboot.h @@ -60,7 +60,7 @@ class MouseReportParser : public HIDReportParser { union { MOUSEINFO mouseInfo; - uint8_t bInfo[sizeof (MOUSEINFO)]; + uint8_t bInfo[sizeof(MOUSEINFO)]; } prevState; public: @@ -140,7 +140,7 @@ protected: union { KBDINFO kbdInfo; - uint8_t bInfo[sizeof (KBDINFO)]; + uint8_t bInfo[sizeof(KBDINFO)]; } prevState; union { @@ -221,13 +221,13 @@ bPollEnable(false), pRptParser(NULL) { Initialize(); - if (pUsb) + if(pUsb) pUsb->RegisterDeviceClass(this); } template void HIDBoot::Initialize() { - for (uint8_t i = 0; i < totalEndpoints; i++) { + for(uint8_t i = 0; i < totalEndpoints; i++) { epInfo[i].epAddr = 0; epInfo[i].maxPktSize = (i) ? 0 : 8; epInfo[i].epAttribs = 0; @@ -240,7 +240,7 @@ void HIDBoot::Initialize() { template uint8_t HIDBoot::Init(uint8_t parent, uint8_t port, bool lowspeed) { - const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR); + const uint8_t constBufSize = sizeof(USB_DEVICE_DESCRIPTOR); uint8_t buf[constBufSize]; uint8_t rcode; @@ -256,16 +256,16 @@ uint8_t HIDBoot::Init(uint8_t parent, uint8_t port, bool lowspeed USBTRACE("BM Init\r\n"); - if (bAddress) + if(bAddress) return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; // Get pointer to pseudo device with address 0 assigned p = addrPool.GetUsbDevicePtr(0); - if (!p) + if(!p) return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; - if (!p->epinfo) { + if(!p->epinfo) { USBTRACE("epinfo\r\n"); return USB_ERROR_EPINFO_IS_NULL; } @@ -281,10 +281,10 @@ uint8_t HIDBoot::Init(uint8_t parent, uint8_t port, bool lowspeed // Get device descriptor rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*) buf); - if (!rcode) + if(!rcode) len = (buf[0] > constBufSize) ? constBufSize : buf[0]; - if (rcode) { + if(rcode) { // Restore p->epinfo p->epinfo = oldep_ptr; @@ -297,7 +297,7 @@ uint8_t HIDBoot::Init(uint8_t parent, uint8_t port, bool lowspeed // Allocate new address according to device class bAddress = addrPool.AllocAddress(parent, false, port); - if (!bAddress) + if(!bAddress) return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; // Extract Max Packet Size from the device descriptor @@ -306,7 +306,7 @@ uint8_t HIDBoot::Init(uint8_t parent, uint8_t port, bool lowspeed // Assign new address to the device rcode = pUsb->setAddr(0, 0, bAddress); - if (rcode) { + if(rcode) { p->lowspeed = false; addrPool.FreeAddress(bAddress); bAddress = 0; @@ -320,15 +320,15 @@ uint8_t HIDBoot::Init(uint8_t parent, uint8_t port, bool lowspeed p = addrPool.GetUsbDevicePtr(bAddress); - if (!p) + if(!p) return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; p->lowspeed = lowspeed; - if (len) + if(len) rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*) buf); - if (rcode) + if(rcode) goto FailGetDevDescr; num_of_conf = ((USB_DEVICE_DESCRIPTOR*) buf)->bNumConfigurations; @@ -336,12 +336,12 @@ uint8_t HIDBoot::Init(uint8_t parent, uint8_t port, bool lowspeed // Assign epInfo to epinfo pointer rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); - if (rcode) + if(rcode) goto FailSetDevTblEntry; //USBTRACE2("NC:", num_of_conf); - for (uint8_t i = 0; i < num_of_conf; i++) { + for(uint8_t i = 0; i < num_of_conf; i++) { ConfigDescParser< USB_CLASS_HID, HID_BOOT_INTF_SUBCLASS, @@ -350,11 +350,11 @@ uint8_t HIDBoot::Init(uint8_t parent, uint8_t port, bool lowspeed rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser); - if (bNumEP > 1) + if(bNumEP > 1) break; } // for - if (bNumEP < 2) + if(bNumEP < 2) return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; //USBTRACE2("\r\nbAddr:", bAddress); @@ -368,20 +368,20 @@ uint8_t HIDBoot::Init(uint8_t parent, uint8_t port, bool lowspeed // Set Configuration Value rcode = pUsb->setConf(bAddress, 0, bConfNum); - if (rcode) + if(rcode) goto FailSetConfDescr; //USBTRACE2("\r\nIf:", bIfaceNum); rcode = SetProtocol(bIfaceNum, HID_BOOT_PROTOCOL); - if (rcode) + if(rcode) goto FailSetProtocol; - if (BOOT_PROTOCOL == 1) { + if(BOOT_PROTOCOL == 1) { rcode = SetIdle(bIfaceNum, 0, 0); - if (rcode) + if(rcode) goto FailSetIdle; } USBTRACE("BM configured\r\n"); @@ -424,7 +424,7 @@ Fail: template void HIDBoot::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *pep) { // If the first configuration satisfies, the others are not concidered. - if (bNumEP > 1 && conf != bConfNum) + if(bNumEP > 1 && conf != bConfNum) return; bConfNum = conf; @@ -432,7 +432,7 @@ void HIDBoot::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t uint8_t index; - if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) { + if((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) { index = epInterruptInIndex; // Fill in the endpoint info structure @@ -461,10 +461,10 @@ template uint8_t HIDBoot::Poll() { uint8_t rcode = 0; - if (!bPollEnable) + if(!bPollEnable) return 0; - if (qNextPollTime <= millis()) { + if(qNextPollTime <= millis()) { qNextPollTime = millis() + 10; const uint8_t const_buff_len = 16; @@ -474,8 +474,8 @@ uint8_t HIDBoot::Poll() { uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[epInterruptInIndex].epAddr, &read, buf); - if (rcode) { - if (rcode != hrNAK) + if(rcode) { + if(rcode != hrNAK) USBTRACE2("Poll:", rcode); return rcode; } @@ -484,7 +484,7 @@ uint8_t HIDBoot::Poll() { //if (read) // Serial.println(""); - if (pRptParser) + if(pRptParser) pRptParser->Parse((HID*)this, 0, (uint8_t) read, buf); } return rcode; diff --git a/hidescriptorparser.cpp b/hidescriptorparser.cpp index ddb001da..2db1e75c 100644 --- a/hidescriptorparser.cpp +++ b/hidescriptorparser.cpp @@ -1,6 +1,6 @@ #include "hidescriptorparser.h" -const char * const ReportDescParserBase::usagePageTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::usagePageTitles0[] PROGMEM = { pstrUsagePageGenericDesktopControls, pstrUsagePageSimulationControls, pstrUsagePageVRControls, @@ -18,7 +18,7 @@ const char * const ReportDescParserBase::usagePageTitles0[] PROGMEM ={ pstrUsagePageUnicode }; -const char * const ReportDescParserBase::usagePageTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::usagePageTitles1[] PROGMEM = { pstrUsagePageBarCodeScanner, pstrUsagePageScale, pstrUsagePageMSRDevices, @@ -26,7 +26,7 @@ const char * const ReportDescParserBase::usagePageTitles1[] PROGMEM ={ pstrUsagePageCameraControl, pstrUsagePageArcade }; -const char * const ReportDescParserBase::genDesktopTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::genDesktopTitles0[] PROGMEM = { pstrUsagePointer, pstrUsageMouse, pstrUsageJoystick, @@ -37,7 +37,7 @@ const char * const ReportDescParserBase::genDesktopTitles0[] PROGMEM ={ pstrUsageTabletPCSystemControls }; -const char * const ReportDescParserBase::genDesktopTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::genDesktopTitles1[] PROGMEM = { pstrUsageX, pstrUsageY, pstrUsageZ, @@ -64,7 +64,7 @@ const char * const ReportDescParserBase::genDesktopTitles1[] PROGMEM ={ pstrUsageFeatureNotification, pstrUsageResolutionMultiplier }; -const char * const ReportDescParserBase::genDesktopTitles2[] PROGMEM ={ +const char * const ReportDescParserBase::genDesktopTitles2[] PROGMEM = { pstrUsageSystemControl, pstrUsageSystemPowerDown, pstrUsageSystemSleep, @@ -86,7 +86,7 @@ const char * const ReportDescParserBase::genDesktopTitles2[] PROGMEM ={ pstrUsageDPadRight, pstrUsageDPadLeft }; -const char * const ReportDescParserBase::genDesktopTitles3[] PROGMEM ={ +const char * const ReportDescParserBase::genDesktopTitles3[] PROGMEM = { pstrUsageSystemDock, pstrUsageSystemUndock, pstrUsageSystemSetup, @@ -97,7 +97,7 @@ const char * const ReportDescParserBase::genDesktopTitles3[] PROGMEM ={ pstrUsageSystemSpeakerMute, pstrUsageSystemHibernate }; -const char * const ReportDescParserBase::genDesktopTitles4[] PROGMEM ={ +const char * const ReportDescParserBase::genDesktopTitles4[] PROGMEM = { pstrUsageSystemDisplayInvert, pstrUsageSystemDisplayInternal, pstrUsageSystemDisplayExternal, @@ -107,7 +107,7 @@ const char * const ReportDescParserBase::genDesktopTitles4[] PROGMEM ={ pstrUsageSystemDisplaySwapPriSec, pstrUsageSystemDisplayLCDAutoscale }; -const char * const ReportDescParserBase::simuTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::simuTitles0[] PROGMEM = { pstrUsageFlightSimulationDevice, pstrUsageAutomobileSimulationDevice, pstrUsageTankSimulationDevice, @@ -121,7 +121,7 @@ const char * const ReportDescParserBase::simuTitles0[] PROGMEM ={ pstrUsageMagicCarpetSimulationDevice, pstrUsageBicycleSimulationDevice }; -const char * const ReportDescParserBase::simuTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::simuTitles1[] PROGMEM = { pstrUsageFlightControlStick, pstrUsageFlightStick, pstrUsageCyclicControl, @@ -129,7 +129,7 @@ const char * const ReportDescParserBase::simuTitles1[] PROGMEM ={ pstrUsageFlightYoke, pstrUsageTrackControl }; -const char * const ReportDescParserBase::simuTitles2[] PROGMEM ={ +const char * const ReportDescParserBase::simuTitles2[] PROGMEM = { pstrUsageAileron, pstrUsageAileronTrim, pstrUsageAntiTorqueControl, @@ -164,7 +164,7 @@ const char * const ReportDescParserBase::simuTitles2[] PROGMEM ={ pstrUsageFrontBrake, pstrUsageRearBrake }; -const char * const ReportDescParserBase::vrTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::vrTitles0[] PROGMEM = { pstrUsageBelt, pstrUsageBodySuit, pstrUsageFlexor, @@ -176,17 +176,17 @@ const char * const ReportDescParserBase::vrTitles0[] PROGMEM ={ pstrUsageVest, pstrUsageAnimatronicDevice }; -const char * const ReportDescParserBase::vrTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::vrTitles1[] PROGMEM = { pstrUsageStereoEnable, pstrUsageDisplayEnable }; -const char * const ReportDescParserBase::sportsCtrlTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::sportsCtrlTitles0[] PROGMEM = { pstrUsageBaseballBat, pstrUsageGolfClub, pstrUsageRowingMachine, pstrUsageTreadmill }; -const char * const ReportDescParserBase::sportsCtrlTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::sportsCtrlTitles1[] PROGMEM = { pstrUsageOar, pstrUsageSlope, pstrUsageRate, @@ -198,7 +198,7 @@ const char * const ReportDescParserBase::sportsCtrlTitles1[] PROGMEM ={ pstrUsageStickType, pstrUsageStickHeight }; -const char * const ReportDescParserBase::sportsCtrlTitles2[] PROGMEM ={ +const char * const ReportDescParserBase::sportsCtrlTitles2[] PROGMEM = { pstrUsagePutter, pstrUsage1Iron, pstrUsage2Iron, @@ -220,12 +220,12 @@ const char * const ReportDescParserBase::sportsCtrlTitles2[] PROGMEM ={ pstrUsage7Wood, pstrUsage9Wood }; -const char * const ReportDescParserBase::gameTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::gameTitles0[] PROGMEM = { pstrUsage3DGameController, pstrUsagePinballDevice, pstrUsageGunDevice }; -const char * const ReportDescParserBase::gameTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::gameTitles1[] PROGMEM = { pstrUsagePointOfView, pstrUsageTurnRightLeft, pstrUsagePitchForwardBackward, @@ -252,7 +252,7 @@ const char * const ReportDescParserBase::gameTitles1[] PROGMEM ={ pstrUsageGamepadFireJump, pstrUsageGamepadTrigger }; -const char * const ReportDescParserBase::genDevCtrlTitles[] PROGMEM ={ +const char * const ReportDescParserBase::genDevCtrlTitles[] PROGMEM = { pstrUsageBatteryStrength, pstrUsageWirelessChannel, pstrUsageWirelessID, @@ -261,7 +261,7 @@ const char * const ReportDescParserBase::genDevCtrlTitles[] PROGMEM ={ pstrUsageSecurityCodeCharErased, pstrUsageSecurityCodeCleared }; -const char * const ReportDescParserBase::ledTitles[] PROGMEM ={ +const char * const ReportDescParserBase::ledTitles[] PROGMEM = { pstrUsageNumLock, pstrUsageCapsLock, pstrUsageScrollLock, @@ -340,7 +340,7 @@ const char * const ReportDescParserBase::ledTitles[] PROGMEM ={ pstrUsageSystemSuspend, pstrUsageExternalPowerConnected }; -const char * const ReportDescParserBase::telTitles0 [] PROGMEM ={ +const char * const ReportDescParserBase::telTitles0 [] PROGMEM = { pstrUsagePhone, pstrUsageAnsweringMachine, pstrUsageMessageControls, @@ -349,7 +349,7 @@ const char * const ReportDescParserBase::telTitles0 [] PROGMEM ={ pstrUsageTelephonyKeyPad, pstrUsageProgrammableButton }; -const char * const ReportDescParserBase::telTitles1 [] PROGMEM ={ +const char * const ReportDescParserBase::telTitles1 [] PROGMEM = { pstrUsageHookSwitch, pstrUsageFlash, pstrUsageFeature, @@ -369,20 +369,20 @@ const char * const ReportDescParserBase::telTitles1 [] PROGMEM ={ pstrUsageCallerID, pstrUsageSend }; -const char * const ReportDescParserBase::telTitles2 [] PROGMEM ={ +const char * const ReportDescParserBase::telTitles2 [] PROGMEM = { pstrUsageSpeedDial, pstrUsageStoreNumber, pstrUsageRecallNumber, pstrUsagePhoneDirectory }; -const char * const ReportDescParserBase::telTitles3 [] PROGMEM ={ +const char * const ReportDescParserBase::telTitles3 [] PROGMEM = { pstrUsageVoiceMail, pstrUsageScreenCalls, pstrUsageDoNotDisturb, pstrUsageMessage, pstrUsageAnswerOnOff }; -const char * const ReportDescParserBase::telTitles4 [] PROGMEM ={ +const char * const ReportDescParserBase::telTitles4 [] PROGMEM = { pstrUsageInsideDialTone, pstrUsageOutsideDialTone, pstrUsageInsideRingTone, @@ -399,7 +399,7 @@ const char * const ReportDescParserBase::telTitles4 [] PROGMEM ={ pstrUsageOutsideRingback, pstrUsageRinger }; -const char * const ReportDescParserBase::telTitles5 [] PROGMEM ={ +const char * const ReportDescParserBase::telTitles5 [] PROGMEM = { pstrUsagePhoneKey0, pstrUsagePhoneKey1, pstrUsagePhoneKey2, @@ -417,7 +417,7 @@ const char * const ReportDescParserBase::telTitles5 [] PROGMEM ={ pstrUsagePhoneKeyC, pstrUsagePhoneKeyD }; -const char * const ReportDescParserBase::consTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles0[] PROGMEM = { pstrUsageConsumerControl, pstrUsageNumericKeyPad, pstrUsageProgrammableButton, @@ -425,12 +425,12 @@ const char * const ReportDescParserBase::consTitles0[] PROGMEM ={ pstrUsageHeadphone, pstrUsageGraphicEqualizer }; -const char * const ReportDescParserBase::consTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles1[] PROGMEM = { pstrUsagePlus10, pstrUsagePlus100, pstrUsageAMPM }; -const char * const ReportDescParserBase::consTitles2[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles2[] PROGMEM = { pstrUsagePower, pstrUsageReset, pstrUsageSleep, @@ -440,7 +440,7 @@ const char * const ReportDescParserBase::consTitles2[] PROGMEM ={ pstrUsageFunctionButtons }; -const char * const ReportDescParserBase::consTitles3[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles3[] PROGMEM = { pstrUsageMenu, pstrUsageMenuPick, pstrUsageMenuUp, @@ -451,7 +451,7 @@ const char * const ReportDescParserBase::consTitles3[] PROGMEM ={ pstrUsageMenuValueIncrease, pstrUsageMenuValueDecrease }; -const char * const ReportDescParserBase::consTitles4[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles4[] PROGMEM = { pstrUsageDataOnScreen, pstrUsageClosedCaption, pstrUsageClosedCaptionSelect, @@ -460,7 +460,7 @@ const char * const ReportDescParserBase::consTitles4[] PROGMEM ={ pstrUsageSnapshot, pstrUsageStill }; -const char * const ReportDescParserBase::consTitles5[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles5[] PROGMEM = { pstrUsageSelection, pstrUsageAssignSelection, pstrUsageModeStep, @@ -499,7 +499,7 @@ const char * const ReportDescParserBase::consTitles5[] PROGMEM ={ pstrUsageWeekly, pstrUsageMonthly }; -const char * const ReportDescParserBase::consTitles6[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles6[] PROGMEM = { pstrUsagePlay, pstrUsagePause, pstrUsageRecord, @@ -532,7 +532,7 @@ const char * const ReportDescParserBase::consTitles6[] PROGMEM ={ pstrUsagePlayPause, pstrUsagePlaySkip }; -const char * const ReportDescParserBase::consTitles7[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles7[] PROGMEM = { pstrUsageVolume, pstrUsageBalance, pstrUsageMute, @@ -545,7 +545,7 @@ const char * const ReportDescParserBase::consTitles7[] PROGMEM ={ pstrUsageVolumeIncrement, pstrUsageVolumeDecrement }; -const char * const ReportDescParserBase::consTitles8[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles8[] PROGMEM = { pstrUsageSpeedSelect, pstrUsagePlaybackSpeed, pstrUsageStandardPlay, @@ -553,7 +553,7 @@ const char * const ReportDescParserBase::consTitles8[] PROGMEM ={ pstrUsageExtendedPlay, pstrUsageSlow }; -const char * const ReportDescParserBase::consTitles9[] PROGMEM ={ +const char * const ReportDescParserBase::consTitles9[] PROGMEM = { pstrUsageFanEnable, pstrUsageFanSpeed, pstrUsageLightEnable, @@ -569,7 +569,7 @@ const char * const ReportDescParserBase::consTitles9[] PROGMEM ={ pstrUsageHoldupAlarm, pstrUsageMedicalAlarm }; -const char * const ReportDescParserBase::consTitlesA[] PROGMEM ={ +const char * const ReportDescParserBase::consTitlesA[] PROGMEM = { pstrUsageBalanceRight, pstrUsageBalanceLeft, pstrUsageBassIncrement, @@ -577,7 +577,7 @@ const char * const ReportDescParserBase::consTitlesA[] PROGMEM ={ pstrUsageTrebleIncrement, pstrUsageTrebleDecrement }; -const char * const ReportDescParserBase::consTitlesB[] PROGMEM ={ +const char * const ReportDescParserBase::consTitlesB[] PROGMEM = { pstrUsageSpeakerSystem, pstrUsageChannelLeft, pstrUsageChannelRight, @@ -590,14 +590,14 @@ const char * const ReportDescParserBase::consTitlesB[] PROGMEM ={ pstrUsageChannelTop, pstrUsageChannelUnknown }; -const char * const ReportDescParserBase::consTitlesC[] PROGMEM ={ +const char * const ReportDescParserBase::consTitlesC[] PROGMEM = { pstrUsageSubChannel, pstrUsageSubChannelIncrement, pstrUsageSubChannelDecrement, pstrUsageAlternateAudioIncrement, pstrUsageAlternateAudioDecrement }; -const char * const ReportDescParserBase::consTitlesD[] PROGMEM ={ +const char * const ReportDescParserBase::consTitlesD[] PROGMEM = { pstrUsageApplicationLaunchButtons, pstrUsageALLaunchButtonConfigTool, pstrUsageALProgrammableButton, @@ -671,7 +671,7 @@ const char * const ReportDescParserBase::consTitlesD[] PROGMEM ={ pstrUsageALResearchSearchBrowser, pstrUsageALAudioPlayer }; -const char * const ReportDescParserBase::consTitlesE[] PROGMEM ={ +const char * const ReportDescParserBase::consTitlesE[] PROGMEM = { pstrUsageGenericGUIAppControls, pstrUsageACNew, pstrUsageACOpen, @@ -814,7 +814,7 @@ const char * const ReportDescParserBase::consTitlesE[] PROGMEM ={ pstrUsageACDistributeHorizontaly, pstrUsageACDistributeVerticaly }; -const char * const ReportDescParserBase::digitTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::digitTitles0[] PROGMEM = { pstrUsageDigitizer, pstrUsagePen, pstrUsageLightPen, @@ -829,13 +829,13 @@ const char * const ReportDescParserBase::digitTitles0[] PROGMEM ={ pstrUsageMultiplePointDigitizer, pstrUsageFreeSpaceWand }; -const char * const ReportDescParserBase::digitTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::digitTitles1[] PROGMEM = { pstrUsageStylus, pstrUsagePuck, pstrUsageFinger }; -const char * const ReportDescParserBase::digitTitles2[] PROGMEM ={ +const char * const ReportDescParserBase::digitTitles2[] PROGMEM = { pstrUsageTipPressure, pstrUsageBarrelPressure, pstrUsageInRange, @@ -860,11 +860,11 @@ const char * const ReportDescParserBase::digitTitles2[] PROGMEM ={ pstrUsageEraser, pstrUsageTabletPick }; -const char * const ReportDescParserBase::aplphanumTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::aplphanumTitles0[] PROGMEM = { pstrUsageAlphanumericDisplay, pstrUsageBitmappedDisplay }; -const char * const ReportDescParserBase::aplphanumTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::aplphanumTitles1[] PROGMEM = { pstrUsageDisplayAttributesReport, pstrUsageASCIICharacterSet, pstrUsageDataReadBack, @@ -912,7 +912,7 @@ const char * const ReportDescParserBase::aplphanumTitles1[] PROGMEM ={ pstrUsageCharAttributeUnderline, pstrUsageCharAttributeBlink }; -const char * const ReportDescParserBase::aplphanumTitles2[] PROGMEM ={ +const char * const ReportDescParserBase::aplphanumTitles2[] PROGMEM = { pstrUsageBitmapSizeX, pstrUsageBitmapSizeY, pstrUsagePageReserved, @@ -935,7 +935,7 @@ const char * const ReportDescParserBase::aplphanumTitles2[] PROGMEM ={ pstrUsageSoftButtonOffset2, pstrUsageSoftButtonReport }; -const char * const ReportDescParserBase::medInstrTitles0[] PROGMEM ={ +const char * const ReportDescParserBase::medInstrTitles0[] PROGMEM = { pstrUsageVCRAcquisition, pstrUsageFreezeThaw, pstrUsageClipStore, @@ -945,18 +945,18 @@ const char * const ReportDescParserBase::medInstrTitles0[] PROGMEM ={ pstrUsagePrint, pstrUsageMicrophoneEnable }; -const char * const ReportDescParserBase::medInstrTitles1[] PROGMEM ={ +const char * const ReportDescParserBase::medInstrTitles1[] PROGMEM = { pstrUsageCine, pstrUsageTransmitPower, pstrUsageVolume, pstrUsageFocus, pstrUsageDepth }; -const char * const ReportDescParserBase::medInstrTitles2[] PROGMEM ={ +const char * const ReportDescParserBase::medInstrTitles2[] PROGMEM = { pstrUsageSoftStepPrimary, pstrUsageSoftStepSecondary }; -const char * const ReportDescParserBase::medInstrTitles3[] PROGMEM ={ +const char * const ReportDescParserBase::medInstrTitles3[] PROGMEM = { pstrUsageZoomSelect, pstrUsageZoomAdjust, pstrUsageSpectralDopplerModeSelect, @@ -968,14 +968,14 @@ const char * const ReportDescParserBase::medInstrTitles3[] PROGMEM ={ pstrUsage2DModeSelect, pstrUsage2DModeAdjust }; -const char * const ReportDescParserBase::medInstrTitles4[] PROGMEM ={ +const char * const ReportDescParserBase::medInstrTitles4[] PROGMEM = { pstrUsageSoftControlSelect, pstrUsageSoftControlAdjust }; void ReportDescParserBase::Parse(const uint16_t len, const uint8_t *pbuf, const uint16_t &offset) { - uint16_t cntdn = (uint16_t) len; - uint8_t *p = (uint8_t*) pbuf; + uint16_t cntdn = (uint16_t)len; + uint8_t *p = (uint8_t*)pbuf; totalSize = 0; @@ -1108,13 +1108,13 @@ uint8_t ReportDescParserBase::ParseItem(uint8_t **pp, uint16_t *pcntdn) { itemParseState = 3; case 3: { - uint8_t data = *((uint8_t*) varBuffer); + uint8_t data = *((uint8_t*)varBuffer); switch (itemPrefix & (TYPE_MASK | TAG_MASK)) { case (TYPE_LOCAL | TAG_LOCAL_USAGE): if (pfUsage) { if (theBuffer.valueSize > 1) - pfUsage(*((uint16_t*) varBuffer)); + pfUsage(*((uint16_t*)varBuffer)); else pfUsage(data); } @@ -1179,7 +1179,7 @@ uint8_t ReportDescParserBase::ParseItem(uint8_t **pp, uint16_t *pcntdn) { case (TYPE_MAIN | TAG_MAIN_INPUT): case (TYPE_MAIN | TAG_MAIN_OUTPUT): case (TYPE_MAIN | TAG_MAIN_FEATURE): - totalSize += (uint16_t) rptSize * (uint16_t) rptCount; + totalSize += (uint16_t)rptSize * (uint16_t)rptCount; rptSize = 0; rptCount = 0; Notify(PSTR("("), 0x80); @@ -1193,7 +1193,7 @@ uint8_t ReportDescParserBase::ParseItem(uint8_t **pp, uint16_t *pcntdn) { return enErrorSuccess; } -ReportDescParserBase::UsagePageFunc ReportDescParserBase::usagePageFunctions[] /*PROGMEM*/ ={ +ReportDescParserBase::UsagePageFunc ReportDescParserBase::usagePageFunctions[] /*PROGMEM*/ = { &ReportDescParserBase::PrintGenericDesktopPageUsage, &ReportDescParserBase::PrintSimulationControlsPageUsage, &ReportDescParserBase::PrintVRControlsPageUsage, @@ -1240,13 +1240,13 @@ void ReportDescParserBase::PrintUsagePage(uint16_t page) { Notify(pstrSpace, 0x80); if (page > 0x00 && page < 0x11) - Notify((char*) pgm_read_word(&usagePageTitles0[page - 1]), 0x80); + Notify((char*)pgm_read_word(&usagePageTitles0[page - 1]), 0x80); else if (page > 0x7f && page < 0x84) Notify(pstrUsagePageMonitor, 0x80); else if (page > 0x83 && page < 0x8c) Notify(pstrUsagePagePower, 0x80); else if (page > 0x8b && page < 0x92) - Notify((char*) pgm_read_word(&usagePageTitles1[page - 0x8c]), 0x80); + Notify((char*)pgm_read_word(&usagePageTitles1[page - 0x8c]), 0x80); else if (page > 0xfeff && page <= 0xffff) Notify(pstrUsagePageVendorDefined, 0x80); else @@ -1283,15 +1283,15 @@ void ReportDescParserBase::PrintGenericDesktopPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x0a) - Notify((char*) pgm_read_word(&genDesktopTitles0[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&genDesktopTitles0[usage - 1]), 0x80); else if (usage > 0x2f && usage < 0x49) - Notify((char*) pgm_read_word(&genDesktopTitles1[usage - 0x30]), 0x80); + Notify((char*)pgm_read_word(&genDesktopTitles1[usage - 0x30]), 0x80); else if (usage > 0x7f && usage < 0x94) - Notify((char*) pgm_read_word(&genDesktopTitles2[usage - 0x80]), 0x80); + Notify((char*)pgm_read_word(&genDesktopTitles2[usage - 0x80]), 0x80); else if (usage > 0x9f && usage < 0xa9) - Notify((char*) pgm_read_word(&genDesktopTitles3[usage - 0xa0]), 0x80); + Notify((char*)pgm_read_word(&genDesktopTitles3[usage - 0xa0]), 0x80); else if (usage > 0xaf && usage < 0xb8) - Notify((char*) pgm_read_word(&genDesktopTitles4[usage - 0xb0]), 0x80); + Notify((char*)pgm_read_word(&genDesktopTitles4[usage - 0xb0]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1300,11 +1300,11 @@ void ReportDescParserBase::PrintSimulationControlsPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x0d) - Notify((char*) pgm_read_word(&simuTitles0[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&simuTitles0[usage - 1]), 0x80); else if (usage > 0x1f && usage < 0x26) - Notify((char*) pgm_read_word(&simuTitles1[usage - 0x20]), 0x80); + Notify((char*)pgm_read_word(&simuTitles1[usage - 0x20]), 0x80); else if (usage > 0xaf && usage < 0xd1) - Notify((char*) pgm_read_word(&simuTitles2[usage - 0xb0]), 0x80); + Notify((char*)pgm_read_word(&simuTitles2[usage - 0xb0]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1313,9 +1313,9 @@ void ReportDescParserBase::PrintVRControlsPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x0b) - Notify((char*) pgm_read_word(&vrTitles0[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&vrTitles0[usage - 1]), 0x80); else if (usage > 0x1f && usage < 0x22) - Notify((char*) pgm_read_word(&vrTitles1[usage - 0x20]), 0x80); + Notify((char*)pgm_read_word(&vrTitles1[usage - 0x20]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1324,11 +1324,11 @@ void ReportDescParserBase::PrintSportsControlsPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x05) - Notify((char*) pgm_read_word(&sportsCtrlTitles0[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&sportsCtrlTitles0[usage - 1]), 0x80); else if (usage > 0x2f && usage < 0x3a) - Notify((char*) pgm_read_word(&sportsCtrlTitles1[usage - 0x30]), 0x80); + Notify((char*)pgm_read_word(&sportsCtrlTitles1[usage - 0x30]), 0x80); else if (usage > 0x4f && usage < 0x64) - Notify((char*) pgm_read_word(&sportsCtrlTitles2[usage - 0x50]), 0x80); + Notify((char*)pgm_read_word(&sportsCtrlTitles2[usage - 0x50]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1337,9 +1337,9 @@ void ReportDescParserBase::PrintGameControlsPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x04) - Notify((char*) pgm_read_word(&gameTitles0[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&gameTitles0[usage - 1]), 0x80); else if (usage > 0x1f && usage < 0x3a) - Notify((char*) pgm_read_word(&gameTitles1[usage - 0x20]), 0x80); + Notify((char*)pgm_read_word(&gameTitles1[usage - 0x20]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1348,7 +1348,7 @@ void ReportDescParserBase::PrintGenericDeviceControlsPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x1f && usage < 0x27) - Notify((char*) pgm_read_word(&genDevCtrlTitles[usage - 0x20]), 0x80); + Notify((char*)pgm_read_word(&genDevCtrlTitles[usage - 0x20]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1357,7 +1357,7 @@ void ReportDescParserBase::PrintLEDPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x4e) - Notify((char*) pgm_read_word(&ledTitles[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&ledTitles[usage - 1]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1366,17 +1366,17 @@ void ReportDescParserBase::PrintTelephonyPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x08) - Notify((char*) pgm_read_word(&telTitles0[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&telTitles0[usage - 1]), 0x80); else if (usage > 0x1f && usage < 0x32) - Notify((char*) pgm_read_word(&telTitles1[usage - 0x1f]), 0x80); + Notify((char*)pgm_read_word(&telTitles1[usage - 0x1f]), 0x80); else if (usage > 0x4f && usage < 0x54) - Notify((char*) pgm_read_word(&telTitles2[usage - 0x4f]), 0x80); + Notify((char*)pgm_read_word(&telTitles2[usage - 0x4f]), 0x80); else if (usage > 0x6f && usage < 0x75) - Notify((char*) pgm_read_word(&telTitles3[usage - 0x6f]), 0x80); + Notify((char*)pgm_read_word(&telTitles3[usage - 0x6f]), 0x80); else if (usage > 0x8f && usage < 0x9f) - Notify((char*) pgm_read_word(&telTitles4[usage - 0x8f]), 0x80); + Notify((char*)pgm_read_word(&telTitles4[usage - 0x8f]), 0x80); else if (usage > 0xaf && usage < 0xc0) - Notify((char*) pgm_read_word(&telTitles5[usage - 0xaf]), 0x80); + Notify((char*)pgm_read_word(&telTitles5[usage - 0xaf]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1385,35 +1385,35 @@ void ReportDescParserBase::PrintConsumerPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x07) - Notify((char*) pgm_read_word(&consTitles0[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&consTitles0[usage - 1]), 0x80); else if (usage > 0x1f && usage < 0x23) - Notify((char*) pgm_read_word(&consTitles1[usage - 0x1f]), 0x80); + Notify((char*)pgm_read_word(&consTitles1[usage - 0x1f]), 0x80); else if (usage > 0x2f && usage < 0x37) - Notify((char*) pgm_read_word(&consTitles2[usage - 0x2f]), 0x80); + Notify((char*)pgm_read_word(&consTitles2[usage - 0x2f]), 0x80); else if (usage > 0x3f && usage < 0x49) - Notify((char*) pgm_read_word(&consTitles3[usage - 0x3f]), 0x80); + Notify((char*)pgm_read_word(&consTitles3[usage - 0x3f]), 0x80); else if (usage > 0x5f && usage < 0x67) - Notify((char*) pgm_read_word(&consTitles4[usage - 0x5f]), 0x80); + Notify((char*)pgm_read_word(&consTitles4[usage - 0x5f]), 0x80); else if (usage > 0x7f && usage < 0xa5) - Notify((char*) pgm_read_word(&consTitles5[usage - 0x7f]), 0x80); + Notify((char*)pgm_read_word(&consTitles5[usage - 0x7f]), 0x80); else if (usage > 0xaf && usage < 0xcf) - Notify((char*) pgm_read_word(&consTitles6[usage - 0xaf]), 0x80); + Notify((char*)pgm_read_word(&consTitles6[usage - 0xaf]), 0x80); else if (usage > 0xdf && usage < 0xeb) - Notify((char*) pgm_read_word(&consTitles7[usage - 0xdf]), 0x80); + Notify((char*)pgm_read_word(&consTitles7[usage - 0xdf]), 0x80); else if (usage > 0xef && usage < 0xf6) - Notify((char*) pgm_read_word(&consTitles8[usage - 0xef]), 0x80); + Notify((char*)pgm_read_word(&consTitles8[usage - 0xef]), 0x80); else if (usage > 0xff && usage < 0x10e) - Notify((char*) pgm_read_word(&consTitles9[usage - 0xff]), 0x80); + Notify((char*)pgm_read_word(&consTitles9[usage - 0xff]), 0x80); else if (usage > 0x14f && usage < 0x156) - Notify((char*) pgm_read_word(&consTitlesA[usage - 0x14f]), 0x80); + Notify((char*)pgm_read_word(&consTitlesA[usage - 0x14f]), 0x80); else if (usage > 0x15f && usage < 0x16b) - Notify((char*) pgm_read_word(&consTitlesB[usage - 0x15f]), 0x80); + Notify((char*)pgm_read_word(&consTitlesB[usage - 0x15f]), 0x80); else if (usage > 0x16f && usage < 0x175) - Notify((char*) pgm_read_word(&consTitlesC[usage - 0x16f]), 0x80); + Notify((char*)pgm_read_word(&consTitlesC[usage - 0x16f]), 0x80); else if (usage > 0x17f && usage < 0x1c8) - Notify((char*) pgm_read_word(&consTitlesD[usage - 0x17f]), 0x80); + Notify((char*)pgm_read_word(&consTitlesD[usage - 0x17f]), 0x80); else if (usage > 0x1ff && usage < 0x29d) - Notify((char*) pgm_read_word(&consTitlesE[usage - 0x1ff]), 0x80); + Notify((char*)pgm_read_word(&consTitlesE[usage - 0x1ff]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1422,11 +1422,11 @@ void ReportDescParserBase::PrintDigitizerPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x0e) - Notify((char*) pgm_read_word(&digitTitles0[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&digitTitles0[usage - 1]), 0x80); else if (usage > 0x1f && usage < 0x23) - Notify((char*) pgm_read_word(&digitTitles1[usage - 0x1f]), 0x80); + Notify((char*)pgm_read_word(&digitTitles1[usage - 0x1f]), 0x80); else if (usage > 0x2f && usage < 0x47) - Notify((char*) pgm_read_word(&digitTitles2[usage - 0x2f]), 0x80); + Notify((char*)pgm_read_word(&digitTitles2[usage - 0x2f]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1435,11 +1435,11 @@ void ReportDescParserBase::PrintAlphanumDisplayPageUsage(uint16_t usage) { Notify(pstrSpace, 0x80); if (usage > 0x00 && usage < 0x03) - Notify((char*) pgm_read_word(&aplphanumTitles0[usage - 1]), 0x80); + Notify((char*)pgm_read_word(&aplphanumTitles0[usage - 1]), 0x80); else if (usage > 0x1f && usage < 0x4e) - Notify((char*) pgm_read_word(&aplphanumTitles1[usage - 0x1f]), 0x80); + Notify((char*)pgm_read_word(&aplphanumTitles1[usage - 0x1f]), 0x80); else if (usage > 0x7f && usage < 0x96) - Notify((char*) pgm_read_word(&digitTitles2[usage - 0x80]), 0x80); + Notify((char*)pgm_read_word(&digitTitles2[usage - 0x80]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1450,17 +1450,17 @@ void ReportDescParserBase::PrintMedicalInstrumentPageUsage(uint16_t usage) { if (usage == 1) Notify(pstrUsageMedicalUltrasound, 0x80); else if (usage > 0x1f && usage < 0x28) - Notify((char*) pgm_read_word(&medInstrTitles0[usage - 0x1f]), 0x80); + Notify((char*)pgm_read_word(&medInstrTitles0[usage - 0x1f]), 0x80); else if (usage > 0x3f && usage < 0x45) - Notify((char*) pgm_read_word(&medInstrTitles1[usage - 0x40]), 0x80); + Notify((char*)pgm_read_word(&medInstrTitles1[usage - 0x40]), 0x80); else if (usage > 0x5f && usage < 0x62) - Notify((char*) pgm_read_word(&medInstrTitles2[usage - 0x60]), 0x80); + Notify((char*)pgm_read_word(&medInstrTitles2[usage - 0x60]), 0x80); else if (usage == 0x70) Notify(pstrUsageDepthGainCompensation, 0x80); else if (usage > 0x7f && usage < 0x8a) - Notify((char*) pgm_read_word(&medInstrTitles3[usage - 0x80]), 0x80); + Notify((char*)pgm_read_word(&medInstrTitles3[usage - 0x80]), 0x80); else if (usage > 0x9f && usage < 0xa2) - Notify((char*) pgm_read_word(&medInstrTitles4[usage - 0xa0]), 0x80); + Notify((char*)pgm_read_word(&medInstrTitles4[usage - 0xa0]), 0x80); else Notify(pstrUsagePageUndefined, 0x80); } @@ -1497,13 +1497,13 @@ uint8_t ReportDescParser2::ParseItem(uint8_t **pp, uint16_t *pcntdn) { itemParseState = 3; case 3: { - uint8_t data = *((uint8_t*) varBuffer); + uint8_t data = *((uint8_t*)varBuffer); switch (itemPrefix & (TYPE_MASK | TAG_MASK)) { case (TYPE_LOCAL | TAG_LOCAL_USAGE): if (pfUsage) { if (theBuffer.valueSize > 1) - pfUsage(*((uint16_t*) varBuffer)); + pfUsage(*((uint16_t*)varBuffer)); else pfUsage(data); } @@ -1536,7 +1536,7 @@ uint8_t ReportDescParser2::ParseItem(uint8_t **pp, uint16_t *pcntdn) { case (TYPE_MAIN | TAG_MAIN_INPUT): OnInputItem(data); - totalSize += (uint16_t) rptSize * (uint16_t) rptCount; + totalSize += (uint16_t)rptSize * (uint16_t)rptCount; rptSize = 0; rptCount = 0; diff --git a/hidescriptorparser.h b/hidescriptorparser.h index 407f6c7e..1c4a1900 100644 --- a/hidescriptorparser.h +++ b/hidescriptorparser.h @@ -119,7 +119,7 @@ protected: MultiValueBuffer theBuffer; MultiByteValueParser valParser; ByteSkipper theSkipper; - uint8_t varBuffer[sizeof (USB_CONFIGURATION_DESCRIPTOR)]; + uint8_t varBuffer[sizeof(USB_CONFIGURATION_DESCRIPTOR)]; uint8_t itemParseState; // Item parser state variable uint8_t itemSize; // Item size diff --git a/hiduniversal.cpp b/hiduniversal.cpp index 9cdb8f66..2ae0ba3c 100644 --- a/hiduniversal.cpp +++ b/hiduniversal.cpp @@ -112,7 +112,7 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*) buf); + rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*)buf); if (!rcode) len = (buf[0] > constBufSize) ? constBufSize : buf[0]; @@ -134,7 +134,7 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) { return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; // Extract Max Packet Size from the device descriptor - epInfo[0].maxPktSize = (uint8_t) ((USB_DEVICE_DESCRIPTOR*) buf)->bMaxPacketSize0; + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; // Assign new address to the device rcode = pUsb->setAddr(0, 0, bAddress); @@ -159,12 +159,12 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; if (len) - rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*) buf); + rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*)buf); if (rcode) goto FailGetDevDescr; - num_of_conf = ((USB_DEVICE_DESCRIPTOR*) buf)->bNumConfigurations; + num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; // Assign epInfo to epinfo pointer rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); @@ -287,7 +287,7 @@ void HIDUniversal::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint if (index) { // Fill in the endpoint info structure epInfo[bNumEP].epAddr = (pep->bEndpointAddress & 0x0F); - epInfo[bNumEP].maxPktSize = (uint8_t) pep->wMaxPacketSize; + epInfo[bNumEP].maxPktSize = (uint8_t)pep->wMaxPacketSize; epInfo[bNumEP].epAttribs = 0; epInfo[bNumEP].bmNakPower = USB_NAK_NOWAIT; @@ -339,7 +339,7 @@ uint8_t HIDUniversal::Poll() { for (uint8_t i = 0; i < bNumIface; i++) { uint8_t index = hidInterfaces[i].epIndex[epInterruptInIndex]; - uint16_t read = (uint16_t) epInfo[index].maxPktSize; + uint16_t read = (uint16_t)epInfo[index].maxPktSize; ZeroMemory(constBuffLen, buf); @@ -371,7 +371,7 @@ uint8_t HIDUniversal::Poll() { HIDReportParser *prs = GetReportParser(((bHasReportId) ? *buf : 0)); if (prs) - prs->Parse(this, bHasReportId, (uint8_t) read, buf); + prs->Parse(this, bHasReportId, (uint8_t)read, buf); } } return rcode; diff --git a/masstorage.cpp b/masstorage.cpp index 1fca5f55..dde10114 100644 --- a/masstorage.cpp +++ b/masstorage.cpp @@ -82,7 +82,7 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, constBufSize, (uint8_t*) buf); + rcode = pUsb->getDevDescr(0, 0, constBufSize, (uint8_t*)buf); // Restore p->epinfo p->epinfo = oldep_ptr; @@ -97,7 +97,7 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) { return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; // Extract Max Packet Size from the device descriptor - epInfo[0].maxPktSize = (uint8_t) ((USB_DEVICE_DESCRIPTOR*) buf)->bMaxPacketSize0; + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; // Assign new address to the device rcode = pUsb->setAddr(0, 0, bAddress); @@ -121,7 +121,7 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; - num_of_conf = ((USB_DEVICE_DESCRIPTOR*) buf)->bNumConfigurations; + num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; // Assign epInfo to epinfo pointer rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); @@ -220,7 +220,7 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) { PrintHex (capacity.data[i], 0x80); Notify(PSTR("\r\n\r\n"), 0x80); // Only 512/1024/2048/4096 are valid values! - uint32_t c = ((uint32_t) capacity.data[4] << 24) + ((uint32_t) capacity.data[5] << 16) + ((uint32_t) capacity.data[6] << 8) + (uint32_t) capacity.data[7]; + uint32_t c = ((uint32_t)capacity.data[4] << 24) + ((uint32_t)capacity.data[5] << 16) + ((uint32_t)capacity.data[6] << 8) + (uint32_t)capacity.data[7]; if (c != 0x0200LU && c != 0x0400LU && c != 0x0800LU && c != 0x1000LU) { rcode = 255; goto FailInvalidSectorSize; @@ -354,7 +354,7 @@ void BulkOnly::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t // Fill in the endpoint info structure epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); - epInfo[index].maxPktSize = (uint8_t) pep->wMaxPacketSize; + epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; epInfo[index].epAttribs = 0; bNumEP++; @@ -611,7 +611,7 @@ uint8_t BulkOnly::Read(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t block cbw.dCBWSignature = MASS_CBW_SIGNATURE; cbw.dCBWTag = ++dCBWTag; - cbw.dCBWDataTransferLength = ((uint32_t) bsize * blocks); + cbw.dCBWDataTransferLength = ((uint32_t)bsize * blocks); cbw.bmCBWFlags = MASS_CMD_DIR_IN, cbw.bmCBWLUN = lun; cbw.bmCBWCBLength = 10; @@ -638,7 +638,7 @@ uint8_t BulkOnly::Read(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t block cbw.dCBWSignature = MASS_CBW_SIGNATURE; cbw.dCBWTag = ++dCBWTag; - cbw.dCBWDataTransferLength = ((uint32_t) bsize * blocks); + cbw.dCBWDataTransferLength = ((uint32_t)bsize * blocks); cbw.bmCBWFlags = MASS_CMD_DIR_IN, cbw.bmCBWLUN = lun; cbw.bmCBWCBLength = 10; @@ -666,7 +666,7 @@ uint8_t BulkOnly::Write(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t bloc cbw.dCBWSignature = MASS_CBW_SIGNATURE; cbw.dCBWTag = ++dCBWTag; - cbw.dCBWDataTransferLength = ((uint32_t) bsize * blocks); + cbw.dCBWDataTransferLength = ((uint32_t)bsize * blocks); cbw.bmCBWFlags = MASS_CMD_DIR_OUT, cbw.bmCBWLUN = lun; cbw.bmCBWCBLength = 10; @@ -681,7 +681,7 @@ uint8_t BulkOnly::Write(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t bloc cbw.CBWCB[3] = ((addr >> 16) & 0xff); cbw.CBWCB[2] = ((addr >> 24) & 0xff); - return HandleSCSIError(Transaction(&cbw, bsize, (void*) buf, 0)); + return HandleSCSIError(Transaction(&cbw, bsize, (void*)buf, 0)); } uint8_t BulkOnly::ModeSense(uint8_t lun, uint8_t pc, uint8_t page, uint8_t subpage, uint8_t len, uint8_t *pbuf) { @@ -714,7 +714,7 @@ uint8_t BulkOnly::Transaction(CommandBlockWrapper *pcbw, uint16_t buf_size, void ErrorMessage (PSTR("CBW.dCBWTag"), pcbw->dCBWTag); - ret = HandleUsbError(pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, sizeof (CommandBlockWrapper), (uint8_t*) pcbw), epDataOutIndex); + ret = HandleUsbError(pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, sizeof (CommandBlockWrapper), (uint8_t*)pcbw), epDataOutIndex); if (ret) { ErrorMessage (PSTR("CBW"), ret); @@ -732,16 +732,16 @@ uint8_t BulkOnly::Transaction(CommandBlockWrapper *pcbw, uint16_t buf_size, void uint8_t rbuf[read]; uint8_t err = 0; ret = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &read, rbuf); - if (ret == hrSUCCESS) ((USBReadParser*) buf)->Parse(read, rbuf, 0); + if (ret == hrSUCCESS) ((USBReadParser*)buf)->Parse(read, rbuf, 0); if (ret == hrSTALL) err = ClearEpHalt(epDataInIndex); if (ret) { ErrorMessage (PSTR("RDR"), err); return MASS_ERR_GENERAL_USB_ERROR; } } else - ret = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &read, (uint8_t*) buf); + ret = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &read, (uint8_t*)buf); } else - ret = pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, read, (uint8_t*) buf); + ret = pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, read, (uint8_t*)buf); ret = HandleUsbError(ret, (pcbw->bmCBWFlags & MASS_CMD_DIR_IN) ? epDataInIndex : epDataOutIndex); @@ -798,7 +798,7 @@ uint8_t BulkOnly::HandleSCSIError(uint8_t status) { switch (status) { case 0: return MASS_ERR_SUCCESS; - //case 4: return MASS_ERR_UNIT_BUSY; + //case 4: return MASS_ERR_UNIT_BUSY; case 2: ErrorMessage (PSTR("Phase"), status); ResetRecovery(); diff --git a/masstorage.h b/masstorage.h index de16d12b..c30f4ae6 100644 --- a/masstorage.h +++ b/masstorage.h @@ -104,7 +104,7 @@ struct Capacity { uint8_t data[8]; //uint32_t dwBlockAddress; //uint32_t dwBlockLength; -}__attribute__((packed)); +} __attribute__((packed)); struct InquiryResponse { uint8_t DeviceType : 5; @@ -136,14 +136,14 @@ struct InquiryResponse { uint8_t VendorID[8]; uint8_t ProductID[16]; uint8_t RevisionID[4]; -}__attribute__((packed)); +} __attribute__((packed)); struct CommandBlockWrapperBase { uint32_t dCBWSignature; uint32_t dCBWTag; uint32_t dCBWDataTransferLength; uint8_t bmCBWFlags; -}__attribute__((packed)); +} __attribute__((packed)); struct CommandBlockWrapper : public CommandBlockWrapperBase { @@ -158,14 +158,14 @@ struct CommandBlockWrapper : public CommandBlockWrapperBase { }; uint8_t CBWCB[16]; -}__attribute__((packed)); +} __attribute__((packed)); struct CommandStatusWrapper { uint32_t dCSWSignature; uint32_t dCSWTag; uint32_t dCSWDataResidue; uint8_t bCSWStatus; -}__attribute__((packed)); +} __attribute__((packed)); struct RequestSenseResponce { uint8_t bResponseCode; @@ -184,7 +184,7 @@ struct RequestSenseResponce { uint8_t bAdditionalSenseQualifier; uint8_t bFieldReplaceableUnitCode; uint8_t SenseKeySpecific[3]; -}__attribute__((packed)); +} __attribute__((packed)); #define MASS_MAX_ENDPOINTS 3 diff --git a/parsetools.cpp b/parsetools.cpp index 469c3c8e..c302dd2a 100644 --- a/parsetools.cpp +++ b/parsetools.cpp @@ -43,7 +43,7 @@ bool PTPListParser::Parse(uint8_t **pp, uint16_t *pcntdn, PTP_ARRAY_EL_FUNC pf, return false; arLen = 0; - arLen = (pBuf->valueSize >= 4) ? *((uint32_t*) pBuf->pValue) : (uint32_t) (*((uint16_t*) pBuf->pValue)); + arLen = (pBuf->valueSize >= 4) ? *((uint32_t*)pBuf->pValue) : (uint32_t)(*((uint16_t*)pBuf->pValue)); arLenCntdn = arLen; nStage = 2; diff --git a/parsetools.h b/parsetools.h index 9fc9568c..59402df2 100644 --- a/parsetools.h +++ b/parsetools.h @@ -32,7 +32,7 @@ e-mail : support@circuitsathome.com struct MultiValueBuffer { uint8_t valueSize; void *pValue; -}__attribute__((packed)); +} __attribute__((packed)); class MultiByteValueParser { uint8_t * pBuf; @@ -72,17 +72,17 @@ public: }; bool Skip(uint8_t **pp, uint16_t *pcntdn, uint16_t bytes_to_skip) { - switch (nStage) { + switch(nStage) { case 0: countDown = bytes_to_skip; nStage++; case 1: - for (; countDown && (*pcntdn); countDown--, (*pp)++, (*pcntdn)--); + for(; countDown && (*pcntdn); countDown--, (*pp)++, (*pcntdn)--); - if (!countDown) + if(!countDown) nStage = 0; }; - return (!countDown); + return(!countDown); }; }; @@ -132,7 +132,7 @@ public: valSize = val_size; prsMode = mode; - if (prsMode == modeRange) { + if(prsMode == modeRange) { arLenCntdn = arLen = 3; nStage = 2; } else { diff --git a/printhex.h b/printhex.h index 262286c0..4a303884 100644 --- a/printhex.h +++ b/printhex.h @@ -26,19 +26,19 @@ void Notifyc(char c, int lvl); template void PrintHex(T val, int lvl) { - int num_nibbles = sizeof (T) * 2; + int num_nibbles = sizeof(T) * 2; do { char v = 48 + (((val >> (num_nibbles - 1) * 4)) & 0x0f); - if (v > 57) v += 7; + if(v > 57) v += 7; Notifyc(v, lvl); - } while (--num_nibbles); + } while(--num_nibbles); } template void PrintBin(T val, int lvl) { - for (T mask = (((T) 1) << ((sizeof (T) << 3) - 1)); mask; mask >>= 1) - if (val & mask) + for(T mask = (((T) 1) << ((sizeof(T) << 3) - 1)); mask; mask >>= 1) + if(val & mask) Notifyc('1', lvl); else Notifyc('0', lvl); @@ -46,21 +46,21 @@ void PrintBin(T val, int lvl) { template void SerialPrintHex(T val) { - int num_nibbles = sizeof (T) * 2; + int num_nibbles = sizeof(T) * 2; do { char v = 48 + (((val >> (num_nibbles - 1) * 4)) & 0x0f); - if (v > 57) v += 7; + if(v > 57) v += 7; Serial.print(v); - } while (--num_nibbles); + } while(--num_nibbles); } template void PrintHex2(Print *prn, T val) { - T mask = (((T) 1) << (((sizeof (T) << 1) - 1) << 2)); + T mask = (((T) 1) << (((sizeof(T) << 1) - 1) << 2)); - while (mask > 1) { - if (val < mask) + while(mask > 1) { + if(val < mask) prn->print("0"); mask >>= 4; diff --git a/usb_ch9.h b/usb_ch9.h index 9a8d3c99..dd854f5c 100644 --- a/usb_ch9.h +++ b/usb_ch9.h @@ -108,7 +108,7 @@ typedef struct { uint8_t iProduct; // Index of String Descriptor describing the product. uint8_t iSerialNumber; // Index of String Descriptor with the device's serial number. uint8_t bNumConfigurations; // Number of possible configurations. -}__attribute__((packed)) USB_DEVICE_DESCRIPTOR; +} __attribute__((packed)) USB_DEVICE_DESCRIPTOR; /* Configuration descriptor structure */ typedef struct { @@ -120,7 +120,7 @@ typedef struct { uint8_t iConfiguration; // Index of String Descriptor describing the configuration. uint8_t bmAttributes; // Configuration characteristics. uint8_t bMaxPower; // Maximum power consumed by this configuration. -}__attribute__((packed)) USB_CONFIGURATION_DESCRIPTOR; +} __attribute__((packed)) USB_CONFIGURATION_DESCRIPTOR; /* Interface descriptor structure */ typedef struct { @@ -133,7 +133,7 @@ typedef struct { uint8_t bInterfaceSubClass; // Subclass code (assigned by the USB-IF). uint8_t bInterfaceProtocol; // Protocol code (assigned by the USB-IF). 0xFF-Vendor specific. uint8_t iInterface; // Index of String Descriptor describing the interface. -}__attribute__((packed)) USB_INTERFACE_DESCRIPTOR; +} __attribute__((packed)) USB_INTERFACE_DESCRIPTOR; /* Endpoint descriptor structure */ typedef struct { @@ -143,7 +143,7 @@ typedef struct { uint8_t bmAttributes; // Endpoint transfer type. uint16_t wMaxPacketSize; // Maximum packet size. uint8_t bInterval; // Polling interval in frames. -}__attribute__((packed)) USB_ENDPOINT_DESCRIPTOR; +} __attribute__((packed)) USB_ENDPOINT_DESCRIPTOR; /* HID descriptor */ typedef struct { @@ -154,11 +154,11 @@ typedef struct { uint8_t bNumDescriptors; // Number of additional class specific descriptors uint8_t bDescrType; // Type of class descriptor uint16_t wDescriptorLength; // Total size of the Report descriptor -}__attribute__((packed)) USB_HID_DESCRIPTOR; +} __attribute__((packed)) USB_HID_DESCRIPTOR; typedef struct { uint8_t bDescrType; // Type of class descriptor uint16_t wDescriptorLength; // Total size of the Report descriptor -}__attribute__((packed)) HID_CLASS_DESCRIPTOR_LEN_AND_TYPE; +} __attribute__((packed)) HID_CLASS_DESCRIPTOR_LEN_AND_TYPE; #endif // _ch9_h_ diff --git a/usbhost.h b/usbhost.h index 95b87a48..2dd60a06 100644 --- a/usbhost.h +++ b/usbhost.h @@ -96,9 +96,9 @@ template< typename SS, typename INTR > void MAX3421e< SS, INTR >::regWr(uint8_t reg, uint8_t data) { SS::Clear(); SPDR = (reg | 0x02); - while (!(SPSR & (1 << SPIF))); + while(!(SPSR & (1 << SPIF))); SPDR = data; - while (!(SPSR & (1 << SPIF))); + while(!(SPSR & (1 << SPIF))); SS::Set(); return; }; @@ -109,14 +109,14 @@ template< typename SS, typename INTR > uint8_t* MAX3421e< SS, INTR >::bytesWr(uint8_t reg, uint8_t nbytes, uint8_t* data_p) { SS::Clear(); SPDR = (reg | 0x02); //set WR bit and send register number - while (nbytes--) { - while (!(SPSR & (1 << SPIF))); //check if previous byte was sent + while(nbytes--) { + while(!(SPSR & (1 << SPIF))); //check if previous byte was sent SPDR = (*data_p); // send next data byte data_p++; // advance data pointer } - while (!(SPSR & (1 << SPIF))); + while(!(SPSR & (1 << SPIF))); SS::Set(); - return ( data_p); + return( data_p); } /* GPIO write */ /*GPIO byte is split between 2 registers, so two writes are needed to write one byte */ @@ -135,11 +135,11 @@ template< typename SS, typename INTR > uint8_t MAX3421e< SS, INTR >::regRd(uint8_t reg) { SS::Clear(); SPDR = reg; - while (!(SPSR & (1 << SPIF))); + while(!(SPSR & (1 << SPIF))); SPDR = 0; //send empty byte - while (!(SPSR & (1 << SPIF))); + while(!(SPSR & (1 << SPIF))); SS::Set(); - return ( SPDR); + return( SPDR); } /* multiple-byte register read */ @@ -148,16 +148,16 @@ template< typename SS, typename INTR > uint8_t* MAX3421e< SS, INTR >::bytesRd(uint8_t reg, uint8_t nbytes, uint8_t* data_p) { SS::Clear(); SPDR = reg; - while (!(SPSR & (1 << SPIF))); //wait - while (nbytes) { + while(!(SPSR & (1 << SPIF))); //wait + while(nbytes) { SPDR = 0; //send empty byte nbytes--; - while (!(SPSR & (1 << SPIF))); + while(!(SPSR & (1 << SPIF))); *data_p = SPDR; data_p++; } SS::Set(); - return ( data_p); + return( data_p); } /* GPIO read. See gpioWr for explanation */ @@ -168,7 +168,7 @@ uint8_t MAX3421e< SS, INTR >::gpioRd() { gpin = regRd(rIOPINS2); //pins 4-7 gpin &= 0xf0; //clean lower nibble gpin |= (regRd(rIOPINS1) >> 4); //shift low bits and OR with upper from previous operation. - return ( gpin); + return( gpin); } /* reset MAX3421E. Returns number of cycles it took for PLL to stabilize after reset @@ -178,12 +178,12 @@ uint16_t MAX3421e< SS, INTR >::reset() { uint16_t i = 0; regWr(rUSBCTL, bmCHIPRES); regWr(rUSBCTL, 0x00); - while (++i) { - if ((regRd(rUSBIRQ) & bmOSCOKIRQ)) { + while(++i) { + if((regRd(rUSBIRQ) & bmOSCOKIRQ)) { break; } } - return ( i); + return( i); } ///* initialize MAX3421E. Set Host mode, pullups, and stuff. Returns 0 if success, -1 if not */ //template< typename SS, typename INTR > @@ -200,8 +200,8 @@ uint16_t MAX3421e< SS, INTR >::reset() { /* initialize MAX3421E. Set Host mode, pullups, and stuff. Returns 0 if success, -1 if not */ template< typename SS, typename INTR > int8_t MAX3421e< SS, INTR >::Init() { - if (reset() == 0) { //OSCOKIRQ hasn't asserted in time - return ( -1); + if(reset() == 0) { //OSCOKIRQ hasn't asserted in time + return( -1); } regWr(rMODE, bmDPPULLDN | bmDMPULLDN | bmHOST); // set pull-downs, Host @@ -209,13 +209,13 @@ int8_t MAX3421e< SS, INTR >::Init() { /* check if device is connected */ regWr(rHCTL, bmSAMPLEBUS); // sample USB bus - while (!(regRd(rHCTL) & bmSAMPLEBUS)); //wait for sample operation to finish + while(!(regRd(rHCTL) & bmSAMPLEBUS)); //wait for sample operation to finish busprobe(); //check if anything is connected regWr(rHIRQ, bmCONDETIRQ); //clear connection detect interrupt regWr(rCPUCTL, 0x01); //enable interrupt pin - return ( 0); + return( 0); } /* probe bus to determine device presence and speed and switch host to this speed */ @@ -224,9 +224,9 @@ void MAX3421e< SS, INTR >::busprobe() { uint8_t bus_sample; bus_sample = regRd(rHRSL); //Get J,K status bus_sample &= (bmJSTATUS | bmKSTATUS); //zero the rest of the byte - switch (bus_sample) { //start full-speed or low-speed host + switch(bus_sample) { //start full-speed or low-speed host case( bmJSTATUS): - if ((regRd(rMODE) & bmLOWSPEED) == 0) { + if((regRd(rMODE) & bmLOWSPEED) == 0) { regWr(rMODE, MODE_FS_HOST); //start full-speed host vbusState = FSHOST; } else { @@ -235,7 +235,7 @@ void MAX3421e< SS, INTR >::busprobe() { } break; case( bmKSTATUS): - if ((regRd(rMODE) & bmLOWSPEED) == 0) { + if((regRd(rMODE) & bmLOWSPEED) == 0) { regWr(rMODE, MODE_LS_HOST); //start low-speed host vbusState = LSHOST; } else { @@ -262,7 +262,7 @@ uint8_t MAX3421e< SS, INTR >::Task(void) { //Serial.println( vbusState, HEX ); pinvalue = INTR::IsSet(); //Read(); //pinvalue = digitalRead( MAX_INT ); - if (pinvalue == 0) { + if(pinvalue == 0) { rcode = IntHandler(); } // pinvalue = digitalRead( MAX_GPX ); @@ -270,7 +270,7 @@ uint8_t MAX3421e< SS, INTR >::Task(void) { // GpxHandler(); // } // usbSM(); //USB state machine - return ( rcode); + return( rcode); } template< typename SS, typename INTR > @@ -281,13 +281,13 @@ uint8_t MAX3421e< SS, INTR >::IntHandler() { //if( HIRQ & bmFRAMEIRQ ) { //->1ms SOF interrupt handler // HIRQ_sendback |= bmFRAMEIRQ; //}//end FRAMEIRQ handling - if (HIRQ & bmCONDETIRQ) { + if(HIRQ & bmCONDETIRQ) { busprobe(); HIRQ_sendback |= bmCONDETIRQ; } /* End HIRQ interrupts handling, clear serviced IRQs */ regWr(rHIRQ, HIRQ_sendback); - return ( HIRQ_sendback); + return( HIRQ_sendback); } //template< typename SS, typename INTR > //uint8_t MAX3421e< SS, INTR >::GpxHandler() diff --git a/usbhub.cpp b/usbhub.cpp index 7693c2c7..52e20cfb 100644 --- a/usbhub.cpp +++ b/usbhub.cpp @@ -74,7 +74,7 @@ uint8_t USBHub::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->lowspeed = lowspeed; // Get device descriptor - rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*) buf); + rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*)buf); p->lowspeed = false; @@ -89,17 +89,17 @@ uint8_t USBHub::Init(uint8_t parent, uint8_t port, bool lowspeed) { // Extract device class from device descriptor // If device class is not a hub return - if (((USB_DEVICE_DESCRIPTOR*) buf)->bDeviceClass != 0x09) + if (((USB_DEVICE_DESCRIPTOR*)buf)->bDeviceClass != 0x09) return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; // Allocate new address according to device class - bAddress = addrPool.AllocAddress(parent, (((USB_DEVICE_DESCRIPTOR*) buf)->bDeviceClass == 0x09) ? true : false, port); + bAddress = addrPool.AllocAddress(parent, (((USB_DEVICE_DESCRIPTOR*)buf)->bDeviceClass == 0x09) ? true : false, port); if (!bAddress) return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; // Extract Max Packet Size from the device descriptor - epInfo[0].maxPktSize = ((USB_DEVICE_DESCRIPTOR*) buf)->bMaxPacketSize0; + epInfo[0].maxPktSize = ((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; // Assign new address to the device rcode = pUsb->setAddr(0, 0, bAddress); @@ -118,7 +118,7 @@ uint8_t USBHub::Init(uint8_t parent, uint8_t port, bool lowspeed) { p->epinfo = oldep_ptr; if (len) - rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*) buf); + rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*)buf); if (rcode) goto FailGetDevDescr; @@ -139,7 +139,7 @@ uint8_t USBHub::Init(uint8_t parent, uint8_t port, bool lowspeed) { goto FailGetHubDescr; // Save number of ports for future use - bNbrPorts = ((HubDescriptor*) buf)->bNbrPorts; + bNbrPorts = ((HubDescriptor*)buf)->bNbrPorts; bInitState = 2; @@ -148,7 +148,7 @@ uint8_t USBHub::Init(uint8_t parent, uint8_t port, bool lowspeed) { rcode = pUsb->getConfDescr(bAddress, 0, 8, 0, buf); if (!rcode) { - cd_len = ((USB_CONFIGURATION_DESCRIPTOR*) buf)->wTotalLength; + cd_len = ((USB_CONFIGURATION_DESCRIPTOR*)buf)->wTotalLength; rcode = pUsb->getConfDescr(bAddress, 0, cd_len, 0, buf); } if (rcode) diff --git a/usbhub.h b/usbhub.h index 98a8ed7a..0150c4b3 100644 --- a/usbhub.h +++ b/usbhub.h @@ -155,11 +155,11 @@ struct HubDescriptor { uint16_t TTThinkTime : 2; uint16_t PortIndicatorsSupported : 1; uint16_t Reserved : 8; - }__attribute__((packed)); + } __attribute__((packed)); uint8_t bPwrOn2PwrGood; uint8_t bHubContrCurrent; -}__attribute__((packed)); +} __attribute__((packed)); struct HubEvent { @@ -168,11 +168,11 @@ struct HubEvent { struct { uint16_t bmStatus; // port status bits uint16_t bmChange; // port status change bits - }__attribute__((packed)); + } __attribute__((packed)); uint32_t bmEvent; uint8_t evtBuff[4]; }; -}__attribute__((packed)); +} __attribute__((packed)); class USBHub : USBDeviceConfig { static bool bResetInitiated; // True when reset is triggered @@ -216,42 +216,42 @@ public: // Clear Hub Feature inline uint8_t USBHub::ClearHubFeature(uint8_t fid) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_CLEAR_HUB_FEATURE, USB_REQUEST_CLEAR_FEATURE, fid, 0, 0, 0, 0, NULL, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_CLEAR_HUB_FEATURE, USB_REQUEST_CLEAR_FEATURE, fid, 0, 0, 0, 0, NULL, NULL)); } // Clear Port Feature inline uint8_t USBHub::ClearPortFeature(uint8_t fid, uint8_t port, uint8_t sel) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_CLEAR_PORT_FEATURE, USB_REQUEST_CLEAR_FEATURE, fid, 0, ((0x0000 | port) | (sel << 8)), 0, 0, NULL, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_CLEAR_PORT_FEATURE, USB_REQUEST_CLEAR_FEATURE, fid, 0, ((0x0000 | port) | (sel << 8)), 0, 0, NULL, NULL)); } // Get Hub Descriptor inline uint8_t USBHub::GetHubDescriptor(uint8_t index, uint16_t nbytes, uint8_t *dataptr) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_HUB_DESCRIPTOR, USB_REQUEST_GET_DESCRIPTOR, index, 0x29, 0, nbytes, nbytes, dataptr, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_HUB_DESCRIPTOR, USB_REQUEST_GET_DESCRIPTOR, index, 0x29, 0, nbytes, nbytes, dataptr, NULL)); } // Get Hub Status inline uint8_t USBHub::GetHubStatus(uint16_t nbytes, uint8_t* dataptr) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_HUB_STATUS, USB_REQUEST_GET_STATUS, 0, 0, 0x0000, nbytes, nbytes, dataptr, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_HUB_STATUS, USB_REQUEST_GET_STATUS, 0, 0, 0x0000, nbytes, nbytes, dataptr, NULL)); } // Get Port Status inline uint8_t USBHub::GetPortStatus(uint8_t port, uint16_t nbytes, uint8_t* dataptr) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_PORT_STATUS, USB_REQUEST_GET_STATUS, 0, 0, port, nbytes, nbytes, dataptr, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_PORT_STATUS, USB_REQUEST_GET_STATUS, 0, 0, port, nbytes, nbytes, dataptr, NULL)); } // Set Hub Descriptor inline uint8_t USBHub::SetHubDescriptor(uint8_t port, uint16_t nbytes, uint8_t* dataptr) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_HUB_DESCRIPTOR, USB_REQUEST_SET_DESCRIPTOR, 0, 0, port, nbytes, nbytes, dataptr, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_HUB_DESCRIPTOR, USB_REQUEST_SET_DESCRIPTOR, 0, 0, port, nbytes, nbytes, dataptr, NULL)); } // Set Hub Feature inline uint8_t USBHub::SetHubFeature(uint8_t fid) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_HUB_FEATURE, USB_REQUEST_SET_FEATURE, fid, 0, 0, 0, 0, NULL, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_HUB_FEATURE, USB_REQUEST_SET_FEATURE, fid, 0, 0, 0, 0, NULL, NULL)); } // Set Port Feature inline uint8_t USBHub::SetPortFeature(uint8_t fid, uint8_t port, uint8_t sel) { - return ( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_PORT_FEATURE, USB_REQUEST_SET_FEATURE, fid, 0, (((0x0000 | sel) << 8) | port), 0, 0, NULL, NULL)); + return( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_PORT_FEATURE, USB_REQUEST_SET_FEATURE, fid, 0, (((0x0000 | sel) << 8) | port), 0, 0, NULL, NULL)); } void PrintHubPortStatus(USB *usbptr, uint8_t addr, uint8_t port, bool print_changes = false); diff --git a/xboxEnums.h b/xboxEnums.h index d66956f0..adf60f9b 100644 --- a/xboxEnums.h +++ b/xboxEnums.h @@ -1,15 +1,15 @@ /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. - + This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). - + Contact information ------------------- - + Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com @@ -22,43 +22,43 @@ /** Enum used to set special LED modes supported by the Xbox controller. */ enum LEDMode { - ROTATING = 0x0A, - FASTBLINK = 0x0B, - SLOWBLINK = 0x0C, - ALTERNATING = 0x0D, + ROTATING = 0x0A, + FASTBLINK = 0x0B, + SLOWBLINK = 0x0C, + ALTERNATING = 0x0D, }; /** Used to set the LEDs on the controllers */ const uint8_t XBOXLEDS[] PROGMEM = { - 0x02, // LED1 - 0x03, // LED2 - 0x04, // LED3 - 0x05, // LED4 - 0x01 // ALL - Used to blink all LEDs - }; -/** Buttons on the controllers */ + 0x02, // LED1 + 0x03, // LED2 + 0x04, // LED3 + 0x05, // LED4 + 0x01 // ALL - Used to blink all LEDs +}; +/** Buttons on the controllers */ const uint16_t XBOXBUTTONS[] PROGMEM = { - 0x0100, // UP - 0x0800, // RIGHT - 0x0200, // DOWN - 0x0400, // LEFT + 0x0100, // UP + 0x0800, // RIGHT + 0x0200, // DOWN + 0x0400, // LEFT - 0x2000, // BACK - 0x1000, // START - 0x4000, // L3 - 0x8000, // R3 + 0x2000, // BACK + 0x1000, // START + 0x4000, // L3 + 0x8000, // R3 - 0,0, // Skip L2 and R2 as these are analog buttons - 0x0001, // L1 - 0x0002, // R1 + 0, 0, // Skip L2 and R2 as these are analog buttons + 0x0001, // L1 + 0x0002, // R1 - 0x0020, // B - 0x0010, // A - 0x0040, // X - 0x0080, // Y + 0x0020, // B + 0x0010, // A + 0x0040, // X + 0x0080, // Y - 0x0004, // XBOX - 0x0008 // SYNC + 0x0004, // XBOX + 0x0008 // SYNC }; #endif \ No newline at end of file