|
2 | 2 | #include "custom_cast.h"
|
3 | 3 | #include "timestamp.h"
|
4 | 4 |
|
| 5 | +#ifndef _WIN32 |
| 6 | +#include <errno.h> |
| 7 | +#endif |
5 | 8 |
|
6 | 9 | #define START_BYTE 0xA0
|
7 | 10 | #define END_BYTE_STANDARD 0xC0
|
@@ -38,148 +41,154 @@ void CytonDaisyWifi::read_thread ()
|
38 | 41 | Byte 33: 0xCX where X is 0-F in hex
|
39 | 42 | */
|
40 | 43 | int res;
|
41 |
| - unsigned char b[OpenBCIWifiShieldBoard::transaction_size]; |
| 44 | + unsigned char b[OpenBCIWifiShieldBoard::package_size]; |
42 | 45 | double accel[3] = {0.};
|
43 | 46 | while (keep_alive)
|
44 | 47 | {
|
45 |
| - res = server_socket->recv (b, OpenBCIWifiShieldBoard::transaction_size); |
46 |
| - if (res != OpenBCIWifiShieldBoard::transaction_size) |
| 48 | + res = server_socket->recv (b, OpenBCIWifiShieldBoard::package_size); |
| 49 | + if (res != OpenBCIWifiShieldBoard::package_size) |
47 | 50 | {
|
| 51 | + safe_logger (spdlog::level::warn, "recv result: {}", res); |
| 52 | + if (res == -1) |
| 53 | + { |
| 54 | +#ifdef _WIN32 |
| 55 | + safe_logger (spdlog::level::warn, "WSAGetLastError is {}", WSAGetLastError ()); |
| 56 | +#else |
| 57 | + safe_logger (spdlog::level::warn, "errno {} message {}", errno, strerror (errno)); |
| 58 | +#endif |
| 59 | + } |
| 60 | + |
48 | 61 | continue;
|
49 | 62 | }
|
| 63 | + |
50 | 64 | double package[30] = {0.};
|
51 | 65 | bool first_sample = false;
|
52 |
| - for (int cur_package = 0; |
53 |
| - cur_package < OpenBCIWifiShieldBoard::num_packages_per_transaction; cur_package++) |
| 66 | + if (b[0] != START_BYTE) |
| 67 | + { |
| 68 | + break; // drop entire transaction for daisy |
| 69 | + } |
| 70 | + unsigned char *bytes = b + 1; // for better consistency between plain cyton and wifi, in |
| 71 | + // plain cyton index is shifted by 1 |
| 72 | + |
| 73 | + if ((bytes[31] < END_BYTE_STANDARD) || (bytes[31] > END_BYTE_MAX)) |
| 74 | + { |
| 75 | + safe_logger (spdlog::level::warn, "Wrong end byte {}", bytes[31]); |
| 76 | + continue; |
| 77 | + } |
| 78 | + |
| 79 | + // place unprocessed bytes to other_channels for all modes |
| 80 | + if ((bytes[0] % 2 == 0) && (first_sample)) |
54 | 81 | {
|
55 |
| - int offset = cur_package * OpenBCIWifiShieldBoard::package_size; |
56 |
| - if (b[0 + offset] != START_BYTE) |
| 82 | + // eeg |
| 83 | + for (int i = 0; i < 8; i++) |
57 | 84 | {
|
58 |
| - break; // drop entire transaction for daisy |
| 85 | + package[i + 9] = eeg_scale * cast_24bit_to_int32 (bytes + 1 + 3 * i); |
59 | 86 | }
|
60 |
| - unsigned char *bytes = b + 1; // for better consistency between plain cyton and wifi, in |
61 |
| - // plain cyton index is shifted by 1 |
62 |
| - |
63 |
| - if ((bytes[31 + offset] < END_BYTE_STANDARD) || (bytes[31 + offset] > END_BYTE_MAX)) |
| 87 | + // need to average other_channels |
| 88 | + package[21] += (double)bytes[25]; |
| 89 | + package[22] += (double)bytes[28]; |
| 90 | + package[23] += (double)bytes[27]; |
| 91 | + package[24] += (double)bytes[28]; |
| 92 | + package[25] += (double)bytes[29]; |
| 93 | + package[26] += (double)bytes[30]; |
| 94 | + package[21] /= 2.0; |
| 95 | + package[22] /= 2.0; |
| 96 | + package[23] /= 2.0; |
| 97 | + package[24] /= 2.0; |
| 98 | + package[25] /= 2.0; |
| 99 | + package[26] /= 2.0; |
| 100 | + package[20] = (double)bytes[31]; |
| 101 | + } |
| 102 | + else |
| 103 | + { |
| 104 | + first_sample = true; |
| 105 | + package[0] = (double)bytes[0]; |
| 106 | + // eeg |
| 107 | + for (int i = 0; i < 8; i++) |
64 | 108 | {
|
65 |
| - safe_logger (spdlog::level::warn, "Wrong end byte {}", bytes[31 + offset]); |
66 |
| - continue; |
| 109 | + package[i + 1] = eeg_scale * cast_24bit_to_int32 (bytes + 1 + 3 * i); |
67 | 110 | }
|
| 111 | + // other_channels |
| 112 | + package[21] = (double)bytes[25]; |
| 113 | + package[22] = (double)bytes[26]; |
| 114 | + package[23] = (double)bytes[27]; |
| 115 | + package[24] = (double)bytes[28]; |
| 116 | + package[25] = (double)bytes[29]; |
| 117 | + package[26] = (double)bytes[30]; |
| 118 | + } |
| 119 | + |
| 120 | + // place processed accel data |
| 121 | + if (bytes[31] == END_BYTE_STANDARD) |
| 122 | + { |
| 123 | + int32_t accel_temp[3] = {0}; |
| 124 | + accel_temp[0] = cast_16bit_to_int32 (bytes + 25); |
| 125 | + accel_temp[1] = cast_16bit_to_int32 (bytes + 27); |
| 126 | + accel_temp[2] = cast_16bit_to_int32 (bytes + 29); |
68 | 127 |
|
69 |
| - // place unprocessed bytes to other_channels for all modes |
70 |
| - if ((bytes[0 + offset] % 2 == 0) && (first_sample)) |
| 128 | + if ((bytes[0] % 2 == 0) && (first_sample)) |
71 | 129 | {
|
72 |
| - // eeg |
73 |
| - for (int i = 0; i < 8; i++) |
| 130 | + // need to average accel data |
| 131 | + if (accel_temp[0] != 0) |
74 | 132 | {
|
75 |
| - package[i + 9] = eeg_scale * cast_24bit_to_int32 (bytes + 1 + 3 * i + offset); |
| 133 | + accel[0] += accel_scale * accel_temp[0]; |
| 134 | + accel[1] += accel_scale * accel_temp[1]; |
| 135 | + accel[2] += accel_scale * accel_temp[2]; |
| 136 | + |
| 137 | + accel[0] /= 2.f; |
| 138 | + accel[1] /= 2.f; |
| 139 | + accel[2] /= 2.f; |
76 | 140 | }
|
77 |
| - // need to average other_channels |
78 |
| - package[21] += (double)bytes[25 + offset]; |
79 |
| - package[22] += (double)bytes[28 + offset]; |
80 |
| - package[23] += (double)bytes[27 + offset]; |
81 |
| - package[24] += (double)bytes[28 + offset]; |
82 |
| - package[25] += (double)bytes[29 + offset]; |
83 |
| - package[26] += (double)bytes[30 + offset]; |
84 |
| - package[21] /= 2.0; |
85 |
| - package[22] /= 2.0; |
86 |
| - package[23] /= 2.0; |
87 |
| - package[24] /= 2.0; |
88 |
| - package[25] /= 2.0; |
89 |
| - package[26] /= 2.0; |
90 |
| - package[20] = (double)bytes[31 + offset]; |
| 141 | + |
| 142 | + package[20] = (double)bytes[31]; |
91 | 143 | }
|
92 | 144 | else
|
93 | 145 | {
|
94 | 146 | first_sample = true;
|
95 |
| - package[0] = (double)bytes[0 + offset]; |
96 |
| - // eeg |
97 |
| - for (int i = 0; i < 8; i++) |
98 |
| - { |
99 |
| - package[i + 1] = eeg_scale * cast_24bit_to_int32 (bytes + 1 + 3 * i + offset); |
100 |
| - } |
101 |
| - // other_channels |
102 |
| - package[21] = (double)bytes[25 + offset]; |
103 |
| - package[22] = (double)bytes[26 + offset]; |
104 |
| - package[23] = (double)bytes[27 + offset]; |
105 |
| - package[24] = (double)bytes[28 + offset]; |
106 |
| - package[25] = (double)bytes[29 + offset]; |
107 |
| - package[26] = (double)bytes[30 + offset]; |
108 |
| - } |
109 |
| - |
110 |
| - // place processed accel data |
111 |
| - if (bytes[31 + offset] == END_BYTE_STANDARD) |
112 |
| - { |
113 |
| - int32_t accel_temp[3] = {0}; |
114 |
| - accel_temp[0] = cast_16bit_to_int32 (bytes + 25 + offset); |
115 |
| - accel_temp[1] = cast_16bit_to_int32 (bytes + 27 + offset); |
116 |
| - accel_temp[2] = cast_16bit_to_int32 (bytes + 29 + offset); |
117 |
| - |
118 |
| - if ((bytes[0 + offset] % 2 == 0) && (first_sample)) |
119 |
| - { |
120 |
| - // need to average accel data |
121 |
| - if (accel_temp[0] != 0) |
122 |
| - { |
123 |
| - accel[0] += accel_scale * accel_temp[0]; |
124 |
| - accel[1] += accel_scale * accel_temp[1]; |
125 |
| - accel[2] += accel_scale * accel_temp[2]; |
| 147 | + package[0] = (double)bytes[0]; |
126 | 148 |
|
127 |
| - accel[0] /= 2.f; |
128 |
| - accel[1] /= 2.f; |
129 |
| - accel[2] /= 2.f; |
130 |
| - } |
131 |
| - |
132 |
| - package[20] = (double)bytes[31 + offset]; |
133 |
| - } |
134 |
| - else |
| 149 | + // accel |
| 150 | + if (accel_temp[0] != 0) |
135 | 151 | {
|
136 |
| - first_sample = true; |
137 |
| - package[0] = (double)bytes[0 + offset]; |
138 |
| - |
139 |
| - // accel |
140 |
| - if (accel_temp[0] != 0) |
141 |
| - { |
142 |
| - accel[0] = accel_scale * accel_temp[0]; |
143 |
| - accel[1] = accel_scale * accel_temp[1]; |
144 |
| - accel[2] = accel_scale * accel_temp[2]; |
145 |
| - } |
| 152 | + accel[0] = accel_scale * accel_temp[0]; |
| 153 | + accel[1] = accel_scale * accel_temp[1]; |
| 154 | + accel[2] = accel_scale * accel_temp[2]; |
146 | 155 | }
|
147 |
| - |
148 |
| - package[17] = accel[0]; |
149 |
| - package[18] = accel[1]; |
150 |
| - package[19] = accel[2]; |
151 | 156 | }
|
152 |
| - // place processed analog data |
153 |
| - if (bytes[31 + offset] == END_BYTE_ANALOG) |
| 157 | + |
| 158 | + package[17] = accel[0]; |
| 159 | + package[18] = accel[1]; |
| 160 | + package[19] = accel[2]; |
| 161 | + } |
| 162 | + // place processed analog data |
| 163 | + if (bytes[31] == END_BYTE_ANALOG) |
| 164 | + { |
| 165 | + if ((bytes[0] % 2 == 0) && (first_sample)) |
154 | 166 | {
|
155 |
| - if ((bytes[0 + offset] % 2 == 0) && (first_sample)) |
156 |
| - { |
157 |
| - // need to average analog data |
158 |
| - package[27] += cast_16bit_to_int32 (bytes + 25 + offset); |
159 |
| - package[28] += cast_16bit_to_int32 (bytes + 27 + offset); |
160 |
| - package[29] += cast_16bit_to_int32 (bytes + 29 + offset); |
161 |
| - package[27] /= 2.0f; |
162 |
| - package[28] /= 2.0f; |
163 |
| - package[29] /= 2.0f; |
164 |
| - package[20] = (double)bytes[31 + offset]; // cyton end byte |
165 |
| - } |
166 |
| - else |
167 |
| - { |
168 |
| - first_sample = true; |
169 |
| - package[0] = (double)bytes[0 + offset]; |
170 |
| - // analog |
171 |
| - package[27] = cast_16bit_to_int32 (bytes + 25 + offset); |
172 |
| - package[28] = cast_16bit_to_int32 (bytes + 27 + offset); |
173 |
| - package[29] = cast_16bit_to_int32 (bytes + 29 + offset); |
174 |
| - } |
| 167 | + // need to average analog data |
| 168 | + package[27] += cast_16bit_to_int32 (bytes + 25); |
| 169 | + package[28] += cast_16bit_to_int32 (bytes + 27); |
| 170 | + package[29] += cast_16bit_to_int32 (bytes + 29); |
| 171 | + package[27] /= 2.0f; |
| 172 | + package[28] /= 2.0f; |
| 173 | + package[29] /= 2.0f; |
| 174 | + package[20] = (double)bytes[31]; // cyton end byte |
175 | 175 | }
|
176 |
| - // commit package |
177 |
| - if ((bytes[0 + offset] % 2 == 0) && (first_sample)) |
| 176 | + else |
178 | 177 | {
|
179 |
| - double timestamp = get_timestamp (); |
180 |
| - db->add_data (timestamp, package); |
181 |
| - streamer->stream_data (package, 30, timestamp); |
| 178 | + first_sample = true; |
| 179 | + package[0] = (double)bytes[0]; |
| 180 | + // analog |
| 181 | + package[27] = cast_16bit_to_int32 (bytes + 25); |
| 182 | + package[28] = cast_16bit_to_int32 (bytes + 27); |
| 183 | + package[29] = cast_16bit_to_int32 (bytes + 29); |
182 | 184 | }
|
183 | 185 | }
|
| 186 | + // commit package |
| 187 | + if ((bytes[0] % 2 == 0) && (first_sample)) |
| 188 | + { |
| 189 | + double timestamp = get_timestamp (); |
| 190 | + db->add_data (timestamp, package); |
| 191 | + streamer->stream_data (package, 30, timestamp); |
| 192 | + } |
184 | 193 | }
|
185 | 194 | }
|
0 commit comments