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

Rename device3/4 to device_imu/mcu

Browse source
This commit is contained in:
wheaney 2024-07-30 10:03:08 -07:00
parent ce3f9f5493
commit a2c8e49856
9 changed files with 1181 additions and 1181 deletions
Download patch file Download diff file Expand all files Collapse all files

4
interface_lib/CMakeLists.txt
View file

@ -12,8 +12,8 @@ add_library(
xrealAirLibrary xrealAirLibrary
src/crc32.c src/crc32.c
src/device.c src/device.c
src/device3.c src/device_imu.c
src/device4.c src/device_mcu.c
src/hid_ids.c src/hid_ids.c
) )

182
interface_lib/include/device3.h
View file

@ -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 <stdbool.h>
#endif
#ifndef __cplusplus
#include <stdint.h>
#else
#include <cstdint>
#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

192
interface_lib/include/device4.h
View file

@ -1,192 +0,0 @@
#pragma once
//
// Created by thejackimonster on 29.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 <stdbool.h>
#endif
#ifndef __cplusplus
#include <stdint.h>
#else
#include <cstdint>
#endif
#define DEVICE4_MSG_R_BRIGHTNESS 0x03
#define DEVICE4_MSG_W_BRIGHTNESS 0x04
#define DEVICE4_MSG_R_DISP_MODE 0x07
#define DEVICE4_MSG_W_DISP_MODE 0x08
#define DEVICE4_MSG_R_GLASSID 0x15
#define DEVICE4_MSG_R_DP7911_FW_VERSION 0x16
#define DEVICE4_MSG_R_DSP_VERSION 0x18
#define DEVICE4_MSG_W_CANCEL_ACTIVATION 0x19
#define DEVICE4_MSG_P_HEARTBEAT 0x1A
#define DEVICE4_MSG_W_SLEEP_TIME 0x1E
#define DEVICE4_MSG_R_DSP_APP_FW_VERSION 0x21
#define DEVICE4_MSG_R_MCU_APP_FW_VERSION 0x26
#define DEVICE4_MSG_R_ACTIVATION_TIME 0x29
#define DEVICE4_MSG_W_ACTIVATION_TIME 0x2A
#define DEVICE4_MSG_R_DP7911_FW_IS_UPDATE 0x3C
#define DEVICE4_MSG_W_UPDATE_DP 0x3D
#define DEVICE4_MSG_W_UPDATE_MCU_APP_FW_PREPARE 0x3E
#define DEVICE4_MSG_W_UPDATE_MCU_APP_FW_START 0x3F
#define DEVICE4_MSG_W_UPDATE_MCU_APP_FW_TRANSMIT 0x40
#define DEVICE4_MSG_W_UPDATE_MCU_APP_FW_FINISH 0x41
#define DEVICE4_MSG_W_BOOT_JUMP_TO_APP 0x42
#define DEVICE4_MSG_W_MCU_APP_JUMP_TO_BOOT 0x44
#define DEVICE4_MSG_W_UPDATE_DSP_APP_FW_PREPARE 0x45
#define DEVICE4_MSG_W_UPDATE_DSP_APP_FW_START 0x46
#define DEVICE4_MSG_W_UPDATE_DSP_APP_FW_TRANSMIT 0x47
#define DEVICE4_MSG_W_UPDATE_DSP_APP_FW_FINISH 0x48
#define DEVICE4_MSG_R_IS_NEED_UPGRADE_DSP_FW 0x49
#define DEVICE4_MSG_W_FORCE_UPGRADE_DSP_FW 0x69
#define DEVICE4_MSG_W_BOOT_UPDATE_MODE 0x1100
#define DEVICE4_MSG_W_BOOT_UPDATE_CONFIRM 0x1101
#define DEVICE4_MSG_W_BOOT_UPDATE_PREPARE 0x1102
#define DEVICE4_MSG_W_BOOT_UPDATE_START 0x1103
#define DEVICE4_MSG_W_BOOT_UPDATE_TRANSMIT 0x1104
#define DEVICE4_MSG_W_BOOT_UPDATE_FINISH 0x1105
#define DEVICE4_MSG_P_START_HEARTBEAT 0x6c02
#define DEVICE4_MSG_P_BUTTON_PRESSED 0x6C05
#define DEVICE4_MSG_P_END_HEARTBEAT 0x6c12
#define DEVICE4_MSG_P_ASYNC_TEXT_LOG 0x6c09
#define DEVICE4_MSG_E_DSP_ONE_PACKGE_WRITE_FINISH 0x6C0E
#define DEVICE4_MSG_E_DSP_UPDATE_PROGRES 0x6C10
#define DEVICE4_MSG_E_DSP_UPDATE_ENDING 0x6C11
#define DEVICE4_BUTTON_PHYS_DISPLAY_TOGGLE 0x1
#define DEVICE4_BUTTON_PHYS_BRIGHTNESS_UP 0x2
#define DEVICE4_BUTTON_PHYS_BRIGHTNESS_DOWN 0x3
#define DEVICE4_DISPLAY_MODE_1920x1080_60 0x1
#define DEVICE4_DISPLAY_MODE_3840x1080_60_SBS 0x3
#define DEVICE4_DISPLAY_MODE_3840x1080_72_SBS 0x4
#define DEVICE4_DISPLAY_MODE_1920x1080_72 0x5
#define DEVICE4_DISPLAY_MODE_1920x1080_60_SBS 0x8
#define DEVICE4_DISPLAY_MODE_3840x1080_90_SBS 0x9
#define DEVICE4_DISPLAY_MODE_1920x1080_90 0xA
#define DEVICE4_DISPLAY_MODE_1920x1080_120 0xB
#define DEVICE4_BUTTON_VIRT_DISPLAY_TOGGLE 0x1
#define DEVICE4_BUTTON_VIRT_BRIGHTNESS_UP 0x6
#define DEVICE4_BUTTON_VIRT_BRIGHTNESS_DOWN 0x7
#define DEVICE4_BUTTON_VIRT_UP 0x8
#define DEVICE4_BUTTON_VIRT_DOWN 0x9
#ifdef __cplusplus
extern "C" {
#endif
enum device4_error_t {
DEVICE4_ERROR_NO_ERROR = 0,
DEVICE4_ERROR_NO_DEVICE = 1,
DEVICE4_ERROR_NO_HANDLE = 2,
DEVICE4_ERROR_NO_ACTIVATION = 3,
DEVICE4_ERROR_WRONG_SIZE = 4,
DEVICE4_ERROR_UNPLUGGED = 5,
DEVICE4_ERROR_UNEXPECTED = 6,
DEVICE4_ERROR_WRONG_HEAD = 7,
DEVICE4_ERROR_INVALID_LENGTH = 8,
DEVICE4_ERROR_NOT_INITIALIZED = 9,
DEVICE4_ERROR_PAYLOAD_FAILED = 10,
DEVICE4_ERROR_UNKNOWN = 11,
};
struct __attribute__((__packed__)) device4_packet_t {
uint8_t head;
uint32_t checksum;
uint16_t length;
uint64_t timestamp;
uint16_t msgid;
uint8_t reserved [5];
union {
char text [42];
uint8_t data [42];
};
};
enum device4_event_t {
DEVICE4_EVENT_UNKNOWN = 0,
DEVICE4_EVENT_SCREEN_ON = 1,
DEVICE4_EVENT_SCREEN_OFF = 2,
DEVICE4_EVENT_BRIGHTNESS_UP = 3,
DEVICE4_EVENT_BRIGHTNESS_DOWN = 4,
DEVICE4_EVENT_MESSAGE = 5,
};
typedef enum device4_error_t device4_error_type;
typedef struct device4_packet_t device4_packet_type;
typedef enum device4_event_t device4_event_type;
typedef void (*device4_event_callback)(
uint64_t timestamp,
device4_event_type event,
uint8_t brightness,
const char* msg
);
struct device4_t {
uint16_t vendor_id;
uint16_t product_id;
void* handle;
bool activated;
char mcu_app_fw_version [42];
char dp_fw_version [42];
char dsp_fw_version [42];
bool active;
uint8_t brightness;
uint8_t disp_mode;
device4_event_callback callback;
};
typedef struct device4_t device4_type;
device4_error_type device4_open(device4_type* device, device4_event_callback callback);
device4_error_type device4_clear(device4_type* device);
device4_error_type device4_read(device4_type* device, int timeout);
device4_error_type device4_poll_display_mode(device4_type* device);
device4_error_type device4_update_display_mode(device4_type* device);
device4_error_type device4_update_mcu_firmware(device4_type* device, const char* path);
device4_error_type device4_close(device4_type* device);
#ifdef __cplusplus
} // extern "C"
#endif

182
interface_lib/include/device_imu.h Normal file
View file

