diff --git a/imu_module/imu_reader.py b/imu_module/imu_reader.py index 3d3e8b5..12a5871 100644 --- a/imu_module/imu_reader.py +++ b/imu_module/imu_reader.py @@ -45,8 +45,9 @@ signal.signal(signal.SIGINT, shutdown) def init_db(): - conn = sqlite3.connect(DB_PATH) + conn = sqlite3.connect(DB_PATH, timeout=10) conn.execute('PRAGMA journal_mode=WAL') + conn.execute('PRAGMA busy_timeout=5000') conn.execute('PRAGMA synchronous=NORMAL') conn.execute('''CREATE TABLE IF NOT EXISTS imu_data ( timestamp REAL, @@ -111,7 +112,7 @@ def main(): continue # Реальное время от старта (монотонные секунды) - t = time.monotonic() - start_time + t = time.time() batch.append(( round(t, 4), diff --git a/lidar_module/lidar_reader.py b/lidar_module/lidar_reader.py index 01701db..8cce983 100644 --- a/lidar_module/lidar_reader.py +++ b/lidar_module/lidar_reader.py @@ -7,7 +7,7 @@ import os # Константы SERIAL_PORT = '/dev/ttyUSB0' -BAUD_RATE = 115200 +BAUD_RATE = 57600 DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db') # Заголовок пакета LDS02RR @@ -32,6 +32,8 @@ signal.signal(signal.SIGINT, shutdown) def init_db(): """Создать таблицу если не существует и очистить""" conn = sqlite3.connect(DB_PATH) + conn.execute("PRAGMA journal_mode=WAL") + conn.execute("PRAGMA synchronous=NORMAL") conn.execute('''CREATE TABLE IF NOT EXISTS lidar_data ( timestamp REAL, angle REAL, @@ -70,7 +72,7 @@ def save_to_db(): conn = init_db() try: - ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1) + ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=0.1) print("Подключено к лидару") except Exception as e: print(f"Ошибка подключения: {e}") @@ -92,7 +94,7 @@ def save_to_db(): packet = bytes(buffer[:PACKET_SIZE]) points = parse_packet(packet) if points: - t = round(time.time() - start_time, 3) + t = round(time.time(), 3) for angle, distance, quality in points: conn.execute( 'INSERT INTO lidar_data VALUES (?, ?, ?, ?)', diff --git a/processing/calculations b/processing/calculations new file mode 100755 index 0000000..9c1b640 --- /dev/null +++ b/processing/calculations @@ -0,0 +1,6 @@ +#!/bin/bash +cd /home/grant/inertial_control/processing +./calc_acc & +./calc_gyro & +./calc_lid & +./calc_main diff --git a/processing/filter_lid.cpp b/processing/filter_lid.cpp new file mode 100644 index 0000000..e242fc7 --- /dev/null +++ b/processing/filter_lid.cpp @@ -0,0 +1,149 @@ +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "sqlite3.h" + +using namespace std; + +// ------------------- НАСТРОЙКИ ФИЛЬТРАЦИИ ------------------- +static const double MIN_DIST_MM = 50.0; // минимальное расстояние (мм) +static const double MAX_DIST_MM = 16000.0; // максимальное расстояние (мм) +static const int MIN_QUALITY = 500; // минимальное качество сигнала +// ----------------------------------------------------------- + +struct LidarPoint { + double timestamp; + double angle; // градусы + double distance_mm; + int quality; +}; + +// Чтение последнего обработанного timestamp из lidar_points +double readLastTimestamp(sqlite3* db) { + double last_ts = 0.0; + sqlite3_stmt* stmt = nullptr; + if (sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM lidar_points", -1, &stmt, nullptr) == SQLITE_OK) { + if (sqlite3_step(stmt) == SQLITE_ROW && sqlite3_column_type(stmt, 0) != SQLITE_NULL) + last_ts = sqlite3_column_double(stmt, 0); + } + sqlite3_finalize(stmt); + return last_ts; +} + +// Чтение новых сырых данных лидара из БД после last_timestamp +vector readLidar(sqlite3* db, double last_timestamp) { + vector data; + sqlite3_stmt* stmt = nullptr; + const char* sql = "SELECT timestamp, angle, distance_mm, quality FROM lidar_data WHERE timestamp > ? ORDER BY timestamp ASC"; + if (sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr) != SQLITE_OK) { + cerr << "Ошибка подготовки запроса: " << sqlite3_errmsg(db) << endl; + return data; + } + sqlite3_bind_double(stmt, 1, last_timestamp); + while (sqlite3_step(stmt) == SQLITE_ROW) { + LidarPoint p; + p.timestamp = sqlite3_column_double(stmt, 0); + p.angle = sqlite3_column_double(stmt, 1); + p.distance_mm = sqlite3_column_double(stmt, 2); + p.quality = sqlite3_column_int(stmt, 3); + data.push_back(p); + } + sqlite3_finalize(stmt); + return data; +} + +int main() { + // 1. Открываем базу данных + sqlite3* db = nullptr; + if (sqlite3_open("/home/grant/inertial_control/inertial_data.db", &db) != SQLITE_OK) { + cerr << "Не удалось открыть БД: " << sqlite3_errmsg(db) << endl; + return 1; + } + + sqlite3_busy_timeout(db, 5000); + + // 2. Включаем WAL и синхронизацию (для производительности) + char* errMsg = nullptr; + sqlite3_exec(db, "PRAGMA journal_mode=WAL;", nullptr, nullptr, &errMsg); + sqlite3_exec(db, "PRAGMA synchronous=NORMAL;", nullptr, nullptr, &errMsg); + + // 3. Создаём таблицу lidar_points один раз при старте + sqlite3_exec(db, + "CREATE TABLE IF NOT EXISTS lidar_points (" + "timestamp REAL, angle REAL, distance_mm REAL, " + "x REAL, y REAL, quality INTEGER)", + nullptr, nullptr, &errMsg); + + // 4. Подготавливаем INSERT + sqlite3_stmt* insStmt; + const char* insertSql = "INSERT INTO lidar_points (timestamp, angle, distance_mm, x, y, quality) VALUES (?,?,?,?,?,?)"; + if (sqlite3_prepare_v2(db, insertSql, -1, &insStmt, nullptr) != SQLITE_OK) { + cerr << "Ошибка подготовки INSERT: " << sqlite3_errmsg(db) << endl; + sqlite3_close(db); + return 1; + } + + // 5. Бесконечный цикл — читаем только новые данные каждые 3 секунды + while (true) { + double last_ts = readLastTimestamp(db); + vector raw = readLidar(db, last_ts); + + if (raw.empty()) { + sleep(3); + continue; + } + + cout << "Новых точек из lidar_data: " << raw.size() << endl; + + int written = 0; + int filtered = 0; + + sqlite3_exec(db, "BEGIN TRANSACTION;", nullptr, nullptr, &errMsg); + + for (const auto& p : raw) { + // Фильтрация по расстоянию и качеству + if (p.distance_mm < MIN_DIST_MM) { filtered++; continue; } + if (p.distance_mm > MAX_DIST_MM) { filtered++; continue; } + if (p.quality < MIN_QUALITY) { filtered++; continue; } + + // Пересчёт в декартовы координаты (метры) + double angle_rad = p.angle * M_PI / 180.0; + double dist_m = p.distance_mm / 1000.0; + double x = dist_m * cos(angle_rad); + double y = dist_m * sin(angle_rad); + + // Привязываем значения + sqlite3_reset(insStmt); + sqlite3_bind_double(insStmt, 1, p.timestamp); + sqlite3_bind_double(insStmt, 2, p.angle); + sqlite3_bind_double(insStmt, 3, p.distance_mm); + sqlite3_bind_double(insStmt, 4, x); + sqlite3_bind_double(insStmt, 5, y); + sqlite3_bind_int(insStmt, 6, p.quality); + + // Выполняем вставку + int stepRes = sqlite3_step(insStmt); + if (stepRes == SQLITE_DONE) { + written++; + } + else { + cerr << "Ошибка вставки: " << sqlite3_errmsg(db) << endl; + } + } + + sqlite3_exec(db, "COMMIT;", nullptr, nullptr, &errMsg); + + cout << "=====================================" << endl; + cout << "Фильтрация: отброшено " << filtered << " точек" << endl; + cout << "Записано в lidar_points: " << written << " точек" << endl; + cout << "=====================================" << endl; + + sleep(3); + } + + sqlite3_finalize(insStmt); + sqlite3_close(db); + return 0; +} diff --git a/processing/final_server_auto.py b/processing/final_server_auto.py index 0495802..b6f8d49 100644 --- a/processing/final_server_auto.py +++ b/processing/final_server_auto.py @@ -1,368 +1,453 @@ -import sqlite3 -import json -import os -from http.server import HTTPServer, BaseHTTPRequestHandler - -DB_PATH = os.path.expanduser('~/inertial_control/inertial_data.db') -PORT = 8050 -TRAJ_LIMIT = 5000 # показываем последние 5000 точек траектории (~60 сек движения) -LIDAR_LIMIT = 2000 # последние точки лидара - -HTML = """ - - - - Inertial Tracker — Реальное время - - - - -
-
INERTIAL TRACKER
-
Нет данных
-
Пройдено: 0.00 м
-
Время: 0:00
-
- -
X: —
Y: —
Z: —
- -
- ◆ Начальная точка (0, 0, 0)
- ━━ Траектория движения
- ●●● Карта окружения (лидар)
- ● Текущее положение -
- -
- Оси X, Y, Z — перемещение в метрах.
- Масштаб одинаковый по всем осям.
- Вращайте сцену мышью. -
- -
Подключение...
-
- - - - -""" - - -def fetch_data(): - """Читает свежие данные из БД в read-only режиме (не мешает писателям).""" - conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True) - try: - # Текущая позиция - cur = conn.execute( - "SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT 1" - ) - row = cur.fetchone() - pos = {'x': row[0], 'y': row[1], 'z': row[2]} if row else {'x': 0.0, 'y': 0.0, 'z': 0.0} - - # Последние N точек траектории (разворачиваем в хронологический порядок) - cur = conn.execute( - f"SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT {TRAJ_LIMIT}" - ) - traj_rows = cur.fetchall()[::-1] - traj = { - 'x': [r[0] for r in traj_rows], - 'y': [r[1] for r in traj_rows], - 'z': [r[2] for r in traj_rows], - } - - # Последние N точек лидара - cur = conn.execute( - f"SELECT x, y, z FROM lidar_points ORDER BY rowid DESC LIMIT {LIDAR_LIMIT}" - ) - lidar_rows = cur.fetchall() - lidar = { - 'x': [r[0] for r in lidar_rows], - 'y': [r[1] for r in lidar_rows], - 'z': [r[2] for r in lidar_rows], - } - finally: - conn.close() - return {'pos': pos, 'traj': traj, 'lidar': lidar} - - -class Handler(BaseHTTPRequestHandler): - def log_message(self, format, *args): - pass - - def do_GET(self): - if self.path == '/': - self.send_response(200) - self.send_header('Content-type', 'text/html; charset=utf-8') - self.end_headers() - self.wfile.write(HTML.encode('utf-8')) - - elif self.path == '/plotly-latest.min.js': - self.send_response(200) - self.send_header('Content-type', 'application/javascript') - self.end_headers() - with open('plotly-latest.min.js', 'rb') as f: - self.wfile.write(f.read()) - - elif self.path.startswith('/api/data'): - try: - data = fetch_data() - payload = json.dumps(data).encode('utf-8') - self.send_response(200) - self.send_header('Content-type', 'application/json') - self.send_header('Cache-Control', 'no-store') - self.end_headers() - self.wfile.write(payload) - except Exception as e: - self.send_response(500) - self.end_headers() - self.wfile.write(str(e).encode('utf-8')) - - elif self.path.startswith('/api/pos'): - try: - conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True) - cur = conn.execute( - "SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT 1" - ) - row = cur.fetchone() - conn.close() - pos = {'x': row[0], 'y': row[1], 'z': row[2]} if row else {'x': 0, 'y': 0, 'z': 0} - self.send_response(200) - self.send_header('Content-type', 'application/json') - self.end_headers() - self.wfile.write(json.dumps(pos).encode('utf-8')) - except Exception as e: - self.send_response(500) - self.end_headers() - self.wfile.write(str(e).encode('utf-8')) - - else: - self.send_response(404) - self.end_headers() - - -if __name__ == '__main__': - print("=" * 50) - print("INERTIAL TRACKER — реальное время") - print(f"Открыть: http://192.168.0.106:{PORT}") - print("=" * 50) - HTTPServer(('0.0.0.0', PORT), Handler).serve_forever() +import sqlite3 +import json +import os +from http.server import HTTPServer, BaseHTTPRequestHandler + +DB_PATH = os.path.expanduser('~/inertial_control/inertial_data.db') +PORT = 8050 +TRAJ_LIMIT = 5000 +LIDAR_LIMIT = 2000 + +HTML = """ + + + + Inertial Tracker — Реальное время + + + + +
+
Нет данных
+
Пройдено: 0.00 м
+
Время: 0:00
+
+ +
X: —
Y: —
Z: —
+ +
+ ◆ Начальная точка (0, 0, 0)
+ ━━ Траектория движения
+ ●●● Карта окружения (лидар)
+ ● Положение робота
+ ━━ Направление (жёлтый луч) +
+ +
Подключение...
+ +
+
+
+
+
+
+
+
+ + + + +""" + + +def fetch_data(): + conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True) + try: + cur = conn.execute( + "SELECT x, y, z, roll, pitch, yaw FROM trajectory ORDER BY timestamp DESC LIMIT 1" + ) + row = cur.fetchone() + if row: + pos = {'x': row[0], 'y': row[1], 'z': row[2]} + orient = {'roll': row[3] or 0.0, 'pitch': row[4] or 0.0, 'yaw': row[5] or 0.0} + else: + pos = {'x': 0.0, 'y': 0.0, 'z': 0.0} + orient = {'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0} + + cur = conn.execute( + f"SELECT x, y, z FROM trajectory ORDER BY timestamp ASC LIMIT {TRAJ_LIMIT}" + ) + traj_rows = cur.fetchall()[::-1] + traj = { + 'x': [r[0] for r in traj_rows], + 'y': [r[1] for r in traj_rows], + 'z': [r[2] for r in traj_rows], + } + + cur = conn.execute( + f"SELECT x, y, z FROM lidar_points ORDER BY rowid DESC LIMIT {LIDAR_LIMIT}" + ) + lidar_rows = cur.fetchall() + lidar = { + 'x': [r[0] for r in lidar_rows], + 'y': [r[1] for r in lidar_rows], + 'z': [r[2] for r in lidar_rows], + } + finally: + conn.close() + return {'pos': pos, 'orient': orient, 'traj': traj, 'lidar': lidar} + + +class Handler(BaseHTTPRequestHandler): + def log_message(self, format, *args): + pass + + def do_GET(self): + if self.path == '/': + self.send_response(200) + self.send_header('Content-type', 'text/html; charset=utf-8') + self.end_headers() + self.wfile.write(HTML.encode('utf-8')) + + elif self.path == '/plotly-latest.min.js': + self.send_response(200) + self.send_header('Content-type', 'application/javascript') + self.end_headers() + with open('plotly-latest.min.js', 'rb') as f: + self.wfile.write(f.read()) + + elif self.path.startswith('/api/data'): + try: + data = fetch_data() + payload = json.dumps(data).encode('utf-8') + self.send_response(200) + self.send_header('Content-type', 'application/json') + self.send_header('Cache-Control', 'no-store') + self.end_headers() + self.wfile.write(payload) + except Exception as e: + self.send_response(500) + self.end_headers() + self.wfile.write(str(e).encode('utf-8')) + + elif self.path.startswith('/api/pos'): + try: + conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True) + cur = conn.execute( + "SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT 1" + ) + row = cur.fetchone() + conn.close() + pos = {'x':row[0],'y':row[1],'z':row[2]} if row else {'x':0,'y':0,'z':0} + self.send_response(200) + self.send_header('Content-type', 'application/json') + self.end_headers() + self.wfile.write(json.dumps(pos).encode('utf-8')) + except Exception as e: + self.send_response(500) + self.end_headers() + self.wfile.write(str(e).encode('utf-8')) + + else: + self.send_response(404) + self.end_headers() + + +if __name__ == '__main__': + print("=" * 50) + print("INERTIAL TRACKER — реальное время") + print(f"Открыть: http://192.168.0.106:{PORT}") + print("=" * 50) + HTTPServer(('0.0.0.0', PORT), Handler).serve_forever() \ No newline at end of file diff --git a/processing/simple_filter_acc.cpp b/processing/simple_filter_acc.cpp new file mode 100644 index 0000000..a1e9a8c --- /dev/null +++ b/processing/simple_filter_acc.cpp @@ -0,0 +1,159 @@ +#include +#include +#include +#include "sqlite3.h" + +using namespace std; + +struct ImuSample { + double timestamp; + double ax, ay, az; // акселерометр (g) + double gx, gy, gz; // гироскоп (°/с) +}; + +// Чтение новых строк из таблицы imu_data после last_timestamp +vector readAllImuData(sqlite3* db, double last_timestamp) { + vector samples; + const char* sql = "SELECT timestamp, ax, ay, az, gx, gy, gz FROM imu_data WHERE timestamp > ? ORDER BY timestamp ASC"; + + sqlite3_stmt* stmt = nullptr; + if (sqlite3_prepare_v2(db, sql, -1, & stmt, nullptr) != SQLITE_OK) { + std::cerr << "Ошибка подготовки запроса: " << sqlite3_errmsg(db) << std::endl; + return samples; + } + sqlite3_bind_double(stmt, 1, last_timestamp); + + while (sqlite3_step(stmt) == SQLITE_ROW) { + ImuSample s; + s.timestamp = sqlite3_column_double(stmt, 0); + s.ax = sqlite3_column_double(stmt, 1); + s.ay = sqlite3_column_double(stmt, 2); + s.az = sqlite3_column_double(stmt, 3); + s.gx = sqlite3_column_double(stmt, 4); + s.gy = sqlite3_column_double(stmt, 5); + s.gz = sqlite3_column_double(stmt, 6); + samples.push_back(s); + } + + sqlite3_finalize(stmt); + return samples; +} + +// Чтение последнего обработанного timestamp из filtered_acc_data +double readLastTimestamp(sqlite3* db) { + double last_ts = 0.0; + sqlite3_stmt* stmt = nullptr; + if (sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM filtered_acc_data", -1, &stmt, nullptr) == SQLITE_OK) { + if (sqlite3_step(stmt) == SQLITE_ROW && sqlite3_column_type(stmt, 0) != SQLITE_NULL) + last_ts = sqlite3_column_double(stmt, 0); + } + sqlite3_finalize(stmt); + return last_ts; +} + +// Запись одной строки (timestamp, ax, ay, az) в таблицу filtered_acc_data +void writeFilteredAccData(sqlite3* db, double timestamp, double ax, double ay, double az) { + // 1. Создаём таблицу, если она не существует + const char* create_sql = + "CREATE TABLE IF NOT EXISTS filtered_acc_data (" + "timestamp REAL, " + "ax REAL, " + "ay REAL, " + "az REAL)"; + char* err_msg = nullptr; + if (sqlite3_exec(db, create_sql, nullptr, nullptr, &err_msg) != SQLITE_OK) { + std::cerr << "Ошибка создания таблицы: " << err_msg << std::endl; + sqlite3_free(err_msg); + return; + } + + // 2. Подготавливаем INSERT + const char* insert_sql = "INSERT INTO filtered_acc_data (timestamp, ax, ay, az) VALUES (?, ?, ?, ?)"; + sqlite3_stmt* stmt = nullptr; + if (sqlite3_prepare_v2(db, insert_sql, -1, &stmt, nullptr) != SQLITE_OK) { + std::cerr << "Ошибка подготовки запроса: " << sqlite3_errmsg(db) << std::endl; + return; + } + + // 3. Привязываем значения + sqlite3_bind_double(stmt, 1, timestamp); + sqlite3_bind_double(stmt, 2, ax); + sqlite3_bind_double(stmt, 3, ay); + sqlite3_bind_double(stmt, 4, az); + + // 4. Выполняем + if (sqlite3_step(stmt) != SQLITE_DONE) { + std::cerr << "Ошибка вставки данных: " << sqlite3_errmsg(db) << std::endl; + } + + // 5. Очищаем + sqlite3_finalize(stmt); +} + + +int main() { + sqlite3* db = nullptr; + + if (sqlite3_open("/home/grant/inertial_control/inertial_data.db", &db) != SQLITE_OK) { + cerr << "Не удалось открыть БД" << endl; + return 1; + } + + sqlite3_busy_timeout(db, 5000); + + char* errMsg = nullptr; + if (sqlite3_exec(db, "PRAGMA journal_mode=WAL;", nullptr, nullptr, &errMsg) != SQLITE_OK) { + cerr << "Ошибка установки WAL режима: " << errMsg << std::endl; + sqlite3_free(errMsg); + } + else { + cout << "WAL режим успешно включен" << endl; + } + + // расчет bias + double bias_x = 0.0, bias_y = 0.0, bias_z = 0.0; + const int WIN_SIZE = 4; + + // Бесконечный цикл — читаем только новые данные каждые 3 секунды + while (true) { + double last_ts = readLastTimestamp(db); + auto all = readAllImuData(db, last_ts); + + if (all.empty()) { + sleep(3); + continue; + } + + cout << "Новых строк: " << all.size() << "\n"; + + // заполнения первичных значений + double acc_xf = 0.0, acc_yf = 0.0, acc_zf = 0.0; + for (size_t i = 0; i < WIN_SIZE && i < all.size(); ++i) { + acc_xf += all[i].ax; + acc_yf += all[i].ay; + acc_zf += all[i].az; + } + + // основной цикл сбора - расчета - записи + sqlite3_exec(db, "BEGIN;", nullptr, nullptr, nullptr); + for (size_t i = WIN_SIZE; i < all.size(); ++i) { + writeFilteredAccData(db, all[i].timestamp, + acc_xf / WIN_SIZE - bias_x, + acc_yf / WIN_SIZE - bias_y, + acc_zf / WIN_SIZE - bias_z); + acc_xf -= all[i - WIN_SIZE].ax; + acc_xf += all[i].ax; + acc_yf -= all[i - WIN_SIZE].ay; + acc_yf += all[i].ay; + acc_zf -= all[i - WIN_SIZE].az; + acc_zf += all[i].az; + } + sqlite3_exec(db, "COMMIT;", nullptr, nullptr, nullptr); + + cout << "записано в filtered_acc_data\n"; + sleep(3); + } + + sqlite3_close(db); + return 0; +} diff --git a/processing/simple_filter_gyro.cpp b/processing/simple_filter_gyro.cpp new file mode 100644 index 0000000..ba8cbe5 --- /dev/null +++ b/processing/simple_filter_gyro.cpp @@ -0,0 +1,180 @@ +#include +#include +#include +#include "sqlite3.h" + +using namespace std; + +struct ImuSample { + double timestamp; + double ax, ay, az; // акселерометр (g) + double gx, gy, gz; // гироскоп (°/с) +}; + +// Чтение новых строк из таблицы imu_data после last_timestamp +vector readAllImuData(sqlite3* db, double last_timestamp) { + vector samples; + const char* sql = "SELECT timestamp, ax, ay, az, gx, gy, gz FROM imu_data WHERE timestamp > ? ORDER BY timestamp ASC"; + + sqlite3_stmt* stmt = nullptr; + if (sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr) != SQLITE_OK) { + cerr << "Ошибка подготовки запроса: " << sqlite3_errmsg(db) << endl; + return samples; + } + sqlite3_bind_double(stmt, 1, last_timestamp); + + while (sqlite3_step(stmt) == SQLITE_ROW) { + ImuSample s; + s.timestamp = sqlite3_column_double(stmt, 0); + s.ax = sqlite3_column_double(stmt, 1); + s.ay = sqlite3_column_double(stmt, 2); + s.az = sqlite3_column_double(stmt, 3); + s.gx = sqlite3_column_double(stmt, 4); + s.gy = sqlite3_column_double(stmt, 5); + s.gz = sqlite3_column_double(stmt, 6); + samples.push_back(s); + } + + sqlite3_finalize(stmt); + return samples; +} + +// Чтение последнего обработанного timestamp из filtered_gyro_data +double readLastTimestamp(sqlite3* db) { + double last_ts = 0.0; + sqlite3_stmt* stmt = nullptr; + if (sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM filtered_gyro_data", -1, &stmt, nullptr) == SQLITE_OK) { + if (sqlite3_step(stmt) == SQLITE_ROW && sqlite3_column_type(stmt, 0) != SQLITE_NULL) + last_ts = sqlite3_column_double(stmt, 0); + } + sqlite3_finalize(stmt); + return last_ts; +} + +// Запись отфильтрованных угловых скоростей в таблицу filtered_gyro_data +void writeFilteredGyroData(sqlite3* db, double timestamp, double gx, double gy, double gz) { + // 1. Создаём таблицу, если она не существует + const char* create_sql = + "CREATE TABLE IF NOT EXISTS filtered_gyro_data (" + "timestamp REAL, " + "gx REAL, " + "gy REAL, " + "gz REAL)"; + char* err_msg = nullptr; + if (sqlite3_exec(db, create_sql, nullptr, nullptr, &err_msg) != SQLITE_OK) { + cerr << "Ошибка создания таблицы filtered_gyro_data: " << err_msg << endl; + sqlite3_free(err_msg); + return; + } + + // 2. Подготавливаем INSERT + const char* insert_sql = "INSERT INTO filtered_gyro_data (timestamp, gx, gy, gz) VALUES (?, ?, ?, ?)"; + sqlite3_stmt* stmt = nullptr; + if (sqlite3_prepare_v2(db, insert_sql, -1, &stmt, nullptr) != SQLITE_OK) { + cerr << "Ошибка подготовки INSERT для gyro: " << sqlite3_errmsg(db) << endl; + return; + } + + // 3. Привязываем значения + sqlite3_bind_double(stmt, 1, timestamp); + sqlite3_bind_double(stmt, 2, gx); + sqlite3_bind_double(stmt, 3, gy); + sqlite3_bind_double(stmt, 4, gz); + + // 4. Выполняем + if (sqlite3_step(stmt) != SQLITE_DONE) { + cerr << "Ошибка вставки gyro: " << sqlite3_errmsg(db) << endl; + } + + // 5. Очищаем + sqlite3_finalize(stmt); +} + +int main() { + sqlite3* db = nullptr; + if (sqlite3_open("/home/grant/inertial_control/inertial_data.db", &db) != SQLITE_OK) { + cerr << "Не удалось открыть БД" << endl; + return 1; + } + + sqlite3_busy_timeout(db, 5000); + + char* errMsg = nullptr; + if (sqlite3_exec(db, "PRAGMA journal_mode=WAL;", nullptr, nullptr, &errMsg) != SQLITE_OK) { + cerr << "Ошибка установки WAL режима: " << errMsg << endl; + sqlite3_free(errMsg); + } + else { + cout << "WAL режим успешно включен" << endl; + } + + // 1. Вычисление bias гироскопа по первым 200 выборкам + double gyro_bias_x = 0.0, gyro_bias_y = 0.0, gyro_bias_z = 0.0; + const int WIN_SIZE = 4; + const int BIAS_SAMPLES = 200; + { + cout << "Ожидание данных для вычисления bias гироскопа..." << endl; + vector init_data; + while (init_data.size() < BIAS_SAMPLES) { + init_data = readAllImuData(db, 0.0); + if ((int)init_data.size() < BIAS_SAMPLES) sleep(1); + } + int n = BIAS_SAMPLES; + for (int i = 0; i < n; i++) { + gyro_bias_x += init_data[i].gx; + gyro_bias_y += init_data[i].gy; + gyro_bias_z += init_data[i].gz; + } + gyro_bias_x /= n; + gyro_bias_y /= n; + gyro_bias_z /= n; + cout << "Bias гироскопа: gx=" << gyro_bias_x + << " gy=" << gyro_bias_y + << " gz=" << gyro_bias_z << endl; + } + + // Бесконечный цикл — читаем только новые данные каждые 3 секунды + while (true) { + double last_ts = readLastTimestamp(db); + auto all = readAllImuData(db, last_ts); + + if (all.empty()) { + sleep(3); + continue; + } + + cout << "Новых строк: " << all.size() << "\n"; + + // 2. Инициализация скользящих сумм для следующего окна + double gyro_xf = 0.0, gyro_yf = 0.0, gyro_zf = 0.0; + for (size_t i = 0; i < WIN_SIZE && i < all.size(); ++i) { + gyro_xf += all[i].gx; + gyro_yf += all[i].gy; + gyro_zf += all[i].gz; + } + + // 3. Основной цикл: вычисляем среднее в окне, вычитаем bias и записываем + sqlite3_exec(db, "BEGIN;", nullptr, nullptr, nullptr); + for (size_t i = WIN_SIZE; i < all.size(); ++i) { + // Записываем отфильтрованные данные + writeFilteredGyroData(db, all[i].timestamp, + gyro_xf / WIN_SIZE - gyro_bias_x, + gyro_yf / WIN_SIZE - gyro_bias_y, + gyro_zf / WIN_SIZE - gyro_bias_z); + + gyro_xf -= all[i - WIN_SIZE].gx; + gyro_xf += all[i].gx; + gyro_yf -= all[i - WIN_SIZE].gy; + gyro_yf += all[i].gy; + gyro_zf -= all[i - WIN_SIZE].gz; + gyro_zf += all[i].gz; + } + sqlite3_exec(db, "COMMIT;", nullptr, nullptr, nullptr); + + cout << "записано в filtered_gyro_data." << endl; + sleep(3); + } + + sqlite3_close(db); + return 0; +} diff --git a/processing/the_main_block.cpp b/processing/the_main_block.cpp new file mode 100644 index 0000000..5049057 --- /dev/null +++ b/processing/the_main_block.cpp @@ -0,0 +1,337 @@ +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "sqlite3.h" + +using namespace std; + +static const double G_MS2 = 9.81; // g -> м/с² +static const double DEG2RAD = M_PI / 180.0; + + +class Kalman1D { +private: + double x; // положение + double v; // скорость + double P[2][2]; // ковариационная матрица 2x2 + double Q; // интенсивность шума процесса (ускорение) + double R; // дисперсия шума измерения + double dt; // временной шаг + +public: + Kalman1D(double q_accel, double r_measure, double init_x = 0.0, double init_v = 0.0, double dt0 = 0.01) + : Q(q_accel), R(r_measure), dt(dt0), x(init_x), v(init_v) + { + P[0][0] = 1.0; + P[0][1] = 0.0; + P[1][0] = 0.0; + P[1][1] = 100.0; + } + + void setDt(double new_dt) { dt = new_dt; } + + void predict() { + double new_x = x + v * dt; + double new_v = v; + + double P00 = P[0][0] + dt * (P[1][0] + P[0][1]) + dt * dt * P[1][1]; + double P01 = P[0][1] + dt * P[1][1]; + double P10 = P[1][0] + dt * P[1][1]; + double P11 = P[1][1]; + + double Qv = Q * dt; + double Qp = Q * dt * dt * dt / 3.0; + double Qpv = Q * dt * dt / 2.0; + + P00 += Qp; + P01 += Qpv; + P10 += Qpv; + P11 += Qv; + + P[0][0] = P00; P[0][1] = P01; + P[1][0] = P10; P[1][1] = P11; + + x = new_x; + v = new_v; + } + + double update(double measurement) { + double y = measurement - x; + double S = P[0][0] + R; + double K_pos = P[0][0] / S; + double K_vel = P[1][0] / S; + + x += K_pos * y; + v += K_vel * y; + + double P00_new = (1.0 - K_pos) * P[0][0]; + double P01_new = (1.0 - K_pos) * P[0][1]; + double P11_new = P[1][1] - K_vel * P[1][0]; + + P[0][0] = P00_new; P[0][1] = P01_new; + P[1][0] = P01_new; P[1][1] = P11_new; + + return x; + } + + double getState() const { return x; } + double getVelocity() const { return v; } +}; + + +// ───────────────────────────────────────────── +// Структуры +// ───────────────────────────────────────────── +struct AccSample { + double timestamp; + double ax, ay, az; +}; + +struct GyroSample { + double timestamp; + double gx, gy, gz; +}; + +// ───────────────────────────────────────────── +// Чтение из БД +// ───────────────────────────────────────────── + +// Читаем последний timestamp и последнее состояние навигации из trajectory +double readLastTrajectoryState(sqlite3* db, + double& px, double& py, double& pz, + double& vx, double& vy, double& vz, + double& roll, double& pitch, double& yaw) +{ + double last_ts = 0.0; + sqlite3_stmt* stmt = nullptr; + if (sqlite3_prepare_v2(db, + "SELECT timestamp, x, y, z, vx, vy, vz, roll, pitch, yaw " + "FROM trajectory ORDER BY timestamp DESC LIMIT 1", + -1, &stmt, nullptr) == SQLITE_OK) + { + if (sqlite3_step(stmt) == SQLITE_ROW) { + last_ts = sqlite3_column_double(stmt, 0); + px = sqlite3_column_double(stmt, 1); + py = sqlite3_column_double(stmt, 2); + pz = sqlite3_column_double(stmt, 3); + vx = sqlite3_column_double(stmt, 4); + vy = sqlite3_column_double(stmt, 5); + vz = sqlite3_column_double(stmt, 6); + roll = sqlite3_column_double(stmt, 7); + pitch = sqlite3_column_double(stmt, 8); + yaw = sqlite3_column_double(stmt, 9); + } + } + sqlite3_finalize(stmt); + return last_ts; +} + +vector readAcc(sqlite3* db, double last_timestamp) { + vector data; + sqlite3_stmt* stmt = nullptr; + sqlite3_prepare_v2(db, + "SELECT timestamp, ax, ay, az " + "FROM filtered_acc_data WHERE timestamp > ? ORDER BY timestamp ASC", + -1, &stmt, nullptr); + sqlite3_bind_double(stmt, 1, last_timestamp); + while (sqlite3_step(stmt) == SQLITE_ROW) { + AccSample s; + s.timestamp = sqlite3_column_double(stmt, 0); + s.ax = sqlite3_column_double(stmt, 1); + s.ay = sqlite3_column_double(stmt, 2); + s.az = sqlite3_column_double(stmt, 3); + data.push_back(s); + } + sqlite3_finalize(stmt); + return data; +} + +vector readGyro(sqlite3* db, double last_timestamp) { + vector data; + sqlite3_stmt* stmt = nullptr; + sqlite3_prepare_v2(db, + "SELECT timestamp, gx, gy, gz " + "FROM filtered_gyro_data WHERE timestamp > ? ORDER BY timestamp ASC", + -1, &stmt, nullptr); + sqlite3_bind_double(stmt, 1, last_timestamp); + while (sqlite3_step(stmt) == SQLITE_ROW) { + GyroSample s; + s.timestamp = sqlite3_column_double(stmt, 0); + s.gx = sqlite3_column_double(stmt, 1); + s.gy = sqlite3_column_double(stmt, 2); + s.gz = sqlite3_column_double(stmt, 3); + data.push_back(s); + } + sqlite3_finalize(stmt); + return data; +} + +// ───────────────────────────────────────────── +// Запись в БД +// ───────────────────────────────────────────── +void createTable(sqlite3* db) { + char* err = nullptr; + sqlite3_exec(db, + "CREATE TABLE IF NOT EXISTS trajectory (" + "timestamp REAL, " + "x REAL, y REAL, z REAL, " + "vx REAL, vy REAL, vz REAL, " + "roll REAL, pitch REAL, yaw REAL)", + nullptr, nullptr, &err); +} + +void writePoint(sqlite3_stmt* stmt, + double ts, + double x, double y, double z, + double vx, double vy, double vz, + double roll, double pitch, double yaw) +{ + sqlite3_reset(stmt); + sqlite3_bind_double(stmt, 1, ts); + sqlite3_bind_double(stmt, 2, x); + sqlite3_bind_double(stmt, 3, y); + sqlite3_bind_double(stmt, 4, z); + sqlite3_bind_double(stmt, 5, vx); + sqlite3_bind_double(stmt, 6, vy); + sqlite3_bind_double(stmt, 7, vz); + sqlite3_bind_double(stmt, 8, roll); + sqlite3_bind_double(stmt, 9, pitch); + sqlite3_bind_double(stmt, 10, yaw); + sqlite3_step(stmt); +} + +// ───────────────────────────────────────────── +// main +// ───────────────────────────────────────────── +int main() +{ + sqlite3* db = nullptr; + if (sqlite3_open("/home/grant/inertial_control/inertial_data.db", &db) != SQLITE_OK) { + cerr << "Не удалось открыть БД" << endl; + return 1; + } + + sqlite3_busy_timeout(db, 5000); + sqlite3_exec(db, "PRAGMA journal_mode=WAL;", nullptr, nullptr, nullptr); + + createTable(db); + + // Начальные условия — читаем из последней точки trajectory + double px = 0.0, py = 0.0, pz = 0.0; + double vx = 0.0, vy = 0.0, vz = 0.0; + double roll = 0.0, pitch = 0.0, yaw = 0.0; + double last_ts = readLastTrajectoryState(db, px, py, pz, vx, vy, vz, roll, pitch, yaw); + + // Инициализация фильтров Калмана один раз при старте + Kalman1D kalmanAx(0.01, 0.1, 0.0, 0.0); + Kalman1D kalmanAy(0.01, 0.1, 0.0, 0.0); + Kalman1D kalmanAz(0.01, 0.1, 0.0, 0.0); + Kalman1D kalmanGx(0.001, 0.01, 0.0, 0.0); + Kalman1D kalmanGy(0.001, 0.01, 0.0, 0.0); + Kalman1D kalmanGz(0.001, 0.01, 0.0, 0.0); + + sqlite3_stmt* ins = nullptr; + sqlite3_prepare_v2(db, + "INSERT INTO trajectory " + "(timestamp,x,y,z,vx,vy,vz,roll,pitch,yaw) " + "VALUES (?,?,?,?,?,?,?,?,?,?)", + -1, &ins, nullptr); + + // Бесконечный цикл — читаем только новые данные каждые 3 секунды + while (true) { + last_ts = readLastTrajectoryState(db, px, py, pz, vx, vy, vz, roll, pitch, yaw); + + auto accData = readAcc(db, last_ts); + auto gyroData = readGyro(db, last_ts); + + if (accData.empty() || gyroData.empty()) { + sleep(3); + continue; + } + + size_t N = min(accData.size(), gyroData.size()); + cout << "Новых: acc=" << accData.size() << " gyro=" << gyroData.size() << endl; + + sqlite3_exec(db, "BEGIN TRANSACTION;", nullptr, nullptr, nullptr); + size_t written = 0; + + for (size_t i = 1; i < N; i++) { + double dt = 0.01; + if (dt <= 0.0 || dt > 0.5) dt = 0.01; + + kalmanAx.setDt(dt); + kalmanAy.setDt(dt); + kalmanAz.setDt(dt); + kalmanGx.setDt(dt); + kalmanGy.setDt(dt); + kalmanGz.setDt(dt); + + // Предсказание и обновление для акселерометра + kalmanAx.predict(); kalmanAx.update(accData[i].ax); + kalmanAy.predict(); kalmanAy.update(accData[i].ay); + kalmanAz.predict(); kalmanAz.update(accData[i].az); + + // Предсказание и обновление для гироскопа + kalmanGx.predict(); kalmanGx.update(gyroData[i].gx); + kalmanGy.predict(); kalmanGy.update(gyroData[i].gy); + kalmanGz.predict(); kalmanGz.update(gyroData[i].gz); + + // Получаем отфильтрованные значения + double ax_f = kalmanAx.getState(); + double ay_f = kalmanAy.getState(); + double az_f = kalmanAz.getState(); + double gx_f = kalmanGx.getState(); + double gy_f = kalmanGy.getState(); + double gz_f = kalmanGz.getState(); + + // Перевод в м/с² + double ax_ms2 = ax_f * G_MS2; + double ay_ms2 = ay_f * G_MS2; + double az_ms2 = az_f * G_MS2; + + // Интегрирование углов (используем отфильтрованные угловые скорости) + roll += gx_f * dt; + pitch += gy_f * dt; + yaw += gz_f * dt; + + double Gx = G_MS2 * sin(pitch * DEG2RAD); + double Gy = G_MS2 * cos(pitch * DEG2RAD) * sin(roll * DEG2RAD) + fmod(sin(yaw * DEG2RAD) * G_MS2, sqrt(2)); + double Gz = G_MS2 * cos(pitch * DEG2RAD) * cos(roll * DEG2RAD) - fmod(sin(yaw * DEG2RAD) * G_MS2, sqrt(2)); + + ax_ms2 -= Gx; + ay_ms2 += Gy; + az_ms2 -= Gz; + + // Интегрирование скорости: v += a * dt + vx += ax_ms2 * dt; + vy += ay_ms2 * dt; + vz += az_ms2 * dt; + + // Интегрирование позиции: p += v * dt + px += vx * dt; + py += vy * dt; + pz += vz * dt; + + // Запись + writePoint(ins, + accData[i].timestamp, + px, py, pz, + vx, vy, vz, + roll, pitch, yaw); + written++; + } + + sqlite3_exec(db, "COMMIT;", nullptr, nullptr, nullptr); + + cout << "Готово. Записано " << written << " точек в trajectory." << endl; + cout << "Позиция в метрах, углы в градусах." << endl; + + sleep(3); + } + + sqlite3_finalize(ins); + sqlite3_close(db); + return 0; +} diff --git a/processing/visualization.py b/processing/visualization.py index e965d12..e0d280a 100644 --- a/processing/visualization.py +++ b/processing/visualization.py @@ -1,122 +1,96 @@ -import open3d as o3d -import numpy as np +import matplotlib.pyplot as plt +import matplotlib.animation as animation +from mpl_toolkits.mplot3d import Axes3D import sqlite3 -import signal -import sys import os -import time +import numpy as np -DB_PATH = '../inertial_data.db' +DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db') -# ===== ЗАВЕРШЕНИЕ ВСЕГО ПРОЕКТА ===== +TRAJECTORY_LIMIT = 5000 +LIDAR_LIMIT = 2000 -def shutdown_all(): - """Останавливает все процессы проекта""" - os.system("pkill -f imu_reader.py") - os.system("pkill -f lidar_reader.py") - os.system("pkill -f calculations") -# ===== ЧТЕНИЕ ИЗ SQLite ===== - -def read_trajectory(conn): - cursor = conn.execute( - "SELECT x, y, z, roll, pitch, yaw FROM trajectory ORDER BY timestamp" - ) - rows = cursor.fetchall() +def read_trajectory(): + # Новое соединение на каждый кадр — иначе видим устаревший снимок БД. + with sqlite3.connect(DB_PATH) as conn: + cursor = conn.execute( + "SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT ?", + (TRAJECTORY_LIMIT,) + ) + rows = cursor.fetchall() if not rows: - return np.zeros((0, 3)), np.zeros((0, 3)) - data = np.array(rows) - return data[:, :3], data[:, 3:] + return np.zeros((0, 3)) + return np.array(rows[::-1]) # от старых к новым -def read_lidar_points(conn): - cursor = conn.execute("SELECT x, y, z FROM lidar_points") - rows = cursor.fetchall() + +def read_lidar_points(): + with sqlite3.connect(DB_PATH) as conn: + cursor = conn.execute( + "SELECT x, y, z FROM lidar_points ORDER BY rowid DESC LIMIT ?", + (LIDAR_LIMIT,) + ) + rows = cursor.fetchall() if not rows: return np.zeros((0, 3)) return np.array(rows) -# ===== ПОВОРОТ ===== - -def get_rotation_matrix(roll, pitch, yaw): - Rx = np.array([ - [1, 0, 0], - [0, np.cos(roll), -np.sin(roll)], - [0, np.sin(roll), np.cos(roll)] - ]) - Ry = np.array([ - [np.cos(pitch), 0, np.sin(pitch)], - [0, 1, 0], - [-np.sin(pitch), 0, np.cos(pitch)] - ]) - Rz = np.array([ - [np.cos(yaw), -np.sin(yaw), 0], - [np.sin(yaw), np.cos(yaw), 0], - [0, 0, 1] - ]) - return Rz @ Ry @ Rx - -# ===== MAIN ===== def main(): - conn = sqlite3.connect(DB_PATH) + fig = plt.figure(figsize=(10, 8)) + ax = fig.add_subplot(111, projection='3d') - # Создаём окно Open3D - vis = o3d.visualization.Visualizer() - vis.create_window(window_name="Инерциальное управление", width=1280, height=720) + def on_close(event): + print("Окно визуализации закрыто.") - pcd = o3d.geometry.PointCloud() - line_set = o3d.geometry.LineSet() - frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2) + fig.canvas.mpl_connect('close_event', on_close) - vis.add_geometry(pcd) - vis.add_geometry(line_set) - vis.add_geometry(frame) + def update(frame): + ax.cla() + ax.set_title("Инерциальное управление") + ax.set_xlabel("X, м") + ax.set_ylabel("Y, м") + ax.set_zlabel("Z, м") - print("Визуализация запущена. Закройте окно для остановки проекта.") + trajectory = read_trajectory() + lidar_points = read_lidar_points() - try: - while True: - # Проверяем закрыто ли окно - if not vis.poll_events(): - print("Окно закрыто — останавливаем проект...") - shutdown_all() - break + # Точка старта — для ориентира всегда показываем + ax.scatter([0], [0], [0], c='black', s=80, marker='x', + label='Старт (0, 0, 0)') - # Читаем новые данные из БД - trajectory, angles = read_trajectory(conn) - lidar_points = read_lidar_points(conn) + if len(lidar_points) > 0: + ax.scatter( + lidar_points[:, 0], + lidar_points[:, 1], + lidar_points[:, 2], + c='blue', s=1, alpha=0.5, label='Лидар' + ) - # Обновляем облако точек лидара - if len(lidar_points) > 0: - pcd.points = o3d.utility.Vector3dVector(lidar_points) - vis.update_geometry(pcd) + if len(trajectory) > 1: + ax.plot( + trajectory[:, 0], + trajectory[:, 1], + trajectory[:, 2], + c='red', linewidth=2, label='Траектория' + ) + # Текущая позиция датчика — последняя точка траектории + ax.scatter( + [trajectory[-1, 0]], + [trajectory[-1, 1]], + [trajectory[-1, 2]], + c='green', s=100, label='Позиция (сейчас)' + ) - # Обновляем траекторию - if len(trajectory) > 1: - lines = [[i, i+1] for i in range(len(trajectory)-1)] - line_set.points = o3d.utility.Vector3dVector(trajectory) - line_set.lines = o3d.utility.Vector2iVector(lines) - vis.update_geometry(line_set) + ax.legend(loc='upper right') - # Обновляем позицию объекта - if len(trajectory) > 0: - pos = trajectory[-1] - roll, pitch, yaw = angles[-1] - R = get_rotation_matrix(roll, pitch, yaw) - frame_new = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2) - frame_new.rotate(R, center=(0, 0, 0)) - frame_new.translate(pos) - frame.vertices = frame_new.vertices - frame.triangles = frame_new.triangles - vis.update_geometry(frame) + ani = animation.FuncAnimation(fig, update, interval=200, cache_frame_data=False) - vis.update_renderer() + print("Визуализация запущена. Закройте окно для выхода.") + print("Чёрный крестик — точка старта (0, 0, 0).") + print("Зелёная точка — текущая позиция датчика.") + print("Красная линия — путь от старта до текущей позиции.") + plt.show() - # Обновляем каждые 200 мс - time.sleep(0.2) - finally: - vis.destroy_window() - conn.close() - -main() \ No newline at end of file +main()