Фикс лидара (ttyUSB0, парсинг пакетов), новые расчёты (CF + ZUPT + bias), улучшенная визуализация
This commit is contained in:
parent
16a9462502
commit
80e2eee87d
|
|
@ -1,40 +1,53 @@
|
|||
"""
|
||||
IMU Reader для MPU-9250.
|
||||
Улучшения относительно прежней версии:
|
||||
- реальные timestamp (time.monotonic), а не счётчик
|
||||
- блочное чтение 14 байт за один I2C-запрос
|
||||
- пачечная запись в SQLite (commit раз в 5 семплов = ~50 мс)
|
||||
- WAL-режим для параллельного чтения
|
||||
- устойчивость к временным ошибкам I2C
|
||||
- точное удержание 100 Гц с компенсацией дрейфа цикла
|
||||
"""
|
||||
import smbus2
|
||||
import time
|
||||
import sqlite3
|
||||
import signal
|
||||
import sys
|
||||
import os
|
||||
|
||||
# Константы
|
||||
# === Конфигурация ===
|
||||
MPU_ADDRESS = 0x68
|
||||
I2C_BUS = 2
|
||||
DB_PATH = '../inertial_data.db'
|
||||
DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db')
|
||||
|
||||
# Регистры MPU-9250
|
||||
PWR_MGMT_1 = 0x6B
|
||||
ACCEL_XOUT_H = 0x3B
|
||||
GYRO_XOUT_H = 0x43
|
||||
|
||||
# Масштабные коэффициенты
|
||||
ACCEL_SCALE = 16384.0
|
||||
GYRO_SCALE = 131.0
|
||||
ACCEL_SCALE = 16384.0 # LSB/g для диапазона ±2g
|
||||
GYRO_SCALE = 131.0 # LSB/(°/с) для диапазона ±250°/с
|
||||
|
||||
# Частота и буферизация
|
||||
SAMPLE_PERIOD = 0.01 # 10 мс = 100 Гц
|
||||
COMMIT_EVERY_N = 5 # коммит раз в 5 семплов (~50 мс)
|
||||
|
||||
# Глобальные переменные для завершения
|
||||
running = True
|
||||
conn = None
|
||||
bus = None
|
||||
|
||||
|
||||
def shutdown(signum, frame):
|
||||
"""Обработчик сигнала завершения"""
|
||||
global running
|
||||
print("Завершение IMU модуля...")
|
||||
print("Завершение IMU модуля...", flush=True)
|
||||
running = False
|
||||
|
||||
# Перехватываем сигналы завершения
|
||||
|
||||
signal.signal(signal.SIGTERM, shutdown)
|
||||
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 imu_data (
|
||||
timestamp REAL,
|
||||
ax REAL, ay REAL, az REAL,
|
||||
|
|
@ -44,49 +57,96 @@ def init_db():
|
|||
conn.commit()
|
||||
return conn
|
||||
|
||||
|
||||
def init_mpu(bus):
|
||||
# Снимаем sleep-бит, MPU начинает работать
|
||||
bus.write_byte_data(MPU_ADDRESS, PWR_MGMT_1, 0x00)
|
||||
time.sleep(0.1)
|
||||
|
||||
def read_raw(bus, reg):
|
||||
high = bus.read_byte_data(MPU_ADDRESS, reg)
|
||||
low = bus.read_byte_data(MPU_ADDRESS, reg + 1)
|
||||
value = (high << 8) | low
|
||||
if value > 32767:
|
||||
value -= 65536
|
||||
return value
|
||||
|
||||
def to_signed(high, low):
|
||||
v = (high << 8) | low
|
||||
return v - 65536 if v > 32767 else v
|
||||
|
||||
|
||||
def read_imu(bus):
|
||||
ax = read_raw(bus, ACCEL_XOUT_H) / ACCEL_SCALE
|
||||
ay = read_raw(bus, ACCEL_XOUT_H + 2) / ACCEL_SCALE
|
||||
az = read_raw(bus, ACCEL_XOUT_H + 4) / ACCEL_SCALE
|
||||
gx = read_raw(bus, GYRO_XOUT_H) / GYRO_SCALE
|
||||
gy = read_raw(bus, GYRO_XOUT_H + 2) / GYRO_SCALE
|
||||
gz = read_raw(bus, GYRO_XOUT_H + 4) / GYRO_SCALE
|
||||
# Одним запросом читаем 14 байт: ax/ay/az(6) + TEMP(2) + gx/gy/gz(6)
|
||||
data = bus.read_i2c_block_data(MPU_ADDRESS, ACCEL_XOUT_H, 14)
|
||||
ax = to_signed(data[0], data[1]) / ACCEL_SCALE
|
||||
ay = to_signed(data[2], data[3]) / ACCEL_SCALE
|
||||
az = to_signed(data[4], data[5]) / ACCEL_SCALE
|
||||
# data[6], data[7] — температура, не используем
|
||||
gx = to_signed(data[8], data[9]) / GYRO_SCALE
|
||||
gy = to_signed(data[10], data[11]) / GYRO_SCALE
|
||||
gz = to_signed(data[12], data[13]) / GYRO_SCALE
|
||||
return ax, ay, az, gx, gy, gz
|
||||
|
||||
def save_to_db():
|
||||
global conn, bus, running
|
||||
|
||||
def main():
|
||||
global running
|
||||
conn = init_db()
|
||||
bus = smbus2.SMBus(I2C_BUS)
|
||||
init_mpu(bus)
|
||||
|
||||
print("Запись данных IMU...")
|
||||
t = 0.0
|
||||
print(f"IMU запущен. Целевая частота: {1/SAMPLE_PERIOD:.0f} Гц", flush=True)
|
||||
|
||||
start_time = time.monotonic()
|
||||
next_tick = start_time + SAMPLE_PERIOD
|
||||
batch = []
|
||||
|
||||
try:
|
||||
while running:
|
||||
ax, ay, az, gx, gy, gz = read_imu(bus)
|
||||
conn.execute(
|
||||
# Спим до следующего тика — без дрейфа цикла
|
||||
now = time.monotonic()
|
||||
sleep_time = next_tick - now
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
# Читаем датчик с защитой от случайных ошибок I2C
|
||||
try:
|
||||
ax, ay, az, gx, gy, gz = read_imu(bus)
|
||||
except OSError as e:
|
||||
print(f"IMU read error: {e}", flush=True)
|
||||
next_tick += SAMPLE_PERIOD
|
||||
continue
|
||||
|
||||
# Реальное время от старта (монотонные секунды)
|
||||
t = time.monotonic() - start_time
|
||||
|
||||
batch.append((
|
||||
round(t, 4),
|
||||
round(ax, 4), round(ay, 4), round(az, 4),
|
||||
round(gx, 4), round(gy, 4), round(gz, 4),
|
||||
))
|
||||
|
||||
# Пачечный коммит
|
||||
if len(batch) >= COMMIT_EVERY_N:
|
||||
conn.executemany(
|
||||
'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
|
||||
batch
|
||||
)
|
||||
conn.commit()
|
||||
batch.clear()
|
||||
|
||||
# Назначаем следующий тик
|
||||
next_tick += SAMPLE_PERIOD
|
||||
|
||||
# Если сильно отстали (например, после долгой паузы) — ресинк,
|
||||
# чтобы не было «штормового» догоняния.
|
||||
if time.monotonic() - next_tick > SAMPLE_PERIOD * 5:
|
||||
next_tick = time.monotonic() + SAMPLE_PERIOD
|
||||
finally:
|
||||
# Сбросим остаток батча
|
||||
if batch:
|
||||
conn.executemany(
|
||||
'INSERT INTO imu_data VALUES (?, ?, ?, ?, ?, ?, ?)',
|
||||
(round(t, 3), round(ax, 4), round(ay, 4), round(az, 4),
|
||||
round(gx, 4), round(gy, 4), round(gz, 4))
|
||||
batch
|
||||
)
|
||||
conn.commit()
|
||||
t += 0.01
|
||||
time.sleep(0.01)
|
||||
finally:
|
||||
bus.close()
|
||||
conn.close()
|
||||
print("IMU модуль остановлен")
|
||||
print("IMU модуль остановлен", flush=True)
|
||||
|
||||
save_to_db()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -3,11 +3,12 @@ import time
|
|||
import sqlite3
|
||||
import struct
|
||||
import signal
|
||||
import os
|
||||
|
||||
# Константы
|
||||
SERIAL_PORT = '/dev/ttyS0'
|
||||
BAUD_RATE = 230400
|
||||
DB_PATH = '../inertial_data.db'
|
||||
SERIAL_PORT = '/dev/ttyUSB0'
|
||||
BAUD_RATE = 115200
|
||||
DB_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'inertial_data.db')
|
||||
|
||||
# Заголовок пакета LDS02RR
|
||||
HEADER = 0xFA
|
||||
|
|
@ -51,11 +52,16 @@ def parse_packet(data):
|
|||
points = []
|
||||
for i in range(4):
|
||||
offset = 4 + i * 4
|
||||
distance_raw = struct.unpack_from('<H', data, offset)[0]
|
||||
quality = struct.unpack_from('<H', data, offset + 2)[0] >> 2
|
||||
distance = distance_raw / 4.0
|
||||
b0 = data[offset]
|
||||
b1 = data[offset + 1]
|
||||
b2 = data[offset + 2]
|
||||
b3 = data[offset + 3]
|
||||
invalid = (b1 >> 7) & 1
|
||||
distance = b0 | ((b1 & 0x3F) << 8)
|
||||
quality = b2 | (b3 << 8)
|
||||
angle = index * 4 + i
|
||||
points.append((angle, distance, quality))
|
||||
if not invalid and distance > 0:
|
||||
points.append((angle, distance, quality))
|
||||
return points
|
||||
|
||||
def save_to_db():
|
||||
|
|
@ -101,4 +107,4 @@ def save_to_db():
|
|||
conn.close()
|
||||
print("Лидар модуль остановлен")
|
||||
|
||||
save_to_db()
|
||||
save_to_db()
|
||||
|
|
|
|||
|
|
@ -7,113 +7,121 @@
|
|||
#include <csignal>
|
||||
#include <sqlite3.h>
|
||||
#include <Eigen/Dense>
|
||||
#include <unistd.h>
|
||||
#include <libgen.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace Eigen;
|
||||
|
||||
// ===== ФЛАГ ЗАВЕРШЕНИЯ =====
|
||||
volatile sig_atomic_t running = 1;
|
||||
void shutdown(int signum) { running = 0; }
|
||||
|
||||
void shutdown(int signum) {
|
||||
running = 0;
|
||||
}
|
||||
struct IMUData { double t; Vector3d acc; Vector3d gyro; };
|
||||
struct LidarData { double t; double angle; double distance; };
|
||||
|
||||
// ===== СТРУКТУРЫ =====
|
||||
// Состояние системы
|
||||
Vector3d pos = Vector3d::Zero(); // позиция (м)
|
||||
Vector3d vel = Vector3d::Zero(); // скорость (м/с)
|
||||
Vector3d angles = Vector3d::Zero(); // углы roll, pitch, yaw (рад)
|
||||
|
||||
struct IMUData {
|
||||
double t;
|
||||
Vector3d acc;
|
||||
Vector3d gyro;
|
||||
};
|
||||
// Bias акселерометра (в единицах g).
|
||||
// При калибровке измеряется начальное значение,
|
||||
// затем автоматически уточняется в каждую остановку (ZUPT).
|
||||
double ax_bias = 0.0;
|
||||
double ay_bias = 0.0;
|
||||
double az_bias = 1.0; // ~1g — гравитация по Z когда датчик горизонтальный
|
||||
|
||||
struct LidarData {
|
||||
double t;
|
||||
double angle;
|
||||
double distance;
|
||||
};
|
||||
// Параметры ZUPT
|
||||
const double ZUPT_ACC_THR = 0.08; // отклонение нормы от 1g
|
||||
const double ZUPT_GYRO_THR = 0.05; // рад/с (~3 °/с)
|
||||
const int ZUPT_WINDOW = 10; // сэмплов подряд (~120 мс)
|
||||
|
||||
struct State {
|
||||
VectorXd x;
|
||||
MatrixXd P;
|
||||
};
|
||||
// Коэффициент комплементарного фильтра
|
||||
// 0.98 = 98% гироскоп (быстро, но дрейфует)
|
||||
// 0.02 = 2% акселерометр (медленно, но абсолютное значение)
|
||||
const double CF_ALPHA = 0.98;
|
||||
|
||||
// ===== ЧТЕНИЕ ИЗ SQLite =====
|
||||
|
||||
vector<IMUData> readIMU(sqlite3* db) {
|
||||
// ─────────────────────────────────────────────
|
||||
// Чтение БД
|
||||
// ─────────────────────────────────────────────
|
||||
vector<IMUData> readIMU(sqlite3* db, double after_ts) {
|
||||
vector<IMUData> data;
|
||||
sqlite3_stmt* stmt;
|
||||
|
||||
const char* sql = "SELECT timestamp, ax, ay, az, gx, gy, gz FROM imu_data ORDER BY timestamp";
|
||||
sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
|
||||
|
||||
sqlite3_prepare_v2(db,
|
||||
"SELECT timestamp, ax, ay, az, gx, gy, gz "
|
||||
"FROM imu_data WHERE timestamp > ? ORDER BY timestamp",
|
||||
-1, &stmt, nullptr);
|
||||
sqlite3_bind_double(stmt, 1, after_ts);
|
||||
while (sqlite3_step(stmt) == SQLITE_ROW) {
|
||||
IMUData d;
|
||||
d.t = sqlite3_column_double(stmt, 0);
|
||||
d.acc.x() = sqlite3_column_double(stmt, 1);
|
||||
d.acc.y() = sqlite3_column_double(stmt, 2);
|
||||
d.acc.z() = sqlite3_column_double(stmt, 3);
|
||||
d.t = sqlite3_column_double(stmt, 0);
|
||||
d.acc.x() = sqlite3_column_double(stmt, 1);
|
||||
d.acc.y() = sqlite3_column_double(stmt, 2);
|
||||
d.acc.z() = sqlite3_column_double(stmt, 3);
|
||||
d.gyro.x() = sqlite3_column_double(stmt, 4);
|
||||
d.gyro.y() = sqlite3_column_double(stmt, 5);
|
||||
d.gyro.z() = sqlite3_column_double(stmt, 6);
|
||||
data.push_back(d);
|
||||
}
|
||||
|
||||
sqlite3_finalize(stmt);
|
||||
return data;
|
||||
}
|
||||
|
||||
vector<LidarData> readLidar(sqlite3* db) {
|
||||
vector<LidarData> readLidar(sqlite3* db, double after_ts) {
|
||||
vector<LidarData> data;
|
||||
sqlite3_stmt* stmt;
|
||||
|
||||
const char* sql = "SELECT timestamp, angle, distance_mm FROM lidar_data ORDER BY timestamp";
|
||||
sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
|
||||
|
||||
sqlite3_prepare_v2(db,
|
||||
"SELECT timestamp, angle, distance_mm "
|
||||
"FROM lidar_data WHERE timestamp > ? ORDER BY timestamp",
|
||||
-1, &stmt, nullptr);
|
||||
sqlite3_bind_double(stmt, 1, after_ts);
|
||||
while (sqlite3_step(stmt) == SQLITE_ROW) {
|
||||
LidarData d;
|
||||
d.t = sqlite3_column_double(stmt, 0);
|
||||
d.angle = sqlite3_column_double(stmt, 1);
|
||||
d.t = sqlite3_column_double(stmt, 0);
|
||||
d.angle = sqlite3_column_double(stmt, 1);
|
||||
d.distance = sqlite3_column_double(stmt, 2);
|
||||
data.push_back(d);
|
||||
}
|
||||
|
||||
sqlite3_finalize(stmt);
|
||||
return data;
|
||||
}
|
||||
|
||||
// ===== ЗАПИСЬ В SQLite =====
|
||||
|
||||
// ─────────────────────────────────────────────
|
||||
// Запись в БД
|
||||
// ─────────────────────────────────────────────
|
||||
void initDB(sqlite3* db) {
|
||||
const char* sql =
|
||||
sqlite3_exec(db,
|
||||
"PRAGMA journal_mode=WAL;"
|
||||
"PRAGMA synchronous=NORMAL;"
|
||||
"CREATE TABLE IF NOT EXISTS trajectory ("
|
||||
"timestamp REAL, x REAL, y REAL, z REAL,"
|
||||
"roll REAL, pitch REAL, yaw REAL);"
|
||||
"DELETE FROM trajectory;"
|
||||
"CREATE TABLE IF NOT EXISTS lidar_points ("
|
||||
"x REAL, y REAL, z REAL);"
|
||||
"DELETE FROM lidar_points;";
|
||||
sqlite3_exec(db, sql, nullptr, nullptr, nullptr);
|
||||
"CREATE TABLE IF NOT EXISTS lidar_points (x REAL, y REAL, z REAL);"
|
||||
"DELETE FROM lidar_points;",
|
||||
nullptr, nullptr, nullptr);
|
||||
}
|
||||
|
||||
void writeTraj(sqlite3* db, double t, Vector3d pos, Vector3d ang) {
|
||||
void writeTraj(sqlite3* db, double t) {
|
||||
sqlite3_stmt* stmt;
|
||||
const char* sql = "INSERT INTO trajectory VALUES (?,?,?,?,?,?,?)";
|
||||
sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
|
||||
sqlite3_prepare_v2(db,
|
||||
"INSERT INTO trajectory VALUES (?,?,?,?,?,?,?)",
|
||||
-1, &stmt, nullptr);
|
||||
sqlite3_bind_double(stmt, 1, t);
|
||||
sqlite3_bind_double(stmt, 2, pos.x());
|
||||
sqlite3_bind_double(stmt, 3, pos.y());
|
||||
sqlite3_bind_double(stmt, 4, pos.z());
|
||||
sqlite3_bind_double(stmt, 5, ang.x());
|
||||
sqlite3_bind_double(stmt, 6, ang.y());
|
||||
sqlite3_bind_double(stmt, 7, ang.z());
|
||||
sqlite3_bind_double(stmt, 5, angles.x());
|
||||
sqlite3_bind_double(stmt, 6, angles.y());
|
||||
sqlite3_bind_double(stmt, 7, angles.z());
|
||||
sqlite3_step(stmt);
|
||||
sqlite3_finalize(stmt);
|
||||
}
|
||||
|
||||
void writeLidarPoint(sqlite3* db, Vector3d p) {
|
||||
sqlite3_stmt* stmt;
|
||||
const char* sql = "INSERT INTO lidar_points VALUES (?,?,?)";
|
||||
sqlite3_prepare_v2(db, sql, -1, &stmt, nullptr);
|
||||
sqlite3_prepare_v2(db, "INSERT INTO lidar_points VALUES (?,?,?)",
|
||||
-1, &stmt, nullptr);
|
||||
sqlite3_bind_double(stmt, 1, p.x());
|
||||
sqlite3_bind_double(stmt, 2, p.y());
|
||||
sqlite3_bind_double(stmt, 3, p.z());
|
||||
|
|
@ -121,60 +129,105 @@ void writeLidarPoint(sqlite3* db, Vector3d p) {
|
|||
sqlite3_finalize(stmt);
|
||||
}
|
||||
|
||||
// ===== ФИЗИКА (без изменений) =====
|
||||
|
||||
// ─────────────────────────────────────────────
|
||||
// Матрица поворота (roll, pitch, yaw)
|
||||
// ─────────────────────────────────────────────
|
||||
Matrix3d rotationMatrix(double roll, double pitch, double yaw) {
|
||||
AngleAxisd rx(roll, Vector3d::UnitX());
|
||||
AngleAxisd rx(roll, Vector3d::UnitX());
|
||||
AngleAxisd ry(pitch, Vector3d::UnitY());
|
||||
AngleAxisd rz(yaw, Vector3d::UnitZ());
|
||||
AngleAxisd rz(yaw, Vector3d::UnitZ());
|
||||
return (rz * ry * rx).toRotationMatrix();
|
||||
}
|
||||
|
||||
void predict(State& s, const IMUData& imu, double dt) {
|
||||
VectorXd& x = s.x;
|
||||
Vector3d pos = x.segment<3>(0);
|
||||
Vector3d vel = x.segment<3>(3);
|
||||
Vector3d angles = x.segment<3>(6);
|
||||
Vector3d lidarToWorld(const LidarData& l) {
|
||||
double angle = l.angle * M_PI / 180.0;
|
||||
double dist = l.distance / 1000.0;
|
||||
Vector3d local(dist * cos(angle), dist * sin(angle), 0);
|
||||
return pos + rotationMatrix(angles[0], angles[1], angles[2]) * local;
|
||||
}
|
||||
|
||||
Vector3d acc = imu.acc * 9.81;
|
||||
// ─────────────────────────────────────────────
|
||||
// predict() — формулы с доски + комплементарный фильтр
|
||||
//
|
||||
// Формулы преподавателя (для всех трёх осей OX, OY, OZ):
|
||||
// V(nT) = V((n-1)T) + a·T
|
||||
// X(nT) = V((n-1)T)·T + a·T²/2
|
||||
// φ = ω·T
|
||||
//
|
||||
// Комплементарный фильтр — решает проблему наклона:
|
||||
// angle = α·(angle + gyro·dt) + (1-α)·angle_from_accel
|
||||
// Гироскоп отслеживает быстрые повороты.
|
||||
// Акселерометр корректирует накопленный дрейф.
|
||||
//
|
||||
// ZUPT — когда стоим: замораживаем всё, уточняем bias.
|
||||
// ─────────────────────────────────────────────
|
||||
void predict(const IMUData& imu, double dt) {
|
||||
// Угловые скорости (рад/с)
|
||||
Vector3d gyro = imu.gyro * M_PI / 180.0;
|
||||
|
||||
Matrix3d R = rotationMatrix(angles[0], angles[1], angles[2]);
|
||||
Vector3d g(0, 0, 9.81);
|
||||
Vector3d acc_world = R * acc - g;
|
||||
// ── Комплементарный фильтр ориентации ──────────────
|
||||
// Углы наклона из акселерометра (абсолютные, но шумные)
|
||||
double roll_acc = atan2(imu.acc.y(), imu.acc.z());
|
||||
double pitch_acc = atan2(-imu.acc.x(),
|
||||
sqrt(imu.acc.y() * imu.acc.y() +
|
||||
imu.acc.z() * imu.acc.z()));
|
||||
|
||||
vel += acc_world * dt;
|
||||
pos += vel * dt + 0.5 * acc_world * dt * dt;
|
||||
angles += gyro * dt;
|
||||
// Комбинируем: быстрая часть (гироскоп) + медленная коррекция (акселерометр)
|
||||
// φ = α·(φ + ω·T) + (1-α)·φ_акселерометр
|
||||
angles.x() = CF_ALPHA * (angles.x() + gyro.x() * dt) + (1.0 - CF_ALPHA) * roll_acc;
|
||||
angles.y() = CF_ALPHA * (angles.y() + gyro.y() * dt) + (1.0 - CF_ALPHA) * pitch_acc;
|
||||
angles.z() += gyro.z() * dt; // yaw — только гироскоп (нет абсолютного источника)
|
||||
|
||||
x.segment<3>(0) = pos;
|
||||
x.segment<3>(3) = vel;
|
||||
x.segment<3>(6) = angles;
|
||||
// ── Линейное ускорение в мировой системе ───────────
|
||||
// Шаг 1: вычитаем bias датчика и переводим в м/с²
|
||||
Vector3d acc_sensor(
|
||||
(imu.acc.x() - ax_bias) * 9.81,
|
||||
(imu.acc.y() - ay_bias) * 9.81,
|
||||
imu.acc.z() * 9.81 // Z: bias не вычитаем, гравитация убирается через матрицу поворота
|
||||
);
|
||||
|
||||
MatrixXd F = MatrixXd::Identity(15, 15);
|
||||
F.block<3, 3>(0, 3) = Matrix3d::Identity() * dt;
|
||||
MatrixXd Q = MatrixXd::Identity(15, 15) * 0.01;
|
||||
s.P = F * s.P * F.transpose() + Q;
|
||||
// Шаг 2: поворачиваем в мировую систему координат
|
||||
Matrix3d R = rotationMatrix(angles.x(), angles.y(), angles.z());
|
||||
Vector3d acc_world = R * acc_sensor;
|
||||
|
||||
// Шаг 3: вычитаем гравитацию (она направлена вниз = +Z в мировой системе)
|
||||
acc_world.z() -= 9.81;
|
||||
|
||||
// ── ZUPT: детектор покоя ────────────────────────────
|
||||
double acc_mag = imu.acc.norm();
|
||||
double gyro_mag = gyro.norm();
|
||||
static int zupt_counter = 0;
|
||||
|
||||
if (abs(acc_mag - 1.0) < ZUPT_ACC_THR && gyro_mag < ZUPT_GYRO_THR) {
|
||||
zupt_counter++;
|
||||
} else {
|
||||
zupt_counter = 0;
|
||||
}
|
||||
|
||||
if (zupt_counter >= ZUPT_WINDOW) {
|
||||
// ── СТОИМ ──────────────────────────────────────
|
||||
// Замораживаем скорость
|
||||
vel = Vector3d::Zero();
|
||||
// Уточняем bias по всем трём осям (медленное скользящее среднее)
|
||||
ax_bias = 0.99 * ax_bias + 0.01 * imu.acc.x();
|
||||
ay_bias = 0.99 * ay_bias + 0.01 * imu.acc.y();
|
||||
az_bias = 0.99 * az_bias + 0.01 * imu.acc.z();
|
||||
// pos не меняем
|
||||
} else {
|
||||
// ── ДВИЖЕМСЯ ───────────────────────────────────
|
||||
// Формулы с доски для OX, OY, OZ:
|
||||
// X(nT) = V((n-1)T)·T + a·T²/2
|
||||
// V(nT) = V((n-1)T) + a·T
|
||||
pos += vel * dt + 0.5 * acc_world * dt * dt;
|
||||
vel += acc_world * dt;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3d lidarToWorld(const State& s, const LidarData& l) {
|
||||
double angle = l.angle * M_PI / 180.0;
|
||||
double dist = l.distance / 1000.0;
|
||||
Vector3d local;
|
||||
local << dist * cos(angle), dist * sin(angle), 0;
|
||||
Vector3d pos = s.x.segment<3>(0);
|
||||
Vector3d ang = s.x.segment<3>(6);
|
||||
Matrix3d R = rotationMatrix(ang[0], ang[1], ang[2]);
|
||||
return pos + R * local;
|
||||
}
|
||||
|
||||
// ===== ICP (без изменений) =====
|
||||
|
||||
// ─────────────────────────────────────────────
|
||||
// ICP — сопоставление сканов лидара
|
||||
// ─────────────────────────────────────────────
|
||||
struct KDNode {
|
||||
Vector3d point;
|
||||
KDNode* left;
|
||||
KDNode* right;
|
||||
int axis;
|
||||
Vector3d point; KDNode* left; KDNode* right; int axis;
|
||||
KDNode(Vector3d p, int ax) : point(p), left(nullptr), right(nullptr), axis(ax) {}
|
||||
};
|
||||
|
||||
|
|
@ -182,27 +235,26 @@ KDNode* buildKDTree(vector<Vector3d>& pts, int depth = 0) {
|
|||
if (pts.empty()) return nullptr;
|
||||
int axis = depth % 3;
|
||||
sort(pts.begin(), pts.end(),
|
||||
[axis](const Vector3d& a, const Vector3d& b) { return a[axis] < b[axis]; });
|
||||
[axis](const Vector3d& a, const Vector3d& b){ return a[axis] < b[axis]; });
|
||||
int mid = pts.size() / 2;
|
||||
KDNode* node = new KDNode(pts[mid], axis);
|
||||
vector<Vector3d> left(pts.begin(), pts.begin() + mid);
|
||||
vector<Vector3d> right(pts.begin() + mid + 1, pts.end());
|
||||
node->left = buildKDTree(left, depth + 1);
|
||||
node->left = buildKDTree(left, depth + 1);
|
||||
node->right = buildKDTree(right, depth + 1);
|
||||
return node;
|
||||
}
|
||||
|
||||
void nearestSearch(KDNode* node, const Vector3d& target,
|
||||
Vector3d& best, double& best_dist) {
|
||||
Vector3d& best, double& best_dist) {
|
||||
if (!node) return;
|
||||
double d = (node->point - target).squaredNorm();
|
||||
if (d < best_dist) { best_dist = d; best = node->point; }
|
||||
int axis = node->axis;
|
||||
KDNode* near = target[axis] < node->point[axis] ? node->left : node->right;
|
||||
KDNode* far = target[axis] < node->point[axis] ? node->right : node->left;
|
||||
nearestSearch(near, target, best, best_dist);
|
||||
double diff = target[axis] - node->point[axis];
|
||||
if (diff * diff < best_dist) nearestSearch(far, target, best, best_dist);
|
||||
KDNode* near_ = target[node->axis] < node->point[node->axis] ? node->left : node->right;
|
||||
KDNode* far_ = target[node->axis] < node->point[node->axis] ? node->right : node->left;
|
||||
nearestSearch(near_, target, best, best_dist);
|
||||
double diff = target[node->axis] - node->point[node->axis];
|
||||
if (diff * diff < best_dist) nearestSearch(far_, target, best, best_dist);
|
||||
}
|
||||
|
||||
Matrix3d computeRotation(const vector<Vector3d>& src, const vector<Vector3d>& dst) {
|
||||
|
|
@ -216,7 +268,8 @@ Matrix3d computeRotation(const vector<Vector3d>& src, const vector<Vector3d>& ds
|
|||
return svd.matrixV() * svd.matrixU().transpose();
|
||||
}
|
||||
|
||||
Vector3d computeTranslation(const vector<Vector3d>& src, const vector<Vector3d>& dst, const Matrix3d& R) {
|
||||
Vector3d computeTranslation(const vector<Vector3d>& src,
|
||||
const vector<Vector3d>& dst, const Matrix3d& R) {
|
||||
Vector3d c1 = Vector3d::Zero(), c2 = Vector3d::Zero();
|
||||
for (size_t i = 0; i < src.size(); i++) { c1 += src[i]; c2 += dst[i]; }
|
||||
c1 /= src.size(); c2 /= dst.size();
|
||||
|
|
@ -225,14 +278,13 @@ Vector3d computeTranslation(const vector<Vector3d>& src, const vector<Vector3d>&
|
|||
|
||||
vector<Vector3d> downsample(const vector<Vector3d>& pts, int step = 5) {
|
||||
vector<Vector3d> out;
|
||||
for (size_t i = 0; i < pts.size(); i += step)
|
||||
out.push_back(pts[i]);
|
||||
for (size_t i = 0; i < pts.size(); i += step) out.push_back(pts[i]);
|
||||
return out;
|
||||
}
|
||||
|
||||
void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst, Matrix3d& R, Vector3d& t) {
|
||||
R = Matrix3d::Identity();
|
||||
t = Vector3d::Zero();
|
||||
void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst,
|
||||
Matrix3d& R, Vector3d& t) {
|
||||
R = Matrix3d::Identity(); t = Vector3d::Zero();
|
||||
vector<Vector3d> dst_copy = dst;
|
||||
KDNode* tree = buildKDTree(dst_copy);
|
||||
for (int iter = 0; iter < 10; iter++) {
|
||||
|
|
@ -251,41 +303,115 @@ void ICP_fast(vector<Vector3d>& src, vector<Vector3d>& dst, Matrix3d& R, Vector3
|
|||
}
|
||||
}
|
||||
|
||||
// ===== MAIN =====
|
||||
// ─────────────────────────────────────────────
|
||||
// Калибровка: датчик лежит горизонтально, неподвижно.
|
||||
// Измеряем начальный bias по всем трём осям.
|
||||
// ─────────────────────────────────────────────
|
||||
void calibrate(sqlite3* db) {
|
||||
cout << "Калибровка (держите датчик горизонтально, неподвижно)..." << endl;
|
||||
|
||||
int main() {
|
||||
signal(SIGTERM, shutdown);
|
||||
signal(SIGINT, shutdown);
|
||||
|
||||
sqlite3* db;
|
||||
sqlite3_open("../inertial_data.db", &db);
|
||||
initDB(db);
|
||||
|
||||
State state;
|
||||
state.x = VectorXd::Zero(15);
|
||||
state.P = MatrixXd::Identity(15, 15) * 0.1;
|
||||
|
||||
vector<Vector3d> prev_scan, curr_scan;
|
||||
size_t lidar_idx = 0;
|
||||
size_t imu_idx = 0;
|
||||
|
||||
cout << "Расчёты запущены..." << endl;
|
||||
double sum_ax = 0, sum_ay = 0, sum_az = 0;
|
||||
int n = 0;
|
||||
double last_ts = 0;
|
||||
bool seen_data = false;
|
||||
auto start = chrono::steady_clock::now();
|
||||
auto first_data_time = chrono::steady_clock::time_point();
|
||||
|
||||
while (running) {
|
||||
auto imu = readIMU(db);
|
||||
auto lidar = readLidar(db);
|
||||
auto imu = readIMU(db, last_ts);
|
||||
for (auto& d : imu) {
|
||||
if (!seen_data) {
|
||||
seen_data = true;
|
||||
first_data_time = chrono::steady_clock::now();
|
||||
cout << "Данные IMU получены, собираю калибровку..." << endl;
|
||||
}
|
||||
sum_ax += d.acc.x();
|
||||
sum_ay += d.acc.y();
|
||||
sum_az += d.acc.z();
|
||||
n++;
|
||||
last_ts = d.t;
|
||||
}
|
||||
|
||||
// Обрабатываем только новые данные
|
||||
for (size_t i = max((size_t)1, imu_idx); i < imu.size(); i++) {
|
||||
double dt = imu[i].t - imu[i-1].t;
|
||||
if (seen_data) {
|
||||
auto elapsed = chrono::steady_clock::now() - first_data_time;
|
||||
if (elapsed >= chrono::seconds(2) && n >= 30) break;
|
||||
} else {
|
||||
auto waited = chrono::steady_clock::now() - start;
|
||||
if (waited >= chrono::seconds(60)) {
|
||||
cout << "IMU не отвечает. Использую стандартный bias." << endl;
|
||||
return;
|
||||
}
|
||||
}
|
||||
this_thread::sleep_for(chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
if (n > 0) {
|
||||
ax_bias = sum_ax / n;
|
||||
ay_bias = sum_ay / n;
|
||||
az_bias = sum_az / n;
|
||||
cout << "Калибровка завершена. Bias: ax=" << ax_bias
|
||||
<< "g, ay=" << ay_bias << "g, az=" << az_bias
|
||||
<< "g (по " << n << " сэмплам)" << endl;
|
||||
cout << "Bias будет уточняться автоматически во время остановок." << endl;
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────
|
||||
// main
|
||||
// ─────────────────────────────────────────────
|
||||
int main() {
|
||||
signal(SIGTERM, shutdown);
|
||||
signal(SIGINT, shutdown);
|
||||
|
||||
sqlite3* db;
|
||||
char exe_path[1024];
|
||||
ssize_t len = readlink("/proc/self/exe", exe_path, sizeof(exe_path)-1);
|
||||
exe_path[len] = '\0';
|
||||
string db_path = string(dirname(exe_path)) + "/../inertial_data.db";
|
||||
sqlite3_open(db_path.c_str(), &db);
|
||||
initDB(db);
|
||||
|
||||
cout << "Расчёты запущены..." << endl;
|
||||
calibrate(db);
|
||||
|
||||
double last_imu_ts = 0;
|
||||
double last_lidar_ts = 0;
|
||||
{
|
||||
sqlite3_stmt* stmt;
|
||||
sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM imu_data",
|
||||
-1, &stmt, nullptr);
|
||||
if (sqlite3_step(stmt) == SQLITE_ROW)
|
||||
last_imu_ts = sqlite3_column_double(stmt, 0);
|
||||
sqlite3_finalize(stmt);
|
||||
|
||||
sqlite3_prepare_v2(db, "SELECT MAX(timestamp) FROM lidar_data",
|
||||
-1, &stmt, nullptr);
|
||||
if (sqlite3_step(stmt) == SQLITE_ROW)
|
||||
last_lidar_ts = sqlite3_column_double(stmt, 0);
|
||||
sqlite3_finalize(stmt);
|
||||
}
|
||||
|
||||
vector<Vector3d> prev_scan, curr_scan;
|
||||
vector<LidarData> pending_lidar;
|
||||
|
||||
while (running) {
|
||||
auto imu = readIMU(db, last_imu_ts);
|
||||
auto new_lidar = readLidar(db, last_lidar_ts);
|
||||
if (!new_lidar.empty()) last_lidar_ts = new_lidar.back().t;
|
||||
pending_lidar.insert(pending_lidar.end(),
|
||||
new_lidar.begin(), new_lidar.end());
|
||||
|
||||
for (size_t i = 0; i < imu.size(); i++) {
|
||||
double prev_ts = (i == 0) ? last_imu_ts : imu[i-1].t;
|
||||
double dt = imu[i].t - prev_ts;
|
||||
if (dt <= 0 || dt > 0.1) continue;
|
||||
|
||||
predict(state, imu[i], dt);
|
||||
predict(imu[i], dt);
|
||||
|
||||
while (lidar_idx < lidar.size() &&
|
||||
lidar[lidar_idx].t <= imu[i].t) {
|
||||
curr_scan.push_back(lidarToWorld(state, lidar[lidar_idx]));
|
||||
lidar_idx++;
|
||||
while (!pending_lidar.empty() &&
|
||||
pending_lidar.front().t <= imu[i].t) {
|
||||
curr_scan.push_back(lidarToWorld(pending_lidar.front()));
|
||||
pending_lidar.erase(pending_lidar.begin());
|
||||
}
|
||||
|
||||
if (curr_scan.size() > 200) {
|
||||
|
|
@ -294,26 +420,21 @@ int main() {
|
|||
vector<Vector3d> dst = downsample(prev_scan, 5);
|
||||
Matrix3d R; Vector3d t;
|
||||
ICP_fast(src, dst, R, t);
|
||||
state.x.segment<3>(0) += t;
|
||||
pos += t;
|
||||
}
|
||||
for (auto& p : curr_scan)
|
||||
writeLidarPoint(db, p);
|
||||
for (auto& p : curr_scan) writeLidarPoint(db, p);
|
||||
prev_scan = curr_scan;
|
||||
curr_scan.clear();
|
||||
}
|
||||
|
||||
Vector3d pos = state.x.segment<3>(0);
|
||||
Vector3d ang = state.x.segment<3>(6);
|
||||
writeTraj(db, imu[i].t, pos, ang);
|
||||
writeTraj(db, imu[i].t);
|
||||
}
|
||||
|
||||
imu_idx = imu.size();
|
||||
|
||||
// Ждём 200 мс перед следующим циклом
|
||||
if (!imu.empty()) last_imu_ts = imu.back().t;
|
||||
this_thread::sleep_for(chrono::milliseconds(200));
|
||||
}
|
||||
|
||||
sqlite3_close(db);
|
||||
cout << "Расчёты остановлены" << endl;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
368
processing/final_server_auto.py
Normal file
368
processing/final_server_auto.py
Normal file
|
|
@ -0,0 +1,368 @@
|
|||
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()
|
||||
Loading…
Reference in New Issue
Block a user