@ -0,0 +1,182 @@
#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 <stdbool.h>
#endif
#ifndef __cplusplus
#include <stdint.h>
#else
#include <cstdint>
#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

192
interface_lib/include/device_mcu.h Normal file
View file

@ -0,0 +1,192 @@
#pragma once
//
// Created by thejackimonster on 29.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 <stdbool.h>
#endif
#ifndef __cplusplus
#include <stdint.h>
#else
#include <cstdint>
#endif
#define DEVICE_MCU_MSG_R_BRIGHTNESS 0x03
#define DEVICE_MCU_MSG_W_BRIGHTNESS 0x04
#define DEVICE_MCU_MSG_R_DISP_MODE 0x07
#define DEVICE_MCU_MSG_W_DISP_MODE 0x08
#define DEVICE_MCU_MSG_R_GLASSID 0x15
#define DEVICE_MCU_MSG_R_DP7911_FW_VERSION 0x16
#define DEVICE_MCU_MSG_R_DSP_VERSION 0x18
#define DEVICE_MCU_MSG_W_CANCEL_ACTIVATION 0x19
#define DEVICE_MCU_MSG_P_HEARTBEAT 0x1A
#define DEVICE_MCU_MSG_W_SLEEP_TIME 0x1E
#define DEVICE_MCU_MSG_R_DSP_APP_FW_VERSION 0x21
#define DEVICE_MCU_MSG_R_MCU_APP_FW_VERSION 0x26
#define DEVICE_MCU_MSG_R_ACTIVATION_TIME 0x29
#define DEVICE_MCU_MSG_W_ACTIVATION_TIME 0x2A
#define DEVICE_MCU_MSG_R_DP7911_FW_IS_UPDATE 0x3C
#define DEVICE_MCU_MSG_W_UPDATE_DP 0x3D
#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_PREPARE 0x3E
#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_START 0x3F
#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_TRANSMIT 0x40
#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_FINISH 0x41
#define DEVICE_MCU_MSG_W_BOOT_JUMP_TO_APP 0x42
#define DEVICE_MCU_MSG_W_MCU_APP_JUMP_TO_BOOT 0x44
#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_PREPARE 0x45
#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_START 0x46
#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_TRANSMIT 0x47
#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_FINISH 0x48
#define DEVICE_MCU_MSG_R_IS_NEED_UPGRADE_DSP_FW 0x49
#define DEVICE_MCU_MSG_W_FORCE_UPGRADE_DSP_FW 0x69
#define DEVICE_MCU_MSG_W_BOOT_UPDATE_MODE 0x1100
#define DEVICE_MCU_MSG_W_BOOT_UPDATE_CONFIRM 0x1101
#define DEVICE_MCU_MSG_W_BOOT_UPDATE_PREPARE 0x1102
#define DEVICE_MCU_MSG_W_BOOT_UPDATE_START 0x1103
#define DEVICE_MCU_MSG_W_BOOT_UPDATE_TRANSMIT 0x1104
#define DEVICE_MCU_MSG_W_BOOT_UPDATE_FINISH 0x1105
#define DEVICE_MCU_MSG_P_START_HEARTBEAT 0x6c02
#define DEVICE_MCU_MSG_P_BUTTON_PRESSED 0x6C05
#define DEVICE_MCU_MSG_P_END_HEARTBEAT 0x6c12
#define DEVICE_MCU_MSG_P_ASYNC_TEXT_LOG 0x6c09
#define DEVICE_MCU_MSG_E_DSP_ONE_PACKGE_WRITE_FINISH 0x6C0E
#define DEVICE_MCU_MSG_E_DSP_UPDATE_PROGRES 0x6C10
#define DEVICE_MCU_MSG_E_DSP_UPDATE_ENDING 0x6C11
#define DEVICE_MCU_BUTTON_PHYS_DISPLAY_TOGGLE 0x1
#define DEVICE_MCU_BUTTON_PHYS_BRIGHTNESS_UP 0x2
#define DEVICE_MCU_BUTTON_PHYS_BRIGHTNESS_DOWN 0x3
#define DEVICE_MCU_DISPLAY_MODE_1920x1080_60 0x1
#define DEVICE_MCU_DISPLAY_MODE_3840x1080_60_SBS 0x3
#define DEVICE_MCU_DISPLAY_MODE_3840x1080_72_SBS 0x4
#define DEVICE_MCU_DISPLAY_MODE_1920x1080_72 0x5
#define DEVICE_MCU_DISPLAY_MODE_1920x1080_60_SBS 0x8
#define DEVICE_MCU_DISPLAY_MODE_3840x1080_90_SBS 0x9
#define DEVICE_MCU_DISPLAY_MODE_1920x1080_90 0xA
#define DEVICE_MCU_DISPLAY_MODE_1920x1080_120 0xB
#define DEVICE_MCU_BUTTON_VIRT_DISPLAY_TOGGLE 0x1
#define DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_UP 0x6
#define DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_DOWN 0x7
#define DEVICE_MCU_BUTTON_VIRT_UP 0x8
#define DEVICE_MCU_BUTTON_VIRT_DOWN 0x9
#ifdef __cplusplus
extern "C" {
#endif
enum device_mcu_error_t {
DEVICE_MCU_ERROR_NO_ERROR = 0,
DEVICE_MCU_ERROR_NO_DEVICE = 1,
DEVICE_MCU_ERROR_NO_HANDLE = 2,
DEVICE_MCU_ERROR_NO_ACTIVATION = 3,
DEVICE_MCU_ERROR_WRONG_SIZE = 4,
DEVICE_MCU_ERROR_UNPLUGGED = 5,
DEVICE_MCU_ERROR_UNEXPECTED = 6,
DEVICE_MCU_ERROR_WRONG_HEAD = 7,
DEVICE_MCU_ERROR_INVALID_LENGTH = 8,
DEVICE_MCU_ERROR_NOT_INITIALIZED = 9,
DEVICE_MCU_ERROR_PAYLOAD_FAILED = 10,
DEVICE_MCU_ERROR_UNKNOWN = 11,
};
struct __attribute__((__packed__)) device_mcu_packet_t {
uint8_t head;
uint32_t checksum;
uint16_t length;
uint64_t timestamp;
uint16_t msgid;
uint8_t reserved [5];
union {
char text [42];
uint8_t data [42];
};
};
enum device_mcu_event_t {
DEVICE_MCU_EVENT_UNKNOWN = 0,
DEVICE_MCU_EVENT_SCREEN_ON = 1,
DEVICE_MCU_EVENT_SCREEN_OFF = 2,
DEVICE_MCU_EVENT_BRIGHTNESS_UP = 3,
DEVICE_MCU_EVENT_BRIGHTNESS_DOWN = 4,
DEVICE_MCU_EVENT_MESSAGE = 5,
};
typedef enum device_mcu_error_t device_mcu_error_type;
typedef struct device_mcu_packet_t device_mcu_packet_type;
typedef enum device_mcu_event_t device_mcu_event_type;
typedef void (*device_mcu_event_callback)(
uint64_t timestamp,
device_mcu_event_type event,
uint8_t brightness,
const char* msg
);
struct device_mcu_t {
uint16_t vendor_id;
uint16_t product_id;
void* handle;
bool activated;
char mcu_app_fw_version [42];
char dp_fw_version [42];
char dsp_fw_version [42];
bool active;
uint8_t brightness;
uint8_t disp_mode;
device_mcu_event_callback callback;
};
typedef struct device_mcu_t device_mcu_type;
device_mcu_error_type device_mcu_open(device_mcu_type* device, device_mcu_event_callback callback);
device_mcu_error_type device_mcu_clear(device_mcu_type* device);
device_mcu_error_type device_mcu_read(device_mcu_type* device, int timeout);
device_mcu_error_type device_mcu_poll_display_mode(device_mcu_type* device);
device_mcu_error_type device_mcu_update_display_mode(device_mcu_type* device);
device_mcu_error_type device_mcu_update_mcu_firmware(device_mcu_type* device, const char* path);
device_mcu_error_type device_mcu_close(device_mcu_type* device);
#ifdef __cplusplus
} // extern "C"
#endif

645
interface_lib/src/device4.c
View file

