From fc5ceb98a9620c6a7808750de750b174e09611e7 Mon Sep 17 00:00:00 2001 From: "matthias.ringwald" Date: Sun, 13 Jun 2010 19:01:46 +0000 Subject: [PATCH] use 4x4 matrix --- example/WiiMoteOpenGLDemo/Classes/EAGLView.h | 2 +- example/WiiMoteOpenGLDemo/Classes/EAGLView.m | 28 +-- .../Classes/WiiMoteOpenGLDemoAppDelegate.m | 6 +- example/WiiMoteOpenGLDemo/rotation.c | 4 +- example/l2cap-server.c | 170 +++++++++++++++++- example/test.c | 11 +- 6 files changed, 186 insertions(+), 35 deletions(-) diff --git a/example/WiiMoteOpenGLDemo/Classes/EAGLView.h b/example/WiiMoteOpenGLDemo/Classes/EAGLView.h index 9d05d2a69..595776ed0 100644 --- a/example/WiiMoteOpenGLDemo/Classes/EAGLView.h +++ b/example/WiiMoteOpenGLDemo/Classes/EAGLView.h @@ -61,7 +61,7 @@ Note that setting the view non-opaque will only work if the EAGL surface has an - (void)setupView; - (void)checkGLError:(BOOL)visibleCheck; -- (void)setRotationMatrix:(float[3][3]) matrix; +- (void)setRotationMatrix:(float[4][4]) matrix; - (void)setRotationX:(int)X Y:(int)Y Z:(int)Z; - (void)loadTexture; diff --git a/example/WiiMoteOpenGLDemo/Classes/EAGLView.m b/example/WiiMoteOpenGLDemo/Classes/EAGLView.m index f1d4bfa1d..9e2ccb081 100644 --- a/example/WiiMoteOpenGLDemo/Classes/EAGLView.m +++ b/example/WiiMoteOpenGLDemo/Classes/EAGLView.m @@ -362,30 +362,14 @@ } } -- (void)setRotationMatrix:(float[3][3]) matrix{ +- (void)setRotationMatrix:(float[4][4]) matrix{ useRotationMatrix = YES; - - // extend 3 by 3 matrix to 4 by 4 + // copy and linearize matrix + int i,j; int pos = 0; - rotationMatrix[pos++] = matrix[0][0]; - rotationMatrix[pos++] = matrix[0][1]; - rotationMatrix[pos++] = matrix[0][2]; - rotationMatrix[pos++] = 0; - - rotationMatrix[pos++] = matrix[1][0]; - rotationMatrix[pos++] = matrix[1][1]; - rotationMatrix[pos++] = matrix[1][2]; - rotationMatrix[pos++] = 0; - - rotationMatrix[pos++] = matrix[2][0]; - rotationMatrix[pos++] = matrix[2][1]; - rotationMatrix[pos++] = matrix[2][2]; - rotationMatrix[pos++] = 0; - - rotationMatrix[pos++] = 0; - rotationMatrix[pos++] = 0; - rotationMatrix[pos++] = 0; - rotationMatrix[pos++] = 1; + for (i=0;i<4;i++) + for (j=0; j<4; j++) + rotationMatrix[pos++] = matrix[i][j]; } - (void)setRotationX:(int)x Y:(int)y Z:(int)z{ diff --git a/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m b/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m index 62822faa8..27930e400 100644 --- a/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m +++ b/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m @@ -48,7 +48,7 @@ // quaternion rotation library float norm(float *vector, int dim); void normalizeVector(float *vector, int dim); -void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[3][3]); +void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[4][4]); BTDevice *device; uint16_t wiiMoteConHandle = 0; @@ -92,7 +92,9 @@ static void bt_data_cb(uint8_t x, uint8_t y, uint8_t z){ accData[1] = addToHistory( histY, 1 * (y - 128)); accData[2] = addToHistory( histZ, 1 * (z - 128)); - float rotationMatrix[3][3]; + printf("%f, %f, %f\n", accData[0], accData[1], accData[2]); + + float rotationMatrix[4][4]; getRotationMatrixFromVectors(restPosition, accData, rotationMatrix); [[theMainApp glView] setRotationMatrix:rotationMatrix]; diff --git a/example/WiiMoteOpenGLDemo/rotation.c b/example/WiiMoteOpenGLDemo/rotation.c index 9584f93c3..98050858e 100644 --- a/example/WiiMoteOpenGLDemo/rotation.c +++ b/example/WiiMoteOpenGLDemo/rotation.c @@ -128,7 +128,7 @@ void getRotationMatrixFromQuartenion(float q[4], float m[4][4]){ } -void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[3][3]){ +void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[4][4]){ normalizeVector(vout,3); float q[4] = {0,0,0,0}; @@ -147,7 +147,7 @@ void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[3][3 q[0] = sqrt(vin_length * vout_length) + dotprod; q[1] = -1*(vin[1]*vout[2] - vin[2]*vout[1]); q[2] = -1*(vin[2]*vout[0] - vin[0]*vout[2]); - q[3] = -1*(vin[0]*vout[1] - vin[1]*vout[0]); + q[3] = 0;//-1*(vin[0]*vout[1] - vin[1]*vout[0]); normalizeVector(q,4); diff --git a/example/l2cap-server.c b/example/l2cap-server.c index 18fd4a96a..cfe5cb8f4 100644 --- a/example/l2cap-server.c +++ b/example/l2cap-server.c @@ -42,6 +42,7 @@ #include #include +#include hci_con_handle_t con_handle; @@ -55,6 +56,8 @@ void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint uint16_t psm; uint16_t local_cid; uint16_t remote_cid; + char pin[20]; + int i; switch (packet_type) { @@ -96,6 +99,25 @@ void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint bt_send_cmd(&l2cap_accept_connection, local_cid); break; + case HCI_EVENT_LINK_KEY_REQUEST: + // link key request + bt_flip_addr(event_addr, &packet[2]); + bt_send_cmd(&hci_link_key_request_negative_reply, &event_addr); + break; + + case HCI_EVENT_PIN_CODE_REQUEST: + // inform about pin code request + printf("Please enter PIN here: "); + fgets(pin, 20, stdin); + i = strlen(pin)-1; + if( pin[i] == '\n') { + pin[i] = '\0'; + } + printf("PIN = '%s'\n", pin); + bt_flip_addr(event_addr, &packet[2]); + bt_send_cmd(&hci_pin_code_request_reply, &event_addr, strlen(pin), pin); + break; + case L2CAP_EVENT_CHANNEL_OPENED: // inform about new l2cap connection bt_flip_addr(event_addr, &packet[3]); @@ -134,8 +156,9 @@ void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint case HCI_EVENT_DISCONNECTION_COMPLETE: // connection closed -> quit tes app - printf("Basebank connection closed, exit.\n"); - exit(0); + printf("Basebank connection closed\n"); + + // exit(0); break; case HCI_EVENT_COMMAND_COMPLETE: @@ -144,7 +167,7 @@ void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint bt_send_cmd(&hci_write_authentication_enable, 0); } if ( COMMAND_COMPLETE_EVENT(packet, hci_write_authentication_enable) ) { - bt_send_cmd(&hci_write_class_of_device, 0x2580); + bt_send_cmd(&hci_write_class_of_device, 0x2540); } default: // other event @@ -158,6 +181,139 @@ void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint } } +void create_serial_port_service(uint8_t * service){ + + // create RFCOMM service: UUID 0x1101, attributes serviceClass + de_create_sequence(service); + + // 0x0001 + uint8_t *serviceClassIDList = de_push_sequence(service); + de_add_number(serviceClassIDList, DE_UINT, DE_SIZE_16, 0x0001); + uint8_t *serviceClasses = de_push_sequence(serviceClassIDList); + de_add_number(serviceClasses, DE_UUID, DE_SIZE_16, 0x1101); + de_pop_sequence(serviceClassIDList, serviceClasses); + de_pop_sequence(service, serviceClassIDList); + + // 0x0004 + uint8_t *protocolDescriptorList = de_push_sequence(service); + de_add_number(protocolDescriptorList, DE_UINT, DE_SIZE_16, 0x0004); + uint8_t * protocolStack = de_push_sequence(protocolDescriptorList); + uint8_t * l2cpProtocol = de_push_sequence(protocolStack); + de_add_number(l2cpProtocol, DE_UUID, DE_SIZE_16, 0x0100); + de_pop_sequence(protocolStack, l2cpProtocol); + uint8_t * rfcommChannel = de_push_sequence(protocolStack); + de_add_number(rfcommChannel, DE_UUID, DE_SIZE_16, 0x0003); + de_add_number(rfcommChannel, DE_UINT, DE_SIZE_8, 0x0001); + de_pop_sequence(protocolStack, rfcommChannel); + de_pop_sequence(protocolDescriptorList, protocolStack); + de_pop_sequence(service, protocolDescriptorList); + + // 0x0005 + uint8_t *browseGroupList = de_push_sequence(service); + de_add_number(browseGroupList, DE_UINT, DE_SIZE_16, 0x0005); + uint8_t *groupList = de_push_sequence(browseGroupList); + de_add_number(groupList, DE_UUID, DE_SIZE_16, 0x1002); + de_pop_sequence(browseGroupList, groupList); + de_pop_sequence(service, browseGroupList); + + // 0x0006 + uint8_t *languageBaseAttributeIDList = de_push_sequence(service); + de_add_number(languageBaseAttributeIDList, DE_UINT, DE_SIZE_16, 0x0006); + uint8_t *languageAttributes = de_push_sequence(languageBaseAttributeIDList); + de_add_number(languageAttributes, DE_UINT, DE_SIZE_16, 0x656e); + de_add_number(languageAttributes, DE_UINT, DE_SIZE_16, 0x006a); + de_add_number(languageAttributes, DE_UINT, DE_SIZE_16, 0x0100); + de_pop_sequence(languageBaseAttributeIDList, languageAttributes); + de_pop_sequence(service, languageBaseAttributeIDList); + + // 0x0100 + uint8_t *serviceName = de_push_sequence(service); + de_add_number(serviceName, DE_UINT, DE_SIZE_16, 0x0100); + de_add_data(serviceName, DE_STRING, 6, (uint8_t *) "RFCOMM"); + de_pop_sequence(service, serviceName); +} + +void create_hid_service(uint8_t * service){ + + // create HID service + de_create_sequence(service); + + + // 0x0001 + uint8_t *serviceClassIDList = de_push_sequence(service); + de_add_number(serviceClassIDList, DE_UINT, DE_SIZE_16, 0x0001); + uint8_t *serviceClasses = de_push_sequence(serviceClassIDList); + de_add_number(serviceClasses, DE_UUID, DE_SIZE_16, 0x1124); + de_pop_sequence(serviceClassIDList, serviceClasses); + de_pop_sequence(service, serviceClassIDList); + + // 0x0004 + uint8_t *protocolDescriptorList = de_push_sequence(service); + de_add_number(protocolDescriptorList, DE_UINT, DE_SIZE_16, 0x0004); + uint8_t * protocolStack = de_push_sequence(protocolDescriptorList); + uint8_t * l2cpProtocol = de_push_sequence(protocolStack); + de_add_number(l2cpProtocol, DE_UUID, DE_SIZE_16, 0x0100); + de_add_number(l2cpProtocol, DE_UUID, DE_SIZE_16, 0x0011); + de_pop_sequence(protocolStack, l2cpProtocol); + uint8_t * hidp = de_push_sequence(protocolStack); + de_add_number(hidp, DE_UUID, DE_SIZE_16, 0x0011); + de_pop_sequence(protocolStack, hidp); + de_pop_sequence(protocolDescriptorList, protocolStack); + de_pop_sequence(service, protocolDescriptorList); + + // 0x0005 + uint8_t *browseGroupList = de_push_sequence(service); + de_add_number(browseGroupList, DE_UINT, DE_SIZE_16, 0x0005); + uint8_t *groupList = de_push_sequence(browseGroupList); + de_add_number(groupList, DE_UUID, DE_SIZE_16, 0x1002); + de_pop_sequence(browseGroupList, groupList); + de_pop_sequence(service, browseGroupList); + + // 0x0006 + uint8_t *languageBaseAttributeIDList = de_push_sequence(service); + de_add_number(languageBaseAttributeIDList, DE_UINT, DE_SIZE_16, 0x0006); + uint8_t *languageAttributes = de_push_sequence(languageBaseAttributeIDList); + de_add_number(languageAttributes, DE_UINT, DE_SIZE_16, 0x656e); + de_add_number(languageAttributes, DE_UINT, DE_SIZE_16, 0x006a); + de_add_number(languageAttributes, DE_UINT, DE_SIZE_16, 0x0100); + de_pop_sequence(languageBaseAttributeIDList, languageAttributes); + de_pop_sequence(service, languageBaseAttributeIDList); + + // 0x0009 + uint8_t *bluetoothProfileDescriptorList = de_push_sequence(service); + de_add_number(bluetoothProfileDescriptorList, DE_UINT, DE_SIZE_16, 0x0009); + uint8_t * profileDescriptors = de_push_sequence(bluetoothProfileDescriptorList); + uint8_t * hidProtocol = de_push_sequence(profileDescriptors); + de_add_number(hidProtocol, DE_UUID, DE_SIZE_16, 0x1124); + de_add_number(hidProtocol, DE_UINT, DE_SIZE_16, 0x0100); + de_pop_sequence(profileDescriptors, hidProtocol); + de_pop_sequence(bluetoothProfileDescriptorList, profileDescriptors); + de_pop_sequence(service, bluetoothProfileDescriptorList); + + // 0x000d + uint8_t *additionalProtocolDescriptorLists = de_push_sequence(service); + de_add_number(additionalProtocolDescriptorLists, DE_UINT, DE_SIZE_16, 0x000d); + protocolDescriptorList = de_push_sequence(additionalProtocolDescriptorLists); + protocolStack = de_push_sequence(protocolDescriptorList); + l2cpProtocol = de_push_sequence(protocolStack); + de_add_number(l2cpProtocol, DE_UUID, DE_SIZE_16, 0x0100); + de_add_number(l2cpProtocol, DE_UUID, DE_SIZE_16, 0x0013); + de_pop_sequence(protocolStack, l2cpProtocol); + hidp = de_push_sequence(protocolStack); + de_add_number(hidp, DE_UUID, DE_SIZE_16, 0x0011); + de_pop_sequence(protocolStack, hidp); + de_pop_sequence(protocolDescriptorList, protocolStack); + de_pop_sequence(additionalProtocolDescriptorLists, protocolDescriptorList); + de_pop_sequence(service, additionalProtocolDescriptorLists); + + // 0x0100 + uint8_t *serviceName = de_push_sequence(service); + de_add_number(serviceName, DE_UINT, DE_SIZE_16, 0x0100); + de_add_data(serviceName, DE_STRING, 16, (uint8_t *) "BTstack Keyboard"); + de_pop_sequence(service, serviceName); +} + + int main (int argc, const char * argv[]){ run_loop_init(RUN_LOOP_POSIX); int err = bt_open(); @@ -168,6 +324,14 @@ int main (int argc, const char * argv[]){ bt_register_packet_handler(packet_handler); bt_send_cmd(&l2cap_register_service, 0x11, 250); bt_send_cmd(&l2cap_register_service, 0x13, 250); + + // done + uint8_t service[200]; + create_hid_service(service); + de_dump_data_element(service); + + bt_send_cmd(&sdp_register_service_record, service); + bt_send_cmd(&btstack_set_power_mode, HCI_POWER_ON ); run_loop_execute(); bt_close(); diff --git a/example/test.c b/example/test.c index 5f1a90d48..35e9203af 100644 --- a/example/test.c +++ b/example/test.c @@ -44,7 +44,8 @@ #include // bd_addr_t addr = {0x00, 0x03, 0xc9, 0x3d, 0x77, 0x43 }; // Think Outside Keyboard -bd_addr_t addr = {0x00, 0x19, 0x1d, 0x90, 0x44, 0x68 }; // WiiMote +// bd_addr_t addr = {0x00, 0x19, 0x1d, 0x90, 0x44, 0x68 }; // WiiMote +bd_addr_t addr = {0x76, 0x6d, 0x62, 0xdb, 0xca, 0x73 }; // iPad hci_con_handle_t con_handle; uint16_t source_cid_interrupt; @@ -113,11 +114,11 @@ void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint } else { source_cid_control = source_cid; // request acceleration data.. - uint8_t setMode31[] = { 0x52, 0x12, 0x00, 0x31 }; - bt_send_l2cap( source_cid, setMode31, sizeof(setMode31)); + // uint8_t setMode31[] = { 0x52, 0x12, 0x00, 0x31 }; + // bt_send_l2cap( source_cid, setMode31, sizeof(setMode31)); // stop blinking - uint8_t setLEDs[] = { 0x52, 0x11, 0x10 }; - bt_send_l2cap( source_cid, setLEDs, sizeof(setLEDs)); + // uint8_t setLEDs[] = { 0x52, 0x11, 0x10 }; + // bt_send_l2cap( source_cid, setLEDs, sizeof(setLEDs)); } } else { printf("L2CAP connection to device ");