Sledeći korak je da steknem predstavu šta vrednosti koje dobijam od sentora predstavljaju u stvarnom svetu. Meni je uvek najbolje da shvatim nešto kada ga vidim. Zato je sledeći korak da crtam svaki od podataka (Ax,Ay,Az,Gx,Gy,Gz) koji dolazi sa senzora u realnom vremenu. Python nije pravi alat za RT projekte, ipak se nadam da će dovoljno biti za nesto ovako jednostavno.
pyGame mi zvuči kao dobar početak za grafički deo. Za razliku kako obično koritim pyGame ovde sam odlučio da okidač za iscrtavanje bude pristizanje novog paketa sa podacima preko serijskog porta. Možda nepotrebno ali da bih smanjio kolicinu bajtova koji prolaze kroz 115200 bps vezu paket sa podacima je binarni i izgleda ovako:
| Byte Offset | Field | Size (bytes) | Format | Description |
|---|---|---|---|---|
| 0 | Sync | 1 | 0xAA | Synchronization byte |
| 1 | Ax | 2 | Big Endian | Acceleration X |
| 3 | Ay | 2 | Big Endian | Acceleration Y |
| 5 | Az | 2 | Big Endian | Acceleration Z |
| 7 | Tmp | 2 | Big Endian | Temperature |
| 9 | Gx | 2 | Big Endian | Gyroscope X |
| 11 | Gy | 2 | Big Endian | Gyroscope Y |
| 13 | Gz | 2 | Big Endian | Gyroscope Z |
| 15 | Δt | 2 | Big Endian | Time elapsed since last sample (microseconds) |
malo sam zakomplikovao parsiranje serijske komunikacije zbog problema sa “kočenjem” prikaza ali se ispostavilo da bi radilo i sa dinamičkim nizom. Izgleda da ne radi podjednako dobro na svim operativnim sistemima. Evo deo koda koji prima i priprema paket za procesiranje.
self.capacity = 100
self.buffer = bytearray(self.capacity)
self.buf_len = 0
...
def _read_loop(self):
while self._running:
raw_data = self.ser.read(self.ser.in_waiting or 1)
if raw_data:
self._write_to_buffer(raw_data)
self._process_buffer()
def _write_to_buffer(self, data: bytes):
data_len = len(data)
free_space = self.capacity - self.buf_len
if data_len > free_space:
if self.buf_len > 0:
self.buffer[:self.buf_len] = self.buffer[self.capacity - self.buf_len:self.capacity]
free_space = self.capacity - self.buf_len
if data_len > free_space:
data = data[:free_space]
data_len = len(data)
self.buffer[self.buf_len:self.buf_len+data_len] = data
self.buf_len += data_len
def _process_buffer(self):
packet_length = 1 + self.raw_data_length
i = 0
while i < self.buf_len:
if self.buffer[i] == self.sync_byte[0]:
if self.buf_len - i >= packet_length:
packet_view = memoryview(self.buffer)[i+1:i+packet_length]
if self.packet_callback:
self.packet_callback(packet_view)
i += packet_length
continue
else:
break
else:
i += 1
if i > 0:
remaining = self.buf_len - i
self.buffer[:remaining] = self.buffer[i:self.buf_len]
self.buf_len = remaining
...
Kad je paket u celosti prihvacen poziva se callaback koji procesira podatka (za sada nista posebno zanimljivo) i u glavnom threadu iscrta prikaz preko pyGame. Obrada padataka iz paketa:
...
self.ACCEL_SCALE = 16384.0 # Za ±2g
self.GYRO_SCALE = 131.0 # Za ±250°/s
...
# Parsiranje raw podataka
accel_x = self._bytes_to_int16_s(packet[0], packet[1])
accel_y = self._bytes_to_int16_s(packet[2], packet[3])
accel_z = self._bytes_to_int16_s(packet[4], packet[5])
temp = self._bytes_to_int16_s(packet[6], packet[7])
gyro_x = self._bytes_to_int16_s(packet[8], packet[9])
gyro_y = self._bytes_to_int16_s(packet[10], packet[11])
gyro_z = self._bytes_to_int16_s(packet[12], packet[13])
self.time_diff = self._bytes_to_uint16(packet[14],packet[15])
# Konverzija u stvarne vrednosti
self.accel_x_g = accel_x / self.ACCEL_SCALE
self.accel_y_g = accel_y / self.ACCEL_SCALE
self.accel_z_g = accel_z / self.ACCEL_SCALE
self.temp_c = (temp / 340.0) + 36.53
self.gyro_x_dps = gyro_x / self.GYRO_SCALE
self.gyro_y_dps = gyro_y / self.GYRO_SCALE
self.gyro_z_dps = gyro_z / self.GYRO_SCALE
...
Konačni rezultat je aplikacija koja u realnom vremenu crta vrednosti ovih 6 parametara
Izvorni programski kod ovog resenja -> ovde
Sledeći korak bi trebalo da bude kalibracija ćiroskopa i akcelerometra, a zatim proba nekomod metoda fuzije podataka sa senzora da dobijem upotrebljive vrednosti za orjentaciju u prostoru.
