1 // Copyright 2016, Joey Ferwerda.
2 // SPDX-License-Identifier: BSL-1.0
3 /*
4 * OpenHMD - Free and Open Source API and drivers for immersive technology.
5 */
6
7 /* Sony PSVR Driver - Packet reading code. */
8
9
10 #include "psvr.h"
11
12 #ifdef _MSC_VER
13 #define inline __inline
14 #endif
15
read8(const unsigned char ** buffer)16 inline static uint8_t read8(const unsigned char** buffer)
17 {
18 uint8_t ret = **buffer;
19 *buffer += 1;
20 return ret;
21 }
22
read16(const unsigned char ** buffer)23 inline static int16_t read16(const unsigned char** buffer)
24 {
25 int16_t ret = **buffer | (*(*buffer + 1) << 8);
26 *buffer += 2;
27 return ret;
28 }
29
read32(const unsigned char ** buffer)30 inline static uint32_t read32(const unsigned char** buffer)
31 {
32 uint32_t ret = **buffer | (*(*buffer + 1) << 8) | (*(*buffer + 2) << 16) | (*(*buffer + 3) << 24);
33 *buffer += 4;
34 return ret;
35 }
36
psvr_decode_sensor_packet(psvr_sensor_packet * pkt,const unsigned char * buffer,int size)37 bool psvr_decode_sensor_packet(psvr_sensor_packet* pkt, const unsigned char* buffer, int size)
38 {
39 if(size != 64){
40 LOGE("invalid psvr sensor packet size (expected 64 but got %d)", size);
41 return false;
42 }
43
44 pkt->buttons = read8(&buffer);
45 buffer += 1; //skip 1
46 pkt->volume = read16(&buffer); //volume
47 buffer += 1; //unknown, skip 1
48 pkt->state = read8(&buffer);
49 buffer += 10; //unknown, skip 10
50 pkt->samples[0].tick = read32(&buffer); //TICK
51 // acceleration
52 for(int i = 0; i < 3; i++){
53 pkt->samples[0].gyro[i] = read16(&buffer);
54 }
55
56 // rotation
57 for(int i = 0; i < 3; i++){
58 pkt->samples[0].accel[i] = read16(&buffer);
59 }//34
60 pkt->samples[1].tick = read32(&buffer);
61 for(int i = 0; i < 3; i++){
62 pkt->samples[1].gyro[i] = read16(&buffer);
63 }
64 for(int i = 0; i < 3; i++){
65 pkt->samples[1].accel[i] = read16(&buffer);
66 }//50
67 buffer += 5; //unknown, skip 5
68 pkt->button_raw = read16(&buffer);
69 pkt->proximity = read16(&buffer); // ~150 (nothing) to 1023 (headset is on)
70 buffer += 6; //unknown, skip 6
71 pkt->seq = read8(&buffer);
72
73 return true;
74 }
75