diff --git a/interface_lib/include/device4.h b/interface_lib/include/device4.h index 1086ad8..48a385c 100644 --- a/interface_lib/include/device4.h +++ b/interface_lib/include/device4.h @@ -34,8 +34,10 @@ #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 @@ -139,6 +141,7 @@ struct device4_t { bool active; uint8_t brightness; + uint8_t disp_mode; device4_event_callback callback; }; diff --git a/interface_lib/src/device4.c b/interface_lib/src/device4.c index 06668f5..2dca24d 100644 --- a/interface_lib/src/device4.c +++ b/interface_lib/src/device4.c @@ -124,7 +124,15 @@ static bool recv_payload_msg(device4_type* device, uint8_t msgid, uint8_t len, u return false; } - memcpy(data, packet.data + 1, len); + const uint16_t data_len = 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; } @@ -234,6 +242,21 @@ device4_type* device4_open(device4_event_callback callback) { return device; } + if (!send_payload_action(device, DEVICE4_MSG_R_DISP_MODE, 0, NULL)) { + perror("Requesting display mode failed!\n"); + return device; + } + + if (!recv_payload_msg(device, DEVICE4_MSG_R_DISP_MODE, 1, &device->disp_mode)) { + perror("Receiving display mode failed!\n"); + return device; + } + +#ifndef NDEBUG + printf("Brightness: %d\n", device->brightness); + printf("Disp-Mode: %d\n", device->disp_mode); +#endif + return device; }