Hayrapetyan/project_inertial-control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update lidar reader (baud 57600), filter_lid with quality, gyro bias calibration

Browse Source
This commit is contained in:
Hayrapetyan 2026-06-16 16:04:20 +03:00
parent 80e2eee87d
commit 7bae553ad7
9 changed files with 1362 additions and 469 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
imu_module/imu_reader.py
View File

@ -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),

8
lidar_module/lidar_reader.py
View File

@ -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 (?, ?, ?, ?)',

6
processing/calculations Executable file
View File

@ -0,0 +1,6 @@
#!/bin/bash
cd /home/grant/inertial_control/processing
./calc_acc &
./calc_gyro &
./calc_lid &
./calc_main

149
processing/filter_lid.cpp Normal file
View File

@ -0,0 +1,149 @@
#define _USE_MATH_DEFINES
#include <iostream>
#include <vector>
#include <cmath>
#include <unistd.h>
#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<LidarPoint> readLidar(sqlite3* db, double last_timestamp) {
vector<LidarPoint> 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<LidarPoint> 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;
}

821
processing/final_server_auto.py
View File

@ -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 = """
<!DOCTYPE html>
<html>
<head>
<title>Inertial Tracker — Реальное время</title>
<script src="plotly-latest.min.js"></script>
<style>
* { box-sizing: border-box; }
body { margin:0; background:#111827; font-family: 'Courier New', monospace; }
#topbar {
position: fixed; top: 0; left: 0; right: 0; height: 48px;
background: #1f2937; border-bottom: 1px solid #374151;
display: flex; align-items: center; padding: 0 16px; gap: 24px;
z-index: 200;
}
#topbar .title { color: #f9fafb; font-size: 15px; font-weight: bold; letter-spacing: 1px; }
#topbar .badge {
padding: 3px 10px; border-radius: 12px; font-size: 12px; font-weight: bold;
}
.badge-moving { background: #065f46; color: #6ee7b7; }
.badge-still { background: #1e3a5f; color: #93c5fd; }
.badge-nodata { background: #3f2a2a; color: #fca5a5; }
#topbar .dist { color: #9ca3af; font-size: 13px; }
#topbar .dist span { color: #e5e7eb; }
#coords {
position: fixed; top: 60px; right: 12px;
background: #1f2937; color: #6ee7b7;
padding: 12px 16px; font-size: 17px; z-index: 100;
border-radius: 10px; border: 1px solid #374151;
line-height: 1.8;
}
#legend {
position: fixed; top: 60px; left: 12px;
background: #1f2937; color: #d1d5db;
padding: 10px 14px; font-size: 13px; z-index: 100;
border-radius: 10px; border: 1px solid #374151;
line-height: 2.0;
}
#hint {
position: fixed; bottom: 42px; left: 12px;
background: #1f2937; color: #6b7280;
padding: 7px 12px; font-size: 11px; z-index: 100;
border-radius: 8px; border: 1px solid #374151;
max-width: 260px; line-height: 1.5;
}
#status {
position: fixed; bottom: 10px; left: 12px;
background: #111827; color: #6b7280;
padding: 4px 10px; font-size: 11px; z-index: 100;
border-radius: 6px;
}
#plot { width: 100%; height: 100vh; padding-top: 48px; }
</style>
</head>
<body>
<div id="topbar">
<div class="title">INERTIAL TRACKER</div>
<div id="motion-badge" class="badge badge-nodata">Нет данных</div>
<div class="dist">Пройдено: <span id="dist-val">0.00</span> м</div>
<div class="dist">Время: <span id="time-val">0:00</span></div>
</div>
<div id="coords">X: —<br>Y: —<br>Z: —</div>
<div id="legend">
<span style="color:#ffffff; font-size:16px;">◆</span> Начальная точка (0, 0, 0)<br>
<span style="color:#f87171; font-size:14px;">━━</span> Траектория движения<br>
<span style="color:#60a5fa; font-size:10px;">●●●</span> Карта окружения (лидар)<br>
<span style="color:#34d399; font-size:16px;">●</span> Текущее положение
</div>
<div id="hint">
Оси X, Y, Z — перемещение в метрах.<br>
Масштаб одинаковый по всем осям.<br>
Вращайте сцену мышью.
</div>
<div id="status">Подключение...</div>
<div id="plot"></div>
<script>
let updateCount = 0;
let lastRange = null;
let totalDist = 0;
let lastPos = null;
let startTime = null;
let sessionTimer = null;
// Вычисляет равные диапазоны для всех трёх осей
// (одинаковый масштаб = квадратный куб, без растяжки по Z)
function squareRanges(allX, allY, allZ) {
function minMax(arr) {
if (!arr || arr.length === 0) return [0, 0];
let mn = arr[0], mx = arr[0];
for (let v of arr) { if (v < mn) mn = v; if (v > mx) mx = v; }
return [mn, mx];
}
const rx = minMax(allX), ry = minMax(allY), rz = minMax(allZ);
const spans = [rx[1]-rx[0], ry[1]-ry[0], rz[1]-rz[0]];
const maxSpan = Math.max(...spans, 0.5); // минимум 0.5 м
const half = maxSpan / 2 * 1.2; // запас 20%
function mid(r) { return (r[0] + r[1]) / 2; }
return [
[mid(rx) - half, mid(rx) + half],
[mid(ry) - half, mid(ry) + half],
[mid(rz) - half, mid(rz) + half]
];
}
function rangeChanged(newR, oldR) {
if (!oldR) return true;
for (let i = 0; i < newR.length; i++) {
if (Math.abs(newR[i][0] - oldR[i][0]) > 0.25) return true;
if (Math.abs(newR[i][1] - oldR[i][1]) > 0.25) return true;
}
return false;
}
function fmtTime(sec) {
const m = Math.floor(sec / 60), s = Math.floor(sec % 60);
return m + ':' + String(s).padStart(2, '0');
}
// Инициализация графика
Plotly.newPlot('plot', [
{
name: 'Начало',
x: [0], y: [0], z: [0],
mode: 'markers',
type: 'scatter3d',
marker: { size: 10, color: 'white', symbol: 'diamond',
line: { color: '#374151', width: 2 } }
},
{
name: 'Траектория',
x: [], y: [], z: [],
mode: 'lines',
type: 'scatter3d',
line: { color: '#f87171', width: 3 }
},
{
name: 'Лидар',
x: [], y: [], z: [],
mode: 'markers',
type: 'scatter3d',
marker: { size: 2, color: '#60a5fa', opacity: 0.5 }
},
{
name: 'Позиция',
x: [0], y: [0], z: [0],
mode: 'markers',
type: 'scatter3d',
marker: { size: 12, color: '#34d399', symbol: 'circle',
line: { color: '#6ee7b7', width: 2 } }
}
], {
scene: {
xaxis: { title: 'X (м)', color: '#9ca3af', gridcolor: '#374151', zerolinecolor: '#6b7280' },
yaxis: { title: 'Y (м)', color: '#9ca3af', gridcolor: '#374151', zerolinecolor: '#6b7280' },
zaxis: { title: 'Z (м)', color: '#9ca3af', gridcolor: '#374151', zerolinecolor: '#6b7280' },
bgcolor: '#111827',
aspectmode: 'cube',
camera: { eye: { x: 1.4, y: 1.4, z: 1.0 } }
},
paper_bgcolor: '#111827',
plot_bgcolor: '#111827',
showlegend: false,
margin: { l: 0, r: 0, b: 0, t: 0 }
});
async function update() {
try {
const r = await fetch('/api/data?t=' + Date.now());
if (!r.ok) throw new Error('HTTP ' + r.status);
const d = await r.json();
updateCount++;
const pos = d.pos;
const hasData = d.traj.x.length > 0;
// Координаты
document.getElementById('coords').innerHTML =
`X: ${pos.x.toFixed(3)} м<br>Y: ${pos.y.toFixed(3)} м<br>Z: ${pos.z.toFixed(3)} м`;
// Таймер сессии
if (hasData && !startTime) {
startTime = Date.now();
sessionTimer = setInterval(() => {
document.getElementById('time-val').textContent =
fmtTime((Date.now() - startTime) / 1000);
}, 1000);
}
// Пройденное расстояние
if (lastPos && hasData) {
const dx = pos.x - lastPos.x,
dy = pos.y - lastPos.y,
dz = pos.z - lastPos.z;
const step = Math.sqrt(dx*dx + dy*dy + dz*dz);
if (step < 0.5) totalDist += step; // фильтр прыжков
}
lastPos = pos;
document.getElementById('dist-val').textContent = totalDist.toFixed(2);
// Статус движения
const badge = document.getElementById('motion-badge');
if (!hasData) {
badge.textContent = 'Нет данных';
badge.className = 'badge badge-nodata';
} else {
const spd = lastPos ? Math.sqrt(
(pos.x-lastPos.x)**2 + (pos.y-lastPos.y)**2 + (pos.z-lastPos.z)**2
) : 0;
if (totalDist > 0.02 && spd > 0.001) {
badge.textContent = '▶ В движении';
badge.className = 'badge badge-moving';
} else {
badge.textContent = '■ В покое';
badge.className = 'badge badge-still';
}
}
// Обновляем трассы
Plotly.restyle('plot', { x:[d.traj.x], y:[d.traj.y], z:[d.traj.z] }, 1);
Plotly.restyle('plot', { x:[d.lidar.x], y:[d.lidar.y], z:[d.lidar.z] }, 2);
Plotly.restyle('plot', { x:[[pos.x]], y:[[pos.y]], z:[[pos.z]] }, 3);
// Автоподгонка с одинаковым масштабом по всем осям
const allX = [0, pos.x].concat(d.traj.x);
const allY = [0, pos.y].concat(d.traj.y);
const allZ = [0, pos.z].concat(d.traj.z);
const newRange = squareRanges(allX, allY, allZ);
if (rangeChanged(newRange, lastRange)) {
lastRange = newRange;
Plotly.relayout('plot', {
'scene.xaxis.range': newRange[0],
'scene.yaxis.range': newRange[1],
'scene.zaxis.range': newRange[2]
});
}
document.getElementById('status').textContent =
`Обновлений: ${updateCount} | Траектория: ${d.traj.x.length} тч | Лидар: ${d.lidar.x.length} тч | ${new Date().toLocaleTimeString()}`;
} catch(e) {
document.getElementById('status').textContent = 'Ошибка: ' + e.message;
}
}
update();
setInterval(update, 200);
</script>
</body>
</html>
"""
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 = """
<!DOCTYPE html>
<html>
<head>
<title>Inertial Tracker — Реальное время</title>
<script src="plotly-latest.min.js"></script>
<style>
* { box-sizing: border-box; }
body { margin:0; background:#0f0f0f; font-family: 'Courier New', monospace; }
#topbar {
position: fixed; top: 0; left: 0; right: 0; height: 48px;
background: #181818; border-bottom: 1px solid #2a2a2a;
display: flex; align-items: center; padding: 0 16px; gap: 24px;
z-index: 200;
}
#topbar .badge {
padding: 3px 10px; border-radius: 12px; font-size: 12px; font-weight: bold;
}
.badge-moving { background: #065f46; color: #6ee7b7; }
.badge-still { background: #1e3a5f; color: #93c5fd; }
.badge-nodata { background: #3f2a2a; color: #fca5a5; }
#topbar .dist { color: #9ca3af; font-size: 13px; }
#topbar .dist span { color: #e5e7eb; }
#coords {
position: fixed; top: 60px; right: 12px;
background: #181818; color: #6ee7b7;
padding: 12px 16px; font-size: 17px; z-index: 100;
border-radius: 10px; border: 1px solid #2a2a2a;
line-height: 1.8;
}
#legend {
position: fixed; top: 60px; left: 12px;
background: #181818; color: #d1d5db;
padding: 10px 14px; font-size: 13px; z-index: 100;
border-radius: 10px; border: 1px solid #2a2a2a;
line-height: 2.0;
}
#status {
position: fixed; bottom: 6px; left: 12px;
background: #0f0f0f; color: #4b5563;
padding: 4px 10px; font-size: 11px; z-index: 100;
border-radius: 6px;
}
#main-wrap {
position: fixed; top: 48px; left: 0; right: 0; bottom: 0;
display: flex; flex-direction: column;
}
#plot3d { flex: 0 0 62%; width: 100%; }
#plots2d {
flex: 1 1 0; display: flex; flex-direction: row;
border-top: 1px solid #2a2a2a;
}
.plot2d { flex: 1 1 0; min-width: 0; }
.plot2d:not(:last-child) { border-right: 1px solid #2a2a2a; }
</style>
</head>
<body>
<div id="topbar">
<div id="motion-badge" class="badge badge-nodata">Нет данных</div>
<div class="dist">Пройдено: <span id="dist-val">0.00</span> м</div>
<div class="dist">Время: <span id="time-val">0:00</span></div>
</div>
<div id="coords">X: —<br>Y: —<br>Z: —</div>
<div id="legend">
<span style="color:#ffffff; font-size:16px;">◆</span> Начальная точка (0, 0, 0)<br>
<span style="color:#f87171; font-size:14px;">━━</span> Траектория движения<br>
<span style="color:#60a5fa; font-size:10px;">●●●</span> Карта окружения (лидар)<br>
<span style="color:#ffffff;">●</span> Положение робота<br>
<span style="color:#fbbf24;">━━</span> Направление (жёлтый луч)
</div>
<div id="status">Подключение...</div>
<div id="main-wrap">
<div id="plot3d"></div>
<div id="plots2d">
<div id="plot-xy" class="plot2d"></div>
<div id="plot-yz" class="plot2d"></div>
<div id="plot-zx" class="plot2d"></div>
</div>
</div>
<script>
let updateCount = 0;
let lastRange = null;
let totalDist = 0;
let lastPos = null;
let startTime = null;
let sessionTimer = null;
const BG = '#0f0f0f';
const GRID = '#1e1e1e';
const ZERO = '#2a2a2a';
const AXIS_COLOR = '#555';
const ARROW_LEN = 0.5; // длина жёлтого луча (м)
// ── Матрица полного поворота ─────────────────────────────────────────
function rotFull(roll, pitch, yaw) {
const cr=Math.cos(roll), sr=Math.sin(roll);
const cp=Math.cos(pitch), sp=Math.sin(pitch);
const cy=Math.cos(yaw), sy=Math.sin(yaw);
return [
[cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr],
[sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr],
[-sp, cp*sr, cp*cr]
];
}
// Применение матрицы поворота к вектору
function applyRot(R, v) {
return [
R[0][0]*v[0] + R[0][1]*v[1] + R[0][2]*v[2],
R[1][0]*v[0] + R[1][1]*v[1] + R[1][2]*v[2],
R[2][0]*v[0] + R[2][1]*v[1] + R[2][2]*v[2]
];
}
// ── Вспомогательные функции ──────────────────────────────────────────
function minMax(arr) {
if (!arr || !arr.length) return [0,0];
let mn=arr[0], mx=arr[0];
for (const v of arr) { if(v<mn)mn=v; if(v>mx)mx=v; }
return [mn, mx];
}
function squareRanges(ax, ay, az) {
const rx=minMax(ax), ry=minMax(ay), rz=minMax(az);
const maxSpan=Math.max(rx[1]-rx[0],ry[1]-ry[0],rz[1]-rz[0],0.5);
const half=maxSpan/2*1.2;
const mid=r=>(r[0]+r[1])/2;
return [[mid(rx)-half,mid(rx)+half],[mid(ry)-half,mid(ry)+half],[mid(rz)-half,mid(rz)+half]];
}
function rangeChanged(nR, oR) {
if (!oR) return true;
for (let i=0;i<nR.length;i++) {
if(Math.abs(nR[i][0]-oR[i][0])>0.25||Math.abs(nR[i][1]-oR[i][1])>0.25) return true;
}
return false;
}
function square2D(ax, ay) {
const rx=minMax(ax), ry=minMax(ay);
const maxSpan=Math.max(rx[1]-rx[0],ry[1]-ry[0],0.5);
const half=maxSpan/2*1.2;
const mid=r=>(r[0]+r[1])/2;
return [[mid(rx)-half,mid(rx)+half],[mid(ry)-half,mid(ry)+half]];
}
function fmtTime(sec) {
const m=Math.floor(sec/60), s=Math.floor(sec%60);
return m+':'+String(s).padStart(2,'0');
}
const darkAxis3D = label => ({
title:label, color:AXIS_COLOR,
gridcolor:GRID, zerolinecolor:ZERO,
backgroundcolor: BG, showbackground: true
});
const darkAxis2D = label => ({
title:label, color:AXIS_COLOR,
gridcolor:GRID, zerolinecolor:ZERO,
tickfont:{color:AXIS_COLOR}
});
const darkLayout2D = (xL, yL) => ({
xaxis: darkAxis2D(xL), yaxis: darkAxis2D(yL),
paper_bgcolor: BG, plot_bgcolor: BG,
showlegend: false,
margin:{l:44,r:10,b:36,t:24},
font:{color:AXIS_COLOR, size:11}
});
// ── Инициализация 3D-графика ─────────────────────────────────────────
// Трейсы:
// 0 – начальная точка (белый ромб)
// 1 – траектория (красная линия)
// 2 – лидар (голубые точки)
// 3 – кружок – текущее положение (белый круг)
// 4 – жёлтый луч направления (линия)
Plotly.newPlot('plot3d', [
{
name:'Начало', x:[0],y:[0],z:[0],
mode:'markers', type:'scatter3d',
marker:{size:10,color:'white',symbol:'diamond',line:{color:'#2a2a2a',width:2}}
},
{
name:'Траектория', x:[],y:[],z:[],
mode:'lines', type:'scatter3d',
line:{color:'#f87171',width:3}
},
{
name:'Лидар', x:[],y:[],z:[],
mode:'markers', type:'scatter3d',
marker:{size:2,color:'#60a5fa',opacity:0.5}
},
{
name:'Положение', x:[0],y:[0],z:[0],
mode:'markers', type:'scatter3d',
marker:{size:12,color:'white',symbol:'circle',line:{color:'#fbbf24',width:2}}
},
{
name:'Направление', x:[0,0], y:[0,0], z:[0,0],
mode:'lines', type:'scatter3d',
line:{color:'#fbbf24',width:4},
hoverinfo:'none'
}
], {
scene: {
xaxis: darkAxis3D('X (м)'),
yaxis: darkAxis3D('Y (м)'),
zaxis: darkAxis3D('Z (м)'),
bgcolor: BG,
aspectmode: 'cube',
camera: {eye:{x:1.4,y:1.4,z:1.0}}
},
paper_bgcolor: BG,
plot_bgcolor: BG,
showlegend: false,
margin:{l:0,r:0,b:0,t:0}
}, {responsive:true});
// ── 2D проекции ───────────────────────────────────────────────────────
function init2D(divId, xL, yL) {
Plotly.newPlot(divId, [
{ x:[0],y:[0], mode:'markers', type:'scatter',
marker:{size:8,color:'white',symbol:'diamond',line:{color:'#2a2a2a',width:1}} },
{ x:[],y:[], mode:'lines', type:'scatter',
line:{color:'#f87171',width:2} },
{ x:[0],y:[0], mode:'markers', type:'scatter',
marker:{size:10,color:'#34d399',line:{color:'#6ee7b7',width:2}} }
], darkLayout2D(xL,yL), {responsive:true});
}
init2D('plot-xy','X (м)','Y (м)');
init2D('plot-yz','Y (м)','Z (м)');
init2D('plot-zx','Z (м)','X (м)');
// ── Главный цикл обновления ────────────────────────────────────────────
async function update() {
try {
const r = await fetch('/api/data?t='+Date.now());
if (!r.ok) throw new Error('HTTP '+r.status);
const d = await r.json();
updateCount++;
const pos = d.pos;
const hasData = d.traj.x.length > 0;
document.getElementById('coords').innerHTML =
`X: ${pos.x.toFixed(3)} м<br>Y: ${pos.y.toFixed(3)} м<br>Z: ${pos.z.toFixed(3)} м`;
if (hasData && !startTime) {
startTime = Date.now();
sessionTimer = setInterval(()=>{
document.getElementById('time-val').textContent =
fmtTime((Date.now()-startTime)/1000);
},1000);
}
if (lastPos && hasData) {
const dx=pos.x-lastPos.x, dy=pos.y-lastPos.y, dz=pos.z-lastPos.z;
const step=Math.sqrt(dx*dx+dy*dy+dz*dz);
if (step<0.5) totalDist+=step;
}
lastPos = pos;
document.getElementById('dist-val').textContent = totalDist.toFixed(2);
const badge = document.getElementById('motion-badge');
if (!hasData) {
badge.textContent='Нет данных'; badge.className='badge badge-nodata';
} else if (totalDist>0.02) {
badge.textContent='▶ В движении'; badge.className='badge badge-moving';
} else {
badge.textContent='■ В покое'; badge.className='badge badge-still';
}
// Обновляем траекторию и лидар
Plotly.restyle('plot3d',{x:[d.traj.x],y:[d.traj.y],z:[d.traj.z]},1);
Plotly.restyle('plot3d',{x:[d.lidar.x],y:[d.lidar.y],z:[d.lidar.z]},2);
// ── Кружок (текущее положение) ────────────────────────────────
Plotly.restyle('plot3d',{x:[pos.x], y:[pos.y], z:[pos.z]}, 3);
// ── Жёлтый луч направления ────────────────────────────────────
const roll = d.orient.roll;
const pitch = d.orient.pitch;
const yaw = d.orient.yaw;
const R = rotFull(roll, pitch, yaw);
const dir = applyRot(R, [ARROW_LEN, 0, 0]); // вектор вперёд
const endX = pos.x + dir[0];
const endY = pos.y + dir[1];
const endZ = pos.z + dir[2];
Plotly.restyle('plot3d',{
x:[[pos.x, endX]], y:[[pos.y, endY]], z:[[pos.z, endZ]]
}, 4);
// Автомасштаб 3D
const allX=[0,pos.x].concat(d.traj.x);
const allY=[0,pos.y].concat(d.traj.y);
const allZ=[0,pos.z].concat(d.traj.z);
const newRange=squareRanges(allX,allY,allZ);
if (rangeChanged(newRange,lastRange)) {
lastRange=newRange;
Plotly.relayout('plot3d',{
'scene.xaxis.range':newRange[0],
'scene.yaxis.range':newRange[1],
'scene.zaxis.range':newRange[2]
});
}
// 2D проекции
const tx=d.traj.x, ty=d.traj.y, tz=d.traj.z;
Plotly.restyle('plot-xy',{x:[tx],y:[ty]},1);
Plotly.restyle('plot-xy',{x:[[pos.x]],y:[[pos.y]]},2);
const rXY=square2D([0,pos.x].concat(tx),[0,pos.y].concat(ty));
Plotly.relayout('plot-xy',{'xaxis.range':rXY[0],'yaxis.range':rXY[1]});
Plotly.restyle('plot-yz',{x:[ty],y:[tz]},1);
Plotly.restyle('plot-yz',{x:[[pos.y]],y:[[pos.z]]},2);
const rYZ=square2D([0,pos.y].concat(ty),[0,pos.z].concat(tz));
Plotly.relayout('plot-yz',{'xaxis.range':rYZ[0],'yaxis.range':rYZ[1]});
Plotly.restyle('plot-zx',{x:[tz],y:[tx]},1);
Plotly.restyle('plot-zx',{x:[[pos.z]],y:[[pos.x]]},2);
const rZX=square2D([0,pos.z].concat(tz),[0,pos.x].concat(tx));
Plotly.relayout('plot-zx',{'xaxis.range':rZX[0],'yaxis.range':rZX[1]});
document.getElementById('status').textContent =
`Обновлений: ${updateCount} | Traj: ${d.traj.x.length} тч | Лидар: ${d.lidar.x.length} тч | ${new Date().toLocaleTimeString()}`;
} catch(e) {
document.getElementById('status').textContent='Ошибка: '+e.message;
}
}
update();
setInterval(update, 200);
</script>
</body>
</html>
"""
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()

159
processing/simple_filter_acc.cpp Normal file
View File

@ -0,0 +1,159 @@
#include <iostream>
#include <vector>
#include <unistd.h>
#include "sqlite3.h"
using namespace std;
struct ImuSample {
double timestamp;
double ax, ay, az; // акселерометр (g)
double gx, gy, gz; // гироскоп (°/с)
};
// Чтение новых строк из таблицы imu_data после last_timestamp
vector<ImuSample> readAllImuData(sqlite3* db, double last_timestamp) {
vector<ImuSample> 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;
}

180
processing/simple_filter_gyro.cpp Normal file
View File

@ -0,0 +1,180 @@
#include <iostream>
#include <vector>
#include <unistd.h>
#include "sqlite3.h"
using namespace std;
struct ImuSample {
double timestamp;
double ax, ay, az; // акселерометр (g)
double gx, gy, gz; // гироскоп (°/с)
};
// Чтение новых строк из таблицы imu_data после last_timestamp
vector<ImuSample> readAllImuData(sqlite3* db, double last_timestamp) {
vector<ImuSample> 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<ImuSample> 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;
}

337
processing/the_main_block.cpp Normal file
View File

@ -0,0 +1,337 @@
#define _USE_MATH_DEFINES
#include <iostream>
#include <vector>
#include <cmath>
#include <unistd.h>
#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<AccSample> readAcc(sqlite3* db, double last_timestamp) {
vector<AccSample> 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<GyroSample> readGyro(sqlite3* db, double last_timestamp) {
vector<GyroSample> 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;
}

166
processing/visualization.py
View File

@ -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()
main()