feature: Get sensor data reading working
This commit is contained in:
parent
ad3045fc29
commit
243d595a35
GPG key ID:
8FA7DD57BA6CEA37
11 changed files with 413 additions and 147 deletions
|
|
@ -9,10 +9,10 @@
|
|||
// 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
|
||||
|
|
@ -25,8 +25,8 @@
|
|||
#include "device_imu.h"
|
||||
#include "device.h"
|
||||
|
||||
#include <Fusion/FusionAxes.h>
|
||||
#include <Fusion/FusionMath.h>
|
||||
#include <FusionAxes.h>
|
||||
#include <FusionMath.h>
|
||||
#include <float.h>
|
||||
#include <json-c/json_object.h>
|
||||
#include <json-c/json_types.h>
|
||||
|
|
@ -35,7 +35,7 @@
|
|||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <Fusion/Fusion.h>
|
||||
#include <Fusion.h>
|
||||
#include <json-c/json.h>
|
||||
|
||||
#include <hidapi/hidapi.h>
|
||||
|
|
@ -78,18 +78,18 @@ struct device_imu_calibration_t {
|
|||
FusionMatrix gyroscopeMisalignment;
|
||||
FusionVector gyroscopeSensitivity;
|
||||
FusionVector gyroscopeOffset;
|
||||
|
||||
|
||||
FusionMatrix accelerometerMisalignment;
|
||||
FusionVector accelerometerSensitivity;
|
||||
FusionVector accelerometerOffset;
|
||||
|
||||
|
||||
FusionMatrix magnetometerMisalignment;
|
||||
FusionVector magnetometerSensitivity;
|
||||
FusionVector magnetometerOffset;
|
||||
|
||||
|
||||
FusionMatrix softIronMatrix;
|
||||
FusionVector hardIronOffset;
|
||||
|
||||
|
||||
FusionQuaternion noises;
|
||||
|
||||
device_imu_camera_calibration_type cam;
|
||||
|
|
@ -100,14 +100,14 @@ static bool send_payload(device_imu_type* device, uint16_t size, const uint8_t*
|
|||
if (payload_size > device->max_payload_size) {
|
||||
payload_size = device->max_payload_size;
|
||||
}
|
||||
|
||||
|
||||
int transferred = hid_write(device->handle, payload, payload_size);
|
||||
|
||||
|
||||
if (transferred != payload_size) {
|
||||
device_imu_error("Sending payload failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
return (transferred == size);
|
||||
}
|
||||
|
||||
|
|
@ -116,9 +116,9 @@ static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payloa
|
|||
if (payload_size > device->max_payload_size) {
|
||||
payload_size = device->max_payload_size;
|
||||
}
|
||||
|
||||
|
||||
int transferred = hid_read(device->handle, payload, payload_size);
|
||||
|
||||
|
||||
if (transferred >= payload_size) {
|
||||
transferred = payload_size;
|
||||
}
|
||||
|
|
@ -126,12 +126,12 @@ static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payloa
|
|||
if (transferred == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
if (transferred != payload_size) {
|
||||
device_imu_error("Receiving payload failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
return (transferred == size);
|
||||
}
|
||||
|
||||
|
|
@ -147,14 +147,14 @@ typedef struct device_imu_payload_packet_t device_imu_payload_packet_type;
|
|||
|
||||
static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_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;
|
||||
|
||||
|
||||
packet.head = 0xAA;
|
||||
packet.length = htole16(packet_len);
|
||||
packet.msgid = msgid;
|
||||
|
||||
|
||||
memcpy(packet.data, data, len);
|
||||
packet.checksum = htole32(
|
||||
crc32_checksum(
|
||||
|
|
@ -162,7 +162,7 @@ static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t le
|
|||
packet.length
|
||||
)
|
||||
);
|
||||
|
||||
|
||||
return send_payload(device, payload_len, (uint8_t*) (&packet));
|
||||
}
|
||||
|
||||
|
|
@ -172,20 +172,20 @@ static bool send_payload_msg_signal(device_imu_type* device, uint8_t msgid, uint
|
|||
|
||||
static bool recv_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, uint8_t* data) {
|
||||
static device_imu_payload_packet_type packet;
|
||||
|
||||
|
||||
packet.head = 0;
|
||||
packet.length = 0;
|
||||
packet.msgid = 0;
|
||||
|
||||
|
||||
const uint16_t packet_len = 3 + len;
|
||||
const uint16_t payload_len = 5 + packet_len;
|
||||
|
||||
|
||||
do {
|
||||
if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) {
|
||||
return false;
|
||||
}
|
||||
} while (packet.msgid != msgid);
|
||||
|
||||
|
||||
memcpy(data, packet.data, len);
|
||||
return true;
|
||||
}
|
||||
|
|
@ -195,7 +195,7 @@ static FusionVector json_object_get_vector(struct json_object* obj) {
|
|||
(json_object_array_length(obj) != 3)) {
|
||||
return FUSION_VECTOR_ZERO;
|
||||
}
|
||||
|
||||
|
||||
FusionVector vector;
|
||||
vector.axis.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0));
|
||||
vector.axis.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1));
|
||||
|
|
@ -208,7 +208,7 @@ static FusionQuaternion json_object_get_quaternion(struct json_object* obj) {
|
|||
(json_object_array_length(obj) != 4)) {
|
||||
return FUSION_IDENTITY_QUATERNION;
|
||||
}
|
||||
|
||||
|
||||
FusionQuaternion quaternion;
|
||||
quaternion.element.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0));
|
||||
quaternion.element.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1));
|
||||
|
|
@ -284,7 +284,7 @@ static void init_device_imu_camera(device_imu_camera_type *camera, json_object *
|
|||
float* cc = sensor->cc;
|
||||
float* fc = sensor->fc;
|
||||
float* kc = sensor->kc;
|
||||
|
||||
|
||||
json_object_get_array_u16(json_object_object_get(dev, "resolution"), &resolution, 2);
|
||||
|
||||
json_object_get_array_f32(json_object_object_get(dev, "cc"), &cc, 2);
|
||||
|
|
@ -303,19 +303,19 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_
|
|||
device_imu_error("No device");
|
||||
return DEVICE_IMU_ERROR_NO_DEVICE;
|
||||
}
|
||||
|
||||
|
||||
memset(device, 0, sizeof(device_imu_type));
|
||||
device->vendor_id = xreal_vendor_id;
|
||||
device->product_id = 0;
|
||||
device->callback = callback;
|
||||
|
||||
|
||||
if (!device_init()) {
|
||||
device_imu_error("Not initialized");
|
||||
return DEVICE_IMU_ERROR_NOT_INITIALIZED;
|
||||
}
|
||||
|
||||
struct hid_device_info* info = hid_enumerate(
|
||||
device->vendor_id,
|
||||
device->vendor_id,
|
||||
device->product_id
|
||||
);
|
||||
|
||||
|
|
@ -336,7 +336,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_
|
|||
}
|
||||
|
||||
hid_free_enumeration(info);
|
||||
|
||||
|
||||
if (!device->handle) {
|
||||
device_imu_error("No handle");
|
||||
return DEVICE_IMU_ERROR_NO_HANDLE;
|
||||
|
|
@ -349,19 +349,19 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_
|
|||
}
|
||||
|
||||
device_imu_clear(device);
|
||||
|
||||
|
||||
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, 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(device_imu_calibration_type));
|
||||
memset(device->calibration, 0, sizeof(device_imu_calibration_type));
|
||||
|
||||
|
|
@ -371,12 +371,12 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_
|
|||
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, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 4, (uint8_t*) &calibration_len)) {
|
||||
const uint16_t max_packet_size = (device->max_payload_size - 8);
|
||||
char* calibration_data = malloc(calibration_len + 1);
|
||||
|
||||
|
||||
uint32_t position = 0;
|
||||
while (position < calibration_len) {
|
||||
const uint32_t remaining = (calibration_len - position);
|
||||
|
|
@ -384,23 +384,23 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_
|
|||
if (!send_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, 0, NULL)) {
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
const uint16_t next = (remaining > max_packet_size? max_packet_size : remaining);
|
||||
|
||||
|
||||
if (!recv_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, next, (uint8_t*) calibration_data + position)) {
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
position += next;
|
||||
}
|
||||
|
||||
calibration_data[calibration_len] = '\0';
|
||||
|
||||
|
||||
struct json_tokener* tokener = json_tokener_new();
|
||||
struct json_object* root = json_tokener_parse_ex(tokener, calibration_data, calibration_len);
|
||||
struct json_object* imu = json_object_object_get(root, "IMU");
|
||||
struct json_object* dev1 = json_object_object_get(imu, "device_1");
|
||||
|
||||
|
||||
FusionVector accel_bias = json_object_get_vector(json_object_object_get(dev1, "accel_bias"));
|
||||
FusionQuaternion accel_q_gyro = json_object_get_quaternion(json_object_object_get(dev1, "accel_q_gyro"));
|
||||
FusionVector gyro_bias = json_object_get_vector(json_object_object_get(dev1, "gyro_bias"));
|
||||
|
|
@ -412,19 +412,19 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_
|
|||
FusionVector scale_mag = json_object_get_vector(json_object_object_get(dev1, "scale_mag"));
|
||||
|
||||
const FusionQuaternion accel_q_mag = FusionQuaternionMultiply(accel_q_gyro, gyro_q_mag);
|
||||
|
||||
|
||||
device->calibration->gyroscopeMisalignment = FusionQuaternionToMatrix(accel_q_gyro);
|
||||
device->calibration->gyroscopeSensitivity = scale_gyro;
|
||||
device->calibration->gyroscopeOffset = gyro_bias;
|
||||
|
||||
|
||||
device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX;
|
||||
device->calibration->accelerometerSensitivity = scale_accel;
|
||||
device->calibration->accelerometerOffset = accel_bias;
|
||||
|
||||
|
||||
device->calibration->magnetometerMisalignment = FusionQuaternionToMatrix(accel_q_mag);
|
||||
device->calibration->magnetometerSensitivity = scale_mag;
|
||||
device->calibration->magnetometerOffset = mag_bias;
|
||||
|
||||
|
||||
device->calibration->noises = imu_noises;
|
||||
|
||||
struct json_object* rgb = json_object_object_get(root, "RGB_camera");
|
||||
|
|
@ -452,7 +452,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_
|
|||
|
||||
device->calibration->cam.num_cameras = cameras? num_cameras : 0;
|
||||
device->calibration->cam.cameras = cameras;
|
||||
|
||||
|
||||
json_tokener_free(tokener);
|
||||
free(calibration_data);
|
||||
}
|
||||
|
|
@ -464,16 +464,16 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_
|
|||
}
|
||||
|
||||
const uint32_t SAMPLE_RATE = 1000;
|
||||
|
||||
|
||||
device->offset = malloc(sizeof(FusionOffset));
|
||||
device->ahrs = malloc(sizeof(FusionAhrs));
|
||||
|
||||
|
||||
if (device->offset) {
|
||||
FusionOffsetInitialise((FusionOffset*) device->offset, SAMPLE_RATE);
|
||||
}
|
||||
|
||||
FusionAhrsInitialise((FusionAhrs*) device->ahrs);
|
||||
|
||||
|
||||
const FusionAhrsSettings settings = {
|
||||
.convention = FusionConventionNed,
|
||||
.gain = 0.5f,
|
||||
|
|
@ -481,7 +481,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_
|
|||
.magneticRejection = 20.0f,
|
||||
.recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */
|
||||
};
|
||||
|
||||
|
||||
FusionAhrsSetSettings((FusionAhrs*) device->ahrs, &settings);
|
||||
return DEVICE_IMU_ERROR_NO_ERROR;
|
||||
}
|
||||
|
|
@ -491,27 +491,27 @@ device_imu_error_type device_imu_reset_calibration(device_imu_type* device) {
|
|||
device_imu_error("No device");
|
||||
return DEVICE_IMU_ERROR_NO_DEVICE;
|
||||
}
|
||||
|
||||
|
||||
if (!device->calibration) {
|
||||
device_imu_error("Not allocated");
|
||||
return DEVICE_IMU_ERROR_NO_ALLOCATION;
|
||||
}
|
||||
|
||||
|
||||
device->calibration->gyroscopeMisalignment = FUSION_IDENTITY_MATRIX;
|
||||
device->calibration->gyroscopeSensitivity = FUSION_VECTOR_ONES;
|
||||
device->calibration->gyroscopeOffset = FUSION_VECTOR_ZERO;
|
||||
|
||||
|
||||
device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX;
|
||||
device->calibration->accelerometerSensitivity = FUSION_VECTOR_ONES;
|
||||
device->calibration->accelerometerOffset = FUSION_VECTOR_ZERO;
|
||||
|
||||
|
||||
device->calibration->magnetometerMisalignment = FUSION_IDENTITY_MATRIX;
|
||||
device->calibration->magnetometerSensitivity = FUSION_VECTOR_ONES;
|
||||
device->calibration->magnetometerOffset = FUSION_VECTOR_ZERO;
|
||||
|
||||
|
||||
device->calibration->softIronMatrix = FUSION_IDENTITY_MATRIX;
|
||||
device->calibration->hardIronOffset = FUSION_VECTOR_ZERO;
|
||||
|
||||
|
||||
device->calibration->noises = FUSION_IDENTITY_QUATERNION;
|
||||
device->calibration->noises.element.w = 0.0f;
|
||||
|
||||
|
|
@ -550,12 +550,12 @@ device_imu_error_type device_imu_load_calibration(device_imu_type* device, const
|
|||
device_imu_error("No device");
|
||||
return DEVICE_IMU_ERROR_NO_DEVICE;
|
||||
}
|
||||
|
||||
|
||||
if (!device->calibration) {
|
||||
device_imu_error("Not allocated");
|
||||
return DEVICE_IMU_ERROR_NO_ALLOCATION;
|
||||
}
|
||||
|
||||
|
||||
FILE* file = fopen(path, "rb");
|
||||
if (!file) {
|
||||
device_imu_error("No file opened");
|
||||
|
|
@ -566,20 +566,20 @@ device_imu_error_type device_imu_load_calibration(device_imu_type* device, const
|
|||
const size_t calibration_size = (
|
||||
sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type)
|
||||
);
|
||||
|
||||
|
||||
size_t count;
|
||||
count = fread(device->calibration, 1, calibration_size, file);
|
||||
|
||||
|
||||
if (calibration_size != count) {
|
||||
device_imu_error("Not fully loaded");
|
||||
result = DEVICE_IMU_ERROR_LOADING_FAILED;
|
||||
}
|
||||
|
||||
|
||||
if (0 != fclose(file)) {
|
||||
device_imu_error("No file closed");
|
||||
return DEVICE_IMU_ERROR_FILE_NOT_CLOSED;
|
||||
}
|
||||
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -588,12 +588,12 @@ device_imu_error_type device_imu_save_calibration(device_imu_type* device, const
|
|||
device_imu_error("No device");
|
||||
return DEVICE_IMU_ERROR_NO_DEVICE;
|
||||
}
|
||||
|
||||
|
||||
if (!device->calibration) {
|
||||
device_imu_error("Not allocated");
|
||||
return DEVICE_IMU_ERROR_NO_ALLOCATION;
|
||||
}
|
||||
|
||||
|
||||
FILE* file = fopen(path, "wb");
|
||||
if (!file) {
|
||||
device_imu_error("No file opened");
|
||||
|
|
@ -604,20 +604,20 @@ device_imu_error_type device_imu_save_calibration(device_imu_type* device, const
|
|||
const size_t calibration_size = (
|
||||
sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type)
|
||||
);
|
||||
|
||||
|
||||
size_t count;
|
||||
count = fwrite(device->calibration, 1, calibration_size, file);
|
||||
|
||||
|
||||
if (calibration_size != count) {
|
||||
device_imu_error("Not fully saved");
|
||||
result = DEVICE_IMU_ERROR_SAVING_FAILED;
|
||||
}
|
||||
|
||||
|
||||
if (0 != fclose(file)) {
|
||||
device_imu_error("No file closed");
|
||||
return DEVICE_IMU_ERROR_FILE_NOT_CLOSED;
|
||||
}
|
||||
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -627,7 +627,7 @@ static void device_imu_callback(device_imu_type* device,
|
|||
if (!device->callback) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
device->callback(timestamp, event, device->ahrs);
|
||||
}
|
||||
|
||||
|
|
@ -668,33 +668,33 @@ static void readIMU_from_packet(const device_imu_packet_type* packet,
|
|||
FusionVector* magnetometer) {
|
||||
int32_t vel_m = pack16bit_signed(packet->angular_multiplier);
|
||||
int32_t vel_d = pack32bit_signed(packet->angular_divisor);
|
||||
|
||||
|
||||
int32_t vel_x = pack24bit_signed(packet->angular_velocity_x);
|
||||
int32_t vel_y = pack24bit_signed(packet->angular_velocity_y);
|
||||
int32_t vel_z = pack24bit_signed(packet->angular_velocity_z);
|
||||
|
||||
|
||||
gyroscope->axis.x = (float) vel_x * (float) vel_m / (float) vel_d;
|
||||
gyroscope->axis.y = (float) vel_y * (float) vel_m / (float) vel_d;
|
||||
gyroscope->axis.z = (float) vel_z * (float) vel_m / (float) vel_d;
|
||||
|
||||
|
||||
int32_t accel_m = pack16bit_signed(packet->acceleration_multiplier);
|
||||
int32_t accel_d = pack32bit_signed(packet->acceleration_divisor);
|
||||
|
||||
|
||||
int32_t accel_x = pack24bit_signed(packet->acceleration_x);
|
||||
int32_t accel_y = pack24bit_signed(packet->acceleration_y);
|
||||
int32_t accel_z = pack24bit_signed(packet->acceleration_z);
|
||||
|
||||
|
||||
accelerometer->axis.x = (float) accel_x * (float) accel_m / (float) accel_d;
|
||||
accelerometer->axis.y = (float) accel_y * (float) accel_m / (float) accel_d;
|
||||
accelerometer->axis.z = (float) accel_z * (float) accel_m / (float) accel_d;
|
||||
|
||||
|
||||
int32_t magnet_m = pack16bit_signed_swap(packet->magnetic_multiplier);
|
||||
int32_t magnet_d = pack32bit_signed_swap(packet->magnetic_divisor);
|
||||
|
||||
|
||||
int16_t magnet_x = pack16bit_signed_bizarre(packet->magnetic_x);
|
||||
int16_t magnet_y = pack16bit_signed_bizarre(packet->magnetic_y);
|
||||
int16_t magnet_z = pack16bit_signed_bizarre(packet->magnetic_z);
|
||||
|
||||
|
||||
magnetometer->axis.x = (float) magnet_x * (float) magnet_m / (float) magnet_d;
|
||||
magnetometer->axis.y = (float) magnet_y * (float) magnet_m / (float) magnet_d;
|
||||
magnetometer->axis.z = (float) magnet_z * (float) magnet_m / (float) magnet_d;
|
||||
|
|
@ -719,17 +719,17 @@ static void iterate_iron_offset_estimation(const FusionVector* magnetometer, Fus
|
|||
max.array[i] = max(max.array[i], magnetometer->array[i]);
|
||||
min.array[i] = min(min.array[i], magnetometer->array[i]);
|
||||
}
|
||||
|
||||
|
||||
const float mx = (max.axis.x - min.axis.x) / 2.0f;
|
||||
const float my = (max.axis.y - min.axis.y) / 2.0f;
|
||||
const float mz = (max.axis.z - min.axis.z) / 2.0f;
|
||||
|
||||
|
||||
const float cx = (min.axis.x + max.axis.x) / 2.0f;
|
||||
const float cy = (min.axis.y + max.axis.y) / 2.0f;
|
||||
const float cz = (min.axis.z + max.axis.z) / 2.0f;
|
||||
|
||||
memset(softIronMatrix, 0, sizeof(*softIronMatrix));
|
||||
|
||||
|
||||
softIronMatrix->element.xx = 1.0f / mx;
|
||||
softIronMatrix->element.yy = 1.0f / my;
|
||||
softIronMatrix->element.zz = 1.0f / mz;
|
||||
|
|
@ -746,57 +746,57 @@ static void apply_calibration(const device_imu_type* device,
|
|||
FusionMatrix gyroscopeMisalignment;
|
||||
FusionVector gyroscopeSensitivity;
|
||||
FusionVector gyroscopeOffset;
|
||||
|
||||
|
||||
FusionMatrix accelerometerMisalignment;
|
||||
FusionVector accelerometerSensitivity;
|
||||
FusionVector accelerometerOffset;
|
||||
|
||||
|
||||
FusionMatrix magnetometerMisalignment;
|
||||
FusionVector magnetometerSensitivity;
|
||||
FusionVector magnetometerOffset;
|
||||
|
||||
|
||||
FusionMatrix softIronMatrix;
|
||||
FusionVector hardIronOffset;
|
||||
|
||||
|
||||
if (device->calibration) {
|
||||
gyroscopeMisalignment = device->calibration->gyroscopeMisalignment;
|
||||
gyroscopeSensitivity = device->calibration->gyroscopeSensitivity;
|
||||
gyroscopeOffset = device->calibration->gyroscopeOffset;
|
||||
|
||||
|
||||
accelerometerMisalignment = device->calibration->accelerometerMisalignment;
|
||||
accelerometerSensitivity = device->calibration->accelerometerSensitivity;
|
||||
accelerometerOffset = device->calibration->accelerometerOffset;
|
||||
|
||||
|
||||
magnetometerMisalignment = device->calibration->magnetometerMisalignment;
|
||||
magnetometerSensitivity = device->calibration->magnetometerSensitivity;
|
||||
magnetometerOffset = device->calibration->magnetometerOffset;
|
||||
|
||||
|
||||
softIronMatrix = device->calibration->softIronMatrix;
|
||||
hardIronOffset = device->calibration->hardIronOffset;
|
||||
} else {
|
||||
gyroscopeMisalignment = FUSION_IDENTITY_MATRIX;
|
||||
gyroscopeSensitivity = FUSION_VECTOR_ONES;
|
||||
gyroscopeOffset = FUSION_VECTOR_ZERO;
|
||||
|
||||
|
||||
accelerometerMisalignment = FUSION_IDENTITY_MATRIX;
|
||||
accelerometerSensitivity = FUSION_VECTOR_ONES;
|
||||
accelerometerOffset = FUSION_VECTOR_ZERO;
|
||||
|
||||
|
||||
magnetometerMisalignment = FUSION_IDENTITY_MATRIX;
|
||||
magnetometerSensitivity = FUSION_VECTOR_ONES;
|
||||
magnetometerOffset = FUSION_VECTOR_ZERO;
|
||||
|
||||
|
||||
softIronMatrix = FUSION_IDENTITY_MATRIX;
|
||||
hardIronOffset = FUSION_VECTOR_ZERO;
|
||||
}
|
||||
|
||||
gyroscopeOffset = FusionVectorMultiplyScalar(
|
||||
gyroscopeOffset,
|
||||
gyroscopeOffset,
|
||||
FusionRadiansToDegrees(1.0f)
|
||||
);
|
||||
|
||||
accelerometerOffset = FusionVectorMultiplyScalar(
|
||||
accelerometerOffset,
|
||||
accelerometerOffset,
|
||||
1.0f / GRAVITY_G
|
||||
);
|
||||
|
||||
|
|
@ -814,14 +814,14 @@ static void apply_calibration(const device_imu_type* device,
|
|||
gyroscopeSensitivity,
|
||||
gyroscopeOffset
|
||||
);
|
||||
|
||||
|
||||
a = FusionCalibrationInertial(
|
||||
a,
|
||||
accelerometerMisalignment,
|
||||
accelerometerSensitivity,
|
||||
accelerometerOffset
|
||||
);
|
||||
|
||||
|
||||
m = FusionCalibrationInertial(
|
||||
m,
|
||||
magnetometerMisalignment,
|
||||
|
|
@ -830,16 +830,16 @@ static void apply_calibration(const device_imu_type* device,
|
|||
);
|
||||
|
||||
iterate_iron_offset_estimation(
|
||||
&m,
|
||||
&softIronMatrix,
|
||||
&m,
|
||||
&softIronMatrix,
|
||||
&hardIronOffset
|
||||
);
|
||||
|
||||
|
||||
if (device->calibration) {
|
||||
device->calibration->softIronMatrix = softIronMatrix;
|
||||
device->calibration->hardIronOffset = hardIronOffset;
|
||||
}
|
||||
|
||||
|
||||
m = FusionCalibrationMagnetic(
|
||||
m,
|
||||
softIronMatrix,
|
||||
|
|
@ -870,32 +870,32 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite
|
|||
device_imu_error("No calibration allocated");
|
||||
return DEVICE_IMU_ERROR_NO_ALLOCATION;
|
||||
}
|
||||
|
||||
|
||||
if (sizeof(device_imu_packet_type) > device->max_payload_size) {
|
||||
device_imu_error("Not proper size");
|
||||
return DEVICE_IMU_ERROR_WRONG_SIZE;
|
||||
}
|
||||
|
||||
|
||||
device_imu_packet_type packet;
|
||||
int transferred;
|
||||
|
||||
|
||||
bool initialized = false;
|
||||
|
||||
|
||||
FusionVector cal_gyroscope;
|
||||
FusionVector cal_accelerometer;
|
||||
|
||||
FusionMatrix softIronMatrix;
|
||||
FusionVector hardIronOffset;
|
||||
|
||||
|
||||
const float factor = iterations > 0? 1.0f / ((float) iterations) : 0.0f;
|
||||
|
||||
FusionVector prev_accel;
|
||||
while (iterations > 0) {
|
||||
memset(&packet, 0, sizeof(device_imu_packet_type));
|
||||
|
||||
|
||||
transferred = hid_read(
|
||||
device->handle,
|
||||
(uint8_t*) &packet,
|
||||
device->handle,
|
||||
(uint8_t*) &packet,
|
||||
sizeof(device_imu_packet_type)
|
||||
);
|
||||
|
||||
|
|
@ -912,17 +912,17 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite
|
|||
device_imu_error("Unexpected packet size");
|
||||
return DEVICE_IMU_ERROR_UNEXPECTED;
|
||||
}
|
||||
|
||||
|
||||
if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
FusionVector gyroscope;
|
||||
FusionVector accelerometer;
|
||||
FusionVector magnetometer;
|
||||
|
||||
|
||||
readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer);
|
||||
|
||||
|
||||
pre_biased_coordinate_system(&gyroscope);
|
||||
pre_biased_coordinate_system(&accelerometer);
|
||||
pre_biased_coordinate_system(&magnetometer);
|
||||
|
|
@ -938,14 +938,14 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite
|
|||
prev_accel = accelerometer;
|
||||
|
||||
iterate_iron_offset_estimation(
|
||||
&magnetometer,
|
||||
&softIronMatrix,
|
||||
&magnetometer,
|
||||
&softIronMatrix,
|
||||
&hardIronOffset
|
||||
);
|
||||
|
||||
|
||||
iterations--;
|
||||
}
|
||||
|
||||
|
||||
if (factor > 0.0f) {
|
||||
if (gyro) {
|
||||
device->calibration->gyroscopeOffset = FusionVectorAdd(
|
||||
|
|
@ -956,7 +956,7 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite
|
|||
)
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
if (accel) {
|
||||
device->calibration->accelerometerOffset = FusionVectorAdd(
|
||||
device->calibration->accelerometerOffset,
|
||||
|
|
@ -966,13 +966,13 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite
|
|||
)
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
if (magnet) {
|
||||
device->calibration->softIronMatrix = softIronMatrix;
|
||||
device->calibration->hardIronOffset = hardIronOffset;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return DEVICE_IMU_ERROR_NO_ERROR;
|
||||
}
|
||||
|
||||
|
|
@ -986,18 +986,18 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) {
|
|||
device_imu_error("No handle");
|
||||
return DEVICE_IMU_ERROR_NO_HANDLE;
|
||||
}
|
||||
|
||||
|
||||
if (sizeof(device_imu_packet_type) > device->max_payload_size) {
|
||||
device_imu_error("Not proper size");
|
||||
return DEVICE_IMU_ERROR_WRONG_SIZE;
|
||||
}
|
||||
|
||||
|
||||
device_imu_packet_type packet;
|
||||
memset(&packet, 0, sizeof(device_imu_packet_type));
|
||||
|
||||
|
||||
int transferred = hid_read_timeout(
|
||||
device->handle,
|
||||
(uint8_t*) &packet,
|
||||
device->handle,
|
||||
(uint8_t*) &packet,
|
||||
sizeof(device_imu_packet_type),
|
||||
timeout
|
||||
);
|
||||
|
|
@ -1006,49 +1006,49 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) {
|
|||
device_imu_error("Device may be unplugged");
|
||||
return DEVICE_IMU_ERROR_UNPLUGGED;
|
||||
}
|
||||
|
||||
|
||||
if (transferred == 0) {
|
||||
return DEVICE_IMU_ERROR_NO_ERROR;
|
||||
}
|
||||
|
||||
|
||||
if (sizeof(device_imu_packet_type) != transferred) {
|
||||
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)) {
|
||||
device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_INIT);
|
||||
return DEVICE_IMU_ERROR_NO_ERROR;
|
||||
}
|
||||
|
||||
|
||||
if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) {
|
||||
device_imu_error("Not matching signature");
|
||||
return DEVICE_IMU_ERROR_WRONG_SIGNATURE;
|
||||
}
|
||||
|
||||
|
||||
const uint64_t delta = timestamp - device->last_timestamp;
|
||||
const float deltaTime = (float) ((double) delta / 1e9);
|
||||
|
||||
|
||||
device->last_timestamp = timestamp;
|
||||
|
||||
|
||||
int16_t temperature = pack16bit_signed(packet.temperature);
|
||||
|
||||
|
||||
// According to the ICM-42688-P datasheet: (offset: 25 °C, sensitivity: 132.48 LSB/°C)
|
||||
device->temperature = ((float) temperature) / 132.48f + 25.0f;
|
||||
|
||||
|
||||
FusionVector gyroscope;
|
||||
FusionVector accelerometer;
|
||||
FusionVector magnetometer;
|
||||
|
||||
|
||||
readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer);
|
||||
apply_calibration(device, &gyroscope, &accelerometer, &magnetometer);
|
||||
|
||||
|
||||
if (device->offset) {
|
||||
gyroscope = FusionOffsetUpdate((FusionOffset*) device->offset, gyroscope);
|
||||
}
|
||||
|
||||
|
||||
#ifndef NDEBUG
|
||||
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);
|
||||
|
|
@ -1073,7 +1073,7 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) {
|
|||
return DEVICE_IMU_ERROR_INVALID_VALUE;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_UPDATE);
|
||||
return DEVICE_IMU_ERROR_NO_ERROR;
|
||||
}
|
||||
|
|
@ -1223,7 +1223,7 @@ device_imu_error_type device_imu_close(device_imu_type* device) {
|
|||
device_imu_error("No device");
|
||||
return DEVICE_IMU_ERROR_NO_DEVICE;
|
||||
}
|
||||
|
||||
|
||||
if (device->calibration) {
|
||||
if (device->calibration->cam.cameras) {
|
||||
for (uint32_t i = 0; i < device->calibration->cam.num_cameras; i++) {
|
||||
|
|
@ -1247,11 +1247,11 @@ device_imu_error_type device_imu_close(device_imu_type* device) {
|
|||
|
||||
free(device->calibration);
|
||||
}
|
||||
|
||||
|
||||
if (device->ahrs) {
|
||||
free(device->ahrs);
|
||||
}
|
||||
|
||||
|
||||
if (device->offset) {
|
||||
free(device->offset);
|
||||
}
|
||||
|
|
@ -1264,7 +1264,7 @@ device_imu_error_type device_imu_close(device_imu_type* device) {
|
|||
|
||||
hid_close(device->handle);
|
||||
}
|
||||
|
||||
|
||||
memset(device, 0, sizeof(device_imu_type));
|
||||
device_exit();
|
||||
|
||||
|
|
|
|||
7
ardriver/xreal/go_ffi.c
Normal file
7
ardriver/xreal/go_ffi.c
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
#include "device_imu.h"
|
||||
|
||||
extern void goIMUEventHandler(uint64_t, device_imu_event_type, device_imu_ahrs_type*);
|
||||
|
||||
void imuEventHandler(uint64_t timestamp, device_imu_event_type event, const device_imu_ahrs_type* ahrs) {
|
||||
goIMUEventHandler(timestamp, event, (device_imu_ahrs_type*)ahrs);
|
||||
}
|
||||
3
ardriver/xreal/go_ffi.h
Normal file
3
ardriver/xreal/go_ffi.h
Normal file
|
|
@ -0,0 +1,3 @@
|
|||
#include "device_imu.h"
|
||||
extern void goIMUEventHandler(uint64_t, device_imu_event_type, device_imu_ahrs_type*);
|
||||
void imuEventHandler(uint64_t timestamp, device_imu_event_type event, const device_imu_ahrs_type* ahrs);
|
||||
|
|
@ -3,10 +3,132 @@
|
|||
|
||||
package xreal
|
||||
|
||||
// #include "evdi_lib.h"
|
||||
// #include "go_ffi.h"
|
||||
// #cgo CFLAGS: -w
|
||||
// #cgo CFLAGS: -w -I./Fusion/
|
||||
// #cgo pkg-config: json-c libusb-1.0 hidapi-libusb
|
||||
// #include "go_ffi.h"
|
||||
// #include "device.h"
|
||||
// #include "device_imu.h"
|
||||
// #include "device_mcu.h"
|
||||
import "C"
|
||||
import (
|
||||
"fmt"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
var IsXrealEnabled = true
|
||||
"git.terah.dev/UnrealXR/unrealxr/ardriver/commons"
|
||||
)
|
||||
|
||||
var (
|
||||
IsXrealEnabled = true
|
||||
deviceEventHandlerMutex = sync.Mutex{}
|
||||
deviceEventListener *commons.AREventListener
|
||||
)
|
||||
|
||||
//export goIMUEventHandler
|
||||
func goIMUEventHandler(_ C.uint64_t, event_type C.device_imu_event_type, ahrs *C.struct_device_imu_ahrs_t) {
|
||||
if deviceEventListener == nil {
|
||||
return
|
||||
}
|
||||
|
||||
if event_type != C.DEVICE_IMU_EVENT_UPDATE {
|
||||
return
|
||||
}
|
||||
|
||||
orientation := C.device_imu_get_orientation(ahrs)
|
||||
euler := C.device_imu_get_euler(orientation)
|
||||
|
||||
deviceEventListener.PitchCallback(float32(euler.pitch))
|
||||
deviceEventListener.RollCallback(float32(euler.roll))
|
||||
deviceEventListener.YawCallback(float32(euler.yaw))
|
||||
}
|
||||
|
||||
// Implements commons.ARDevice
|
||||
type XrealDevice struct {
|
||||
eventListener *commons.AREventListener
|
||||
imuDevice *C.struct_device_imu_t
|
||||
deviceIsOpen bool
|
||||
}
|
||||
|
||||
func (device *XrealDevice) Initialize() error {
|
||||
if device.deviceIsOpen {
|
||||
return fmt.Errorf("device is already open")
|
||||
}
|
||||
|
||||
device.imuDevice = &C.struct_device_imu_t{}
|
||||
|
||||
// (*[0]byte) is a FUBAR way to cast a pointer to a function, but unsafe.Pointer doesn't work:
|
||||
// cannot use unsafe.Pointer(_Cgo_ptr(_Cfpvar_fp_imuEventHandler)) (value of type unsafe.Pointer) as *[0]byte value in variable declaration
|
||||
if C.DEVICE_IMU_ERROR_NO_ERROR != C.device_imu_open(device.imuDevice, (*[0]byte)(C.imuEventHandler)) {
|
||||
return fmt.Errorf("failed to open IMU device")
|
||||
}
|
||||
|
||||
C.device_imu_clear(device.imuDevice)
|
||||
C.device_imu_calibrate(device.imuDevice, 1000, true, true, false)
|
||||
|
||||
device.deviceIsOpen = true
|
||||
|
||||
// let's hope this doesn't cause race conditions
|
||||
go func() {
|
||||
for device.eventListener == nil {
|
||||
time.Sleep(time.Millisecond * 10)
|
||||
}
|
||||
|
||||
for {
|
||||
if !device.deviceIsOpen {
|
||||
break
|
||||
}
|
||||
|
||||
// I'm sorry.
|
||||
deviceEventHandlerMutex.Lock()
|
||||
deviceEventListener = device.eventListener
|
||||
status := C.device_imu_read(device.imuDevice, -1)
|
||||
deviceEventHandlerMutex.Unlock()
|
||||
|
||||
if status != C.DEVICE_IMU_ERROR_NO_ERROR {
|
||||
break
|
||||
}
|
||||
}
|
||||
|
||||
device.deviceIsOpen = false
|
||||
C.device_imu_close(device.imuDevice)
|
||||
}()
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (device *XrealDevice) End() error {
|
||||
if !device.deviceIsOpen {
|
||||
return fmt.Errorf("device is not open")
|
||||
}
|
||||
|
||||
C.device_imu_close(device.imuDevice)
|
||||
device.deviceIsOpen = false
|
||||
return nil
|
||||
}
|
||||
|
||||
func (device *XrealDevice) IsPollingLibrary() bool {
|
||||
return false
|
||||
}
|
||||
|
||||
func (device *XrealDevice) IsEventBasedLibrary() bool {
|
||||
return true
|
||||
}
|
||||
|
||||
func (device *XrealDevice) Poll() error {
|
||||
return fmt.Errorf("not supported")
|
||||
}
|
||||
|
||||
func (device *XrealDevice) RegisterEventListeners(listener *commons.AREventListener) {
|
||||
device.eventListener = listener
|
||||
}
|
||||
|
||||
func New() (*XrealDevice, error) {
|
||||
device := &XrealDevice{}
|
||||
err := device.Initialize()
|
||||
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
return device, nil
|
||||
}
|
||||
|
|
|
|||
7
ardriver/xreal/xreal_debug_logging.go
Normal file
7
ardriver/xreal/xreal_debug_logging.go
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
//go:build xreal && !xreal_debug_logging
|
||||
// +build xreal,!xreal_debug_logging
|
||||
|
||||
package xreal
|
||||
|
||||
// #cgo CFLAGS: -DNDEBUG
|
||||
import "C"
|
||||
|
|
@ -3,4 +3,42 @@
|
|||
|
||||
package xreal
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
|
||||
"git.terah.dev/UnrealXR/unrealxr/ardriver/commons"
|
||||
)
|
||||
|
||||
var IsXrealEnabled = false
|
||||
|
||||
// Implements commons.ARDevice
|
||||
type XrealDevice struct {
|
||||
}
|
||||
|
||||
func (device *XrealDevice) Initialize() error {
|
||||
return fmt.Errorf("xreal is not enabled")
|
||||
}
|
||||
|
||||
func (device *XrealDevice) End() error {
|
||||
return fmt.Errorf("xreal is not enabled")
|
||||
}
|
||||
|
||||
func (device *XrealDevice) IsPollingLibrary() bool {
|
||||
return false
|
||||
}
|
||||
|
||||
func (device *XrealDevice) IsEventBasedLibrary() bool {
|
||||
return false
|
||||
}
|
||||
|
||||
func (device *XrealDevice) Poll() error {
|
||||
return fmt.Errorf("xreal is not enabled")
|
||||
}
|
||||
|
||||
func (device *XrealDevice) RegisterEventListeners(*commons.AREventListener) error {
|
||||
return fmt.Errorf("xreal is not enabled")
|
||||
}
|
||||
|
||||
func New() (*XrealDevice, error) {
|
||||
return nil, fmt.Errorf("xreal is not enabled")
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue