diff --git a/imu_module/imu_reader.py b/imu_module/imu_reader.py index aada7ec..3d3e8b5 100644 --- a/imu_module/imu_reader.py +++ b/imu_module/imu_reader.py @@ -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() \ No newline at end of file + +if __name__ == '__main__': + main() diff --git a/lidar_module/lidar_reader.py b/lidar_module/lidar_reader.py index bfc41e1..01701db 100644 --- a/lidar_module/lidar_reader.py +++ b/lidar_module/lidar_reader.py @@ -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('> 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() \ No newline at end of file +save_to_db() diff --git a/processing/calculations.cpp b/processing/calculations.cpp index e60cb2a..cc4d750 100644 --- a/processing/calculations.cpp +++ b/processing/calculations.cpp @@ -7,113 +7,121 @@ #include #include #include +#include +#include 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 readIMU(sqlite3* db) { +// ───────────────────────────────────────────── +// Чтение БД +// ───────────────────────────────────────────── +vector readIMU(sqlite3* db, double after_ts) { vector 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 readLidar(sqlite3* db) { +vector readLidar(sqlite3* db, double after_ts) { vector 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& 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 left(pts.begin(), pts.begin() + mid); vector 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& src, const vector& dst) { @@ -216,7 +268,8 @@ Matrix3d computeRotation(const vector& src, const vector& ds return svd.matrixV() * svd.matrixU().transpose(); } -Vector3d computeTranslation(const vector& src, const vector& dst, const Matrix3d& R) { +Vector3d computeTranslation(const vector& src, + const vector& 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& src, const vector& vector downsample(const vector& pts, int step = 5) { vector 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& src, vector& dst, Matrix3d& R, Vector3d& t) { - R = Matrix3d::Identity(); - t = Vector3d::Zero(); +void ICP_fast(vector& src, vector& dst, + Matrix3d& R, Vector3d& t) { + R = Matrix3d::Identity(); t = Vector3d::Zero(); vector dst_copy = dst; KDNode* tree = buildKDTree(dst_copy); for (int iter = 0; iter < 10; iter++) { @@ -251,41 +303,115 @@ void ICP_fast(vector& src, vector& 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 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 prev_scan, curr_scan; + vector 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 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; -} \ No newline at end of file +} diff --git a/processing/final_server_auto.py b/processing/final_server_auto.py new file mode 100644 index 0000000..0495802 --- /dev/null +++ b/processing/final_server_auto.py @@ -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 = """ + + + + Inertial Tracker — Реальное время + + + + +
+
INERTIAL TRACKER
+
Нет данных
+
Пройдено: 0.00 м
+
Время: 0:00
+
+ +
X: —
Y: —
Z: —
+ +
+ ◆ Начальная точка (0, 0, 0)
+ ━━ Траектория движения
+ ●●● Карта окружения (лидар)
+ ● Текущее положение +
+ +
+ Оси X, Y, Z — перемещение в метрах.
+ Масштаб одинаковый по всем осям.
+ Вращайте сцену мышью. +
+ +
Подключение...
+
+ + + + +""" + + +def fetch_data(): + """Читает свежие данные из БД в read-only режиме (не мешает писателям).""" + conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True) + try: + # Текущая позиция + cur = conn.execute( + "SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT 1" + ) + row = cur.fetchone() + pos = {'x': row[0], 'y': row[1], 'z': row[2]} if row else {'x': 0.0, 'y': 0.0, 'z': 0.0} + + # Последние N точек траектории (разворачиваем в хронологический порядок) + cur = conn.execute( + f"SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT {TRAJ_LIMIT}" + ) + traj_rows = cur.fetchall()[::-1] + traj = { + 'x': [r[0] for r in traj_rows], + 'y': [r[1] for r in traj_rows], + 'z': [r[2] for r in traj_rows], + } + + # Последние N точек лидара + cur = conn.execute( + f"SELECT x, y, z FROM lidar_points ORDER BY rowid DESC LIMIT {LIDAR_LIMIT}" + ) + lidar_rows = cur.fetchall() + lidar = { + 'x': [r[0] for r in lidar_rows], + 'y': [r[1] for r in lidar_rows], + 'z': [r[2] for r in lidar_rows], + } + finally: + conn.close() + return {'pos': pos, 'traj': traj, 'lidar': lidar} + + +class Handler(BaseHTTPRequestHandler): + def log_message(self, format, *args): + pass + + def do_GET(self): + if self.path == '/': + self.send_response(200) + self.send_header('Content-type', 'text/html; charset=utf-8') + self.end_headers() + self.wfile.write(HTML.encode('utf-8')) + + elif self.path == '/plotly-latest.min.js': + self.send_response(200) + self.send_header('Content-type', 'application/javascript') + self.end_headers() + with open('plotly-latest.min.js', 'rb') as f: + self.wfile.write(f.read()) + + elif self.path.startswith('/api/data'): + try: + data = fetch_data() + payload = json.dumps(data).encode('utf-8') + self.send_response(200) + self.send_header('Content-type', 'application/json') + self.send_header('Cache-Control', 'no-store') + self.end_headers() + self.wfile.write(payload) + except Exception as e: + self.send_response(500) + self.end_headers() + self.wfile.write(str(e).encode('utf-8')) + + elif self.path.startswith('/api/pos'): + try: + conn = sqlite3.connect(f'file:{DB_PATH}?mode=ro', uri=True) + cur = conn.execute( + "SELECT x, y, z FROM trajectory ORDER BY timestamp DESC LIMIT 1" + ) + row = cur.fetchone() + conn.close() + pos = {'x': row[0], 'y': row[1], 'z': row[2]} if row else {'x': 0, 'y': 0, 'z': 0} + self.send_response(200) + self.send_header('Content-type', 'application/json') + self.end_headers() + self.wfile.write(json.dumps(pos).encode('utf-8')) + except Exception as e: + self.send_response(500) + self.end_headers() + self.wfile.write(str(e).encode('utf-8')) + + else: + self.send_response(404) + self.end_headers() + + +if __name__ == '__main__': + print("=" * 50) + print("INERTIAL TRACKER — реальное время") + print(f"Открыть: http://192.168.0.106:{PORT}") + print("=" * 50) + HTTPServer(('0.0.0.0', PORT), Handler).serve_forever()