diff --git a/examples/debug_d3/src/debug.c b/examples/debug_d3/src/debug.c index 394f3a9..0d524f8 100644 --- a/examples/debug_d3/src/debug.c +++ b/examples/debug_d3/src/debug.c @@ -56,7 +56,7 @@ int main(int argc, const char** argv) { device3_clear(dev3); while (dev3) { - if (device3_read(dev3, 0) < 0) { + if (device3_read(dev3, -1) < 0) { break; } } diff --git a/examples/debug_d4/src/debug.c b/examples/debug_d4/src/debug.c index 8446d41..e6cdcde 100644 --- a/examples/debug_d4/src/debug.c +++ b/examples/debug_d4/src/debug.c @@ -58,7 +58,7 @@ int main(int argc, const char** argv) { device4_clear(dev4); while (dev4) { - if (device4_read(dev4, 0) < 0) { + if (device4_read(dev4, 1000) < 0) { break; } } diff --git a/interface_lib/src/device3.c b/interface_lib/src/device3.c index 93f06a8..0f3d791 100644 --- a/interface_lib/src/device3.c +++ b/interface_lib/src/device3.c @@ -84,9 +84,9 @@ static bool recv_payload(device3_type* device, uint8_t size, uint8_t* payload) { transferred = payload_size; } - /*if (transferred == 0) { + if (transferred == 0) { return false; - }*/ + } if (transferred != payload_size) { perror("ERROR: receiving payload failed\n"); @@ -223,8 +223,11 @@ device3_type* device3_open(device3_event_callback callback) { perror("No handle!\n"); return device; } + + device3_clear(device); if (!send_payload_msg(device, DEVICE3_MSG_GET_STATIC_ID, 0, NULL)) { + perror("Failed sending payload to get static id!\n"); return device; } @@ -239,6 +242,7 @@ device3_type* device3_open(device3_event_callback callback) { device3_reset_calibration(device); 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; } @@ -292,6 +296,7 @@ device3_type* device3_open(device3_event_callback callback) { } if (!send_payload_msg_signal(device, DEVICE3_MSG_START_IMU_DATA, 0x1)) { + perror("Failed sending payload to start imu data stream!\n"); return device; } @@ -585,8 +590,10 @@ static void apply_calibration(const device3_type* device, hardIronOffset.axis.y = cy / my; hardIronOffset.axis.z = cz / mz; - device->calibration->softIronMatrix = softIronMatrix; - device->calibration->hardIronOffset = hardIronOffset; + if (device->calibration) { + device->calibration->softIronMatrix = softIronMatrix; + device->calibration->hardIronOffset = hardIronOffset; + } *magnetometer = FusionCalibrationMagnetic( *magnetometer, @@ -602,7 +609,7 @@ static void apply_calibration(const 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) { @@ -636,9 +643,9 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool MAX_PACKET_SIZE ); - /*if (transferred == 0) { + if (transferred == 0) { continue; - }*/ + } if (MAX_PACKET_SIZE != transferred) { perror("Not expected issue!\n"); @@ -730,15 +737,16 @@ int device3_read(device3_type* device, int timeout) { device3_packet_type packet; memset(&packet, 0, sizeof(device3_packet_type)); - int transferred = hid_read( + int transferred = hid_read_timeout( device->handle, (uint8_t*) &packet, - MAX_PACKET_SIZE + MAX_PACKET_SIZE, + timeout ); - /*if (transferred == 0) { + if (transferred == 0) { return 1; - }*/ + } if (MAX_PACKET_SIZE != transferred) { perror("Not expected issue!\n"); @@ -774,21 +782,25 @@ int device3_read(device3_type* device, int timeout) { readIMU_from_packet(&packet, &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("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); - FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime); - //FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); + if (device->ahrs) { + FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime); + //FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); + } device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE); return 0; } 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; a.x = acceleration.axis.x; 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) { - FusionVector acceleration = FusionAhrsGetLinearAcceleration((const FusionAhrs*) ahrs); + FusionVector acceleration = ahrs? FusionAhrsGetLinearAcceleration((const FusionAhrs*) ahrs) : FUSION_VECTOR_ZERO; device3_vec3_type a; a.x = acceleration.axis.x; 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) { - FusionQuaternion quaternion = FusionAhrsGetQuaternion((const FusionAhrs*) ahrs); + FusionQuaternion quaternion = ahrs? FusionAhrsGetQuaternion((const FusionAhrs*) ahrs) : FUSION_IDENTITY_QUATERNION; device3_quat_type q; q.x = quaternion.element.x; q.y = quaternion.element.y; @@ -853,4 +865,5 @@ void device3_close(device3_type* device) { } free(device); + hid_exit(); } diff --git a/interface_lib/src/device4.c b/interface_lib/src/device4.c index a0b13ef..b57a22b 100644 --- a/interface_lib/src/device4.c +++ b/interface_lib/src/device4.c @@ -24,14 +24,53 @@ #include "device4.h" +#include +#include #include #include #include #include +#include "crc32.h" + #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* 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; while (it) { - if (it->interface_number == 3) { + if (it->interface_number == 4) { device->handle = hid_open_path(it->path); break; } @@ -71,30 +110,18 @@ device4_type* device4_open(device4_event_callback callback) { perror("No handle!\n"); return device; } - - uint8_t initial_brightness_payload [16] = { - 0xfd, 0x1e, 0xb9, 0xf0, - 0x68, 0x11, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 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"); + + device4_clear(device); + + /*if (!send_payload_action(device, DEVICE4_ACTION_BRIGHTNESS_COMMAND, 0, NULL)) { + perror("Sending brightness command action failed!\n"); return device; } - + + if (0 > device4_read(device, 1000)) { + perror("Reading error!\n"); + }*/ + return device; } @@ -111,7 +138,7 @@ static void device4_callback(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) { @@ -133,16 +160,16 @@ int device4_read(device4_type* device, int timeout) { MAX_PACKET_SIZE, timeout ); - - /*if (transferred == 0) { + + if (transferred == 0) { return 1; - }*/ + } if (MAX_PACKET_SIZE != transferred) { perror("Not expected issue!\n"); return -3; } - + const uint32_t timestamp = packet.timestamp; 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); + hid_exit(); }