@ -1,645 +0,0 @@
//
// Created by thejackimonster on 29.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.
//
#include "device4.h"
#include "device.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <hidapi/hidapi.h>
#include "crc32.h"
#include "hid_ids.h"
#ifndef NDEBUG
#define device4_error(msg) fprintf(stderr, "ERROR: %s\n", msg)
#else
#define device4_error(msg) (0)
#endif
#define MAX_PACKET_SIZE 64
#define PACKET_HEAD 0xFD
static bool send_payload(device4_type* device, uint8_t size, const uint8_t* payload) {
int payload_size = size;
if (payload_size > MAX_PACKET_SIZE) {
payload_size = MAX_PACKET_SIZE;
}
int transferred = hid_write(device->handle, payload, payload_size);
if (transferred != payload_size) {
device4_error("Sending payload failed");
return false;
}
return (transferred == size);
}
static bool recv_payload(device4_type* device, uint8_t size, uint8_t* payload) {
int payload_size = size;
if (payload_size > MAX_PACKET_SIZE) {
payload_size = MAX_PACKET_SIZE;
}
int transferred = hid_read(device->handle, payload, payload_size);
if (transferred >= payload_size) {
transferred = payload_size;
}
if (transferred == 0) {
return false;
}
if (transferred != payload_size) {
device4_error("Receiving payload failed");
return false;
}
return (transferred == size);
}
static bool send_payload_action(device4_type* device, uint16_t msgid, uint8_t len, const uint8_t* data) {
static device4_packet_type packet;
const uint16_t packet_len = 17 + len;
const uint16_t payload_len = 5 + packet_len;
packet.head = PACKET_HEAD;
packet.length = htole16(packet_len);
packet.timestamp = htole64(0);
packet.msgid = htole16(msgid);
memset(packet.reserved, 0, 5);
memcpy(packet.data, data, len);
packet.checksum = htole32(
crc32_checksum(
(const uint8_t*) (&packet.length),
packet.length
)
);
return send_payload(device, payload_len, (uint8_t*) (&packet));
}
static bool recv_payload_msg(device4_type* device, uint16_t msgid, uint8_t len, uint8_t* data) {
static device4_packet_type packet;
packet.head = 0;
packet.length = 0;
packet.msgid = 0;
const uint16_t packet_len = 18 + len;
const uint16_t payload_len = 5 + packet_len;
if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) {
return false;
}
if (packet.head != PACKET_HEAD) {
device4_error("Invalid payload received");
return false;
}
if (le16toh(packet.msgid) != msgid) {
device4_error("Unexpected payload received");
return false;
}
const uint8_t status = packet.data[0];
if (status != 0) {
device4_error("Payload status failed");
return false;
}
const uint16_t data_len = le16toh(packet.length) - 18;
if (len <= data_len) {
memcpy(data, packet.data + 1, len);
} else {
memcpy(data, packet.data + 1, data_len);
memset(data + data_len, 0, len - data_len);
}
return true;
}
static bool do_payload_action(device4_type* device, uint16_t msgid, uint8_t len, const uint8_t* data) {
if (!send_payload_action(device, msgid, len, data)) {
return false;
}
const uint16_t attempts_per_second = (device->active? 60 : 1);
uint16_t attempts = attempts_per_second * 5;
while (attempts > 0) {
if (recv_payload_msg(device, msgid, 0, NULL)) {
return true;
}
attempts--;
}
return false;
}
device4_error_type device4_open(device4_type* device, device4_event_callback callback) {
if (!device) {
device4_error("No device");
return DEVICE4_ERROR_NO_DEVICE;
}
memset(device, 0, sizeof(device4_type));
device->vendor_id = xreal_vendor_id;
device->product_id = 0;
device->callback = callback;
if (!device_init()) {
device4_error("Not initialized");
return DEVICE4_ERROR_NOT_INITIALIZED;
}
struct hid_device_info* info = hid_enumerate(
device->vendor_id,
device->product_id
);
struct hid_device_info* it = info;
while (it) {
if (is_xreal_product_id(it->product_id) && it->interface_number == 4) {
#ifndef NDEBUG
printf("Found device with product_id: 0x%x\n", it->product_id);
#endif
device->product_id = it->product_id;
device->handle = hid_open_path(it->path);
break;
}
it = it->next;
}
hid_free_enumeration(info);
if (!device->handle) {
device4_error("No handle");
return DEVICE4_ERROR_NO_HANDLE;
}
device4_clear(device);
if (!send_payload_action(device, DEVICE4_MSG_R_ACTIVATION_TIME, 0, NULL)) {
device4_error("Requesting activation time failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
uint8_t activated;
if (!recv_payload_msg(device, DEVICE4_MSG_R_ACTIVATION_TIME, 1, &activated)) {
device4_error("Receiving activation time failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
device->activated = (activated != 0);
if (!device->activated) {
device4_error("Device is not activated");
return DEVICE4_ERROR_NO_ACTIVATION;
}
if (!send_payload_action(device, DEVICE4_MSG_R_MCU_APP_FW_VERSION, 0, NULL)) {
device4_error("Requesting current MCU app firmware version");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE4_MSG_R_MCU_APP_FW_VERSION, 41, (uint8_t*) device->mcu_app_fw_version)) {
device4_error("Receiving current MCU app firmware version failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
if (!send_payload_action(device, DEVICE4_MSG_R_DP7911_FW_VERSION, 0, NULL)) {
device4_error("Requesting current DP firmware version");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE4_MSG_R_DP7911_FW_VERSION, 41, (uint8_t*) device->dp_fw_version)) {
device4_error("Receiving current DP firmware version failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
if (!send_payload_action(device, DEVICE4_MSG_R_DSP_APP_FW_VERSION, 0, NULL)) {
device4_error("Requesting current DSP app firmware version");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE4_MSG_R_DSP_APP_FW_VERSION, 41, (uint8_t*) device->dsp_fw_version)) {
device4_error("Receiving current DSP app firmware version failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
#ifndef NDEBUG
printf("MCU: %s\n", device->mcu_app_fw_version);
printf("DP: %s\n", device->dp_fw_version);
printf("DSP: %s\n", device->dsp_fw_version);
#endif
if (!send_payload_action(device, DEVICE4_MSG_R_BRIGHTNESS, 0, NULL)) {
device4_error("Requesting initial brightness failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE4_MSG_R_BRIGHTNESS, 1, &device->brightness)) {
device4_error("Receiving initial brightness failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
if (!send_payload_action(device, DEVICE4_MSG_R_DISP_MODE, 0, NULL)) {
device4_error("Requesting display mode failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE4_MSG_R_DISP_MODE, 1, &device->disp_mode)) {
device4_error("Receiving display mode failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
#ifndef NDEBUG
printf("Brightness: %d\n", device->brightness);
printf("Disp-Mode: %d\n", device->disp_mode);
#endif
return DEVICE4_ERROR_NO_ERROR;
}
static void device4_callback(device4_type* device,
uint64_t timestamp,
device4_event_type event,
uint8_t brightness,
const char* msg) {
if (!device->callback) {
return;
}
device->callback(timestamp, event, brightness, msg);
}
device4_error_type device4_clear(device4_type* device) {
return device4_read(device, 10);
}
device4_error_type device4_read(device4_type* device, int timeout) {
if (!device) {
device4_error("No device");
return DEVICE4_ERROR_NO_DEVICE;
}
if (!device->handle) {
device4_error("No handle");
return DEVICE4_ERROR_NO_HANDLE;
}
if (MAX_PACKET_SIZE != sizeof(device4_packet_type)) {
device4_error("Not proper size");
return DEVICE4_ERROR_WRONG_SIZE;
}
device4_packet_type packet;
memset(&packet, 0, sizeof(device4_packet_type));
int transferred = hid_read_timeout(
device->handle,
(uint8_t*) &packet,
MAX_PACKET_SIZE,
timeout
);
if (transferred == -1) {
device4_error("Device may be unplugged");
return DEVICE4_ERROR_UNPLUGGED;
}
if (transferred == 0) {
return DEVICE4_ERROR_NO_ERROR;
}
if (MAX_PACKET_SIZE != transferred) {
device4_error("Unexpected packet size");
return DEVICE4_ERROR_UNEXPECTED;
}
if (packet.head != PACKET_HEAD) {
device4_error("Wrong packet head");
return DEVICE4_ERROR_WRONG_HEAD;
}
const uint32_t timestamp = le32toh(packet.timestamp);
const uint16_t msgid = le16toh(packet.msgid);
const uint16_t length = le16toh(packet.length);
const size_t data_len = (size_t) &(packet.data) - (size_t) &(packet.length);
#ifndef NDEBUG
printf("MSG: %d = %04x (%d)\n", msgid, msgid, length);
if (length > 11) {
for (int i = 0; i < length - 11; i++) {
printf("%02x ", packet.data[i]);
}
printf("\n");
}
#endif
switch (msgid) {
case DEVICE4_MSG_P_START_HEARTBEAT: {
break;
}
case DEVICE4_MSG_P_BUTTON_PRESSED: {
const uint8_t phys_button = packet.data[0];
const uint8_t virt_button = packet.data[4];
const uint8_t value = packet.data[8];
switch (virt_button) {
case DEVICE4_BUTTON_VIRT_DISPLAY_TOGGLE:
device->active = value;
if (device->active) {
device4_callback(
device,
timestamp,
DEVICE4_EVENT_SCREEN_ON,
device->brightness,
NULL
);
} else {
device4_callback(
device,
timestamp,
DEVICE4_EVENT_SCREEN_OFF,
device->brightness,
NULL
);
}
break;
case DEVICE4_BUTTON_VIRT_BRIGHTNESS_UP:
device->brightness = value;
device4_callback(
device,
timestamp,
DEVICE4_EVENT_BRIGHTNESS_UP,
device->brightness,
NULL
);
break;
case DEVICE4_BUTTON_VIRT_BRIGHTNESS_DOWN:
device->brightness = value;
device4_callback(
device,
timestamp,
DEVICE4_EVENT_BRIGHTNESS_DOWN,
device->brightness,
NULL
);
break;
default:
break;
}
break;
}
case DEVICE4_MSG_P_ASYNC_TEXT_LOG: {
const char* text = packet.text;
const size_t text_len = strlen(text);
device->active = true;
if (data_len + text_len != length) {
device4_error("Not matching length");
return DEVICE4_ERROR_INVALID_LENGTH;
}
device4_callback(
device,
timestamp,
DEVICE4_EVENT_MESSAGE,
device->brightness,
text
);
break;
}
case DEVICE4_MSG_P_END_HEARTBEAT: {
break;
}
default:
device4_callback(
device,
timestamp,
DEVICE4_EVENT_UNKNOWN,
device->brightness,
NULL
);
break;
}
return DEVICE4_ERROR_NO_ERROR;
}
device4_error_type device4_poll_display_mode(device4_type* device) {
if (!device) {
device4_error("No device");
return DEVICE4_ERROR_NO_DEVICE;
}
if (!device->handle) {
device4_error("No handle");
return DEVICE4_ERROR_NO_HANDLE;
}
if (!send_payload_action(device, DEVICE4_MSG_R_DISP_MODE, 0, NULL)) {
device4_error("Requesting display mode failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE4_MSG_R_DISP_MODE, 1, &device->disp_mode)) {
device4_error("Receiving display mode failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
return DEVICE4_ERROR_NO_ERROR;
}
device4_error_type device4_update_display_mode(device4_type* device) {
if (!device) {
device4_error("No device");
return DEVICE4_ERROR_NO_DEVICE;
}
if (!device->handle) {
device4_error("No handle");
return DEVICE4_ERROR_NO_HANDLE;
}
if (!do_payload_action(device, DEVICE4_MSG_W_DISP_MODE, 1, &device->disp_mode)) {
device4_error("Sending display mode failed");
return DEVICE4_ERROR_PAYLOAD_FAILED;
}
return DEVICE4_ERROR_NO_ERROR;
}
device4_error_type device4_update_mcu_firmware(device4_type* device, const char* path) {
if (!device) {
device4_error("No device");
return DEVICE4_ERROR_NO_DEVICE;
}
if (!device->handle) {
device4_error("No handle");
return DEVICE4_ERROR_NO_HANDLE;
}
if (!device->activated) {
device4_error("Device is not activated");
return DEVICE4_ERROR_NO_ACTIVATION;
}
size_t firmware_len = 0;
uint8_t* firmware = NULL;
FILE* file = fopen(path, "rb");
bool result = DEVICE4_ERROR_UNKNOWN;
if (file) {
fseek(file, 0, SEEK_END);
firmware_len = ftell(file);
if (firmware_len > 0) {
firmware = (uint8_t*) malloc(firmware_len);
fread(firmware, sizeof(uint8_t), firmware_len, file);
}
fclose(file);
}
device4_clear(device);
printf("Prepare upload: %lu\n", firmware_len);
if (!do_payload_action(device, DEVICE4_MSG_W_UPDATE_MCU_APP_FW_PREPARE, 0, NULL)) {
device4_error("Failed preparing the device for MCU firmware update!\n");
goto cleanup;
}
if (!do_payload_action(device, DEVICE4_MSG_W_MCU_APP_JUMP_TO_BOOT, 0, NULL)) {
device4_error("Failed mcu app jumping to boot");
goto cleanup;
}
if (!send_payload_action(device, DEVICE4_MSG_R_ACTIVATION_TIME, 0, NULL)) {
device4_error("Requesting activation time failed");
goto cleanup;
}
uint8_t activated;
if (!recv_payload_msg(device, DEVICE4_MSG_R_ACTIVATION_TIME, 1, &activated)) {
device4_error("Receiving activation time failed");
goto cleanup;
}
if (!activated) {
device4_error("Device is not activated");
goto jump_to_app;
}
size_t offset = 0;
while (offset < firmware_len) {
const size_t remaining = firmware_len - offset;
printf("Upload: %lu / %lu\n", offset, firmware_len);
uint8_t len;
uint16_t msgid;
if (offset == 0) {
len = remaining > 24? 24 : remaining;
msgid = DEVICE4_MSG_W_UPDATE_MCU_APP_FW_START;
} else {
len = remaining > 42? 42 : remaining;
msgid = DEVICE4_MSG_W_UPDATE_MCU_APP_FW_TRANSMIT;
}
if (!do_payload_action(device, msgid, len, firmware + offset)) {
device4_error("Failed sending firmware upload");
goto jump_to_app;
}
offset += len;
}
printf("Finish upload");
if (!do_payload_action(device, DEVICE4_MSG_W_UPDATE_MCU_APP_FW_FINISH, 0, NULL)) {
device4_error("Failed finishing firmware upload");
goto jump_to_app;
}
result = DEVICE4_ERROR_NO_ERROR;
jump_to_app:
if (!do_payload_action(device, DEVICE4_MSG_W_BOOT_JUMP_TO_APP, 0, NULL)) {
device4_error("Failed boot jumping back to app");
goto cleanup;
}
cleanup:
if (firmware) {
free(firmware);
}
return result;
}
device4_error_type device4_close(device4_type* device) {
if (!device) {
device4_error("No device");
return DEVICE4_ERROR_NO_DEVICE;
}
if (device->handle) {
hid_close(device->handle);
}
memset(device, 0, sizeof(device4_type));
device_exit();
return DEVICE4_ERROR_NO_ERROR;
}

272
interface_lib/src/device3.c → interface_lib/src/device_imu.c
View file

@ -22,7 +22,7 @@
// THE SOFTWARE. // THE SOFTWARE.
// //
#include "device3.h" #include "device_imu.h"
#include "device.h" #include "device.h"
#include <Fusion/FusionAxes.h> #include <Fusion/FusionAxes.h>
@ -43,12 +43,12 @@
#define GRAVITY_G (9.806f) #define GRAVITY_G (9.806f)
#ifndef NDEBUG #ifndef NDEBUG
#define device3_error(msg) fprintf(stderr, "ERROR: %s\n", msg) #define device_imu_error(msg) fprintf(stderr, "ERROR: %s\n", msg)
#else #else
#define device3_error(msg) (0) #define device_imu_error(msg) (0)
#endif #endif
struct device3_calibration_t { struct device_imu_calibration_t {
FusionMatrix gyroscopeMisalignment; FusionMatrix gyroscopeMisalignment;
FusionVector gyroscopeSensitivity; FusionVector gyroscopeSensitivity;
FusionVector gyroscopeOffset; FusionVector gyroscopeOffset;
@ -69,7 +69,7 @@ struct device3_calibration_t {
#define MAX_PACKET_SIZE 64 #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; int payload_size = size;
if (payload_size > MAX_PACKET_SIZE) { if (payload_size > MAX_PACKET_SIZE) {
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); int transferred = hid_write(device->handle, payload, payload_size);
if (transferred != payload_size) { if (transferred != payload_size) {
device3_error("Sending payload failed"); device_imu_error("Sending payload failed");
return false; return false;
} }
return (transferred == size); 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; int payload_size = size;
if (payload_size > MAX_PACKET_SIZE) { if (payload_size > MAX_PACKET_SIZE) {
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) { if (transferred != payload_size) {
device3_error("Receiving payload failed"); device_imu_error("Receiving payload failed");
return false; return false;
} }
return (transferred == size); return (transferred == size);
} }
struct __attribute__((__packed__)) device3_payload_packet_t { struct __attribute__((__packed__)) device_imu_payload_packet_t {
uint8_t head; uint8_t head;
uint32_t checksum; uint32_t checksum;
uint16_t length; uint16_t length;
@ -117,10 +117,10 @@ struct __attribute__((__packed__)) device3_payload_packet_t {
uint8_t data [56]; 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 bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint8_t len, const uint8_t* data) {
static device3_payload_packet_type packet; static device_imu_payload_packet_type packet;
const uint16_t packet_len = 3 + len; const uint16_t packet_len = 3 + len;
const uint16_t payload_len = 5 + packet_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)); 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); 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 bool recv_payload_msg(device_imu_type* device, uint8_t msgid, uint8_t len, uint8_t* data) {
static device3_payload_packet_type packet; static device_imu_payload_packet_type packet;
packet.head = 0; packet.head = 0;
packet.length = 0; packet.length = 0;
@ -193,20 +193,20 @@ static FusionQuaternion json_object_get_quaternion(struct json_object* obj) {
return quaternion; 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) { if (!device) {
device3_error("No device"); device_imu_error("No device");
return DEVICE3_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->vendor_id = xreal_vendor_id;
device->product_id = 0; device->product_id = 0;
device->callback = callback; device->callback = callback;
if (!device_init()) { if (!device_init()) {
device3_error("Not initialized"); device_imu_error("Not initialized");
return DEVICE3_ERROR_NOT_INITIALIZED; return DEVICE_IMU_ERROR_NOT_INITIALIZED;
} }
struct hid_device_info* info = hid_enumerate( 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); hid_free_enumeration(info);
if (!device->handle) { if (!device->handle) {
device3_error("No handle"); device_imu_error("No handle");
return DEVICE3_ERROR_NO_HANDLE; return DEVICE_IMU_ERROR_NO_HANDLE;
} }
if (!send_payload_msg_signal(device, DEVICE3_MSG_START_IMU_DATA, 0x0)) { if (!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x0)) {
device3_error("Failed sending payload to stop imu data stream"); device_imu_error("Failed sending payload to stop imu data stream");
return DEVICE3_ERROR_PAYLOAD_FAILED; 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)) { if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 0, NULL)) {
device3_error("Failed sending payload to get static id"); device_imu_error("Failed sending payload to get static id");
return DEVICE3_ERROR_PAYLOAD_FAILED; return DEVICE_IMU_ERROR_PAYLOAD_FAILED;
} }
uint32_t static_id = 0; 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; device->static_id = static_id;
} else { } else {
device->static_id = 0x20220101; device->static_id = 0x20220101;
} }
device->calibration = malloc(sizeof(device3_calibration_type)); device->calibration = malloc(sizeof(device_imu_calibration_type));
device3_reset_calibration(device); device_imu_reset_calibration(device);
if (!send_payload_msg(device, DEVICE3_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) { if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) {
device3_error("Failed sending payload to get calibration data length"); device_imu_error("Failed sending payload to get calibration data length");
return DEVICE3_ERROR_PAYLOAD_FAILED; return DEVICE_IMU_ERROR_PAYLOAD_FAILED;
} }
uint32_t calibration_len = 0; 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); char* calibration_data = malloc(calibration_len + 1);
uint32_t position = 0; uint32_t position = 0;
while (position < calibration_len) { while (position < calibration_len) {
const uint32_t remaining = (calibration_len - position); 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; break;
} }
const uint8_t next = (remaining > 56? 56 : remaining); 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; break;
} }
@ -320,9 +320,9 @@ device3_error_type device3_open(device3_type* device, device3_event_callback cal
free(calibration_data); free(calibration_data);
} }
if (!send_payload_msg_signal(device, DEVICE3_MSG_START_IMU_DATA, 0x1)) { if (!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x1)) {
device3_error("Failed sending payload to start imu data stream"); device_imu_error("Failed sending payload to start imu data stream");
return DEVICE3_ERROR_PAYLOAD_FAILED; return DEVICE_IMU_ERROR_PAYLOAD_FAILED;
} }
const uint32_t SAMPLE_RATE = 1000; 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); 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) { if (!device) {
device3_error("No device"); device_imu_error("No device");
return DEVICE3_ERROR_NO_DEVICE; return DEVICE_IMU_ERROR_NO_DEVICE;
} }
if (!device->calibration) { if (!device->calibration) {
device3_error("Not allocated"); device_imu_error("Not allocated");
return DEVICE3_ERROR_NO_ALLOCATION; return DEVICE_IMU_ERROR_NO_ALLOCATION;
} }
device->calibration->gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; 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; 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) { if (!device) {
device3_error("No device"); device_imu_error("No device");
return DEVICE3_ERROR_NO_DEVICE; return DEVICE_IMU_ERROR_NO_DEVICE;
} }
if (!device->calibration) { if (!device->calibration) {
device3_error("Not allocated"); device_imu_error("Not allocated");
return DEVICE3_ERROR_NO_ALLOCATION; return DEVICE_IMU_ERROR_NO_ALLOCATION;
} }
FILE* file = fopen(path, "rb"); FILE* file = fopen(path, "rb");
if (!file) { if (!file) {
device3_error("No file opened"); device_imu_error("No file opened");
return DEVICE3_ERROR_FILE_NOT_OPEN; 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; 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) { if (sizeof(device_imu_calibration_type) != count) {
device3_error("Not fully loaded"); device_imu_error("Not fully loaded");
result = DEVICE3_ERROR_LOADING_FAILED; result = DEVICE_IMU_ERROR_LOADING_FAILED;
} }
if (0 != fclose(file)) { if (0 != fclose(file)) {
device3_error("No file closed"); device_imu_error("No file closed");
return DEVICE3_ERROR_FILE_NOT_CLOSED; return DEVICE_IMU_ERROR_FILE_NOT_CLOSED;
} }
return result; 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) { if (!device) {
device3_error("No device"); device_imu_error("No device");
return DEVICE3_ERROR_NO_DEVICE; return DEVICE_IMU_ERROR_NO_DEVICE;
} }
if (!device->calibration) { if (!device->calibration) {
device3_error("Not allocated"); device_imu_error("Not allocated");
return DEVICE3_ERROR_NO_ALLOCATION; return DEVICE_IMU_ERROR_NO_ALLOCATION;
} }
FILE* file = fopen(path, "wb"); FILE* file = fopen(path, "wb");
if (!file) { if (!file) {
device3_error("No file opened"); device_imu_error("No file opened");
return DEVICE3_ERROR_FILE_NOT_OPEN; 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; 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) { if (sizeof(device_imu_calibration_type) != count) {
device3_error("Not fully saved"); device_imu_error("Not fully saved");
result = DEVICE3_ERROR_SAVING_FAILED; result = DEVICE_IMU_ERROR_SAVING_FAILED;
} }
if (0 != fclose(file)) { if (0 != fclose(file)) {
device3_error("No file closed"); device_imu_error("No file closed");
return DEVICE3_ERROR_FILE_NOT_CLOSED; return DEVICE_IMU_ERROR_FILE_NOT_CLOSED;
} }
return result; return result;
} }
static void device3_callback(device3_type* device, static void device_imu_callback(device_imu_type* device,
uint64_t timestamp, uint64_t timestamp,
device3_event_type event) { device_imu_event_type event) {
if (!device->callback) { if (!device->callback) {
return; return;
} }
@ -489,7 +489,7 @@ static int16_t pack16bit_signed_bizarre(const uint8_t* data) {
return (int16_t) unsigned_value; 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* gyroscope,
FusionVector* accelerometer, FusionVector* accelerometer,
FusionVector* magnetometer) { FusionVector* magnetometer) {
@ -566,7 +566,7 @@ static void iterate_iron_offset_estimation(const FusionVector* magnetometer, Fus
hardIronOffset->axis.z = cz; hardIronOffset->axis.z = cz;
} }
static void apply_calibration(const device3_type* device, static void apply_calibration(const device_imu_type* device,
FusionVector* gyroscope, FusionVector* gyroscope,
FusionVector* accelerometer, FusionVector* accelerometer,
FusionVector* magnetometer) { FusionVector* magnetometer) {
@ -678,32 +678,32 @@ static void apply_calibration(const device3_type* device,
post_biased_coordinate_system(&m, magnetometer); post_biased_coordinate_system(&m, magnetometer);
} }
device3_error_type device3_clear(device3_type* device) { device_imu_error_type device_imu_clear(device_imu_type* device) {
return device3_read(device, 10); 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) { if (!device) {
device3_error("No device"); device_imu_error("No device");
return DEVICE3_ERROR_NO_DEVICE; return DEVICE_IMU_ERROR_NO_DEVICE;
} }
if (!device->handle) { if (!device->handle) {
device3_error("No handle"); device_imu_error("No handle");
return DEVICE3_ERROR_NO_HANDLE; return DEVICE_IMU_ERROR_NO_HANDLE;
} }
if (!device->calibration) { if (!device->calibration) {
device3_error("No calibration allocated"); device_imu_error("No calibration allocated");
return DEVICE3_ERROR_NO_ALLOCATION; return DEVICE_IMU_ERROR_NO_ALLOCATION;
} }
if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) { if (MAX_PACKET_SIZE != sizeof(device_imu_packet_type)) {
device3_error("Not proper size"); device_imu_error("Not proper size");
return DEVICE3_ERROR_WRONG_SIZE; return DEVICE_IMU_ERROR_WRONG_SIZE;
} }
device3_packet_type packet; device_imu_packet_type packet;
int transferred; int transferred;
bool initialized = false; bool initialized = false;
@ -718,7 +718,7 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations,
FusionVector prev_accel; FusionVector prev_accel;
while (iterations > 0) { while (iterations > 0) {
memset(&packet, 0, sizeof(device3_packet_type)); memset(&packet, 0, sizeof(device_imu_packet_type));
transferred = hid_read( transferred = hid_read(
device->handle, device->handle,
@ -727,8 +727,8 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations,
); );
if (transferred == -1) { if (transferred == -1) {
device3_error("Device may be unplugged"); device_imu_error("Device may be unplugged");
return DEVICE3_ERROR_UNPLUGGED; return DEVICE_IMU_ERROR_UNPLUGGED;
} }
if (transferred == 0) { if (transferred == 0) {
@ -736,8 +736,8 @@ device3_error_type device3_calibrate(device3_type* device, uint32_t iterations,
} }
if (MAX_PACKET_SIZE != transferred) { if (MAX_PACKET_SIZE != transferred) {
device3_error("Unexpected packet size"); device_imu_error("Unexpected packet size");
return DEVICE3_ERROR_UNEXPECTED; return DEVICE_IMU_ERROR_UNEXPECTED;
} }
if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { 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) { if (!device) {
device3_error("No device"); device_imu_error("No device");
return DEVICE3_ERROR_NO_DEVICE; return DEVICE_IMU_ERROR_NO_DEVICE;
} }
if (!device->handle) { if (!device->handle) {
device3_error("No handle"); device_imu_error("No handle");
return DEVICE3_ERROR_NO_HANDLE; return DEVICE_IMU_ERROR_NO_HANDLE;
} }
if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) { if (MAX_PACKET_SIZE != sizeof(device_imu_packet_type)) {
device3_error("Not proper size"); device_imu_error("Not proper size");
return DEVICE3_ERROR_WRONG_SIZE; return DEVICE_IMU_ERROR_WRONG_SIZE;
} }
device3_packet_type packet; device_imu_packet_type packet;
memset(&packet, 0, sizeof(device3_packet_type)); memset(&packet, 0, sizeof(device_imu_packet_type));
int transferred = hid_read_timeout( int transferred = hid_read_timeout(
device->handle, device->handle,
@ -830,29 +830,29 @@ device3_error_type device3_read(device3_type* device, int timeout) {
); );
if (transferred == -1) { if (transferred == -1) {
device3_error("Device may be unplugged"); device_imu_error("Device may be unplugged");
return DEVICE3_ERROR_UNPLUGGED; return DEVICE_IMU_ERROR_UNPLUGGED;
} }
if (transferred == 0) { if (transferred == 0) {
return DEVICE3_ERROR_NO_ERROR; return DEVICE_IMU_ERROR_NO_ERROR;
} }
if (MAX_PACKET_SIZE != transferred) { if (MAX_PACKET_SIZE != transferred) {
device3_error("Unexpected packet size"); device_imu_error("Unexpected packet size");
return DEVICE3_ERROR_UNEXPECTED; return DEVICE_IMU_ERROR_UNEXPECTED;
} }
const uint64_t timestamp = le64toh(packet.timestamp); const uint64_t timestamp = le64toh(packet.timestamp);
if ((packet.signature[0] == 0xaa) && (packet.signature[1] == 0x53)) { if ((packet.signature[0] == 0xaa) && (packet.signature[1] == 0x53)) {
device3_callback(device, timestamp, DEVICE3_EVENT_INIT); device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_INIT);
return DEVICE3_ERROR_NO_ERROR; return DEVICE_IMU_ERROR_NO_ERROR;
} }
if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) {
device3_error("Not matching signature"); device_imu_error("Not matching signature");
return DEVICE3_ERROR_WRONG_SIGNATURE; return DEVICE_IMU_ERROR_WRONG_SIGNATURE;
} }
const uint64_t delta = timestamp - device->last_timestamp; 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); 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 // 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 // the gyro/accel/magnet readings
if (isnan(orientation.x) || isnan(orientation.y) || isnan(orientation.z) || isnan(orientation.w)) { if (isnan(orientation.x) || isnan(orientation.y) || isnan(orientation.z) || isnan(orientation.w)) {
device3_error("Invalid orientation reading"); device_imu_error("Invalid orientation reading");
return DEVICE3_ERROR_INVALID_VALUE; return DEVICE_IMU_ERROR_INVALID_VALUE;
} }
} }
device3_callback(device, timestamp, DEVICE3_EVENT_UPDATE); device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_UPDATE);
return DEVICE3_ERROR_NO_ERROR; 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; 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.x = acceleration.axis.x;
a.y = acceleration.axis.y; a.y = acceleration.axis.y;
a.z = acceleration.axis.z; a.z = acceleration.axis.z;
return a; 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; 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.x = acceleration.axis.x;
a.y = acceleration.axis.y; a.y = acceleration.axis.y;
a.z = acceleration.axis.z; a.z = acceleration.axis.z;
return a; 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; 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.x = quaternion.element.x;
q.y = quaternion.element.y; q.y = quaternion.element.y;
q.z = quaternion.element.z; q.z = quaternion.element.z;
@ -933,24 +933,24 @@ device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs) {
return q; 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; FusionQuaternion quaternion;
quaternion.element.x = quat.x; quaternion.element.x = quat.x;
quaternion.element.y = quat.y; quaternion.element.y = quat.y;
quaternion.element.z = quat.z; quaternion.element.z = quat.z;
quaternion.element.w = quat.w; quaternion.element.w = quat.w;
FusionEuler euler = FusionQuaternionToEuler(quaternion); FusionEuler euler = FusionQuaternionToEuler(quaternion);
device3_euler_type e; device_imu_euler_type e;
e.roll = euler.angle.roll; e.roll = euler.angle.roll;
e.pitch = euler.angle.pitch; e.pitch = euler.angle.pitch;
e.yaw = euler.angle.yaw; e.yaw = euler.angle.yaw;
return e; return e;
} }
device3_error_type device3_close(device3_type* device) { device_imu_error_type device_imu_close(device_imu_type* device) {
if (!device) { if (!device) {
device3_error("No device"); device_imu_error("No device");
return DEVICE3_ERROR_NO_DEVICE; return DEVICE_IMU_ERROR_NO_DEVICE;
} }
if (device->calibration) { if (device->calibration) {
@ -969,8 +969,8 @@ device3_error_type device3_close(device3_type* device) {
hid_close(device->handle); hid_close(device->handle);
} }
memset(device, 0, sizeof(device3_type)); memset(device, 0, sizeof(device_imu_type));
device_exit(); device_exit();
return DEVICE3_ERROR_NO_ERROR; return DEVICE_IMU_ERROR_NO_ERROR;
} }

645
interface_lib/src/device_mcu.c Normal file
View file

@ -0,0 +1,645 @@
//
// Created by thejackimonster on 29.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.
//
#include "device_mcu.h"
#include "device.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <hidapi/hidapi.h>
#include "crc32.h"
#include "hid_ids.h"
#ifndef NDEBUG
#define device_mcu_error(msg) fprintf(stderr, "ERROR: %s\n", msg)
#else
#define device_mcu_error(msg) (0)
#endif
#define MAX_PACKET_SIZE 64
#define PACKET_HEAD 0xFD
static bool send_payload(device_mcu_type* device, uint8_t size, const uint8_t* payload) {
int payload_size = size;
if (payload_size > MAX_PACKET_SIZE) {
payload_size = MAX_PACKET_SIZE;
}
int transferred = hid_write(device->handle, payload, payload_size);
if (transferred != payload_size) {
device_mcu_error("Sending payload failed");
return false;
}
return (transferred == size);
}
static bool recv_payload(device_mcu_type* device, uint8_t size, uint8_t* payload) {
int payload_size = size;
if (payload_size > MAX_PACKET_SIZE) {
payload_size = MAX_PACKET_SIZE;
}
int transferred = hid_read(device->handle, payload, payload_size);
if (transferred >= payload_size) {
transferred = payload_size;
}
if (transferred == 0) {
return false;
}
if (transferred != payload_size) {
device_mcu_error("Receiving payload failed");
return false;
}
return (transferred == size);
}
static bool send_payload_action(device_mcu_type* device, uint16_t msgid, uint8_t len, const uint8_t* data) {
static device_mcu_packet_type packet;
const uint16_t packet_len = 17 + len;
const uint16_t payload_len = 5 + packet_len;
packet.head = PACKET_HEAD;
packet.length = htole16(packet_len);
packet.timestamp = htole64(0);
packet.msgid = htole16(msgid);
memset(packet.reserved, 0, 5);
memcpy(packet.data, data, len);
packet.checksum = htole32(
crc32_checksum(
(const uint8_t*) (&packet.length),
packet.length
)
);
return send_payload(device, payload_len, (uint8_t*) (&packet));
}
static bool recv_payload_msg(device_mcu_type* device, uint16_t msgid, uint8_t len, uint8_t* data) {
static device_mcu_packet_type packet;
packet.head = 0;
packet.length = 0;
packet.msgid = 0;
const uint16_t packet_len = 18 + len;
const uint16_t payload_len = 5 + packet_len;
if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) {
return false;
}
if (packet.head != PACKET_HEAD) {
device_mcu_error("Invalid payload received");
return false;
}
if (le16toh(packet.msgid) != msgid) {
device_mcu_error("Unexpected payload received");
return false;
}
const uint8_t status = packet.data[0];
if (status != 0) {
device_mcu_error("Payload status failed");
return false;
}
const uint16_t data_len = le16toh(packet.length) - 18;
if (len <= data_len) {
memcpy(data, packet.data + 1, len);
} else {
memcpy(data, packet.data + 1, data_len);
memset(data + data_len, 0, len - data_len);
}
return true;
}
static bool do_payload_action(device_mcu_type* device, uint16_t msgid, uint8_t len, const uint8_t* data) {
if (!send_payload_action(device, msgid, len, data)) {
return false;
}
const uint16_t attempts_per_second = (device->active? 60 : 1);
uint16_t attempts = attempts_per_second * 5;
while (attempts > 0) {
if (recv_payload_msg(device, msgid, 0, NULL)) {
return true;
}
attempts--;
}
return false;
}
device_mcu_error_type device_mcu_open(device_mcu_type* device, device_mcu_event_callback callback) {
if (!device) {
device_mcu_error("No device");
return DEVICE_MCU_ERROR_NO_DEVICE;
}
memset(device, 0, sizeof(device_mcu_type));
device->vendor_id = xreal_vendor_id;
device->product_id = 0;
device->callback = callback;
if (!device_init()) {
device_mcu_error("Not initialized");
return DEVICE_MCU_ERROR_NOT_INITIALIZED;
}
struct hid_device_info* info = hid_enumerate(
device->vendor_id,
device->product_id
);
struct hid_device_info* it = info;
while (it) {
if (is_xreal_product_id(it->product_id) && it->interface_number == 4) {
#ifndef NDEBUG
printf("Found device with product_id: 0x%x\n", it->product_id);
#endif
device->product_id = it->product_id;
device->handle = hid_open_path(it->path);
break;
}
it = it->next;
}
hid_free_enumeration(info);
if (!device->handle) {
device_mcu_error("No handle");
return DEVICE_MCU_ERROR_NO_HANDLE;
}
device_mcu_clear(device);
if (!send_payload_action(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 0, NULL)) {
device_mcu_error("Requesting activation time failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
uint8_t activated;
if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 1, &activated)) {
device_mcu_error("Receiving activation time failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
device->activated = (activated != 0);
if (!device->activated) {
device_mcu_error("Device is not activated");
return DEVICE_MCU_ERROR_NO_ACTIVATION;
}
if (!send_payload_action(device, DEVICE_MCU_MSG_R_MCU_APP_FW_VERSION, 0, NULL)) {
device_mcu_error("Requesting current MCU app firmware version");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_MCU_APP_FW_VERSION, 41, (uint8_t*) device->mcu_app_fw_version)) {
device_mcu_error("Receiving current MCU app firmware version failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
if (!send_payload_action(device, DEVICE_MCU_MSG_R_DP7911_FW_VERSION, 0, NULL)) {
device_mcu_error("Requesting current DP firmware version");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DP7911_FW_VERSION, 41, (uint8_t*) device->dp_fw_version)) {
device_mcu_error("Receiving current DP firmware version failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
if (!send_payload_action(device, DEVICE_MCU_MSG_R_DSP_APP_FW_VERSION, 0, NULL)) {
device_mcu_error("Requesting current DSP app firmware version");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DSP_APP_FW_VERSION, 41, (uint8_t*) device->dsp_fw_version)) {
device_mcu_error("Receiving current DSP app firmware version failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
#ifndef NDEBUG
printf("MCU: %s\n", device->mcu_app_fw_version);
printf("DP: %s\n", device->dp_fw_version);
printf("DSP: %s\n", device->dsp_fw_version);
#endif
if (!send_payload_action(device, DEVICE_MCU_MSG_R_BRIGHTNESS, 0, NULL)) {
device_mcu_error("Requesting initial brightness failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_BRIGHTNESS, 1, &device->brightness)) {
device_mcu_error("Receiving initial brightness failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
if (!send_payload_action(device, DEVICE_MCU_MSG_R_DISP_MODE, 0, NULL)) {
device_mcu_error("Requesting display mode failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DISP_MODE, 1, &device->disp_mode)) {
device_mcu_error("Receiving display mode failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
#ifndef NDEBUG
printf("Brightness: %d\n", device->brightness);
printf("Disp-Mode: %d\n", device->disp_mode);
#endif
return DEVICE_MCU_ERROR_NO_ERROR;
}
static void device_mcu_callback(device_mcu_type* device,
uint64_t timestamp,
device_mcu_event_type event,
uint8_t brightness,
const char* msg) {
if (!device->callback) {
return;
}
device->callback(timestamp, event, brightness, msg);
}
device_mcu_error_type device_mcu_clear(device_mcu_type* device) {
return device_mcu_read(device, 10);
}
device_mcu_error_type device_mcu_read(device_mcu_type* device, int timeout) {
if (!device) {
device_mcu_error("No device");
return DEVICE_MCU_ERROR_NO_DEVICE;
}
if (!device->handle) {
device_mcu_error("No handle");
return DEVICE_MCU_ERROR_NO_HANDLE;
}
if (MAX_PACKET_SIZE != sizeof(device_mcu_packet_type)) {
device_mcu_error("Not proper size");
return DEVICE_MCU_ERROR_WRONG_SIZE;
}
device_mcu_packet_type packet;
memset(&packet, 0, sizeof(device_mcu_packet_type));
int transferred = hid_read_timeout(
device->handle,
(uint8_t*) &packet,
MAX_PACKET_SIZE,
timeout
);
if (transferred == -1) {
device_mcu_error("Device may be unplugged");
return DEVICE_MCU_ERROR_UNPLUGGED;
}
if (transferred == 0) {
return DEVICE_MCU_ERROR_NO_ERROR;
}
if (MAX_PACKET_SIZE != transferred) {
device_mcu_error("Unexpected packet size");
return DEVICE_MCU_ERROR_UNEXPECTED;
}
if (packet.head != PACKET_HEAD) {
device_mcu_error("Wrong packet head");
return DEVICE_MCU_ERROR_WRONG_HEAD;
}
const uint32_t timestamp = le32toh(packet.timestamp);
const uint16_t msgid = le16toh(packet.msgid);
const uint16_t length = le16toh(packet.length);
const size_t data_len = (size_t) &(packet.data) - (size_t) &(packet.length);
#ifndef NDEBUG
printf("MSG: %d = %04x (%d)\n", msgid, msgid, length);
if (length > 11) {
for (int i = 0; i < length - 11; i++) {
printf("%02x ", packet.data[i]);
}
printf("\n");
}
#endif
switch (msgid) {
case DEVICE_MCU_MSG_P_START_HEARTBEAT: {
break;
}
case DEVICE_MCU_MSG_P_BUTTON_PRESSED: {
const uint8_t phys_button = packet.data[0];
const uint8_t virt_button = packet.data[4];
const uint8_t value = packet.data[8];
switch (virt_button) {
case DEVICE_MCU_BUTTON_VIRT_DISPLAY_TOGGLE:
device->active = value;
if (device->active) {
device_mcu_callback(
device,
timestamp,
DEVICE_MCU_EVENT_SCREEN_ON,
device->brightness,
NULL
);
} else {
device_mcu_callback(
device,
timestamp,
DEVICE_MCU_EVENT_SCREEN_OFF,
device->brightness,
NULL
);
}
break;
case DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_UP:
device->brightness = value;
device_mcu_callback(
device,
timestamp,
DEVICE_MCU_EVENT_BRIGHTNESS_UP,
device->brightness,
NULL
);
break;
case DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_DOWN:
device->brightness = value;
device_mcu_callback(
device,
timestamp,
DEVICE_MCU_EVENT_BRIGHTNESS_DOWN,
device->brightness,
NULL
);
break;
default:
break;
}
break;
}
case DEVICE_MCU_MSG_P_ASYNC_TEXT_LOG: {
const char* text = packet.text;
const size_t text_len = strlen(text);
device->active = true;
if (data_len + text_len != length) {
device_mcu_error("Not matching length");
return DEVICE_MCU_ERROR_INVALID_LENGTH;
}
device_mcu_callback(
device,
timestamp,
DEVICE_MCU_EVENT_MESSAGE,
device->brightness,
text
);
break;
}
case DEVICE_MCU_MSG_P_END_HEARTBEAT: {
break;
}
default:
device_mcu_callback(
device,
timestamp,
DEVICE_MCU_EVENT_UNKNOWN,
device->brightness,
NULL
);
break;
}
return DEVICE_MCU_ERROR_NO_ERROR;
}
device_mcu_error_type device_mcu_poll_display_mode(device_mcu_type* device) {
if (!device) {
device_mcu_error("No device");
return DEVICE_MCU_ERROR_NO_DEVICE;
}
if (!device->handle) {
device_mcu_error("No handle");
return DEVICE_MCU_ERROR_NO_HANDLE;
}
if (!send_payload_action(device, DEVICE_MCU_MSG_R_DISP_MODE, 0, NULL)) {
device_mcu_error("Requesting display mode failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DISP_MODE, 1, &device->disp_mode)) {
device_mcu_error("Receiving display mode failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
return DEVICE_MCU_ERROR_NO_ERROR;
}
device_mcu_error_type device_mcu_update_display_mode(device_mcu_type* device) {
if (!device) {
device_mcu_error("No device");
return DEVICE_MCU_ERROR_NO_DEVICE;
}
if (!device->handle) {
device_mcu_error("No handle");
return DEVICE_MCU_ERROR_NO_HANDLE;
}
if (!do_payload_action(device, DEVICE_MCU_MSG_W_DISP_MODE, 1, &device->disp_mode)) {
device_mcu_error("Sending display mode failed");
return DEVICE_MCU_ERROR_PAYLOAD_FAILED;
}
return DEVICE_MCU_ERROR_NO_ERROR;
}
device_mcu_error_type device_mcu_update_mcu_firmware(device_mcu_type* device, const char* path) {
if (!device) {
device_mcu_error("No device");
return DEVICE_MCU_ERROR_NO_DEVICE;
}
if (!device->handle) {
device_mcu_error("No handle");
return DEVICE_MCU_ERROR_NO_HANDLE;
}
if (!device->activated) {
device_mcu_error("Device is not activated");
return DEVICE_MCU_ERROR_NO_ACTIVATION;
}
size_t firmware_len = 0;
uint8_t* firmware = NULL;
FILE* file = fopen(path, "rb");
bool result = DEVICE_MCU_ERROR_UNKNOWN;
if (file) {
fseek(file, 0, SEEK_END);
firmware_len = ftell(file);
if (firmware_len > 0) {
firmware = (uint8_t*) malloc(firmware_len);
fread(firmware, sizeof(uint8_t), firmware_len, file);
}
fclose(file);
}
device_mcu_clear(device);
printf("Prepare upload: %lu\n", firmware_len);
if (!do_payload_action(device, DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_PREPARE, 0, NULL)) {
device_mcu_error("Failed preparing the device for MCU firmware update!\n");
goto cleanup;
}
if (!do_payload_action(device, DEVICE_MCU_MSG_W_MCU_APP_JUMP_TO_BOOT, 0, NULL)) {
device_mcu_error("Failed mcu app jumping to boot");
goto cleanup;
}
if (!send_payload_action(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 0, NULL)) {
device_mcu_error("Requesting activation time failed");
goto cleanup;
}
uint8_t activated;
if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 1, &activated)) {
device_mcu_error("Receiving activation time failed");
goto cleanup;
}
if (!activated) {
device_mcu_error("Device is not activated");
goto jump_to_app;
}
size_t offset = 0;
while (offset < firmware_len) {
const size_t remaining = firmware_len - offset;
printf("Upload: %lu / %lu\n", offset, firmware_len);
uint8_t len;
uint16_t msgid;
if (offset == 0) {
len = remaining > 24? 24 : remaining;
msgid = DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_START;
} else {
len = remaining > 42? 42 : remaining;
msgid = DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_TRANSMIT;
}
if (!do_payload_action(device, msgid, len, firmware + offset)) {
device_mcu_error("Failed sending firmware upload");
goto jump_to_app;
}
offset += len;
}
printf("Finish upload");
if (!do_payload_action(device, DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_FINISH, 0, NULL)) {
device_mcu_error("Failed finishing firmware upload");
goto jump_to_app;
}
result = DEVICE_MCU_ERROR_NO_ERROR;
jump_to_app:
if (!do_payload_action(device, DEVICE_MCU_MSG_W_BOOT_JUMP_TO_APP, 0, NULL)) {
device_mcu_error("Failed boot jumping back to app");
goto cleanup;
}
cleanup:
if (firmware) {
free(firmware);
}
return result;
}
device_mcu_error_type device_mcu_close(device_mcu_type* device) {
if (!device) {
device_mcu_error("No device");
return DEVICE_MCU_ERROR_NO_DEVICE;
}
if (device->handle) {
hid_close(device->handle);
}
memset(device, 0, sizeof(device_mcu_type));
device_exit();
return DEVICE_MCU_ERROR_NO_ERROR;
}

48
src/driver.c
View file

@ -22,8 +22,8 @@
// THE SOFTWARE. // THE SOFTWARE.
// //
#include "device3.h" #include "device_imu.h"
#include "device4.h" #include "device_mcu.h"
#include <stdio.h> #include <stdio.h>
#include <sys/wait.h> #include <sys/wait.h>
@ -32,16 +32,16 @@
#include <math.h> #include <math.h>
void test3(uint64_t timestamp, void test3(uint64_t timestamp,
device3_event_type event, device_imu_event_type event,
const device3_ahrs_type* ahrs) { const device_imu_ahrs_type* ahrs) {
static device3_quat_type old; static device_imu_quat_type old;
static float dmax = -1.0f; static float dmax = -1.0f;
if (event != DEVICE3_EVENT_UPDATE) { if (event != DEVICE_IMU_EVENT_UPDATE) {
return; 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 dx = (old.x - q.x) * (old.x - q.x);
const float dy = (old.y - q.y) * (old.y - q.y); 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); 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) { 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", e0.roll, e0.pitch, e0.yaw);
printf("Roll: %f; Pitch: %f; Yaw: %f\n", e.roll, e.pitch, e.yaw); printf("Roll: %f; Pitch: %f; Yaw: %f\n", e.roll, e.pitch, e.yaw);
@ -75,17 +75,17 @@ void test3(uint64_t timestamp,
} }
void test4(uint64_t timestamp, void test4(uint64_t timestamp,
device4_event_type event, device_mcu_event_type event,
uint8_t brightness, uint8_t brightness,
const char* msg) { const char* msg) {
switch (event) { switch (event) {
case DEVICE4_EVENT_MESSAGE: case DEVICE_MCU_EVENT_MESSAGE:
printf("Message: `%s`\n", msg); printf("Message: `%s`\n", msg);
break; break;
case DEVICE4_EVENT_BRIGHTNESS_UP: case DEVICE_MCU_EVENT_BRIGHTNESS_UP:
printf("Increase Brightness: %u\n", brightness); printf("Increase Brightness: %u\n", brightness);
break; break;
case DEVICE4_EVENT_BRIGHTNESS_DOWN: case DEVICE_MCU_EVENT_BRIGHTNESS_DOWN:
printf("Decrease Brightness: %u\n", brightness); printf("Decrease Brightness: %u\n", brightness);
break; break;
default: default:
@ -102,28 +102,28 @@ int main(int argc, const char** argv) {
} }
if (pid == 0) { if (pid == 0) {
device3_type dev3; device_imu_type dev3;
if (DEVICE3_ERROR_NO_ERROR != device3_open(&dev3, test3)) { if (DEVICE_IMU_ERROR_NO_ERROR != device_imu_open(&dev3, test3)) {
return 1; return 1;
} }
device3_clear(&dev3); device_imu_clear(&dev3);
device3_calibrate(&dev3, 1000, true, true, false); device_imu_calibrate(&dev3, 1000, true, true, false);
while (DEVICE3_ERROR_NO_ERROR == device3_read(&dev3, -1)); while (DEVICE_IMU_ERROR_NO_ERROR == device_imu_read(&dev3, -1));
device3_close(&dev3); device_imu_close(&dev3);
return 0; return 0;
} else { } else {
int status = 0; int status = 0;
device4_type dev4; device_mcu_type dev4;
if (DEVICE4_ERROR_NO_ERROR != device4_open(&dev4, test4)) { if (DEVICE_MCU_ERROR_NO_ERROR != device_mcu_open(&dev4, test4)) {
status = 1; status = 1;
goto exit; goto exit;
} }
device4_clear(&dev4); device_mcu_clear(&dev4);
while (DEVICE4_ERROR_NO_ERROR == device4_read(&dev4, -1)); while (DEVICE_MCU_ERROR_NO_ERROR == device_mcu_read(&dev4, -1));
device4_close(&dev4); device_mcu_close(&dev4);
exit: exit:
if (pid != waitpid(pid, &status, 0)) { if (pid != waitpid(pid, &status, 0)) {