UnrealXR/nrealAirLinuxDriver
Archived
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Correct timeouts and remove broken brightness command

Browse source 
Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>
This commit is contained in:
TheJackiMonster 2023-04-30 14:24:13 +02:00
parent 0a85530624
commit 8cf3221944
No known key found for this signature in database
GPG key ID: D850A5F772E880F9
4 changed files with 88 additions and 47 deletions
Download patch file Download diff file Expand all files Collapse all files

2
examples/debug_d3/src/debug.c
View file

@ -56,7 +56,7 @@ int main(int argc, const char** argv) {
device3_clear(dev3); device3_clear(dev3);
while (dev3) { while (dev3) {
if (device3_read(dev3, 0) < 0) { if (device3_read(dev3, -1) < 0) {
break; break;
} }
} }

2
examples/debug_d4/src/debug.c
View file

@ -58,7 +58,7 @@ int main(int argc, const char** argv) {
device4_clear(dev4); device4_clear(dev4);
while (dev4) { while (dev4) {
if (device4_read(dev4, 0) < 0) { if (device4_read(dev4, 1000) < 0) {
break; break;
} }
} }

47
interface_lib/src/device3.c
View file

@ -84,9 +84,9 @@ static bool recv_payload(device3_type* device, uint8_t size, uint8_t* payload) {
transferred = payload_size; transferred = payload_size;
} }
/*if (transferred == 0) { if (transferred == 0) {
return false; return false;
}*/ }
if (transferred != payload_size) { if (transferred != payload_size) {
perror("ERROR: receiving payload failed\n"); perror("ERROR: receiving payload failed\n");
@ -223,8 +223,11 @@ device3_type* device3_open(device3_event_callback callback) {
perror("No handle!\n"); perror("No handle!\n");
return device; return device;
} }
device3_clear(device);
if (!send_payload_msg(device, DEVICE3_MSG_GET_STATIC_ID, 0, NULL)) { if (!send_payload_msg(device, DEVICE3_MSG_GET_STATIC_ID, 0, NULL)) {
perror("Failed sending payload to get static id!\n");
return device; return device;
} }
@ -239,6 +242,7 @@ device3_type* device3_open(device3_event_callback callback) {
device3_reset_calibration(device); device3_reset_calibration(device);
if (!send_payload_msg(device, DEVICE3_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) { if (!send_payload_msg(device, DEVICE3_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) {
perror("Failed sending payload to get calibration data length!\n");
return device; return device;
} }
@ -292,6 +296,7 @@ device3_type* device3_open(device3_event_callback callback) {
} }
if (!send_payload_msg_signal(device, DEVICE3_MSG_START_IMU_DATA, 0x1)) { if (!send_payload_msg_signal(device, DEVICE3_MSG_START_IMU_DATA, 0x1)) {
perror("Failed sending payload to start imu data stream!\n");
return device; return device;
} }
@ -585,8 +590,10 @@ static void apply_calibration(const device3_type* device,
hardIronOffset.axis.y = cy / my; hardIronOffset.axis.y = cy / my;
hardIronOffset.axis.z = cz / mz; hardIronOffset.axis.z = cz / mz;
device->calibration->softIronMatrix = softIronMatrix; if (device->calibration) {
device->calibration->hardIronOffset = hardIronOffset; device->calibration->softIronMatrix = softIronMatrix;
device->calibration->hardIronOffset = hardIronOffset;
}
*magnetometer = FusionCalibrationMagnetic( *magnetometer = FusionCalibrationMagnetic(
*magnetometer, *magnetometer,
@ -602,7 +609,7 @@ static void apply_calibration(const device3_type* device,
} }
void device3_clear(device3_type* device) { void device3_clear(device3_type* device) {
device3_read(device, 0); device3_read(device, 10);
} }
int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet) { int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet) {
@ -636,9 +643,9 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
MAX_PACKET_SIZE MAX_PACKET_SIZE
); );
/*if (transferred == 0) { if (transferred == 0) {
continue; continue;
}*/ }
if (MAX_PACKET_SIZE != transferred) { if (MAX_PACKET_SIZE != transferred) {
perror("Not expected issue!\n"); perror("Not expected issue!\n");
@ -730,15 +737,16 @@ int device3_read(device3_type* device, int timeout) {
device3_packet_type packet; device3_packet_type packet;
memset(&packet, 0, sizeof(device3_packet_type)); memset(&packet, 0, sizeof(device3_packet_type));
int transferred = hid_read( int transferred = hid_read_timeout(
device->handle, device->handle,
(uint8_t*) &packet, (uint8_t*) &packet,
MAX_PACKET_SIZE MAX_PACKET_SIZE,
timeout
); );
/*if (transferred == 0) { if (transferred == 0) {
return 1; return 1;
}*/ }
if (MAX_PACKET_SIZE != transferred) { if (MAX_PACKET_SIZE != transferred) {
perror("Not expected issue!\n"); perror("Not expected issue!\n");
@ -774,21 +782,25 @@ int device3_read(device3_type* device, int timeout) {
readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer);
apply_calibration(device, &gyroscope, &accelerometer, &magnetometer); apply_calibration(device, &gyroscope, &accelerometer, &magnetometer);
gyroscope = FusionOffsetUpdate((FusionOffset*) device->offset, gyroscope); if (device->offset) {
gyroscope = FusionOffsetUpdate((FusionOffset*) device->offset, gyroscope);
}
//printf("G: %.2f %.2f %.2f\n", gyroscope.axis.x, gyroscope.axis.y, gyroscope.axis.z); //printf("G: %.2f %.2f %.2f\n", gyroscope.axis.x, gyroscope.axis.y, gyroscope.axis.z);
//printf("A: %.2f %.2f %.2f\n", accelerometer.axis.x, accelerometer.axis.y, accelerometer.axis.z); //printf("A: %.2f %.2f %.2f\n", accelerometer.axis.x, accelerometer.axis.y, accelerometer.axis.z);
//printf("M: %.2f %.2f %.2f\n", magnetometer.axis.x, magnetometer.axis.y, magnetometer.axis.z); //printf("M: %.2f %.2f %.2f\n", magnetometer.axis.x, magnetometer.axis.y, magnetometer.axis.z);
FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime); if (device->ahrs) {
//FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
//FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime);
}
device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE); device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE);
return 0; return 0;
} }
device3_vec3_type device3_get_earth_acceleration(const device3_ahrs_type* ahrs) { device3_vec3_type device3_get_earth_acceleration(const device3_ahrs_type* ahrs) {
FusionVector acceleration = FusionAhrsGetEarthAcceleration((const FusionAhrs*) ahrs); FusionVector acceleration = ahrs? FusionAhrsGetEarthAcceleration((const FusionAhrs*) ahrs) : FUSION_VECTOR_ZERO;
device3_vec3_type a; device3_vec3_type a;
a.x = acceleration.axis.x; a.x = acceleration.axis.x;
a.y = acceleration.axis.y; a.y = acceleration.axis.y;
@ -797,7 +809,7 @@ device3_vec3_type device3_get_earth_acceleration(const device3_ahrs_type* ahrs)
} }
device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs) { device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs) {
FusionVector acceleration = FusionAhrsGetLinearAcceleration((const FusionAhrs*) ahrs); FusionVector acceleration = ahrs? FusionAhrsGetLinearAcceleration((const FusionAhrs*) ahrs) : FUSION_VECTOR_ZERO;
device3_vec3_type a; device3_vec3_type a;
a.x = acceleration.axis.x; a.x = acceleration.axis.x;
a.y = acceleration.axis.y; a.y = acceleration.axis.y;
@ -806,7 +818,7 @@ device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs)
} }
device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs) { device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs) {
FusionQuaternion quaternion = FusionAhrsGetQuaternion((const FusionAhrs*) ahrs); FusionQuaternion quaternion = ahrs? FusionAhrsGetQuaternion((const FusionAhrs*) ahrs) : FUSION_IDENTITY_QUATERNION;
device3_quat_type q; device3_quat_type q;
q.x = quaternion.element.x; q.x = quaternion.element.x;
q.y = quaternion.element.y; q.y = quaternion.element.y;
@ -853,4 +865,5 @@ void device3_close(device3_type* device) {
} }
free(device); free(device);
hid_exit();
} }

84
interface_lib/src/device4.c
View file

@ -24,14 +24,53 @@
#include "device4.h" #include "device4.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <hidapi/hidapi.h> #include <hidapi/hidapi.h>
#include "crc32.h"
#define MAX_PACKET_SIZE 64 #define MAX_PACKET_SIZE 64
static bool send_payload(device4_type* device, uint8_t size, const uint8_t* payload) {
int payload_size = size;
if (payload_size > MAX_PACKET_SIZE) {
payload_size = MAX_PACKET_SIZE;
}
int transferred = hid_write(device->handle, payload, payload_size);
if (transferred != payload_size) {
perror("ERROR: sending payload failed\n");
return false;
}
return (transferred == size);
}
static bool send_payload_action(device4_type* device, uint8_t action, uint8_t len, const uint8_t* data) {
static device4_packet_type packet;
const uint16_t packet_len = 11 + len;
const uint16_t payload_len = 5 + packet_len;
packet.signature = 0xFD;
packet.length = packet_len;
memset(packet._padding0, 0, 4);
packet.timestamp = 0;
packet.action = action;
memset(packet._padding1, 0, 6);
memcpy(packet.data, data, len);
packet.checksum = crc32_checksum((const uint8_t*) (&packet.length), packet.length);
return send_payload(device, payload_len, (uint8_t*) (&packet));
}
device4_type* device4_open(device4_event_callback callback) { device4_type* device4_open(device4_event_callback callback) {
device4_type* device = (device4_type*) malloc(sizeof(device4_type)); device4_type* device = (device4_type*) malloc(sizeof(device4_type));
@ -57,7 +96,7 @@ device4_type* device4_open(device4_event_callback callback) {
struct hid_device_info* it = info; struct hid_device_info* it = info;
while (it) { while (it) {
if (it->interface_number == 3) { if (it->interface_number == 4) {
device->handle = hid_open_path(it->path); device->handle = hid_open_path(it->path);
break; break;
} }
@ -71,30 +110,18 @@ device4_type* device4_open(device4_event_callback callback) {
perror("No handle!\n"); perror("No handle!\n");
return device; return device;
} }
uint8_t initial_brightness_payload [16] = { device4_clear(device);
0xfd, 0x1e, 0xb9, 0xf0,
0x68, 0x11, 0x00, 0x00, /*if (!send_payload_action(device, DEVICE4_ACTION_BRIGHTNESS_COMMAND, 0, NULL)) {
0x00, 0x00, 0x00, 0x00, perror("Sending brightness command action failed!\n");
0x00, 0x00, 0x00, 0x03
};
int size = MAX_PACKET_SIZE;
if (sizeof(initial_brightness_payload) < size) {
size = sizeof(initial_brightness_payload);
}
int transferred = hid_write(
device->handle,
initial_brightness_payload,
size
);
if (transferred != sizeof(initial_brightness_payload)) {
perror("ERROR\n");
return device; return device;
} }
if (0 > device4_read(device, 1000)) {
perror("Reading error!\n");
}*/
return device; return device;
} }
@ -111,7 +138,7 @@ static void device4_callback(device4_type* device,
} }
void device4_clear(device4_type* device) { void device4_clear(device4_type* device) {
device4_read(device, 0); device4_read(device, 10);
} }
int device4_read(device4_type* device, int timeout) { int device4_read(device4_type* device, int timeout) {
@ -133,16 +160,16 @@ int device4_read(device4_type* device, int timeout) {
MAX_PACKET_SIZE, MAX_PACKET_SIZE,
timeout timeout
); );
/*if (transferred == 0) { if (transferred == 0) {
return 1; return 1;
}*/ }
if (MAX_PACKET_SIZE != transferred) { if (MAX_PACKET_SIZE != transferred) {
perror("Not expected issue!\n"); perror("Not expected issue!\n");
return -3; return -3;
} }
const uint32_t timestamp = packet.timestamp; const uint32_t timestamp = packet.timestamp;
const size_t data_len = (size_t) &(packet.data) - (size_t) &(packet.length); const size_t data_len = (size_t) &(packet.data) - (size_t) &(packet.length);
@ -266,4 +293,5 @@ void device4_close(device4_type* device) {
} }
free(device); free(device);
hid_exit();
} }