diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 480eb10..c0822b9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,5 +1,5 @@ -add_subdirectory(debug_d3) +add_subdirectory(debug_imu) add_subdirectory(debug_d4) add_subdirectory(mcu_firmware) diff --git a/examples/debug_d3/CMakeLists.txt b/examples/debug_imu/CMakeLists.txt similarity index 56% rename from examples/debug_d3/CMakeLists.txt rename to examples/debug_imu/CMakeLists.txt index f626a9b..1255db7 100644 --- a/examples/debug_d3/CMakeLists.txt +++ b/examples/debug_imu/CMakeLists.txt @@ -1,17 +1,17 @@ cmake_minimum_required(VERSION 3.16) -project(xrealAirDebugD3 C) +project(xrealAirDebugIMU C) set(CMAKE_C_STANDARD 17) add_executable( - xrealAirDebugD3 + xrealAirDebugIMU src/debug.c ) -target_include_directories(xrealAirDebugD3 +target_include_directories(xrealAirDebugIMU BEFORE PUBLIC ${XREAL_AIR_INCLUDE_DIR} ) -target_link_libraries(xrealAirDebugD3 +target_link_libraries(xrealAirDebugIMU ${XREAL_AIR_LIBRARY} ) diff --git a/examples/debug_d3/src/debug.c b/examples/debug_imu/src/debug.c similarity index 70% rename from examples/debug_d3/src/debug.c rename to examples/debug_imu/src/debug.c index a622a54..24dd5c3 100644 --- a/examples/debug_d3/src/debug.c +++ b/examples/debug_imu/src/debug.c @@ -1,7 +1,7 @@ // // Created by thejackimonster on 05.04.23. // -// Copyright (c) 2023 thejackimonster. All rights reserved. +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -22,23 +22,23 @@ // THE SOFTWARE. // -#include "device3.h" +#include "device_imu.h" #include void test3(uint64_t timestamp, - device3_event_type event, - const device3_ahrs_type* ahrs) { - device3_quat_type orientation; - device3_euler_type euler; + device_imu_event_type event, + const device_imu_ahrs_type* ahrs) { + device_imu_quat_type orientation; + device_imu_euler_type euler; switch (event) { - case DEVICE3_EVENT_INIT: + case DEVICE_IMU_EVENT_INIT: printf("Initialized\n"); break; - case DEVICE3_EVENT_UPDATE: - orientation = device3_get_orientation(ahrs); - euler = device3_get_euler(orientation); + case DEVICE_IMU_EVENT_UPDATE: + orientation = device_imu_get_orientation(ahrs); + euler = device_imu_get_euler(orientation); printf("Roll: %.2f; Pitch: %.2f; Yaw: %.2f\n", euler.roll, euler.pitch, euler.yaw); break; default: @@ -47,13 +47,13 @@ void test3(uint64_t timestamp, } int main(int argc, const char** argv) { - device3_type dev3; - if (DEVICE3_ERROR_NO_ERROR != device3_open(&dev3, test3)) { + device_imu_type dev3; + if (DEVICE_IMU_ERROR_NO_ERROR != device_imu_open(&dev3, test3)) { return 1; } - device3_clear(&dev3); - while (DEVICE3_ERROR_NO_ERROR == device3_read(&dev3, -1)); - device3_close(&dev3); + device_imu_clear(&dev3); + while (DEVICE_IMU_ERROR_NO_ERROR == device_imu_read(&dev3, -1)); + device_imu_close(&dev3); return 0; } diff --git a/interface_lib/CMakeLists.txt b/interface_lib/CMakeLists.txt index 7a75094..df592ba 100644 --- a/interface_lib/CMakeLists.txt +++ b/interface_lib/CMakeLists.txt @@ -12,7 +12,7 @@ add_library( xrealAirLibrary src/crc32.c src/device.c - src/device3.c + src/device_imu.c src/device4.c src/hid_ids.c ) diff --git a/interface_lib/include/device3.h b/interface_lib/include/device3.h deleted file mode 100644 index 79f5662..0000000 --- a/interface_lib/include/device3.h +++ /dev/null @@ -1,182 +0,0 @@ -#pragma once -// -// Created by thejackimonster on 30.03.23. -// -// Copyright (c) 2023 thejackimonster. All rights reserved. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// - -#ifndef __cplusplus -#include -#endif - -#ifndef __cplusplus -#include -#else -#include -#endif - -#define DEVICE3_MSG_GET_CAL_DATA_LENGTH 0x14 -#define DEVICE3_MSG_CAL_DATA_GET_NEXT_SEGMENT 0x15 -#define DEVICE3_MSG_ALLOCATE_CAL_DATA_BUFFER 0x16 -#define DEVICE3_MSG_WRITE_CAL_DATA_SEGMENT 0x17 -#define DEVICE3_MSG_FREE_CAL_BUFFER 0x18 -#define DEVICE3_MSG_START_IMU_DATA 0x19 -#define DEVICE3_MSG_GET_STATIC_ID 0x1A -#define DEVICE3_MSG_UNKNOWN 0x1D - -#ifdef __cplusplus -extern "C" { -#endif - -enum device3_error_t { - DEVICE3_ERROR_NO_ERROR = 0, - DEVICE3_ERROR_NO_DEVICE = 1, - DEVICE3_ERROR_NO_HANDLE = 2, - DEVICE3_ERROR_NO_ALLOCATION = 3, - DEVICE3_ERROR_WRONG_SIZE = 4, - DEVICE3_ERROR_FILE_NOT_OPEN = 5, - DEVICE3_ERROR_FILE_NOT_CLOSED = 6, - DEVICE3_ERROR_LOADING_FAILED = 7, - DEVICE3_ERROR_SAVING_FAILED = 8, - DEVICE3_ERROR_UNPLUGGED = 9, - DEVICE3_ERROR_UNEXPECTED = 10, - DEVICE3_ERROR_WRONG_SIGNATURE = 11, - DEVICE3_ERROR_INVALID_VALUE = 12, - DEVICE3_ERROR_NOT_INITIALIZED = 13, - DEVICE3_ERROR_PAYLOAD_FAILED = 14, - DEVICE3_ERROR_UNKNOWN = 15, -}; - -struct __attribute__((__packed__)) device3_packet_t { - uint8_t signature [2]; - uint8_t temperature [2]; - uint64_t timestamp; - uint8_t angular_multiplier [2]; - uint8_t angular_divisor [4]; - uint8_t angular_velocity_x [3]; - uint8_t angular_velocity_y [3]; - uint8_t angular_velocity_z [3]; - uint8_t acceleration_multiplier [2]; - uint8_t acceleration_divisor [4]; - uint8_t acceleration_x [3]; - uint8_t acceleration_y [3]; - uint8_t acceleration_z [3]; - uint8_t magnetic_multiplier [2]; - uint8_t magnetic_divisor [4]; - uint8_t magnetic_x [2]; - uint8_t magnetic_y [2]; - uint8_t magnetic_z [2]; - uint32_t checksum; - uint8_t _padding [6]; -}; - -enum device3_event_t { - DEVICE3_EVENT_UNKNOWN = 0, - DEVICE3_EVENT_INIT = 1, - DEVICE3_EVENT_UPDATE = 2, -}; - -struct device3_ahrs_t; -struct device3_calibration_t; - -struct device3_vec3_t { - float x; - float y; - float z; -}; - -struct device3_quat_t { - float x; - float y; - float z; - float w; -}; - -struct device3_euler_t { - float roll; - float pitch; - float yaw; -}; - -typedef enum device3_error_t device3_error_type; -typedef struct device3_packet_t device3_packet_type; -typedef enum device3_event_t device3_event_type; - -typedef struct device3_ahrs_t device3_ahrs_type; -typedef struct device3_calibration_t device3_calibration_type; - -typedef struct device3_vec3_t device3_vec3_type; -typedef struct device3_quat_t device3_quat_type; -typedef struct device3_euler_t device3_euler_type; - -typedef void (*device3_event_callback)( - uint64_t timestamp, - device3_event_type event, - const device3_ahrs_type* ahrs -); - -struct device3_t { - uint16_t vendor_id; - uint16_t product_id; - - void* handle; - - uint32_t static_id; - - uint64_t last_timestamp; - float temperature; // (in °C) - - void* offset; - device3_ahrs_type* ahrs; - - device3_event_callback callback; - device3_calibration_type* calibration; -}; - -typedef struct device3_t device3_type; - -device3_error_type device3_open(device3_type* device, device3_event_callback callback); - -device3_error_type device3_reset_calibration(device3_type* device); - -device3_error_type device3_load_calibration(device3_type* device, const char* path); - -device3_error_type device3_save_calibration(device3_type* device, const char* path); - -device3_error_type device3_clear(device3_type* device); - -device3_error_type device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet); - -device3_error_type device3_read(device3_type* device, int timeout); - -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_quat_type device3_get_orientation(const device3_ahrs_type* ahrs); - -device3_euler_type device3_get_euler(device3_quat_type quat); - -device3_error_type device3_close(device3_type* device); - -#ifdef __cplusplus -} // extern "C" -#endif diff --git a/interface_lib/include/device_imu.h b/interface_lib/include/device_imu.h new file mode 100644 index 0000000..c356975 --- /dev/null +++ b/interface_lib/include/device_imu.h @@ -0,0 +1,182 @@ +#pragma once +// +// Created by thejackimonster on 30.03.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +#define DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH 0x14 +#define DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT 0x15 +#define DEVICE_IMU_MSG_ALLOCATE_CAL_DATA_BUFFER 0x16 +#define DEVICE_IMU_MSG_WRITE_CAL_DATA_SEGMENT 0x17 +#define DEVICE_IMU_MSG_FREE_CAL_BUFFER 0x18 +#define DEVICE_IMU_MSG_START_IMU_DATA 0x19 +#define DEVICE_IMU_MSG_GET_STATIC_ID 0x1A +#define DEVICE_IMU_MSG_UNKNOWN 0x1D + +#ifdef __cplusplus +extern "C" { +#endif + +enum device_imu_error_t { + DEVICE_IMU_ERROR_NO_ERROR = 0, + DEVICE_IMU_ERROR_NO_DEVICE = 1, + DEVICE_IMU_ERROR_NO_HANDLE = 2, + DEVICE_IMU_ERROR_NO_ALLOCATION = 3, + DEVICE_IMU_ERROR_WRONG_SIZE = 4, + DEVICE_IMU_ERROR_FILE_NOT_OPEN = 5, + DEVICE_IMU_ERROR_FILE_NOT_CLOSED = 6, + DEVICE_IMU_ERROR_LOADING_FAILED = 7, + DEVICE_IMU_ERROR_SAVING_FAILED = 8, + DEVICE_IMU_ERROR_UNPLUGGED = 9, + DEVICE_IMU_ERROR_UNEXPECTED = 10, + DEVICE_IMU_ERROR_WRONG_SIGNATURE = 11, + DEVICE_IMU_ERROR_INVALID_VALUE = 12, + DEVICE_IMU_ERROR_NOT_INITIALIZED = 13, + DEVICE_IMU_ERROR_PAYLOAD_FAILED = 14, + DEVICE_IMU_ERROR_UNKNOWN = 15, +}; + +struct __attribute__((__packed__)) device_imu_packet_t { + uint8_t signature [2]; + uint8_t temperature [2]; + uint64_t timestamp; + uint8_t angular_multiplier [2]; + uint8_t angular_divisor [4]; + uint8_t angular_velocity_x [3]; + uint8_t angular_velocity_y [3]; + uint8_t angular_velocity_z [3]; + uint8_t acceleration_multiplier [2]; + uint8_t acceleration_divisor [4]; + uint8_t acceleration_x [3]; + uint8_t acceleration_y [3]; + uint8_t acceleration_z [3]; + uint8_t magnetic_multiplier [2]; + uint8_t magnetic_divisor [4]; + uint8_t magnetic_x [2]; + uint8_t magnetic_y [2]; + uint8_t magnetic_z [2]; + uint32_t checksum; + uint8_t _padding [6]; +}; + +enum device_imu_event_t { + DEVICE_IMU_EVENT_UNKNOWN = 0, + DEVICE_IMU_EVENT_INIT = 1, + DEVICE_IMU_EVENT_UPDATE = 2, +}; + +struct device_imu_ahrs_t; +struct device_imu_calibration_t; + +struct device_imu_vec3_t { + float x; + float y; + float z; +}; + +struct device_imu_quat_t { + float x; + float y; + float z; + float w; +}; + +struct device_imu_euler_t { + float roll; + float pitch; + float yaw; +}; + +typedef enum device_imu_error_t device_imu_error_type; +typedef struct device_imu_packet_t device_imu_packet_type; +typedef enum device_imu_event_t device_imu_event_type; + +typedef struct device_imu_ahrs_t device_imu_ahrs_type; +typedef struct device_imu_calibration_t device_imu_calibration_type; + +typedef struct device_imu_vec3_t device_imu_vec3_type; +typedef struct device_imu_quat_t device_imu_quat_type; +typedef struct device_imu_euler_t device_imu_euler_type; + +typedef void (*device_imu_event_callback)( + uint64_t timestamp, + device_imu_event_type event, + const device_imu_ahrs_type* ahrs +); + +struct device_imu_t { + uint16_t vendor_id; + uint16_t product_id; + + void* handle; + + uint32_t static_id; + + uint64_t last_timestamp; + float temperature; // (in °C) + + void* offset; + device_imu_ahrs_type* ahrs; + + device_imu_event_callback callback; + device_imu_calibration_type* calibration; +}; + +typedef struct device_imu_t device_imu_type; + +device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_callback callback); + +device_imu_error_type device_imu_reset_calibration(device_imu_type* device); + +device_imu_error_type device_imu_load_calibration(device_imu_type* device, const char* path); + +device_imu_error_type device_imu_save_calibration(device_imu_type* device, const char* path); + +device_imu_error_type device_imu_clear(device_imu_type* device); + +device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet); + +device_imu_error_type device_imu_read(device_imu_type* device, int timeout); + +device_imu_vec3_type device_imu_get_earth_acceleration(const device_imu_ahrs_type* ahrs); + +device_imu_vec3_type device_imu_get_linear_acceleration(const device_imu_ahrs_type* ahrs); + +device_imu_quat_type device_imu_get_orientation(const device_imu_ahrs_type* ahrs); + +device_imu_euler_type device_imu_get_euler(device_imu_quat_type quat); + +device_imu_error_type device_imu_close(device_imu_type* device); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/interface_lib/src/device3.c b/interface_lib/src/device_imu.c similarity index 76% rename from interface_lib/src/device3.c rename to interface_lib/src/device_imu.c index f97ba3e..613b17f 100644 --- a/interface_lib/src/device3.c +++ b/interface_lib/src/device_imu.c @@ -22,7 +22,7 @@ // THE SOFTWARE. // -#include "device3.h" +#include "device_imu.h" #include "device.h" #include @@ -43,12 +43,12 @@ #define GRAVITY_G (9.806f) #ifndef NDEBUG -#define device3_error(msg) fprintf(stderr, "ERROR: %s\n", msg) +#define device_imu_error(msg) fprintf(stderr, "ERROR: %s\n", msg) #else -#define device3_error(msg) (0) +#define device_imu_error(msg) (0) #endif -struct device3_calibration_t { +struct device_imu_calibration_t { FusionMatrix gyroscopeMisalignment; FusionVector gyroscopeSensitivity; FusionVector gyroscopeOffset; @@ -69,7 +69,7 @@ struct device3_calibration_t { #define MAX_PACKET_SIZE 64 -static bool send_payload(device3_type* device, uint8_t size, const uint8_t* payload) { +static bool send_payload(device_imu_type* device, uint8_t size, const uint8_t* payload) { int payload_size = size; if (payload_size > MAX_PACKET_SIZE) { payload_size = MAX_PACKET_SIZE; @@ -78,14 +78,14 @@ static bool send_payload(device3_type* device, uint8_t size, const uint8_t* payl int transferred = hid_write(device->handle, payload, payload_size); if (transferred != payload_size) { - device3_error("Sending payload failed"); + device_imu_error("Sending payload failed"); return false; } return (transferred == size); } -static bool recv_payload(device3_type* device, uint8_t size, uint8_t* payload) { +static bool recv_payload(device_imu_type* device, uint8_t size, uint8_t* payload) { int payload_size = size; if (payload_size > MAX_PACKET_SIZE) { payload_size = MAX_PACKET_SIZE; @@ -102,14 +102,14 @@ static bool recv_payload(device3_type* device, uint8_t size, uint8_t* payload) { } if (transferred != payload_size) { - device3_error("Receiving payload failed"); + device_imu_error("Receiving payload failed"); return false; } return (transferred == size); } -struct __attribute__((__packed__)) device3_payload_packet_t { +struct __attribute__((__packed__)) device_imu_payload_packet_t { uint8_t head; uint32_t checksum; uint16_t length; @@ -117,10 +117,10 @@ struct __attribute__((__packed__)) device3_payload_packet_t { uint8_t data [56]; }; -typedef struct device3_payload_packet_t device3_payload_packet_type; +typedef struct device_imu_payload_packet_t device_imu_payload_packet_type; -static bool send_payload_msg(device3_type* device, uint8_t msgid, uint8_t len, const uint8_t* data) { - static device3_payload_packet_type packet; +static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint8_t len, const uint8_t* data) { + static device_imu_payload_packet_type packet; const uint16_t packet_len = 3 + len; const uint16_t payload_len = 5 + packet_len; @@ -140,12 +140,12 @@ static bool send_payload_msg(device3_type* device, uint8_t msgid, uint8_t len, c return send_payload(device, payload_len, (uint8_t*) (&packet)); } -static bool send_payload_msg_signal(device3_type* device, uint8_t msgid, uint8_t signal) { +static bool send_payload_msg_signal(device_imu_type* device, uint8_t msgid, uint8_t signal) { return send_payload_msg(device, msgid, 1, &signal); } -static bool recv_payload_msg(device3_type* device, uint8_t msgid, uint8_t len, uint8_t* data) { - static device3_payload_packet_type packet; +static bool recv_payload_msg(device_imu_type* device, uint8_t msgid, uint8_t len, uint8_t* data) { + static device_imu_payload_packet_type packet; packet.head = 0; packet.length = 0; @@ -193,20 +193,20 @@ static FusionQuaternion json_object_get_quaternion(struct json_object* obj) { return quaternion; } -device3_error_type device3_open(device3_type* device, device3_event_callback callback) { +device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_callback callback) { if (!device) { - device3_error("No device"); - return DEVICE3_ERROR_NO_DEVICE; + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; } - memset(device, 0, sizeof(device3_type)); + memset(device, 0, sizeof(device_imu_type)); device->vendor_id = xreal_vendor_id; device->product_id = 0; device->callback = callback; if (!device_init()) { - device3_error("Not initialized"); - return DEVICE3_ERROR_NOT_INITIALIZED; + device_imu_error("Not initialized"); + return DEVICE_IMU_ERROR_NOT_INITIALIZED; } struct hid_device_info* info = hid_enumerate( @@ -231,52 +231,52 @@ device3_error_type device3_open(device3_type* device, device3_event_callback cal hid_free_enumeration(info); if (!device->handle) { - device3_error("No handle"); - return DEVICE3_ERROR_NO_HANDLE; + device_imu_error("No handle"); + return DEVICE_IMU_ERROR_NO_HANDLE; } - if (!send_payload_msg_signal(device, DEVICE3_MSG_START_IMU_DATA, 0x0)) { - device3_error("Failed sending payload to stop imu data stream"); - return DEVICE3_ERROR_PAYLOAD_FAILED; + if (!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x0)) { + device_imu_error("Failed sending payload to stop imu data stream"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } - device3_clear(device); + device_imu_clear(device); - if (!send_payload_msg(device, DEVICE3_MSG_GET_STATIC_ID, 0, NULL)) { - device3_error("Failed sending payload to get static id"); - return DEVICE3_ERROR_PAYLOAD_FAILED; + if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 0, NULL)) { + device_imu_error("Failed sending payload to get static id"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } uint32_t static_id = 0; - if (recv_payload_msg(device, DEVICE3_MSG_GET_STATIC_ID, 4, (uint8_t*) &static_id)) { + if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 4, (uint8_t*) &static_id)) { device->static_id = static_id; } else { device->static_id = 0x20220101; } - device->calibration = malloc(sizeof(device3_calibration_type)); - device3_reset_calibration(device); + device->calibration = malloc(sizeof(device_imu_calibration_type)); + device_imu_reset_calibration(device); - if (!send_payload_msg(device, DEVICE3_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) { - device3_error("Failed sending payload to get calibration data length"); - return DEVICE3_ERROR_PAYLOAD_FAILED; + if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) { + device_imu_error("Failed sending payload to get calibration data length"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } uint32_t calibration_len = 0; - if (recv_payload_msg(device, DEVICE3_MSG_GET_CAL_DATA_LENGTH, 4, (uint8_t*) &calibration_len)) { + if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 4, (uint8_t*) &calibration_len)) { char* calibration_data = malloc(calibration_len + 1); uint32_t position = 0; while (position < calibration_len) { const uint32_t remaining = (calibration_len - position); - if (!send_payload_msg(device, DEVICE3_MSG_CAL_DATA_GET_NEXT_SEGMENT, 0, NULL)) { + if (!send_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, 0, NULL)) { break; } const uint8_t next = (remaining > 56? 56 : remaining); - if (!recv_payload_msg(device, DEVICE3_MSG_CAL_DATA_GET_NEXT_SEGMENT, next, (uint8_t*) calibration_data + position)) { + if (!recv_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, next, (uint8_t*) calibration_data + position)) { break; } @@ -320,9 +320,9 @@ device3_error_type device3_open(device3_type* device, device3_event_callback cal free(calibration_data); } - if (!send_payload_msg_signal(device, DEVICE3_MSG_START_IMU_DATA, 0x1)) { - device3_error("Failed sending payload to start imu data stream"); - return DEVICE3_ERROR_PAYLOAD_FAILED; + if (!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x1)) { + device_imu_error("Failed sending payload to start imu data stream"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } const uint32_t SAMPLE_RATE = 1000; @@ -345,18 +345,18 @@ device3_error_type device3_open(device3_type* device, device3_event_callback cal }; FusionAhrsSetSettings((FusionAhrs*) device->ahrs, &settings); - return DEVICE3_ERROR_NO_ERROR; + return DEVICE_IMU_ERROR_NO_ERROR; } -device3_error_type device3_reset_calibration(device3_type* device) { +device_imu_error_type device_imu_reset_calibration(device_imu_type* device) { if (!device) { - device3_error("No device"); - return DEVICE3_ERROR_NO_DEVICE; + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; } if (!device->calibration) { - device3_error("Not allocated"); - return DEVICE3_ERROR_NO_ALLOCATION; + device_imu_error("Not allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; } device->calibration->gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; @@ -378,79 +378,79 @@ device3_error_type device3_reset_calibration(device3_type* device) { device->calibration->noises.element.w = 0.0f; } -device3_error_type device3_load_calibration(device3_type* device, const char* path) { +device_imu_error_type device_imu_load_calibration(device_imu_type* device, const char* path) { if (!device) { - device3_error("No device"); - return DEVICE3_ERROR_NO_DEVICE; + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; } if (!device->calibration) { - device3_error("Not allocated"); - return DEVICE3_ERROR_NO_ALLOCATION; + device_imu_error("Not allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; } FILE* file = fopen(path, "rb"); if (!file) { - device3_error("No file opened"); - return DEVICE3_ERROR_FILE_NOT_OPEN; + device_imu_error("No file opened"); + return DEVICE_IMU_ERROR_FILE_NOT_OPEN; } - device3_error_type result = DEVICE3_ERROR_NO_ERROR; + device_imu_error_type result = DEVICE_IMU_ERROR_NO_ERROR; size_t count; - count = fread(device->calibration, 1, sizeof(device3_calibration_type), file); + count = fread(device->calibration, 1, sizeof(device_imu_calibration_type), file); - if (sizeof(device3_calibration_type) != count) { - device3_error("Not fully loaded"); - result = DEVICE3_ERROR_LOADING_FAILED; + if (sizeof(device_imu_calibration_type) != count) { + device_imu_error("Not fully loaded"); + result = DEVICE_IMU_ERROR_LOADING_FAILED; } if (0 != fclose(file)) { - device3_error("No file closed"); - return DEVICE3_ERROR_FILE_NOT_CLOSED; + device_imu_error("No file closed"); + return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; } return result; } -device3_error_type device3_save_calibration(device3_type* device, const char* path) { +device_imu_error_type device_imu_save_calibration(device_imu_type* device, const char* path) { if (!device) { - device3_error("No device"); - return DEVICE3_ERROR_NO_DEVICE; + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; } if (!device->calibration) { - device3_error("Not allocated"); - return DEVICE3_ERROR_NO_ALLOCATION; + device_imu_error("Not allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; } FILE* file = fopen(path, "wb"); if (!file) { - device3_error("No file opened"); - return DEVICE3_ERROR_FILE_NOT_OPEN; + device_imu_error("No file opened"); + return DEVICE_IMU_ERROR_FILE_NOT_OPEN; } - device3_error_type result = DEVICE3_ERROR_NO_ERROR; + device_imu_error_type result = DEVICE_IMU_ERROR_NO_ERROR; size_t count; - count = fwrite(device->calibration, 1, sizeof(device3_calibration_type), file); + count = fwrite(device->calibration, 1, sizeof(device_imu_calibration_type), file); - if (sizeof(device3_calibration_type) != count) { - device3_error("Not fully saved"); - result = DEVICE3_ERROR_SAVING_FAILED; + if (sizeof(device_imu_calibration_type) != count) { + device_imu_error("Not fully saved"); + result = DEVICE_IMU_ERROR_SAVING_FAILED; } if (0 != fclose(file)) { - device3_error("No file closed"); - return DEVICE3_ERROR_FILE_NOT_CLOSED; + device_imu_error("No file closed"); + return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; } return result; } -static void device3_callback(device3_type* device, +static void device_imu_callback(device_imu_type* device, uint64_t timestamp, - device3_event_type event) { + device_imu_event_type event) { if (!device->callback) { return; } @@ -489,7 +489,7 @@ static int16_t pack16bit_signed_bizarre(const uint8_t* data) { return (int16_t) unsigned_value; } -static void readIMU_from_packet(const device3_packet_type* packet, +static void readIMU_from_packet(const device_imu_packet_type* packet, FusionVector* gyroscope, FusionVector* accelerometer, FusionVector* magnetometer) { @@ -566,7 +566,7 @@ static void iterate_iron_offset_estimation(const FusionVector* magnetometer, Fus hardIronOffset->axis.z = cz; } -static void apply_calibration(const device3_type* device, +static void apply_calibration(const device_imu_type* device, FusionVector* gyroscope, FusionVector* accelerometer, FusionVector* magnetometer) { @@ -678,32 +678,32 @@ static void apply_calibration(const device3_type* device, post_biased_coordinate_system(&m, magnetometer); } -device3_error_type device3_clear(device3_type* device) { - return device3_read(device, 10); +device_imu_error_type device_imu_clear(device_imu_type* device) { + return device_imu_read(device, 10); } -device3_error_type device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet) { +device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet) { if (!device) { - device3_error("No device"); - return DEVICE3_ERROR_NO_DEVICE; + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; } if (!device->handle) { - device3_error("No handle"); - return DEVICE3_ERROR_NO_HANDLE; + device_imu_error("No handle"); + return DEVICE_IMU_ERROR_NO_HANDLE; } if (!device->calibration) { - device3_error("No calibration allocated"); - return DEVICE3_ERROR_NO_ALLOCATION; + device_imu_error("No calibration allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; } - if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) { - device3_error("Not proper size"); - return DEVICE3_ERROR_WRONG_SIZE; + if (MAX_PACKET_SIZE != sizeof(device_imu_packet_type)) { + device_imu_error("Not proper size"); + return DEVICE_IMU_ERROR_WRONG_SIZE; } - device3_packet_type packet; + device_imu_packet_type packet; int transferred; bool initialized = false; @@ -718,7 +718,7 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations, FusionVector prev_accel; while (iterations > 0) { - memset(&packet, 0, sizeof(device3_packet_type)); + memset(&packet, 0, sizeof(device_imu_packet_type)); transferred = hid_read( device->handle, @@ -727,8 +727,8 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations, ); if (transferred == -1) { - device3_error("Device may be unplugged"); - return DEVICE3_ERROR_UNPLUGGED; + device_imu_error("Device may be unplugged"); + return DEVICE_IMU_ERROR_UNPLUGGED; } if (transferred == 0) { @@ -736,8 +736,8 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations, } if (MAX_PACKET_SIZE != transferred) { - device3_error("Unexpected packet size"); - return DEVICE3_ERROR_UNEXPECTED; + device_imu_error("Unexpected packet size"); + return DEVICE_IMU_ERROR_UNEXPECTED; } if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { @@ -800,27 +800,27 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations, } } - return DEVICE3_ERROR_NO_ERROR; + return DEVICE_IMU_ERROR_NO_ERROR; } -device3_error_type device3_read(device3_type* device, int timeout) { +device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { if (!device) { - device3_error("No device"); - return DEVICE3_ERROR_NO_DEVICE; + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; } if (!device->handle) { - device3_error("No handle"); - return DEVICE3_ERROR_NO_HANDLE; + device_imu_error("No handle"); + return DEVICE_IMU_ERROR_NO_HANDLE; } - if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) { - device3_error("Not proper size"); - return DEVICE3_ERROR_WRONG_SIZE; + if (MAX_PACKET_SIZE != sizeof(device_imu_packet_type)) { + device_imu_error("Not proper size"); + return DEVICE_IMU_ERROR_WRONG_SIZE; } - device3_packet_type packet; - memset(&packet, 0, sizeof(device3_packet_type)); + device_imu_packet_type packet; + memset(&packet, 0, sizeof(device_imu_packet_type)); int transferred = hid_read_timeout( device->handle, @@ -830,29 +830,29 @@ device3_error_type device3_read(device3_type* device, int timeout) { ); if (transferred == -1) { - device3_error("Device may be unplugged"); - return DEVICE3_ERROR_UNPLUGGED; + device_imu_error("Device may be unplugged"); + return DEVICE_IMU_ERROR_UNPLUGGED; } if (transferred == 0) { - return DEVICE3_ERROR_NO_ERROR; + return DEVICE_IMU_ERROR_NO_ERROR; } if (MAX_PACKET_SIZE != transferred) { - device3_error("Unexpected packet size"); - return DEVICE3_ERROR_UNEXPECTED; + device_imu_error("Unexpected packet size"); + return DEVICE_IMU_ERROR_UNEXPECTED; } const uint64_t timestamp = le64toh(packet.timestamp); if ((packet.signature[0] == 0xaa) && (packet.signature[1] == 0x53)) { - device3_callback(device, timestamp, DEVICE3_EVENT_INIT); - return DEVICE3_ERROR_NO_ERROR; + device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_INIT); + return DEVICE_IMU_ERROR_NO_ERROR; } if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { - device3_error("Not matching signature"); - return DEVICE3_ERROR_WRONG_SIGNATURE; + device_imu_error("Not matching signature"); + return DEVICE_IMU_ERROR_WRONG_SIGNATURE; } const uint64_t delta = timestamp - device->last_timestamp; @@ -891,41 +891,41 @@ device3_error_type device3_read(device3_type* device, int timeout) { FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); } - const device3_quat_type orientation = device3_get_orientation(device->ahrs); + const device_imu_quat_type orientation = device_imu_get_orientation(device->ahrs); // TODO: fix detection of this case; quat.x as a nan value is only a side-effect of some issue with ahrs or // the gyro/accel/magnet readings if (isnan(orientation.x) || isnan(orientation.y) || isnan(orientation.z) || isnan(orientation.w)) { - device3_error("Invalid orientation reading"); - return DEVICE3_ERROR_INVALID_VALUE; + device_imu_error("Invalid orientation reading"); + return DEVICE_IMU_ERROR_INVALID_VALUE; } } - device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE); - return DEVICE3_ERROR_NO_ERROR; + device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_UPDATE); + return DEVICE_IMU_ERROR_NO_ERROR; } -device3_vec3_type device3_get_earth_acceleration(const device3_ahrs_type* ahrs) { +device_imu_vec3_type device_imu_get_earth_acceleration(const device_imu_ahrs_type* ahrs) { FusionVector acceleration = ahrs? FusionAhrsGetEarthAcceleration((const FusionAhrs*) ahrs) : FUSION_VECTOR_ZERO; - device3_vec3_type a; + device_imu_vec3_type a; a.x = acceleration.axis.x; a.y = acceleration.axis.y; a.z = acceleration.axis.z; return a; } -device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs) { +device_imu_vec3_type device_imu_get_linear_acceleration(const device_imu_ahrs_type* ahrs) { FusionVector acceleration = ahrs? FusionAhrsGetLinearAcceleration((const FusionAhrs*) ahrs) : FUSION_VECTOR_ZERO; - device3_vec3_type a; + device_imu_vec3_type a; a.x = acceleration.axis.x; a.y = acceleration.axis.y; a.z = acceleration.axis.z; return a; } -device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs) { +device_imu_quat_type device_imu_get_orientation(const device_imu_ahrs_type* ahrs) { FusionQuaternion quaternion = ahrs? FusionAhrsGetQuaternion((const FusionAhrs*) ahrs) : FUSION_IDENTITY_QUATERNION; - device3_quat_type q; + device_imu_quat_type q; q.x = quaternion.element.x; q.y = quaternion.element.y; q.z = quaternion.element.z; @@ -933,24 +933,24 @@ device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs) { return q; } -device3_euler_type device3_get_euler(device3_quat_type quat) { +device_imu_euler_type device_imu_get_euler(device_imu_quat_type quat) { FusionQuaternion quaternion; quaternion.element.x = quat.x; quaternion.element.y = quat.y; quaternion.element.z = quat.z; quaternion.element.w = quat.w; FusionEuler euler = FusionQuaternionToEuler(quaternion); - device3_euler_type e; + device_imu_euler_type e; e.roll = euler.angle.roll; e.pitch = euler.angle.pitch; e.yaw = euler.angle.yaw; return e; } -device3_error_type device3_close(device3_type* device) { +device_imu_error_type device_imu_close(device_imu_type* device) { if (!device) { - device3_error("No device"); - return DEVICE3_ERROR_NO_DEVICE; + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; } if (device->calibration) { @@ -969,8 +969,8 @@ device3_error_type device3_close(device3_type* device) { hid_close(device->handle); } - memset(device, 0, sizeof(device3_type)); + memset(device, 0, sizeof(device_imu_type)); device_exit(); - return DEVICE3_ERROR_NO_ERROR; + return DEVICE_IMU_ERROR_NO_ERROR; } diff --git a/src/driver.c b/src/driver.c index e33e4bc..449374e 100644 --- a/src/driver.c +++ b/src/driver.c @@ -1,7 +1,7 @@ // // Created by thejackimonster on 29.03.23. // -// Copyright (c) 2023 thejackimonster. All rights reserved. +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -22,7 +22,7 @@ // THE SOFTWARE. // -#include "device3.h" +#include "device_imu.h" #include "device4.h" #include @@ -32,16 +32,16 @@ #include void test3(uint64_t timestamp, - device3_event_type event, - const device3_ahrs_type* ahrs) { - static device3_quat_type old; + device_imu_event_type event, + const device_imu_ahrs_type* ahrs) { + static device_imu_quat_type old; static float dmax = -1.0f; - if (event != DEVICE3_EVENT_UPDATE) { + if (event != DEVICE_IMU_EVENT_UPDATE) { return; } - device3_quat_type q = device3_get_orientation(ahrs); + device_imu_quat_type q = device_imu_get_orientation(ahrs); const float dx = (old.x - q.x) * (old.x - q.x); const float dy = (old.y - q.y) * (old.y - q.y); @@ -56,10 +56,10 @@ void test3(uint64_t timestamp, dmax = (d > dmax? d : dmax); } - device3_euler_type e = device3_get_euler(q); + device_imu_euler_type e = device_imu_get_euler(q); if (d >= 0.00005f) { - device3_euler_type e0 = device3_get_euler(old); + device_imu_euler_type e0 = device_imu_get_euler(old); printf("Roll: %f; Pitch: %f; Yaw: %f\n", e0.roll, e0.pitch, e0.yaw); printf("Roll: %f; Pitch: %f; Yaw: %f\n", e.roll, e.pitch, e.yaw); @@ -102,15 +102,15 @@ int main(int argc, const char** argv) { } if (pid == 0) { - device3_type dev3; - if (DEVICE3_ERROR_NO_ERROR != device3_open(&dev3, test3)) { + device_imu_type dev3; + if (DEVICE_IMU_ERROR_NO_ERROR != device_imu_open(&dev3, test3)) { return 1; } - device3_clear(&dev3); - device3_calibrate(&dev3, 1000, true, true, false); - while (DEVICE3_ERROR_NO_ERROR == device3_read(&dev3, -1)); - device3_close(&dev3); + device_imu_clear(&dev3); + device_imu_calibrate(&dev3, 1000, true, true, false); + while (DEVICE_IMU_ERROR_NO_ERROR == device_imu_read(&dev3, -1)); + device_imu_close(&dev3); return 0; } else { int status = 0;