diff --git a/interface_lib/include/device3.h b/interface_lib/include/device3.h index 3d19182..d4a3b28 100644 --- a/interface_lib/include/device3.h +++ b/interface_lib/include/device3.h @@ -122,8 +122,6 @@ struct device3_t { device3_event_callback callback; device3_calibration_type* calibration; - - bool ready; }; typedef struct device3_t device3_type; @@ -140,7 +138,7 @@ void device3_clear(device3_type* device); int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet); -int device3_read(device3_type* device, int timeout, bool silent); +int device3_read(device3_type* device, int timeout); device3_vec3_type device3_get_earth_acceleration(const device3_ahrs_type* ahrs); diff --git a/interface_lib/src/device3.c b/interface_lib/src/device3.c index 2d80a8b..16feaf5 100644 --- a/interface_lib/src/device3.c +++ b/interface_lib/src/device3.c @@ -36,6 +36,12 @@ #include "crc32.h" +#ifndef NDEBUG +#define device3_error(msg) fprintf(stderr, "ERROR: %s\n", msg) +#else +#define device3_error(msg) (0) +#endif + struct device3_calibration_t { FusionMatrix gyroscopeMisalignment; FusionVector gyroscopeSensitivity; @@ -66,7 +72,7 @@ static bool send_payload(device3_type* device, uint8_t size, const uint8_t* payl int transferred = hid_write(device->handle, payload, payload_size); if (transferred != payload_size) { - fprintf(stderr, "ERROR: sending payload failed\n"); + device3_error("Sending payload failed"); return false; } @@ -90,7 +96,7 @@ static bool recv_payload(device3_type* device, uint8_t size, uint8_t* payload) { } if (transferred != payload_size) { - fprintf(stderr, "ERROR: receiving payload failed\n"); + device3_error("Receiving payload failed"); return false; } @@ -180,6 +186,7 @@ device3_type* device3_open(device3_event_callback callback) { device3_type* device = (device3_type*) malloc(sizeof(device3_type)); if (!device) { + device3_error("Not allocated"); return NULL; } @@ -189,6 +196,7 @@ device3_type* device3_open(device3_event_callback callback) { device->callback = callback; if (0 != hid_init()) { + device3_error("Not initialized"); return device; } @@ -210,12 +218,14 @@ device3_type* device3_open(device3_event_callback callback) { hid_free_enumeration(info); if (!device->handle) { + device3_error("No handle"); return device; } device3_clear(device); if (!send_payload_msg(device, DEVICE3_MSG_GET_STATIC_ID, 0, NULL)) { + device3_error("Failed sending payload to get static id"); return device; } @@ -230,6 +240,7 @@ device3_type* device3_open(device3_event_callback callback) { device3_reset_calibration(device); if (!send_payload_msg(device, DEVICE3_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) { + device3_error("Failed sending payload to get calibration data length"); return device; } @@ -285,6 +296,7 @@ device3_type* device3_open(device3_event_callback callback) { } if (!send_payload_msg_signal(device, DEVICE3_MSG_START_IMU_DATA, 0x1)) { + device3_error("Failed sending payload to start imu data stream"); return device; } @@ -306,19 +318,17 @@ device3_type* device3_open(device3_event_callback callback) { FusionAhrsSetSettings((FusionAhrs*) device->ahrs, &settings); - device->ready = true; - return device; } void device3_reset_calibration(device3_type* device) { if (!device) { - fprintf(stderr, "No device!\n"); + device3_error("No device"); return; } if (!device->calibration) { - fprintf(stderr, "Not allocated!\n"); + device3_error("Not allocated"); return; } @@ -343,18 +353,18 @@ void device3_reset_calibration(device3_type* device) { int device3_load_calibration(device3_type* device, const char* path) { if (!device) { - fprintf(stderr, "No device!\n"); + device3_error("No device"); return -1; } if (!device->calibration) { - fprintf(stderr, "Not allocated!\n"); + device3_error("Not allocated"); return -2; } FILE* file = fopen(path, "rb"); if (!file) { - fprintf(stderr, "No file opened!\n"); + device3_error("No file opened"); return -3; } @@ -362,11 +372,11 @@ int device3_load_calibration(device3_type* device, const char* path) { count = fread(device->calibration, 1, sizeof(device3_calibration_type), file); if (sizeof(device3_calibration_type) != count) { - fprintf(stderr, "Not fully loaded!\n"); + device3_error("Not fully loaded"); } if (0 != fclose(file)) { - fprintf(stderr, "No file closed!\n"); + device3_error("No file closed"); return -4; } @@ -375,18 +385,18 @@ int device3_load_calibration(device3_type* device, const char* path) { int device3_save_calibration(device3_type* device, const char* path) { if (!device) { - fprintf(stderr, "No device!\n"); + device3_error("No device"); return -1; } if (!device->calibration) { - fprintf(stderr, "Not allocated!\n"); + device3_error("Not allocated"); return -2; } FILE* file = fopen(path, "wb"); if (!file) { - fprintf(stderr, "No file opened!\n"); + device3_error("No file opened"); return -3; } @@ -394,11 +404,11 @@ int device3_save_calibration(device3_type* device, const char* path) { count = fwrite(device->calibration, 1, sizeof(device3_calibration_type), file); if (sizeof(device3_calibration_type) != count) { - fprintf(stderr, "Not fully saved!\n"); + device3_error("Not fully saved"); } if (0 != fclose(file)) { - fprintf(stderr, "No file closed!\n"); + device3_error("No file closed"); return -4; } @@ -605,12 +615,12 @@ void device3_clear(device3_type* device) { int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet) { if (!device) { - fprintf(stderr, "No device!\n"); + device3_error("No device"); return -1; } if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) { - fprintf(stderr, "Not proper size!\n"); + device3_error("Not proper size"); return -2; } @@ -644,7 +654,7 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool } if (MAX_PACKET_SIZE != transferred) { - fprintf(stderr, "HID read error: unexpected packet size\n"); + device3_error("Not expected issue"); return -3; } @@ -719,19 +729,20 @@ int device3_calibrate(device3_type* device, uint32_t iterations, bool gyro, bool return 0; } -int device3_read(device3_type* device, int timeout, bool silent) { - if (!device || !device->ready) { - if (!silent) { - fprintf(stderr, "No device!\n"); - } +int device3_read(device3_type* device, int timeout) { + if (!device) { + device3_error("No device"); return -1; } + + if (!device->handle) { + device3_error("No handle"); + return -2; + } if (MAX_PACKET_SIZE != sizeof(device3_packet_type)) { - if (!silent) { - fprintf(stderr, "Not proper size!\n"); - } - return -2; + device3_error("Not proper size"); + return -3; } device3_packet_type packet; @@ -745,10 +756,8 @@ int device3_read(device3_type* device, int timeout, bool silent) { ); if (transferred == -1) { - if (!silent) { - fprintf(stderr, "HID read error: device may be unplugged\n"); - } - return -5; + device3_error("Device may be unplugged"); + return -4; } if (transferred == 0) { @@ -756,10 +765,8 @@ int device3_read(device3_type* device, int timeout, bool silent) { } if (MAX_PACKET_SIZE != transferred) { - if (!silent) { - fprintf(stderr, "HID read error: unexpected packet size\n"); - } - return -3; + device3_error("Unexpected packet size"); + return -5; } const uint64_t timestamp = packet.timestamp; @@ -770,10 +777,8 @@ int device3_read(device3_type* device, int timeout, bool silent) { } if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { - if (!silent) { - fprintf(stderr, "Mismatched signature! Try unplugging then replugging your device.\n"); - } - return -4; + device3_error("Not matching signature"); + return -6; } const uint64_t delta = timestamp - device->last_timestamp; @@ -804,13 +809,13 @@ int device3_read(device3_type* device, int timeout, bool silent) { if (device->ahrs) { FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); + const device3_quat_type orientation = device3_get_orientation(device->ahrs); + // TODO: fix detection of this case; quat.x as a nan value is only a side-effect of some issue with ahrs or // the gyro/accel/magnet readings - if (isnan(device3_get_orientation(device->ahrs).x)) { - if (!silent) { - fprintf(stderr, "Invalid device reading\n"); - } - return -5; + if (isnan(orientation.x) || isnan(orientation.y) || isnan(orientation.z) || isnan(orientation.w)) { + device3_error("Invalid orientation reading"); + return -7; } } @@ -862,6 +867,7 @@ device3_vec3_type device3_get_euler(device3_quat_type quat) { void device3_close(device3_type* device) { if (!device) { + device3_error("No device"); return; } diff --git a/interface_lib/src/device4.c b/interface_lib/src/device4.c index 878b090..4341e8e 100644 --- a/interface_lib/src/device4.c +++ b/interface_lib/src/device4.c @@ -35,6 +35,12 @@ #include "crc32.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 @@ -47,7 +53,7 @@ static bool send_payload(device4_type* device, uint8_t size, const uint8_t* payl int transferred = hid_write(device->handle, payload, payload_size); if (transferred != payload_size) { - fprintf(stderr, "ERROR: sending payload failed\n"); + device4_error("Sending payload failed"); return false; } @@ -71,7 +77,7 @@ static bool recv_payload(device4_type* device, uint8_t size, uint8_t* payload) { } if (transferred != payload_size) { - fprintf(stderr, "ERROR: receiving payload failed\n"); + device4_error("Receiving payload failed"); return false; } @@ -111,19 +117,19 @@ static bool recv_payload_msg(device4_type* device, uint16_t msgid, uint8_t len, } if (packet.head != PACKET_HEAD) { - perror("ERROR: invalid payload received\n"); + device4_error("Invalid payload received"); return false; } if (packet.msgid != msgid) { - perror("ERROR: unexpected payload received\n"); + device4_error("Unexpected payload received"); return false; } const uint8_t status = packet.data[0]; if (status != 0) { - perror("ERROR: payload status failed\n"); + device4_error("Payload status failed"); return false; } @@ -156,7 +162,6 @@ static bool do_payload_action(device4_type* device, uint16_t msgid, uint8_t len, attempts--; } - perror("ERROR: payload status failed\n"); return false; } @@ -164,7 +169,7 @@ device4_type* device4_open(device4_event_callback callback) { device4_type* device = (device4_type*) malloc(sizeof(device4_type)); if (!device) { - fprintf(stderr, "Not allocated!\n"); + device4_error("Not allocated"); return NULL; } @@ -174,7 +179,7 @@ device4_type* device4_open(device4_event_callback callback) { device->callback = callback; if (0 != hid_init()) { - fprintf(stderr, "Not initialized!\n"); + device4_error("Not initialized"); return device; } @@ -196,57 +201,57 @@ device4_type* device4_open(device4_event_callback callback) { hid_free_enumeration(info); if (!device->handle) { - fprintf(stderr, "No handle!\n"); + device4_error("No handle"); return device; } device4_clear(device); if (!send_payload_action(device, DEVICE4_MSG_R_ACTIVATION_TIME, 0, NULL)) { - fprintf(stderr, "Requesting activation time failed!\n"); + device4_error("Requesting activation time failed"); return device; } uint8_t activated; if (!recv_payload_msg(device, DEVICE4_MSG_R_ACTIVATION_TIME, 1, &activated)) { - fprintf(stderr, "Receiving activation time failed!\n"); + device4_error("Receiving activation time failed"); return device; } device->activated = (activated != 0); if (!device->activated) { - fprintf(stderr, "Device is not activated!\n"); + device4_error("Device is not activated"); return device; } if (!send_payload_action(device, DEVICE4_MSG_R_MCU_APP_FW_VERSION, 0, NULL)) { - fprintf(stderr, "Requesting current MCU app firmware version!\n"); + device4_error("Requesting current MCU app firmware version"); return device; } if (!recv_payload_msg(device, DEVICE4_MSG_R_MCU_APP_FW_VERSION, 41, (uint8_t*) device->mcu_app_fw_version)) { - fprintf(stderr, "Receiving current MCU app firmware version failed!\n"); + device4_error("Receiving current MCU app firmware version failed"); return device; } if (!send_payload_action(device, DEVICE4_MSG_R_DP7911_FW_VERSION, 0, NULL)) { - fprintf(stderr, "Requesting current DP firmware version!\n"); + device4_error("Requesting current DP firmware version"); return device; } if (!recv_payload_msg(device, DEVICE4_MSG_R_DP7911_FW_VERSION, 41, (uint8_t*) device->dp_fw_version)) { - fprintf(stderr, "Receiving current DP firmware version failed!\n"); + device4_error("Receiving current DP firmware version failed"); return device; } if (!send_payload_action(device, DEVICE4_MSG_R_DSP_APP_FW_VERSION, 0, NULL)) { - fprintf(stderr, "Requesting current DSP app firmware version!\n"); + device4_error("Requesting current DSP app firmware version"); return device; } if (!recv_payload_msg(device, DEVICE4_MSG_R_DSP_APP_FW_VERSION, 41, (uint8_t*) device->dsp_fw_version)) { - fprintf(stderr, "Receiving current DSP app firmware version failed!\n"); + device4_error("Receiving current DSP app firmware version failed"); return device; } @@ -257,22 +262,22 @@ device4_type* device4_open(device4_event_callback callback) { #endif if (!send_payload_action(device, DEVICE4_MSG_R_BRIGHTNESS, 0, NULL)) { - fprintf(stderr, "Requesting initial brightness failed!\n"); + device4_error("Requesting initial brightness failed"); return device; } if (!recv_payload_msg(device, DEVICE4_MSG_R_BRIGHTNESS, 1, &device->brightness)) { - fprintf(stderr, "Receiving initial brightness failed!\n"); + device4_error("Receiving initial brightness failed"); return device; } if (!send_payload_action(device, DEVICE4_MSG_R_DISP_MODE, 0, NULL)) { - fprintf(stderr, "Requesting display mode failed!\n"); + device4_error("Requesting display mode failed"); return device; } if (!recv_payload_msg(device, DEVICE4_MSG_R_DISP_MODE, 1, &device->disp_mode)) { - fprintf(stderr, "Receiving display mode failed!\n"); + device4_error("Receiving display mode failed"); return device; } @@ -301,13 +306,19 @@ void device4_clear(device4_type* device) { } int device4_read(device4_type* device, int timeout) { - if ((!device) || (!device->handle)) { + if (!device) { + device4_error("No device"); return -1; } + + if (!device->handle) { + device4_error("No handle"); + return -2; + } if (MAX_PACKET_SIZE != sizeof(device4_packet_type)) { - fprintf(stderr, "Not proper size!\n"); - return -2; + device4_error("Not proper size"); + return -3; } device4_packet_type packet; @@ -325,13 +336,13 @@ int device4_read(device4_type* device, int timeout) { } if (MAX_PACKET_SIZE != transferred) { - fprintf(stderr, "Reading failed!\n"); - return -3; + device4_error("Reading failed"); + return -4; } if (packet.head != PACKET_HEAD) { - fprintf(stderr, "Wrong packet!\n"); - return -4; + device4_error("Wrong packet"); + return -5; } const uint32_t timestamp = packet.timestamp; @@ -415,8 +426,8 @@ int device4_read(device4_type* device, int timeout) { device->active = true; if (data_len + text_len != packet.length) { - fprintf(stderr, "Not matching length!\n"); - return -5; + device4_error("Not matching length"); + return -6; } device4_callback( @@ -451,7 +462,7 @@ bool device4_update_mcu_firmware(device4_type* device, const char* path) { } if (!device->activated) { - fprintf(stderr, "Device is not activated!\n"); + device4_error("Device is not activated"); return false; } @@ -478,28 +489,28 @@ bool device4_update_mcu_firmware(device4_type* device, const char* path) { printf("Prepare upload: %lu\n", firmware_len); if (!do_payload_action(device, DEVICE4_MSG_W_UPDATE_MCU_APP_FW_PREPARE, 0, NULL)) { - fprintf(stderr, "Failed preparing the device for MCU firmware update!\n"); + 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)) { - fprintf(stderr, "Failed mcu app jumping to boot!\n"); + device4_error("Failed mcu app jumping to boot"); goto cleanup; } if (!send_payload_action(device, DEVICE4_MSG_R_ACTIVATION_TIME, 0, NULL)) { - perror("Requesting activation time failed!\n"); + device4_error("Requesting activation time failed"); return device; } uint8_t activated; if (!recv_payload_msg(device, DEVICE4_MSG_R_ACTIVATION_TIME, 1, &activated)) { - perror("Receiving activation time failed!\n"); + device4_error("Receiving activation time failed"); return device; } if (!activated) { - perror("Device is not activated!\n"); + device4_error("Device is not activated"); goto jump_to_app; } @@ -521,17 +532,17 @@ bool device4_update_mcu_firmware(device4_type* device, const char* path) { } if (!do_payload_action(device, msgid, len, firmware + offset)) { - fprintf(stderr, "Failed sending firmware upload!\n"); + device4_error("Failed sending firmware upload"); goto jump_to_app; } offset += len; } - printf("Finish upload!\n"); + printf("Finish upload"); if (!do_payload_action(device, DEVICE4_MSG_W_UPDATE_MCU_APP_FW_FINISH, 0, NULL)) { - fprintf(stderr, "Failed finishing firmware upload!\n"); + device4_error("Failed finishing firmware upload"); goto jump_to_app; } @@ -539,7 +550,7 @@ bool device4_update_mcu_firmware(device4_type* device, const char* path) { jump_to_app: if (!do_payload_action(device, DEVICE4_MSG_W_BOOT_JUMP_TO_APP, 0, NULL)) { - fprintf(stderr, "Failed boot jumping back to app!\n"); + device4_error("Failed boot jumping back to app"); goto cleanup; }