デバイスの紹介

QuickLouTE（クイックルート）は、IMUやGNSS、LTE通信を活用して移動状態を推定し、記録・アップロードするデバイスです。
BLEで接続できるみちびき「災危通報」受信機能も搭載しており、災害時や電波の届きにくい場所では活躍することが期待できます。

タイムライン生成&ビューア

各種センサーからの情報で状態を推定し、SDに記録すると同時にCloudflare Workersに送信し、Cloudflare D1にデータを保存します。その後、データをReact製のビューアで読み込みます。

みちびき「災危通報」受信機能

みちびきから「災危通報」を受信したあと、BLEでPCやスマートフォンに送信することで可視化を行います。

制作の動機

私は旅行で、行ったことがない場所を開拓することが好きです。
自宅に帰ってきたあと、しばらく経って忘れかけてきたころに思い返すことがありますが、そのときにログは重要です。
今はスマホのアプリについたタイムライン機能を使っていますが、以下の不満点がありました。

• 精度が悪い
  • 時間の解像度が低く、N時からある場所に滞在してもN+1時からの滞在など遅れて記録されていることが多い
  • 移動手段の判別ができてない (徒歩/自転車/電車などがまとめて自動車判定)
• ログの反映が遅い
• いつサービス終了するかわからない
• スマホの電池消費が増えてそう
• 位置情報へのアクセスを常時許可するのは微妙に抵抗ある

これらの不満点を解決するために、制作デバイスに以下のような条件を設定しました。

• GNSSで移動のログをつける
• 移動手段(徒歩・ランニング/自転車/列車/バス・自動車/飛行機)を判別し記録
• 遅延なくログを確認できるように
• 可能な限り処理をSpresense上で行う

使用部品

• Spresense メインボード
• LTE拡張ボード
• BLE1507 (BLE serialization firmware)
• MPU6886 (M5STACK-U095)
• Groveケーブル
• オス-メス ジャンパー線
• microSDHCカード KIOXIA製 32GB
• 3Dプリンタ製ケース/フタ
• モバイルバッテリー
• nanoSIMカード

制作過程

各モジュールの動作確認

まず、提供いただいたモジュールを各種サンプルコードを使い、動作確認しました。
今回は使い慣れているArduino IDEを用いて開発を行っています。
セットアップは公式ドキュメント通りにやれば迷うところはないと思います。(https://developer.spresense.sony-semicon.com/development-guides/?page=arduino_set_up&lang=ja)
インストール後、Arduino IDEのFile→Examplesより"Example for Spresense"が選択できますが、網羅的にサンプルコードが用意されており、開発を始めやすかったです。ありがとうございます！

提供いただいた部品と確認した機能

• Spresense メインボード (CXD5602PWBMAIN1E)
  → Lチカ, GNSS受信("gnss_nmea.ino"), QZQSM受信("gnss_qzqsm.ino")
• Spresense LTE拡張ボード (CXD5602PWBLM1JUL)
  → HTTP受信("LteWebClient.ino"), SD("Files.ino")
• BLE1507 (BLE serialization firmware)
  → BLE notify("BLE1507_notify.ino") + アプリ "LightBlue" on Google Pixel 7

Spresenseについて

以前からGNSS搭載という珍しいマイコンとして認知はしており、IO電圧も1.1Vと例を見ない構成で面白いと思っていました。
開発を始める前に、Spresense Arduino 開発ガイドや、CXD5602のデータシートを眺めていたところ、電源回りの機能が充実している印象をもちました。しっかりと使わないコアの電源は切れるようになっていたので、今回はシングルコアで開発することにしました。いつかしっかりと調整して消費電力を減らしてみたいです。

ケース作成

まずは基板を保護するケースが必要です。公式で販売されているケースはないということで自作しました。
公式サイト(https://developer.spresense.sony-semicon.com/development-guides/?page=hw_design&lang=ja)の寸法や3Dモデルを利用し3DCADで設計を行いました。
3Dモデルは以下の場所に公開しています。
https://drive.google.com/drive/folders/1eTaw1Ai7B0ob_xA8lnPOSTXNtiaKFoHm?usp=sharing

多色印刷対応3Dプリンターで作成した完成写真が以下になります。

みちびき「災危通報」受信デバイスの作成

実現したいこと&利用技術選択

みちびき「災危通報」のメリットとして考えられるのは、可用性です。宇宙からブロードキャストしているため、災害時に携帯ネットワークなどの地上通信インフラが損傷したとしても情報を取得できます。よって活用する場面というのは災害時や圏外にいるときと想定できます。
まず、Spresenseに画面をつけることを考えました。画面があれば生成したタイムラインの確認も用意に行えると思いましたが、数インチ程度では結局視認性が悪いほか、消費電力やリソースも増えます。今回はSpresenseの特徴である低消費電力を活用できるよう、表示機能は他のデバイスに任せることにしました。
Spresenseで受信だけして表示はスマホ/PCで行う上で、送信方法としてはLTE, Bluetooth, Wi-Fi, (音声)を考えました。このうち災害時に屋外や窓際で受信するということを考えるとLTEと音声は不適であり、通信の単純さを考えるとBluetoothがよさそうです。
Bluetoothで受け取る側について、PC/スマホ両対応を行いたいとおもいました。ネイティブアプリとWeb Bluetoothが選択肢にありましたが、同時に開発できるWeb Bluetoothを選択しました。

「災危通報」仕様調査

みちびきの災害・危機管理情報にはDCRとDCXの2種類があります。

DCR（Disaster and Crisis Report）

気象庁や内閣府などの公的機関からの情報を配信します。仕様はみちびき公式サイトで公開されています。

主な特徴：

• L1S信号で送信（1575.42 MHz）
• 250ビット/秒の低速通信で、1メッセージは約4秒で送信完了
• NMEAフォーマットでは$QZQSMセンテンスとして出力される
• ペイロードは212ビットで、HEX形式で53文字

DCRのメッセージカテゴリ：

DCX（Disaster and Crisis eXtended）

DCRを拡張した形式で、自治体などからのより詳細な情報を配信します。仕様はみちびき公式サイトで公開されています。

DCXの特徴：

• DCRと同じL1S信号で送信
• L-Alert（公共情報コモンズ）と連携し、自治体からの避難情報などを配信
• より詳細な地域情報や避難所情報を含むことが可能
• メッセージ構造がDCRとは異なり、CAMFフィールドなどを持つ

DCXのメッセージカテゴリ：

デコーダーでは、メッセージタイプを判別してDCR/DCXそれぞれに対応したパース処理を行っています。

SpresenseのGNSSライブラリでは、QZ_L1Sを選択することでこれらのメッセージを受信できます。

QZQSMデコーダー作成

受信したQZQSMメッセージを人間が読める形式にデコードするWebアプリを作成しました。

QZQSMのペイロードは212ビットで、各フィールドがビット単位で定義されています。JavaScriptでビット抽出を行う関数を実装しました。

function getBits(hexStr, startBit, length) {
    // HEX文字列をビット配列に変換
    let bits = '';
    for (const c of hexStr) {
        bits += parseInt(c, 16).toString(2).padStart(4, '0');
    }
    return parseInt(bits.substring(startBit, startBit + length), 2);
}

地域コードのデコード

災害情報には影響を受ける地域がビットマップ形式で含まれています。これを都道府県名に変換するテーブルを実装しました。

地図上への表示

Leaflet.jsを使用して、影響を受ける地域を地図上にハイライト表示します。日本の都道府県境界データ（GeoJSON）を組み込み、オフラインでも動作するようにしました。

BLE仕様調査

BLE（Bluetooth Low Energy）でデータを送信する際、MTU（Maximum Transmission Unit）の制限があります。
BLEのデフォルトMTUは23バイトで、実際のペイロードは20バイトが上限のようです。QZQSMのNMEAセンテンスは約80文字程度あるため、1回のNotificationでは送信できません。そこで、メッセージを20バイトずつに分割して送信し、受信側で再構築する方式を採用しました。

BLEでQZQSM送信

Gnss.select(GPS);
Gnss.select(QZ_L1CA);
Gnss.select(QZ_L1S);  // 災危通報用

QZ_L1Sを選択することで、災危通報を受信できるようになります。

NMEA_InitMask();
NMEA_SetMask(NMEA_QZQSM_ON);  // QZQSMのみ出力

不要なNMEAセンテンスを抑制し、QZQSMのみを出力するようにしています。

分割送信の実装

void sendSplitMessage(const char *message) {
    int len = strlen(message);
    int index = 0;
    const int CHUNK_SIZE = 20;

    while (index < len) {
        int currentSize = min(CHUNK_SIZE, len - index);
        ble1507->writeNotify((uint8_t*)(message + index), currentSize);
        delay(30);  // 受信側の処理時間を確保
        index += currentSize;
    }
}

30msの遅延は分割送信で送った情報が受信側で全て受け取れるように設けています

BLE受信側制作

Web Bluetooth APIを使用して、ブラウザからBLEデバイスに接続します。

接続処理

const BLE_SERVICE_UUID = 0x3802;
const BLE_CHAR_UUID = 0x4a02;
const BLE_DEVICE_NAME = 'SPR-PERIPHERAL';

async function connectBLE() {
    const device = await navigator.bluetooth.requestDevice({
        filters: [{ name: BLE_DEVICE_NAME }],
        optionalServices: [BLE_SERVICE_UUID]
    });

    const server = await device.gatt.connect();
    const service = await server.getPrimaryService(BLE_SERVICE_UUID);
    const characteristic = await service.getCharacteristic(BLE_CHAR_UUID);

    await characteristic.startNotifications();
    characteristic.addEventListener('characteristicvaluechanged', handleNotification);
}

分割メッセージの再構築

let bleBuffer = '';

function handleNotification(event) {
    const chunk = new TextDecoder().decode(event.target.value);
    bleBuffer += chunk;

    // 改行または'*'（NMEAチェックサム開始）で完了判定
    if (bleBuffer.includes('*') && bleBuffer.length >= bleBuffer.lastIndexOf('*') + 3) {
        processQzqsm(bleBuffer);
        bleBuffer = '';
    }
}

受信したチャンクをバッファに蓄積し、NMEAセンテンスが完成したらデコード処理を実行します。

統合テスト

SpresenseとPCを実際にWeb Bluetoothで接続し、表示を行いました。
テスト用のサンプルメッセージとして、火山情報・海上警報・L-Alertの3種類を用意し、いつでもデコーダーの動作確認ができるようにしています。

タイムライン生成機能制作 (デバイス)

実験

まず、状態を推定するためにGNSS+加速度を使うことが考えられました。
GNSSで速度を測定することで、大まかな分類を行い、詰めきれない部分を加速度で補完する想定を持っていました。様々な状態を判別するのにX, Y, Z軸それぞれの加速度について解析することで電車のような一定の加速度や車のようなランダムな加速度などを判別できるかと考えたからです。
しかし、実際にはデバイスの向きが変わるためなかなか難しく、最終的には加速度の大きさを算出し、その波形データを処理して使う方法が一番安定しました。

利用技術選択

その後も様々な実験を行った結果、移動手段の判別には、以下のセンサー・情報を組み合わせて使用するとよいことがわかりました。

データ取得の間隔は次の通りに設定してあります。

• 加速度: 50ms（20Hz）- 高速サンプリングでFFT解析に十分なデータを確保
• 速度: 1s - GNSSの更新レート
• クラウド送信: 10s

LTE時刻同期によるTTFF高速化

実験を行うとき測位に時間がかかり、困りました。
GNSSのCold Start時、TTFF(Time To First Fix)は通常30秒〜数分かかりますが、Hot StartすることでTTFFを大幅に短縮できます。Spresense SDK 開発ガイドを参照すると、直近の測位データなどを保存しておくことでHot Startが可能になり測位時間を大幅に短縮できるそうです。3分ごとに測位データを保存し、電源投入時にはそれらの情報に加えてLTEネットワークから取得した時刻をGNSSに設定することで、測位高速化を図っています。

// LTEから時刻を取得
unsigned long currentTime = lteAccess.getTime();

// JST→UTC変換（LTEはJSTで返ってくる）
if (currentTime > 9 * 3600) {
    currentTime -= 9 * 3600;
}

// GNSSに時刻を設定
SpGnssTime gnssTime;
time_t t = (time_t)currentTime;
struct tm *tm = gmtime(&t);
gnssTime.year = tm->tm_year + 1900;
gnssTime.month = tm->tm_mon + 1;
// ... 以下省略

Gnss.setTime(&gnssTime);
Gnss.start(HOT_START);  // Hot Startで高速測位

移動手段判別機能

状態推定は10秒ごとに行い、以下の5つの状態を判別します：

判定

[10秒分のデータ収集]
        ↓
[速度 ≧ 25km/h ?]
    Yes → [駅から300m以内?]
              Yes → 列車(4)
              No  → 自動車(2)
    No  ↓
[加速度変化が少ない?]
    Yes → 静止(0) ※5サイクル連続で確定
    No  ↓
[FFTで0.8-5Hz検出 かつ 最大加速度>1.75G?]
    Yes → 徒歩/ランニング(1)
    No  → 移動中(3)

状態がチャタリング（頻繁に切り替わる）しないよう、静止への遷移は5サイクル連続で条件を満たした場合のみ行います。また、列車状態では速度が5km/h以上ある限り列車状態を維持します（駅での減速時に誤判定しないため）。

徒歩・ランニング判別 (FFT)

歩行・ランニング状態を判別するために加速度のグラフを眺めていると特徴的な周期(1Hz-4Hzあたり)が見えてきました。

この周波数を検出するために、FFT（高速フーリエ変換）を使うことにしました。

サンプリングは後述する状態判定にも用いるため高速に行っています。

• サンプルレート: 20Hz（50ms間隔）
• FFTサイズ: 256点
• 検出範囲: 0.8Hz〜5.0Hz

FFT実装

ライブラリを使用するまでもないと思ったので実装しました。

void simpleFFT(float *re, float *im, int n) {
    // ビット反転による並び替え
    int j = 0;
    for (int i = 0; i < n - 1; i++) {
        if (i < j) {
            swap(re[i], re[j]);
            swap(im[i], im[j]);
        }
        int k = n / 2;
        while (k <= j) { j -= k; k /= 2; }
        j += k;
    }

    // バタフライ演算
    for (int len = 2; len <= n; len <<= 1) {
        float ang = -2 * M_PI / len;
        float wlen_re = cos(ang), wlen_im = sin(ang);
        for (int i = 0; i < n; i += len) {
            float w_re = 1, w_im = 0;
            for (int k = 0; k < len / 2; k++) {
                // ... バタフライ演算
            }
        }
    }
}

判定ロジック

// DCオフセット除去
float mean = 0;
for (int i = 0; i < accIndex; i++) mean += accData[i];
mean /= accIndex;
for (int i = 0; i < FFT_N; i++) vReal[i] = (i < accIndex) ? accData[i] - mean : 0;

// FFT実行
simpleFFT(vReal, vImag, FFT_N);

// ピーク周波数検出（0.8Hz〜5.0Hz）
float maxMag = 0;
int peakBin = 0;
for (int i = 8; i < 64; i++) {
    float mag = sqrt(vReal[i]*vReal[i] + vImag[i]*vImag[i]);
    if (mag > maxMag) { maxMag = mag; peakBin = i; }
}
float peakFreq = peakBin * (20.0 / 256.0);

// 歩行/ランニング判定
if (peakFreq >= 0.8 && peakFreq <= 5.0 && maxAccel > 1.75 * 9.8) {
    status = STATUS_WALK_RUN;
}

加速度の閾値(1.75G)は、ただ揺れているだけではなく、歩行していることを確認するためのものです。静止状態では1G付近、歩行では1.75〜2G程度、ランニングでは2G以上の瞬間的な加速度が発生することを確認しています。

列車判別 (LTE)

速度だけでは自動車と列車を区別できないため、外部APIを活用して速度が上がった地点(>25km/h)と最寄り駅との距離を判定材料にしています。

HeartRails Express API

無料で利用できる駅情報APIで、緯度経度から最寄り駅とその距離を取得できます。

http://express.heartrails.com/api/json?method=getStations&x={経度}&y={緯度}

レスポンス：

{
  "response": {
    "station": [
      { "name": "博多", "distance": "150m", ... }
    ]
  }
}

レスポンス処理

bool isStationNearby(float lat, float lon) {
    LTEClient stClient;
    if (!stClient.connect("express.heartrails.com", 80)) return false;

    // HTTPリクエスト送信
    stClient.print("GET /api/json?method=getStations&x=");
    stClient.print(String(lon, 6));
    stClient.print("&y=");
    stClient.print(String(lat, 6));
    stClient.println(" HTTP/1.1");
    // ...

    // ストリームパーサーで "distance":"XXXm" を探す
    // 距離が300m以内ならtrueを返す
}

メモリ節約のため、JSONライブラリは使用せず、文字列のストリームパーサーで"distance":"パターンをマッチングしています。

判定ロジック

速度 ≧ 25km/h
    ↓
isStationNearby() 呼び出し
    ↓
距離 ≦ 300m → 列車
距離 > 300m → 自動車

LTEが利用できるためにとれる有用な解決策だと思います。
これで列車との判別はある程度つくようになりましたが、プライバシーの問題や駅前で自動車にのったときなど完璧ではないので列車の状態時に静止したときはすぐに最寄りの駅との距離を毎回測定しています。

車/自転車判別 (AI)

現状では自動車と自転車の判別は実装できていません。両者は速度帯が重なる（自転車でも20-30km/h程度出る）ため、単純なルールベースでは難しいです。
実際に加速度のデータに対してNNCで1D CNNを使ってみたり、FFTした結果に対してのAffineを試しましたが、安定して判別することができませんでした。今後の課題としたいです。

タイムライン生成機能制作 (クラウド+ビューア)

利用技術選択

バックエンド: Cloudflare Workers + D1

選定理由：

• 無料枠が大きい: Workers は1日10万リクエスト、D1 は5GBまで無料
• グローバルエッジ: 低レイテンシでアクセス可能
• サーバーレス: インフラ管理不要
• HTTPで直接アクセス可能: LTEからの送信がシンプルに

DBスキーマ

CREATE TABLE sensor_logs (
    id INTEGER PRIMARY KEY AUTOINCREMENT,
    millis INTEGER,
    latitude REAL,
    longitude REAL,
    altitude REAL,
    acceleration REAL,
    status INTEGER,
    lte_rssi REAL,
    gps_fix INTEGER,
    speed REAL,
    created_at INTEGER DEFAULT (unixepoch())
);

フロントエンド: React + Vite + Leaflet

選定理由：

• React: 自分が使い慣れており、コンポーネントベースで状態管理が容易
• Vite: 高速なビルド
• Leaflet: オープンソースの地図ライブラリ、商用利用も無料
• react-leaflet: ReactとLeafletの統合

タイムライン表示

APIから取得したログデータを、状態ごとにグループ化して表示します。

セグメント化

連続する同じ状態のログをひとつの「セグメント」としてまとめます：

const segments: Segment[] = [];
let currentSegment: Segment | null = null;

for (const log of logs) {
    if (!currentSegment || currentSegment.status !== log.status) {
        if (currentSegment) segments.push(currentSegment);
        currentSegment = {
            status: log.status,
            startTime: log.created_at,
            endTime: log.created_at,
            logs: [log]
        };
    } else {
        currentSegment.endTime = log.created_at;
        currentSegment.logs.push(log);
    }
}

状態ごとの色分け

地図にプロット

Leafletを使用して、移動ルートを地図上に表示します。

Catmull-Rom スプライン補間

GPSの点データをそのまま直線で結ぶとカクカクしたルートになります。Catmull-Romスプライン補間を使用して、滑らかな曲線を生成しています。

function interpolateSpline(points: LatLngExpression[], numPointsPerSegment = 10): LatLngExpression[] {
    const result: LatLngExpression[] = [];

    for (let i = 0; i < points.length - 1; i++) {
        const p0 = points[Math.max(0, i - 1)];
        const p1 = points[i];
        const p2 = points[i + 1];
        const p3 = points[Math.min(points.length - 1, i + 2)];

        for (let t = 0; t < numPointsPerSegment; t++) {
            const s = t / numPointsPerSegment;
            // Catmull-Rom 補間計算
            const lat = 0.5 * (2*p1[0] + (-p0[0]+p2[0])*s + ...);
            const lng = 0.5 * (2*p1[1] + (-p0[1]+p2[1])*s + ...);
            result.push([lat, lng]);
        }
    }
    return result;
}

再生モード（振り返り機能）

スライダーを操作して、その日の移動を時系列で振り返ることができます。再生ボタンを押すと自動的にスライダーが進み、移動の様子をアニメーションで確認できます。

const startPlayback = () => {
    setIsPlaying(true);
    playIntervalRef.current = setInterval(() => {
        setPlaybackIndex(prev => {
            if (prev >= logs.length - 1) {
                setIsPlaying(false);
                return prev;
            }
            return prev + 1;
        });
    }, 500);  // 0.5秒ごとに1点進む
};

状態編集機能

推定された状態が間違っている場合、手動で修正できる機能を実装しました。

時間範囲での一括更新

タイムラインの各セグメントをクリックすると、開始・終了時刻と状態を編集できるモーダルが表示されます。

async function bulkUpdateLogs(startTime: number, endTime: number, status: number) {
    const response = await fetch(`${API_BASE}/api/logs/bulk-update`, {
        method: 'PATCH',
        headers: {
            'Content-Type': 'application/json',
            'X-API-KEY': API_KEY
        },
        body: JSON.stringify({ start_time: startTime, end_time: endTime, status })
    });
    return response.json();
}

バックエンド側では、指定された時間範囲内のすべてのログの状態を一括更新します：

UPDATE sensor_logs SET status = ? WHERE created_at >= ? AND created_at <= ?

動作している画面

消費電力

PC(MacBook Pro) - USB検流計(KWS-X1) - USB-C to USB-A - Spresense
の構成において(GNSS:測位中, LTE:接続, シリアル通信:接続)
10秒間ログを取っている間 → 0.4W-0.5W
FFT計算やLTE通信時 → 0.7-0.8W
の電力を消費していることがわかりました。
十分小さく満足していますが、これから更に減らすためには

• 動作クロックを下げる
• SDではなくFlashに書き込む
• USB給電ではなくバッテリーからの給電を試す
• 状態によって測定頻度を変え、DeepSleepを活用する

今後の展望

実現できたこと

• GNSS + IMU + LTE を組み合わせた状態推定デバイス
• FFTによる歩行/ランニング検出
• 外部APIを活用した列車/自動車の判別
• リアルタイムでのクラウドアップロード
• Webベースのタイムライン/地図ビューア
• みちびき災危通報の受信とBLE転送

今後やりたいこと

1. 自転車判別の実装

   • 機械学習モデルの導入
   • Spresense上でのエッジ推論

2. バス判別

   • バス停APIの活用
   • 速度パターンの分析

3. 省電力化

   • Deep Sleep モードの活用
   • 移動検出時のみGNSS起動

4. オフライン対応強化

   • SD書き込みの最適化
   • 圏外時のバッファリングと再送

5. UIの改善

   • より長い範囲での統計表示
     • （1週間の活動や1年のまとめなど）
   • カレンダービュー

6. ケースの改良

   • 防水対応
   • クリップ/ストラップ取り付け

ソースコード

/*
 * QuickLouTE
 */

// Debug: Comment out to disable LTE
#define USE_LTE

// --- Status Codes ---
#define STATUS_STATIONARY 0
#define STATUS_WALK_RUN   1
#define STATUS_CAR        2
#define STATUS_MOVEMENT   3
#define STATUS_TRAIN      4

#include <GNSS.h>
#include <Wire.h>
#include <SDHCI.h>
#include <File.h>
#ifdef USE_LTE
#include <LTE.h>
#endif

// --- LTE Settings (povo 2.0) ---
#define APP_LTE_APN "povo.jp"
#define APP_LTE_USER_NAME "user@povo.jp"
#define APP_LTE_PASSWORD "povo"
#define APP_LTE_IP_TYPE (LTE_NET_IPTYPE_V4V6)
#define APP_LTE_AUTH_TYPE (LTE_NET_AUTHTYPE_CHAP)
#define APP_LTE_RAT (LTE_NET_RAT_CATM)

// 送信先サーバー設定 (HTTP)
const char server[] = "YOUR_SERVER_URL";
const char path[] = "/";
const int port = 80; // HTTPポート

const char apiKey[] = "YOUR_API_KEY";

// LTEクライアント
#ifdef USE_LTE
LTE lteAccess;
LTEClient client;
#endif

// --- Upload Settings ---
const unsigned long UPLOAD_INTERVAL_MS = 10000; // 送信頻度
const unsigned long BACKUP_INTERVAL_MS = 180000; // Flash保存頻度 (3分)
const unsigned long LOG_INTERVAL_MS = 50;
const int BUFFER_SIZE = 20;

// --- Sensor Settings ---
#define MPU6886_ADDR 0x68
#define MPU6886_PWR_MGMT_1 0x6B
#define MPU6886_ACCEL_XOUT 0x3B
const float accScale = 16384.0;

// --- GNSS Settings ---
static SpGnss Gnss;
static SpNavData LatestNavData;
static bool hasGnssUpdate = false;
static enum ParamSat {
  eSatGps,
  eSatGlonass,
  eSatGpsSbas,
  eSatGpsGlonass,
  eSatGpsBeidou,
  eSatGpsGalileo,
  eSatGpsQz1c,
  eSatGpsGlonassQz1c,
  eSatGpsBeidouQz1c,
  eSatGpsGalileoQz1c,
  eSatGpsQz1cQz1S
} satType = eSatGpsGlonassQz1c;

// --- SD & Buffer ---
SDClass SD;
String currentFileName = "";
String dataBuffer = "";
int bufferCounter = 0;

// --- Helper Functions ---
bool setupMPU6886()
{
  Wire1.begin();
  Wire1.setClock(100000);
  Wire1.beginTransmission(MPU6886_ADDR);
  Wire1.write(MPU6886_PWR_MGMT_1);
  Wire1.write(0x00);
  if (Wire1.endTransmission() != 0)
  {
    return false;
  }
  return true;
}

void readAccel(float *ax, float *ay, float *az)
{
  Wire1.beginTransmission(MPU6886_ADDR);
  Wire1.write(MPU6886_ACCEL_XOUT);
  Wire1.endTransmission(false);
  Wire1.requestFrom(MPU6886_ADDR, 6);
  if (Wire1.available() >= 6)
  {
    int16_t raw_ax = Wire1.read() << 8 | Wire1.read();
    int16_t raw_ay = Wire1.read() << 8 | Wire1.read();
    int16_t raw_az = Wire1.read() << 8 | Wire1.read();
    *ax = raw_ax / accScale;
    *ay = raw_ay / accScale;
    *az = raw_az / accScale;
  }
  else
  {
    *ax = 0;
    *ay = 0;
    *az = 0;
  }
}

float calcTotalAccel(float ax, float ay, float az)
{
  return sqrt(ax * ax + ay * ay + az * az) * 9.8;
}

bool setupSD()
{
  if (!SD.begin())
    return false;
  int FileCount = 0;
  do
  {
    currentFileName = "LOG_" + String(FileCount+1000) + ".CSV";
    FileCount++;
    if (FileCount > 9000)
      return false;
  } while (SD.exists(currentFileName));

  File f = SD.open(currentFileName, FILE_WRITE);
  if (!f)
    return false;
  f.println("millis,DateTime,Lat,Lon,numSat,Acc,CurrentStatus");
  f.close();
  return true;
}

// --- API Helper ---
bool isStationNearby(float lat, float lon)
{

#ifdef USE_LTE
  LTEClient stClient;
  // HeartRails Express API (Japan Only)
  // http://express.heartrails.com/api/json?method=getStations&x={lon}&y={lat}
  const char *host = "express.heartrails.com";
  
  // Use separate client connection
  if (!stClient.connect(host, 80))
  {
    Serial.println("Station API Connection Failed.");
    return false;
  }

  stClient.print("GET /api/json?method=getStations&x=");
  stClient.print(String(lon, 6));
  stClient.print("&y=");
  stClient.print(String(lat, 6));
  stClient.println(" HTTP/1.1");
  stClient.print("Host: ");
  stClient.println(host);
  stClient.println("Connection: close");
  stClient.println();

  unsigned long timeout = millis();
  String match = "\"distance\":\"";
  int matchIdx = 0;
  bool found = false;

  // Stream parser to find "distance":"XXXXm" or "XXXXkm"
  while (stClient.connected() && millis() - timeout < 10000)
  {
    if (stClient.available())
    {
      char c = stClient.read();
      // Simple pattern matching for "distance":"
      if (c == match[matchIdx])
      {
        matchIdx++;
        if (matchIdx == match.length())
        {
          // Found tag, now read value until quote
          String distStr = "";
          unsigned long valTimeout = millis();
          while (stClient.connected() && millis() - valTimeout < 2000)
          {
             if (stClient.available())
             {
               char valC = stClient.read();
               if (valC == '"') break; 
               distStr += valC;
             }
          }
          
          float distVal = 99999.0;
          if (distStr.endsWith("km"))
          {
             distVal = distStr.substring(0, distStr.length()-2).toFloat() * 1000.0;
          }
          else if (distStr.endsWith("m"))
          {
             distVal = distStr.substring(0, distStr.length()-1).toFloat();
          }
          
          Serial.print("Station Dist: ");
          Serial.println(distVal);
          
          if (distVal <= 300.0) found = true;
          break; // Found the nearest station, exit loop
        }
      }
      else
      {
        // Reset match
        matchIdx = (c == match[0]) ? 1 : 0; 
      }
    }
  }
  stClient.stop();
  return found;
#else
  Serial.println("Station API check skipped (USE_LTE not defined).");
  return false;
#endif
}

void setupLTE()
{
#ifdef USE_LTE
  Serial.println("Connecting to LTE Network...");
  if (lteAccess.begin() != LTE_SEARCHING)
  {
    Serial.println("LTE Begin Failed.");
    ledOn(PIN_LED3);
    while (1)
      ;
  }
  if (lteAccess.attach(APP_LTE_RAT, APP_LTE_APN, APP_LTE_USER_NAME, APP_LTE_PASSWORD, APP_LTE_AUTH_TYPE, APP_LTE_IP_TYPE) != LTE_READY)
  {
    Serial.println("LTE Attach Failed.");
    ledOn(PIN_LED3);
    while (1)
      ;
  }
  Serial.println("LTE Connected!");
#else
  Serial.println("LTE setup skipped (USE_LTE not defined).");
#endif
}

// HTTP POST送信
void postData(unsigned long currentMillis, float lat, float lon, float alt, float accel, int status, int gpsfix, float speed)
{
#ifdef USE_LTE
  String payload = String(currentMillis) + "," +
                   String(lat, 6) + "," +
                   String(lon, 6) + "," +
                   String(alt, 4) + "," +
                   String(accel, 3) + "," +
                   String(status) + "," +
                   String(1) + "," +
                   String(gpsfix) + "," +
                   String(speed);

  Serial.println("Posting data via HTTP...");

  if (client.connect(server, port))
  {
    Serial.println("Connected to server.");
    client.print("POST ");
    client.print(path);
    client.println(" HTTP/1.1");
    client.print("Host: ");
    client.println(server);
    client.println("Content-Type: text/plain");
    client.print("X-API-KEY: ");
    client.println(apiKey);
    client.print("Content-Length: ");
    client.println(payload.length());
    client.println("Connection: close");
    client.println();

    client.print(payload);

    // レスポンス確認 (デバッグ用)
    unsigned long timeout = millis();
    while (client.connected() && millis() - timeout < 3000)
    {
      if (client.available())
      {
        char c = client.read();
        Serial.print(c); // レスポンスをシリアルに出力して確認
      }
    }
    client.stop();
    Serial.println("\nPost finished.");
  }
  else
  {
    Serial.println("Connection failed");
  }
#else
  Serial.println("Data post skipped (USE_LTE not defined).");
#endif
}

void setup()
{
  Serial.begin(115200);

  ledOn(PIN_LED0);
  ledOn(PIN_LED1);
  ledOn(PIN_LED2);
  ledOn(PIN_LED3);
  delay(1000);
  ledOff(PIN_LED0);
  ledOff(PIN_LED1);
  ledOff(PIN_LED2);
  ledOff(PIN_LED3);

  if (!setupSD())
  {
    ledOn(PIN_LED3);
    while (1)
      ;
  }
  
  if (!setupMPU6886()) 
  {
    Serial.println("MPU6886 Setup Failed");
    while (1)
    {
      ledOn(PIN_LED3);
      delay(100);
      ledOff(PIN_LED3);
      delay(100);
    }
  }

  ledOn(PIN_LED2);
  setupLTE();
  ledOff(PIN_LED2);

  // --- Station API Test (Debug) ---
#ifdef USE_LTE
  Serial.println("Testing Station API...");
  // Test coordinate: 33.5893486, 130.4207793 (Hakata Station)
  if (isStationNearby(33.5893486, 130.4207793))
  {
    Serial.println("[TEST] Station found!");
  }
  else
  {
    Serial.println("[TEST] No station found.");
  }
#endif

  Gnss.setDebugMode(PrintInfo);

  if (Gnss.begin() != 0)
  {
    Serial.println("GNSS begin error");
    ledOn(PIN_LED3);
    while (1)
      ;
  }

  // LTEから時刻を取得しGNSSに設定 (TTFF高速化)
#ifdef USE_LTE
  unsigned long currentTime = lteAccess.getTime();
  if (currentTime != 0)
  {
    Serial.print("LTE Time(Raw): ");
    Serial.println(currentTime);

    // LTE網から取得した時刻がJST(UTC+9)の場合があるため、UTCに補正する (-9時間)
    // 厳密にはGPS時刻とUTCはうるう秒の差があるが、TTFF短縮目的ではUTC時刻設定で十分機能する
    if (currentTime > 9 * 3600)
    {
      currentTime -= 9 * 3600;
    }

    Serial.print("LTE Time(UTC): ");
    Serial.println(currentTime);

    SpGnssTime gnssTime;
    time_t t = (time_t)currentTime;
    struct tm *tm = gmtime(&t);

    gnssTime.year = tm->tm_year + 1900;
    gnssTime.month = tm->tm_mon + 1;
    gnssTime.day = tm->tm_mday;
    gnssTime.hour = tm->tm_hour;
    gnssTime.minute = tm->tm_min;
    gnssTime.sec = tm->tm_sec;
    gnssTime.usec = 0;

    Gnss.setTime(&gnssTime);
    Serial.println("GNSS time synchronized with LTE.");
  }
  else
  {
    Serial.println("Failed to get time from LTE");
  }
#endif

  if (Gnss.start(HOT_START) != 0)
  {
    Serial.println("GNSS start error");
    ledOn(PIN_LED3);
    while (1)
      ;
  }
  Gnss.select(GPS);
  Gnss.select(GLONASS);
  Gnss.select(QZ_L1CA);

  dataBuffer.reserve(2048);
  Serial.println("System Ready.");
}

// --- FFT Helper ---
void simpleFFT(float *re, float *im, int n)
{
  int j = 0;
  for (int i = 0; i < n - 1; i++)
  {
    if (i < j)
    {
      float temp = re[i];
      re[i] = re[j];
      re[j] = temp;
      temp = im[i];
      im[i] = im[j];
      im[j] = temp;
    }
    int k = n / 2;
    while (k <= j)
    {
      j -= k;
      k /= 2;
    }
    j += k;
  }
  for (int len = 2; len <= n; len <<= 1)
  {
    float ang = 2 * M_PI / len * -1;
    float wlen_re = cos(ang);
    float wlen_im = sin(ang);
    for (int i = 0; i < n; i += len)
    {
      float w_re = 1;
      float w_im = 0;
      for (int k = 0; k < len / 2; k++)
      {
        float u_re = re[i + k];
        float u_im = im[i + k];
        float v_re = re[i + k + len / 2] * w_re - im[i + k + len / 2] * w_im;
        float v_im = re[i + k + len / 2] * w_im + im[i + k + len / 2] * w_re;
        re[i + k] = u_re + v_re;
        im[i + k] = u_im + v_im;
        re[i + k + len / 2] = u_re - v_re;
        im[i + k + len / 2] = u_im - v_im;
        float temp_w_re = w_re * wlen_re - w_im * wlen_im;
        w_im = w_re * wlen_im + w_im * wlen_re;
        w_re = temp_w_re;
      }
    }
  }
}

int currentStatus = STATUS_STATIONARY;

void loop()
{
  static unsigned long lastUploadTime = 0;
  static unsigned long lastBackupTime = 0;
  static bool ledState = false;

  const int ACC_SAMPLES = 200;
  const int FFT_N = 256;
  float accData[ACC_SAMPLES];
  float speedData[10];
  int accIndex = 0;
  int speedIndex = 0;
  int accHighCount = 0;

  unsigned long batchStartTime = millis();
  unsigned long lastAccTime = 0;
  unsigned long lastSpeedTime = 0;

  // Reset speed buffer
  for (int i = 0; i < 10; i++)
    speedData[i] = 0.0;

  // 10-second Data Collection Loop
  while (millis() - batchStartTime < 10000)
  {
    unsigned long now = millis();

    // GNSS Update
    if (Gnss.waitUpdate(0))
    {
      Gnss.getNavData(&LatestNavData);
      hasGnssUpdate = true;
      if (LatestNavData.posDataExist && LatestNavData.posFixMode != FixInvalid)
        ledOn(PIN_LED1);
      else
        ledOff(PIN_LED1);
    }

    // Acc 50ms interval
    if (now - lastAccTime >= 50)
    {
      // First run fix or increment
      if (lastAccTime == 0)
        lastAccTime = now;
      else
        lastAccTime += 50;

      // Anti-drift
      if (now - lastAccTime > 100)
        lastAccTime = now;

      float ax, ay, az;
      readAccel(&ax, &ay, &az);
      float totalAccel = calcTotalAccel(ax, ay, az);
      if (totalAccel < 0.1)
      {
        Serial.println("Warning: Low Accel Value! Re-initializing MPU6886...");
        setupMPU6886();
        delay(500);
        readAccel(&ax, &ay, &az);
        totalAccel = calcTotalAccel(ax, ay, az);
      }

      // Store to array
      if (accIndex < ACC_SAMPLES)
      {
        accData[accIndex++] = totalAccel;
        // Threshold Check ( > 1.2G)
        // 1G ~ 9.8m/s^2. 1.2G ~ 11.76
        if (totalAccel > 1.15 * 9.8)
        {
          accHighCount++;
        }
      }

      // --- Logging Logic ---
      String line = "";
      line += String(now) + ",";

      float lat = 0.0, lon = 0.0, alt = 0.0, vel = 0.0;
      int numSat = 0, gpsfix = 0;

      if (hasGnssUpdate)
      {
        char timeBuf[32];
        snprintf(timeBuf, sizeof(timeBuf), "%04d/%02d/%02d %02d:%02d:%02d",
                 LatestNavData.time.year, LatestNavData.time.month, LatestNavData.time.day,
                 LatestNavData.time.hour, LatestNavData.time.minute, LatestNavData.time.sec);
        line += String(timeBuf) + ",";
        if (LatestNavData.posFixMode != FixInvalid && LatestNavData.posDataExist)
        {
          lat = LatestNavData.latitude;
          lon = LatestNavData.longitude;
          alt = LatestNavData.altitude;
          vel = LatestNavData.velocity;
        }
        gpsfix = LatestNavData.posFixMode;
        numSat = LatestNavData.numSatellites;
        line += String(lat, 6) + "," + String(lon, 6) + "," + String(numSat);
      }
      else
      {
        line += "Waiting_GNSS,---,---,0";
      }
      line += "," + String(totalAccel, 3);
      line += "," + String(currentStatus);

      dataBuffer += line + "\n";
      bufferCounter++;

      if (bufferCounter >= BUFFER_SIZE)
      {
        File f = SD.open(currentFileName, FILE_WRITE);
        if (f)
        {
          f.print(dataBuffer);
          f.close();
        }
        dataBuffer = "";
        bufferCounter = 0;
        ledState = !ledState;
        if (ledState)
          ledOn(PIN_LED0);
        else
          ledOff(PIN_LED0);
      }
    }

    // Speed 1s interval
    if (now - lastSpeedTime >= 1000)
    {
      lastSpeedTime = now;
      if (speedIndex < 10)
      {
        if (hasGnssUpdate && LatestNavData.posDataExist)
        {
          speedData[speedIndex++] = LatestNavData.velocity; // m/s
        }
        else
        {
          speedData[speedIndex++] = 0.0;
        }
      }
    }
  } // End while 10s

  // Analysis Phase
  float avgSpeed = 0;
  if (speedIndex > 0)
  {
    for (int i = 0; i < speedIndex; i++)
      avgSpeed += speedData[i];
    avgSpeed /= speedIndex;
  }
  float avgSpeedKmh = avgSpeed * 3.6;
  float peakFreq = 0.0;

  // Calculate Max Acceleration
  float maxAccel = 0.0;
  for (int i = 0; i < accIndex; i++)
  {
    if (accData[i] > maxAccel)
      maxAccel = accData[i];
  }

  // Decision Logic
  int calculatedStatus = STATUS_STATIONARY;

  // Train Latching: If already TRAIN and speed >= 5km/h, keep TRAIN status
  if (currentStatus == STATUS_TRAIN && avgSpeedKmh >= 5.0)
  {
    calculatedStatus = STATUS_TRAIN;
  }
  else if (avgSpeedKmh >= 25.0)
  {
    if (hasGnssUpdate && isStationNearby(LatestNavData.latitude, LatestNavData.longitude))
    {
      calculatedStatus = STATUS_TRAIN;
    }
    else
    {
      calculatedStatus = STATUS_CAR;
    }
  }
  else if (accHighCount < 5)
  {
    calculatedStatus = STATUS_STATIONARY; // Stationary (threshold < 5)
  }
  else
  {
    // FFT Check
    float vReal[FFT_N];
    float vImag[FFT_N];

    // Remove DC
    float mean = 0;
    for (int i = 0; i < accIndex; i++)
      mean += accData[i];
    if (accIndex > 0)
      mean /= accIndex;

    for (int i = 0; i < FFT_N; i++)
    {
      vImag[i] = 0;
      if (i < accIndex)
        vReal[i] = accData[i] - mean;
      else
        vReal[i] = 0;
    }

    simpleFFT(vReal, vImag, FFT_N);

    // Find Peak (Magnitude)
    float maxMag = 0;
    int peakBin = 0;
    // Search 0.6Hz to 5Hz.
    // Hz = bin * (SampleRate / N) = bin * (20 / 256) = bin * 0.078125
    // 0.6 / 0.078 = 7.6 -> 8
    // 5.0 / 0.078 = 64
    for (int i = 8; i < 64; i++)
    {
      float mag = sqrt(vReal[i] * vReal[i] + vImag[i] * vImag[i]);
      if (mag > maxMag)
      {
        maxMag = mag;
        peakBin = i;
      }
    }

    peakFreq = peakBin * (20.0 / 256.0);

    // Walk/Run Range
    // Threshold > 1.75G (approx 17.15 m/s^2)
    if (peakFreq >= 0.8 && peakFreq <= 5.0 && maxAccel > 1.75 * 9.8)
    {
      calculatedStatus = STATUS_WALK_RUN;
    }
    else
    {
      // Movement detected (accHighCount >= 5) but frequency is out of walk/run range or too weak
      calculatedStatus = STATUS_MOVEMENT;
    }
  }

  // --- Hysteresis Logic ---
  static int stillCount = 0;

  if (calculatedStatus == STATUS_STATIONARY)
  {
    if (currentStatus == STATUS_TRAIN)
    {
       // Check if near station
       bool stationNearby = false;
       if (hasGnssUpdate)
       {
           // Note: Re-using the API call if necessary. 
           // In original logic, it was called if Speed >= 25. Now we call if Speed is low (Still candidate)
           stationNearby = isStationNearby(LatestNavData.latitude, LatestNavData.longitude);
       }

       if (!stationNearby)
       {
           // Immediately stop if not near a station
           currentStatus = STATUS_STATIONARY;
           stillCount = 0;
       }
       else
       {
           // Near station -> wait 5 cycles
           stillCount++;
           if (stillCount >= 5)
           {
               currentStatus = STATUS_STATIONARY;
               stillCount = 0;
           }
       }
    }
    else if (currentStatus == STATUS_WALK_RUN || currentStatus == STATUS_CAR || currentStatus == STATUS_MOVEMENT)
    {
       // Wait 5 cycles before switching to Still
       stillCount++;
       if (stillCount >= 5)
       {
           currentStatus = STATUS_STATIONARY;
           stillCount = 0;
       }
    }
    else
    {
        // Already Still, or uninitialized
        currentStatus = STATUS_STATIONARY;
        stillCount = 0;
    }
  }
  else
  {
      // Moving -> Switch immediately
      currentStatus = calculatedStatus;
      stillCount = 0;
  }

  Serial.print("Analysis: AccCnt=");
  Serial.print(accHighCount);
  Serial.print(", MaxAcc=");
  Serial.print(maxAccel/9.8);
  Serial.print(", AvgSpd=");
  Serial.print(avgSpeedKmh);
  Serial.print(", PeakFreq=");
  Serial.print(peakFreq);
  Serial.print(", Status=");
  Serial.println(currentStatus);

  // --- LTE送信 (HTTP) ---
  if (millis() - lastUploadTime >= UPLOAD_INTERVAL_MS)
  {
    lastUploadTime = millis();
    // --- Ephemeris Backup ---
    if (millis() - lastBackupTime >= BACKUP_INTERVAL_MS)
    {
      if (hasGnssUpdate && LatestNavData.posFixMode != FixInvalid)
      {
        lastBackupTime = millis();
        if (Gnss.saveEphemeris() == 0)
        {
          Serial.println("Ephemeris & Position saved to Flash.");
        }
        else
        {
          Serial.println("Failed to save Ephemeris.");
        }
      }
    }

    float lat = hasGnssUpdate ? LatestNavData.latitude : 0;
    float lon = hasGnssUpdate ? LatestNavData.longitude : 0;
    float alt = hasGnssUpdate ? LatestNavData.altitude : 0;
    float spd = hasGnssUpdate ? LatestNavData.velocity : 0;
    float accel_last = (accIndex > 0) ? accData[accIndex - 1] : 0;
    
    postData(millis(), lat, lon, alt, accel_last, currentStatus, LatestNavData.posFixMode, spd);
  }
}