Merge branch 'main' of http://31.128.43.79:3000/Hayrapetyan/project_inertial-control

imu_module/imu_reader.py

@@ -0,0 +1,68 @@
+import smbus2 #библиотека Python для работы с I2C шиной
+import time
+import csv
+import struct
+
+# Константы
+MPU_ADDRESS = 0x68
+I2C_BUS = 1
+
+# Регистры MPU-9250
+PWR_MGMT_1 = 0x6B      # управление питанием
+ACCEL_XOUT_H = 0x3B    # начало данных акселерометра
+GYRO_XOUT_H = 0x43     # начало данных гироскопа
+
+# Масштабные коэффициенты (диапазон ±2g и ±250°/с)
+ACCEL_SCALE = 16384.0  # LSB/g
+GYRO_SCALE = 131.0     # LSB/(°/с)
+
+def init_mpu(bus):
+    """Разбудить датчик"""
+    bus.write_byte_data(MPU_ADDRESS, PWR_MGMT_1, 0x00)
+    time.sleep(0.1)
+
+def read_raw(bus, reg):
+    """Читать 16-битное значение из двух регистров"""
+    high = bus.read_byte_data(MPU_ADDRESS, reg)
+    low = bus.read_byte_data(MPU_ADDRESS, reg + 1)
+    value = (high << 8) | low
+    # Перевод в знаковое число
+    if value > 32767:
+        value -= 65536
+    return value
+
+def read_imu(bus):
+    """Читать и вернуть данные акселерометра и гироскопа"""
+    ax = read_raw(bus, ACCEL_XOUT_H) / ACCEL_SCALE
+    ay = read_raw(bus, ACCEL_XOUT_H + 2) / ACCEL_SCALE
+    az = read_raw(bus, ACCEL_XOUT_H + 4) / ACCEL_SCALE
+
+    gx = read_raw(bus, GYRO_XOUT_H) / GYRO_SCALE
+    gy = read_raw(bus, GYRO_XOUT_H + 2) / GYRO_SCALE
+    gz = read_raw(bus, GYRO_XOUT_H + 4) / GYRO_SCALE
+
+    return ax, ay, az, gx, gy, gz
+
+def save_to_csv(filename, duration_seconds):
+    """Записать данные в CSV файл"""
+    bus = smbus2.SMBus(I2C_BUS)
+    init_mpu(bus)
+
+    with open(filename, 'w', newline='') as f:
+        writer = csv.writer(f)
+        writer.writerow(['timestamp', 'ax', 'ay', 'az', 'gx', 'gy', 'gz'])
+
+        t = 0.0
+        while t < duration_seconds:
+            ax, ay, az, gx, gy, gz = read_imu(bus)
+            writer.writerow([round(t, 3), 
+                           round(ax, 4), round(ay, 4), round(az, 4),
+                           round(gx, 4), round(gy, 4), round(gz, 4)])
+            t += 0.01
+            time.sleep(0.01)
+
+    bus.close()
+
+# Запуск — записываем 5 секунд
+save_to_csv('imu.csv', 5)
+print("Готово!")

lidar_module/lidar_reader.py

@@ -0,0 +1,87 @@
+import serial
+import csv
+import time
+import struct
+
+# Константы
+SERIAL_PORT = '/dev/ttyS0'  # UART порт на MangoPi
+BAUD_RATE = 230400          # скорость передачи LDS02RR
+DURATION = 5                # секунд записи
+
+# Заголовок пакета LDS02RR
+HEADER = 0xFA
+PACKET_SIZE = 22            # размер пакета в байтах
+
+def parse_packet(data):
+    """Парсить один пакет данных лидара"""
+    if len(data) < PACKET_SIZE:
+        return None
+    
+    if data[0] != HEADER:
+        return None
+
+    # Индекс угла (0xA0 = 0°, 0xF9 = 359°)
+    index = data[1] - 0xA0
+    if index < 0 or index > 89:
+        return None
+
+    # Скорость вращения
+    speed = struct.unpack_from('<H', data, 2)[0] / 64.0
+
+    points = []
+    for i in range(4):
+        offset = 4 + i * 4
+        distance_raw = struct.unpack_from('<H', data, offset)[0]
+        quality = struct.unpack_from('<H', data, offset + 2)[0] >> 8
+
+        distance = distance_raw / 4.0  # в мм
+        angle = index * 4 + i          # угол в градусах
+
+        points.append((angle, distance, quality))
+
+    return points
+
+def save_to_csv(filename, duration_seconds):
+    """Читать данные лидара и записывать в CSV"""
+    try:
+        ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
+        print(f"Подключено к {SERIAL_PORT}")
+    except Exception as e:
+        print(f"Ошибка подключения: {e}")
+        return
+
+    with open(filename, 'w', newline='') as f:
+        writer = csv.writer(f)
+        writer.writerow(['timestamp', 'angle', 'distance_mm', 'quality'])
+
+        start_time = time.time()
+        buffer = bytearray()
+
+        while time.time() - start_time < duration_seconds:
+            data = ser.read(PACKET_SIZE)
+            if not data:
+                continue
+
+            buffer.extend(data)
+
+            # Ищем заголовок пакета
+            while len(buffer) >= PACKET_SIZE:
+                if buffer[0] == HEADER:
+                    packet = bytes(buffer[:PACKET_SIZE])
+                    points = parse_packet(packet)
+
+                    if points:
+                        t = round(time.time() - start_time, 3)
+                        for angle, distance, quality in points:
+                            writer.writerow([t, angle, 
+                                           round(distance, 1), 
+                                           quality])
+                    buffer = buffer[PACKET_SIZE:]
+                else:
+                    buffer.pop(0)
+
+    ser.close()
+    print("Готово!")
+
+# Запуск
+save_to_csv('lidar.csv', DURATION)

run.sh

@@ -0,0 +1,22 @@
+#!/bin/bash
+echo "Запуск системы..."
+
+# Запуск модуля акселерометра
+echo "Запуск IMU модуля..."
+python3 imu_module/imu_reader.py &
+
+# Запуск модуля лидара
+echo "Запуск лидар модуля..."
+python3 lidar_module/lidar_reader.py &
+
+# Ждём пока данные запишутся
+wait
+
+# Компиляция и запуск расчётов
+echo "Запуск расчётов..."
+g++ processing/calculations.cpp -o processing/calculations
+./processing/calculations
+
+# Запуск визуализации
+echo "Запуск визуализации..."
+python3 processing/visualization.py