Merge device changes from https://github.com/wheaney/xrealAirLinuxDriver
This commit is contained in:
parent
e08f98db93
commit
da91b813f0
|
|
@ -1,3 +1,6 @@
|
||||||
[submodule "modules/Fusion"]
|
[submodule "interface_lib/Fusion"]
|
||||||
path = modules/Fusion
|
path = interface_lib/Fusion
|
||||||
url = https://github.com/TheJackiMonster/Fusion.git
|
url = https://github.com/xioTechnologies/Fusion.git
|
||||||
|
[submodule "interface_lib/hidapi"]
|
||||||
|
path = interface_lib/hidapi
|
||||||
|
url = https://github.com/libusb/hidapi.git
|
||||||
|
|
|
||||||
|
|
@ -3,10 +3,6 @@ project(nrealAirLinuxDriver C)
|
||||||
|
|
||||||
set(CMAKE_C_STANDARD 17)
|
set(CMAKE_C_STANDARD 17)
|
||||||
|
|
||||||
add_subdirectory(modules/Fusion/Fusion)
|
|
||||||
set(FUSION_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/modules/Fusion)
|
|
||||||
set(FUSION_LIBRARY Fusion)
|
|
||||||
|
|
||||||
add_subdirectory(interface_lib)
|
add_subdirectory(interface_lib)
|
||||||
add_subdirectory(examples)
|
add_subdirectory(examples)
|
||||||
|
|
||||||
|
|
@ -19,9 +15,9 @@ target_include_directories(nrealAirLinuxDriver
|
||||||
)
|
)
|
||||||
|
|
||||||
target_include_directories(nrealAirLinuxDriver
|
target_include_directories(nrealAirLinuxDriver
|
||||||
BEFORE PUBLIC ${NRA_INCLUDE_DIR}
|
BEFORE PUBLIC ${NREAL_AIR_INCLUDE_DIR}
|
||||||
)
|
)
|
||||||
|
|
||||||
target_link_libraries(nrealAirLinuxDriver
|
target_link_libraries(nrealAirLinuxDriver
|
||||||
${NRA_LIBRARY} ${FUSION_LIBRARY}
|
${NREAL_AIR_LIBRARY} ${FUSION_LIBRARY}
|
||||||
)
|
)
|
||||||
|
|
@ -9,9 +9,9 @@ add_executable(
|
||||||
)
|
)
|
||||||
|
|
||||||
target_include_directories(nrealAirDebugD3
|
target_include_directories(nrealAirDebugD3
|
||||||
BEFORE PUBLIC ${NRA_INCLUDE_DIR}
|
BEFORE PUBLIC ${NREAL_AIR_INCLUDE_DIR}
|
||||||
)
|
)
|
||||||
|
|
||||||
target_link_libraries(nrealAirDebugD3
|
target_link_libraries(nrealAirDebugD3
|
||||||
${NRA_LIBRARY}
|
${NREAL_AIR_LIBRARY}
|
||||||
)
|
)
|
||||||
|
|
|
||||||
|
|
@ -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, -1) < 0) {
|
if (device3_read(dev3, -1, false) < 0) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -9,9 +9,9 @@ add_executable(
|
||||||
)
|
)
|
||||||
|
|
||||||
target_include_directories(nrealAirDebugD4
|
target_include_directories(nrealAirDebugD4
|
||||||
BEFORE PUBLIC ${NRA_INCLUDE_DIR}
|
BEFORE PUBLIC ${NREAL_AIR_INCLUDE_DIR}
|
||||||
)
|
)
|
||||||
|
|
||||||
target_link_libraries(nrealAirDebugD4
|
target_link_libraries(nrealAirDebugD4
|
||||||
${NRA_LIBRARY}
|
${NREAL_AIR_LIBRARY}
|
||||||
)
|
)
|
||||||
|
|
|
||||||
|
|
@ -9,9 +9,9 @@ add_executable(
|
||||||
)
|
)
|
||||||
|
|
||||||
target_include_directories(nrealAirUpgradeMCU
|
target_include_directories(nrealAirUpgradeMCU
|
||||||
BEFORE PUBLIC ${NRA_INCLUDE_DIR}
|
BEFORE PUBLIC ${NREAL_AIR_INCLUDE_DIR}
|
||||||
)
|
)
|
||||||
|
|
||||||
target_link_libraries(nrealAirUpgradeMCU
|
target_link_libraries(nrealAirUpgradeMCU
|
||||||
${NRA_LIBRARY}
|
${NREAL_AIR_LIBRARY}
|
||||||
)
|
)
|
||||||
|
|
|
||||||
|
|
@ -3,8 +3,12 @@ project(nrealAirLibrary C)
|
||||||
|
|
||||||
set(CMAKE_C_STANDARD 17)
|
set(CMAKE_C_STANDARD 17)
|
||||||
|
|
||||||
find_package(hidapi REQUIRED)
|
add_subdirectory(hidapi)
|
||||||
find_package(json-c REQUIRED)
|
find_package(json-c REQUIRED CONFIG)
|
||||||
|
|
||||||
|
add_subdirectory(Fusion/Fusion)
|
||||||
|
set(FUSION_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/interface_lib/Fusion)
|
||||||
|
set(FUSION_LIBRARY Fusion)
|
||||||
|
|
||||||
add_library(
|
add_library(
|
||||||
nrealAirLibrary
|
nrealAirLibrary
|
||||||
|
|
@ -13,17 +17,19 @@ add_library(
|
||||||
src/device4.c
|
src/device4.c
|
||||||
)
|
)
|
||||||
|
|
||||||
|
target_compile_options(nrealAirLibrary PRIVATE -fPIC)
|
||||||
|
|
||||||
target_include_directories(nrealAirLibrary
|
target_include_directories(nrealAirLibrary
|
||||||
BEFORE PUBLIC include
|
BEFORE PUBLIC include
|
||||||
)
|
)
|
||||||
|
|
||||||
target_include_directories(nrealAirLibrary
|
target_include_directories(nrealAirLibrary
|
||||||
SYSTEM BEFORE PRIVATE ${FUSION_INCLUDE_DIR}
|
BEFORE PRIVATE ${FUSION_INCLUDE_DIR}
|
||||||
)
|
)
|
||||||
|
|
||||||
target_link_libraries(nrealAirLibrary
|
target_link_libraries(nrealAirLibrary
|
||||||
hidapi::hidapi json-c::json-c ${FUSION_LIBRARY} m
|
PRIVATE hidapi::hidapi json-c::json-c ${FUSION_LIBRARY}
|
||||||
)
|
)
|
||||||
|
|
||||||
set(NRA_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include PARENT_SCOPE)
|
set(NREAL_AIR_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include PARENT_SCOPE)
|
||||||
set(NRA_LIBRARY nrealAirLibrary PARENT_SCOPE)
|
set(NREAL_AIR_LIBRARY nrealAirLibrary PARENT_SCOPE)
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
Subproject commit 0fd6785d3e3e2f8c358603db47ff5dadc0e338af
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
Subproject commit 09ab35ffa9dce60a993f4e55cdeaf0952645f9b7
|
||||||
|
|
@ -122,6 +122,8 @@ struct device3_t {
|
||||||
|
|
||||||
device3_event_callback callback;
|
device3_event_callback callback;
|
||||||
device3_calibration_type* calibration;
|
device3_calibration_type* calibration;
|
||||||
|
|
||||||
|
bool ready;
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef struct device3_t device3_type;
|
typedef struct device3_t device3_type;
|
||||||
|
|
@ -138,7 +140,7 @@ void device3_clear(device3_type* device);
|
||||||
|
|
||||||
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);
|
||||||
|
|
||||||
int device3_read(device3_type* device, int timeout);
|
int device3_read(device3_type* device, int timeout, bool silent);
|
||||||
|
|
||||||
device3_vec3_type device3_get_earth_acceleration(const device3_ahrs_type* ahrs);
|
device3_vec3_type device3_get_earth_acceleration(const device3_ahrs_type* ahrs);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -26,12 +26,13 @@
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
#include <stdbool.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
#include <Fusion/Fusion.h>
|
#include <Fusion/Fusion.h>
|
||||||
#include <json-c/json.h>
|
#include <json-c/json.h>
|
||||||
|
|
||||||
#include <hidapi/hidapi.h>
|
#include <../hidapi/hidapi/hidapi.h>
|
||||||
|
|
||||||
#include "crc32.h"
|
#include "crc32.h"
|
||||||
|
|
||||||
|
|
@ -65,7 +66,7 @@ static bool send_payload(device3_type* device, uint8_t size, const uint8_t* payl
|
||||||
int transferred = hid_write(device->handle, payload, payload_size);
|
int transferred = hid_write(device->handle, payload, payload_size);
|
||||||
|
|
||||||
if (transferred != payload_size) {
|
if (transferred != payload_size) {
|
||||||
perror("ERROR: sending payload failed\n");
|
fprintf(stderr, "ERROR: sending payload failed\n");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -89,7 +90,7 @@ static bool recv_payload(device3_type* device, uint8_t size, uint8_t* payload) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (transferred != payload_size) {
|
if (transferred != payload_size) {
|
||||||
perror("ERROR: receiving payload failed\n");
|
fprintf(stderr, "ERROR: receiving payload failed\n");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -179,7 +180,6 @@ device3_type* device3_open(device3_event_callback callback) {
|
||||||
device3_type* device = (device3_type*) malloc(sizeof(device3_type));
|
device3_type* device = (device3_type*) malloc(sizeof(device3_type));
|
||||||
|
|
||||||
if (!device) {
|
if (!device) {
|
||||||
perror("Not allocated!\n");
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -189,7 +189,6 @@ device3_type* device3_open(device3_event_callback callback) {
|
||||||
device->callback = callback;
|
device->callback = callback;
|
||||||
|
|
||||||
if (0 != hid_init()) {
|
if (0 != hid_init()) {
|
||||||
perror("Not initialized!\n");
|
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -211,14 +210,12 @@ device3_type* device3_open(device3_event_callback callback) {
|
||||||
hid_free_enumeration(info);
|
hid_free_enumeration(info);
|
||||||
|
|
||||||
if (!device->handle) {
|
if (!device->handle) {
|
||||||
perror("No handle!\n");
|
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
device3_clear(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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -233,7 +230,6 @@ 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -287,7 +283,6 @@ 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -304,21 +299,24 @@ device3_type* device3_open(device3_event_callback callback) {
|
||||||
.gain = 0.5f,
|
.gain = 0.5f,
|
||||||
.accelerationRejection = 10.0f,
|
.accelerationRejection = 10.0f,
|
||||||
.magneticRejection = 20.0f,
|
.magneticRejection = 20.0f,
|
||||||
.rejectionTimeout = 5 * SAMPLE_RATE, /* 5 seconds */
|
.recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */
|
||||||
};
|
};
|
||||||
|
|
||||||
FusionAhrsSetSettings((FusionAhrs*) device->ahrs, &settings);
|
FusionAhrsSetSettings((FusionAhrs*) device->ahrs, &settings);
|
||||||
|
|
||||||
|
device->ready = true;
|
||||||
|
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
void device3_reset_calibration(device3_type* device) {
|
void device3_reset_calibration(device3_type* device) {
|
||||||
if (!device) {
|
if (!device) {
|
||||||
perror("No device!\n");
|
fprintf(stderr, "No device!\n");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!device->calibration) {
|
if (!device->calibration) {
|
||||||
perror("Not allocated!\n");
|
fprintf(stderr, "Not allocated!\n");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -343,18 +341,18 @@ void device3_reset_calibration(device3_type* device) {
|
||||||
|
|
||||||
int device3_load_calibration(device3_type* device, const char* path) {
|
int device3_load_calibration(device3_type* device, const char* path) {
|
||||||
if (!device) {
|
if (!device) {
|
||||||
perror("No device!\n");
|
fprintf(stderr, "No device!\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!device->calibration) {
|
if (!device->calibration) {
|
||||||
perror("Not allocated!\n");
|
fprintf(stderr, "Not allocated!\n");
|
||||||
return -2;
|
return -2;
|
||||||
}
|
}
|
||||||
|
|
||||||
FILE* file = fopen(path, "rb");
|
FILE* file = fopen(path, "rb");
|
||||||
if (!file) {
|
if (!file) {
|
||||||
perror("No file opened!\n");
|
fprintf(stderr, "No file opened!\n");
|
||||||
return -3;
|
return -3;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -362,11 +360,11 @@ int device3_load_calibration(device3_type* device, const char* path) {
|
||||||
count = fread(device->calibration, 1, sizeof(device3_calibration_type), file);
|
count = fread(device->calibration, 1, sizeof(device3_calibration_type), file);
|
||||||
|
|
||||||
if (sizeof(device3_calibration_type) != count) {
|
if (sizeof(device3_calibration_type) != count) {
|
||||||
perror("Not fully loaded!\n");
|
fprintf(stderr, "Not fully loaded!\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (0 != fclose(file)) {
|
if (0 != fclose(file)) {
|
||||||
perror("No file closed!\n");
|
fprintf(stderr, "No file closed!\n");
|
||||||
return -4;
|
return -4;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -375,18 +373,18 @@ int device3_load_calibration(device3_type* device, const char* path) {
|
||||||
|
|
||||||
int device3_save_calibration(device3_type* device, const char* path) {
|
int device3_save_calibration(device3_type* device, const char* path) {
|
||||||
if (!device) {
|
if (!device) {
|
||||||
perror("No device!\n");
|
fprintf(stderr, "No device!\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!device->calibration) {
|
if (!device->calibration) {
|
||||||
perror("Not allocated!\n");
|
fprintf(stderr, "Not allocated!\n");
|
||||||
return -2;
|
return -2;
|
||||||
}
|
}
|
||||||
|
|
||||||
FILE* file = fopen(path, "wb");
|
FILE* file = fopen(path, "wb");
|
||||||
if (!file) {
|
if (!file) {
|
||||||
perror("No file opened!\n");
|
fprintf(stderr, "No file opened!\n");
|
||||||
return -3;
|
return -3;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -394,11 +392,11 @@ int device3_save_calibration(device3_type* device, const char* path) {
|
||||||
count = fwrite(device->calibration, 1, sizeof(device3_calibration_type), file);
|
count = fwrite(device->calibration, 1, sizeof(device3_calibration_type), file);
|
||||||
|
|
||||||
if (sizeof(device3_calibration_type) != count) {
|
if (sizeof(device3_calibration_type) != count) {
|
||||||
perror("Not fully saved!\n");
|
fprintf(stderr, "Not fully saved!\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (0 != fclose(file)) {
|
if (0 != fclose(file)) {
|
||||||
perror("No file closed!\n");
|
fprintf(stderr, "No file closed!\n");
|
||||||
return -4;
|
return -4;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -600,17 +598,17 @@ static void apply_calibration(const device3_type* device,
|
||||||
}
|
}
|
||||||
|
|
||||||
void device3_clear(device3_type* device) {
|
void device3_clear(device3_type* device) {
|
||||||
device3_read(device, 10);
|
device3_read(device, 10, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
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) {
|
||||||
if (!device) {
|
if (!device) {
|
||||||
perror("No device!\n");
|
fprintf(stderr, "No device!\n");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) {
|
if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) {
|
||||||
perror("Not proper size!\n");
|
fprintf(stderr, "Not proper size!\n");
|
||||||
return -2;
|
return -2;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -634,12 +632,17 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
|
||||||
MAX_PACKET_SIZE
|
MAX_PACKET_SIZE
|
||||||
);
|
);
|
||||||
|
|
||||||
|
if (transferred == -1) {
|
||||||
|
fprintf(stderr, "HID read error: device may be unplugged\n");
|
||||||
|
return -5;
|
||||||
|
}
|
||||||
|
|
||||||
if (transferred == 0) {
|
if (transferred == 0) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (MAX_PACKET_SIZE != transferred) {
|
if (MAX_PACKET_SIZE != transferred) {
|
||||||
perror("Not expected issue!\n");
|
fprintf(stderr, "HID read error: unexpected packet size\n");
|
||||||
return -3;
|
return -3;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -714,14 +717,18 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int device3_read(device3_type* device, int timeout) {
|
int device3_read(device3_type* device, int timeout, bool silent) {
|
||||||
if (!device) {
|
if (!device || !device->ready) {
|
||||||
perror("No device!\n");
|
if (!silent) {
|
||||||
|
fprintf(stderr, "No device!\n");
|
||||||
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) {
|
if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) {
|
||||||
perror("Not proper size!\n");
|
if (!silent) {
|
||||||
|
fprintf(stderr, "Not proper size!\n");
|
||||||
|
}
|
||||||
return -2;
|
return -2;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -735,12 +742,21 @@ int device3_read(device3_type* device, int timeout) {
|
||||||
timeout
|
timeout
|
||||||
);
|
);
|
||||||
|
|
||||||
|
if (transferred == -1) {
|
||||||
|
if (!silent) {
|
||||||
|
fprintf(stderr, "HID read error: device may be unplugged\n");
|
||||||
|
}
|
||||||
|
return -5;
|
||||||
|
}
|
||||||
|
|
||||||
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");
|
if (!silent) {
|
||||||
|
fprintf(stderr, "HID read error: unexpected packet size\n");
|
||||||
|
}
|
||||||
return -3;
|
return -3;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -752,7 +768,9 @@ int device3_read(device3_type* device, int timeout) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) {
|
if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) {
|
||||||
perror("Not matching signature!\n");
|
if (!silent) {
|
||||||
|
fprintf(stderr, "Mismatched signature! Try unplugging then replugging your device.\n");
|
||||||
|
}
|
||||||
return -4;
|
return -4;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -782,8 +800,16 @@ int device3_read(device3_type* device, int timeout) {
|
||||||
//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);
|
||||||
|
|
||||||
if (device->ahrs) {
|
if (device->ahrs) {
|
||||||
FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
|
FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime);
|
||||||
//FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime);
|
|
||||||
|
// 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(device3_get_orientation(device->ahrs).x)) {
|
||||||
|
if (!silent) {
|
||||||
|
fprintf(stderr, "Invalid device reading\n");
|
||||||
|
}
|
||||||
|
return -5;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE);
|
device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE);
|
||||||
|
|
@ -834,7 +860,6 @@ device3_vec3_type device3_get_euler(device3_quat_type quat) {
|
||||||
|
|
||||||
void device3_close(device3_type* device) {
|
void device3_close(device3_type* device) {
|
||||||
if (!device) {
|
if (!device) {
|
||||||
perror("No device!\n");
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -31,7 +31,7 @@
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
|
|
||||||
#include <hidapi/hidapi.h>
|
#include <../hidapi/hidapi/hidapi.h>
|
||||||
|
|
||||||
#include "crc32.h"
|
#include "crc32.h"
|
||||||
|
|
||||||
|
|
@ -47,7 +47,7 @@ static bool send_payload(device4_type* device, uint8_t size, const uint8_t* payl
|
||||||
int transferred = hid_write(device->handle, payload, payload_size);
|
int transferred = hid_write(device->handle, payload, payload_size);
|
||||||
|
|
||||||
if (transferred != payload_size) {
|
if (transferred != payload_size) {
|
||||||
perror("ERROR: sending payload failed\n");
|
fprintf(stderr, "ERROR: sending payload failed\n");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -71,7 +71,7 @@ static bool recv_payload(device4_type* device, uint8_t size, uint8_t* payload) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (transferred != payload_size) {
|
if (transferred != payload_size) {
|
||||||
perror("ERROR: receiving payload failed\n");
|
fprintf(stderr, "ERROR: receiving payload failed\n");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -160,7 +160,7 @@ 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));
|
||||||
|
|
||||||
if (!device) {
|
if (!device) {
|
||||||
perror("Not allocated!\n");
|
fprintf(stderr, "Not allocated!\n");
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -170,7 +170,7 @@ device4_type* device4_open(device4_event_callback callback) {
|
||||||
device->callback = callback;
|
device->callback = callback;
|
||||||
|
|
||||||
if (0 != hid_init()) {
|
if (0 != hid_init()) {
|
||||||
perror("Not initialized!\n");
|
fprintf(stderr, "Not initialized!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -192,57 +192,57 @@ device4_type* device4_open(device4_event_callback callback) {
|
||||||
hid_free_enumeration(info);
|
hid_free_enumeration(info);
|
||||||
|
|
||||||
if (!device->handle) {
|
if (!device->handle) {
|
||||||
perror("No handle!\n");
|
fprintf(stderr, "No handle!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
device4_clear(device);
|
device4_clear(device);
|
||||||
|
|
||||||
if (!send_payload_action(device, DEVICE4_MSG_R_ACTIVATION_TIME, 0, NULL)) {
|
if (!send_payload_action(device, DEVICE4_MSG_R_ACTIVATION_TIME, 0, NULL)) {
|
||||||
perror("Requesting activation time failed!\n");
|
fprintf(stderr, "Requesting activation time failed!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t activated;
|
uint8_t activated;
|
||||||
if (!recv_payload_msg(device, DEVICE4_MSG_R_ACTIVATION_TIME, 1, &activated)) {
|
if (!recv_payload_msg(device, DEVICE4_MSG_R_ACTIVATION_TIME, 1, &activated)) {
|
||||||
perror("Receiving activation time failed!\n");
|
fprintf(stderr, "Receiving activation time failed!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
device->activated = (activated != 0);
|
device->activated = (activated != 0);
|
||||||
|
|
||||||
if (!device->activated) {
|
if (!device->activated) {
|
||||||
perror("Device is not activated!\n");
|
fprintf(stderr, "Device is not activated!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!send_payload_action(device, DEVICE4_MSG_R_MCU_APP_FW_VERSION, 0, NULL)) {
|
if (!send_payload_action(device, DEVICE4_MSG_R_MCU_APP_FW_VERSION, 0, NULL)) {
|
||||||
perror("Requesting current MCU app firmware version!\n");
|
fprintf(stderr, "Requesting current MCU app firmware version!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!recv_payload_msg(device, DEVICE4_MSG_R_MCU_APP_FW_VERSION, 41, (uint8_t*) device->mcu_app_fw_version)) {
|
if (!recv_payload_msg(device, DEVICE4_MSG_R_MCU_APP_FW_VERSION, 41, (uint8_t*) device->mcu_app_fw_version)) {
|
||||||
perror("Receiving current MCU app firmware version failed!\n");
|
fprintf(stderr, "Receiving current MCU app firmware version failed!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!send_payload_action(device, DEVICE4_MSG_R_DP7911_FW_VERSION, 0, NULL)) {
|
if (!send_payload_action(device, DEVICE4_MSG_R_DP7911_FW_VERSION, 0, NULL)) {
|
||||||
perror("Requesting current DP firmware version!\n");
|
fprintf(stderr, "Requesting current DP firmware version!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!recv_payload_msg(device, DEVICE4_MSG_R_DP7911_FW_VERSION, 41, (uint8_t*) device->dp_fw_version)) {
|
if (!recv_payload_msg(device, DEVICE4_MSG_R_DP7911_FW_VERSION, 41, (uint8_t*) device->dp_fw_version)) {
|
||||||
perror("Receiving current DP firmware version failed!\n");
|
fprintf(stderr, "Receiving current DP firmware version failed!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!send_payload_action(device, DEVICE4_MSG_R_DSP_APP_FW_VERSION, 0, NULL)) {
|
if (!send_payload_action(device, DEVICE4_MSG_R_DSP_APP_FW_VERSION, 0, NULL)) {
|
||||||
perror("Requesting current DSP app firmware version!\n");
|
fprintf(stderr, "Requesting current DSP app firmware version!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!recv_payload_msg(device, DEVICE4_MSG_R_DSP_APP_FW_VERSION, 41, (uint8_t*) device->dsp_fw_version)) {
|
if (!recv_payload_msg(device, DEVICE4_MSG_R_DSP_APP_FW_VERSION, 41, (uint8_t*) device->dsp_fw_version)) {
|
||||||
perror("Receiving current DSP app firmware version failed!\n");
|
fprintf(stderr, "Receiving current DSP app firmware version failed!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -253,22 +253,22 @@ device4_type* device4_open(device4_event_callback callback) {
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (!send_payload_action(device, DEVICE4_MSG_R_BRIGHTNESS, 0, NULL)) {
|
if (!send_payload_action(device, DEVICE4_MSG_R_BRIGHTNESS, 0, NULL)) {
|
||||||
perror("Requesting initial brightness failed!\n");
|
fprintf(stderr, "Requesting initial brightness failed!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!recv_payload_msg(device, DEVICE4_MSG_R_BRIGHTNESS, 1, &device->brightness)) {
|
if (!recv_payload_msg(device, DEVICE4_MSG_R_BRIGHTNESS, 1, &device->brightness)) {
|
||||||
perror("Receiving initial brightness failed!\n");
|
fprintf(stderr, "Receiving initial brightness failed!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!send_payload_action(device, DEVICE4_MSG_R_DISP_MODE, 0, NULL)) {
|
if (!send_payload_action(device, DEVICE4_MSG_R_DISP_MODE, 0, NULL)) {
|
||||||
perror("Requesting display mode failed!\n");
|
fprintf(stderr, "Requesting display mode failed!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!recv_payload_msg(device, DEVICE4_MSG_R_DISP_MODE, 1, &device->disp_mode)) {
|
if (!recv_payload_msg(device, DEVICE4_MSG_R_DISP_MODE, 1, &device->disp_mode)) {
|
||||||
perror("Receiving display mode failed!\n");
|
fprintf(stderr, "Receiving display mode failed!\n");
|
||||||
return device;
|
return device;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -302,7 +302,7 @@ int device4_read(device4_type* device, int timeout) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (MAX_PACKET_SIZE != sizeof(device4_packet_type)) {
|
if (MAX_PACKET_SIZE != sizeof(device4_packet_type)) {
|
||||||
perror("Not proper size!\n");
|
fprintf(stderr, "Not proper size!\n");
|
||||||
return -2;
|
return -2;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -321,12 +321,12 @@ int device4_read(device4_type* device, int timeout) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (MAX_PACKET_SIZE != transferred) {
|
if (MAX_PACKET_SIZE != transferred) {
|
||||||
perror("Reading failed!\n");
|
fprintf(stderr, "Reading failed!\n");
|
||||||
return -3;
|
return -3;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (packet.head != PACKET_HEAD) {
|
if (packet.head != PACKET_HEAD) {
|
||||||
perror("Wrong packet!\n");
|
fprintf(stderr, "Wrong packet!\n");
|
||||||
return -4;
|
return -4;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -411,7 +411,7 @@ int device4_read(device4_type* device, int timeout) {
|
||||||
device->active = true;
|
device->active = true;
|
||||||
|
|
||||||
if (data_len + text_len != packet.length) {
|
if (data_len + text_len != packet.length) {
|
||||||
perror("Not matching length!\n");
|
fprintf(stderr, "Not matching length!\n");
|
||||||
return -5;
|
return -5;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -447,7 +447,7 @@ bool device4_update_mcu_firmware(device4_type* device, const char* path) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!device->activated) {
|
if (!device->activated) {
|
||||||
perror("Device is not activated!\n");
|
fprintf(stderr, "Device is not activated!\n");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -474,12 +474,12 @@ bool device4_update_mcu_firmware(device4_type* device, const char* path) {
|
||||||
printf("Prepare upload: %lu\n", firmware_len);
|
printf("Prepare upload: %lu\n", firmware_len);
|
||||||
|
|
||||||
if (!do_payload_action(device, DEVICE4_MSG_W_UPDATE_MCU_APP_FW_PREPARE, 0, NULL)) {
|
if (!do_payload_action(device, DEVICE4_MSG_W_UPDATE_MCU_APP_FW_PREPARE, 0, NULL)) {
|
||||||
perror("Failed preparing the device for MCU firmware update!\n");
|
fprintf(stderr, "Failed preparing the device for MCU firmware update!\n");
|
||||||
goto cleanup;
|
goto cleanup;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!do_payload_action(device, DEVICE4_MSG_W_MCU_APP_JUMP_TO_BOOT, 0, NULL)) {
|
if (!do_payload_action(device, DEVICE4_MSG_W_MCU_APP_JUMP_TO_BOOT, 0, NULL)) {
|
||||||
perror("Failed mcu app jumping to boot!\n");
|
fprintf(stderr, "Failed mcu app jumping to boot!\n");
|
||||||
goto cleanup;
|
goto cleanup;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -501,7 +501,7 @@ bool device4_update_mcu_firmware(device4_type* device, const char* path) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!do_payload_action(device, msgid, len, firmware)) {
|
if (!do_payload_action(device, msgid, len, firmware)) {
|
||||||
perror("Failed sending firmware upload!\n");
|
fprintf(stderr, "Failed sending firmware upload!\n");
|
||||||
goto jump_to_app;
|
goto jump_to_app;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -509,7 +509,7 @@ bool device4_update_mcu_firmware(device4_type* device, const char* path) {
|
||||||
printf("Finish upload!\n");
|
printf("Finish upload!\n");
|
||||||
|
|
||||||
if (!do_payload_action(device, DEVICE4_MSG_W_UPDATE_MCU_APP_FW_FINISH, 0, NULL)) {
|
if (!do_payload_action(device, DEVICE4_MSG_W_UPDATE_MCU_APP_FW_FINISH, 0, NULL)) {
|
||||||
perror("Failed finishing firmware upload!\n");
|
fprintf(stderr, "Failed finishing firmware upload!\n");
|
||||||
goto jump_to_app;
|
goto jump_to_app;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -517,7 +517,7 @@ bool device4_update_mcu_firmware(device4_type* device, const char* path) {
|
||||||
|
|
||||||
jump_to_app:
|
jump_to_app:
|
||||||
if (!do_payload_action(device, DEVICE4_MSG_W_BOOT_JUMP_TO_APP, 0, NULL)) {
|
if (!do_payload_action(device, DEVICE4_MSG_W_BOOT_JUMP_TO_APP, 0, NULL)) {
|
||||||
perror("Failed boot jumping back to app!\n");
|
fprintf(stderr, "Failed boot jumping back to app!\n");
|
||||||
goto cleanup;
|
goto cleanup;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1 +0,0 @@
|
||||||
Subproject commit f69fd25df008ca11e709579fea4b784537f8911e
|
|
||||||
|
|
@ -106,7 +106,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, 0, false) < 0) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue