BeaconRadio — носимый SOS-маячок на LoRaWAN + GPS

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
СИМУЛЯТОР ДЕТЕКТОРА ПАДЕНИЯ маячка (пара к Module_2_Beacon_v3_step2_GPS.ino).

Зачем это самый важный стенд проекта: маячок носит человек с деменцией, и
критическая величина здесь — не координата и не батарея, а РЕШЕНИЕ «человек
упал / не упал». Пропущенное падение — беда не замечена; ложная тревога —
человека дёргают зря и к сигналу перестают относиться серьёзно. Оба провала
живут в одном месте — в детекторе падения по акселерометру MPU6050, и увидеть
его работу на живом железе трудно (падать с платой никто не будет).

Стенд СИНТЕЗИРУЕТ след модуля ускорения |a| (в единицах g) для типовых событий
и прогоняет ТУ ЖЕ трёхфазную машину состояний, что зашита в прошивке
(struct FallDetector в .ino):

  ФАЗА 1 (невесомость): |a| < FF_THR дольше FF_MS  → началось свободное падение
  ФАЗА 2 (удар):        |a| > IMPACT_THR в окне IMPACT_WIN_MS после невесомости
  ФАЗА 3 (неподвижность): после успокоения SETTLE_MS дисперсия |a| за STILL_MS
                          мала (σ² < STILL_SIGMA²) → ПАДЕНИЕ ПОДТВЕРЖДЕНО

Ключевая идея — падение подтверждается ТОЛЬКО если сложились ВСЕ ТРИ фазы
подряд. Поэтому «уронил телефон» (удар без невесомости-стояния-после) и «сел
резко» (нет свободного падения) не дают тревоги, а настоящее падение с
последующей неподвижностью — даёт. Ученик двигает пороги ползунками и видит
на ROC-табло, как растут пропуски и ложные срабатывания.

Значения порогов — 1:1 с прошивкой (FP_* в FallDetector).

Запуск:
    uv run fall_detector_sim.py
Без uv:
    pip install numpy matplotlib && python fall_detector_sim.py
"""

# /// script
# requires-python = ">=3.11"
# dependencies = ["numpy>=1.26", "matplotlib>=3.8"]
# ///

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider, Button

# --- Пороги детектора (1:1 с FallDetector в Module_2_Beacon_v3_step2_GPS.ino) ---
FF_THR_G_DEF = 0.4        # порог невесомости, g
FF_MS = 60                # невесомость должна длиться, мс
IMPACT_THR_G_DEF = 2.8    # порог удара, g
IMPACT_WIN_MS = 500       # удар должен прийти в этом окне после невесомости
SETTLE_MS = 1500          # «звон» после удара не считаем
STILL_MS = 8000           # длительность окна оценки неподвижности
STILL_SIGMA_DEF = 0.05    # порог СКО неподвижности, g (в коде sigma^2 < 0.05^2)

RATE_HZ = 100.0           # частота выборки MPU (SMPLRT в прошивке ~100 Гц)
DT_MS = 1000.0 / RATE_HZ


# =====================================================================
# СИНТЕЗ следа |a| для типовых событий
# =====================================================================
def add_walk(a, t, t0, t1, rng):
    """Обычная ходьба: |a| колеблется вокруг 1g."""
    m = (t >= t0) & (t < t1)
    a[m] += 0.25 * np.sin(2 * np.pi * 2.0 * (t[m] - t0)) + rng.normal(0, 0.06, m.sum())


def add_fall(a, t, tc, rng, ff_dur=0.12, impact=6.0):
    """Настоящее падение: невесомость → удар → неподвижность (лежит)."""
    # 1) свободное падение: |a| проваливается почти в 0 на ff_dur секунд
    ff = (t >= tc) & (t < tc + ff_dur)
    a[ff] = 0.15 + rng.normal(0, 0.03, ff.sum())
    # 2) удар: резкий пик сразу после невесомости
    imp_t = tc + ff_dur
    imp = (t >= imp_t) & (t < imp_t + 0.05)
    a[imp] = impact + rng.normal(0, 0.5, imp.sum())
    # 3) звон 1.5 c, затем лежит неподвижно (|a|≈1g, очень малый разброс)
    ring = (t >= imp_t + 0.05) & (t < imp_t + 0.05 + 1.5)
    a[ring] = 1.0 + 0.4 * np.exp(-3 * (t[ring] - imp_t)) * rng.normal(0, 1, ring.sum())
    still = (t >= imp_t + 1.55) & (t < imp_t + 1.55 + 9.0)
    a[still] = 1.0 + rng.normal(0, 0.015, still.sum())   # почти неподвижно


def add_drop(a, t, tc, rng):
    """Ложное: УРОНИЛ устройство — удар есть, но после него подобрали (движение)."""
    ff = (t >= tc) & (t < tc + 0.10)
    a[ff] = 0.15 + rng.normal(0, 0.03, ff.sum())
    imp = (t >= tc + 0.10) & (t < tc + 0.15)
    a[imp] = 5.0 + rng.normal(0, 0.5, imp.sum())
    # после падения устройство ПОДБИРАЮТ — идёт движение, НЕ неподвижность
    move = (t >= tc + 0.15) & (t < tc + 0.15 + 9.0)
    a[move] = 1.0 + rng.normal(0, 0.35, move.sum())


def add_sit(a, t, tc, rng):
    """Ложное: СЕЛ резко — удар средний, но БЕЗ свободного падения перед ним."""
    imp = (t >= tc) & (t < tc + 0.08)
    a[imp] = 3.2 + rng.normal(0, 0.3, imp.sum())          # удар без невесомости
    still = (t >= tc + 0.08) & (t < tc + 0.08 + 9.0)
    a[still] = 1.0 + rng.normal(0, 0.02, still.sum())     # потом сидит спокойно


def build_scene(seed):
    """Возвращает (t, a, truth_falls) — сцена с 1 падением и 2 провокациями."""
    rng = np.random.default_rng(seed)
    dur = 40.0
    n = int(dur * RATE_HZ)
    t = np.arange(n) / RATE_HZ
    a = np.ones(n)
    add_walk(a, t, 0, dur, rng)
    events = [
        ("fall", 8.0),     # настоящее падение (ground truth)
        ("drop", 20.0),    # уронил устройство (провокация)
        ("sit", 30.0),     # сел резко (провокация)
    ]
    truth = []
    for kind, tc in events:
        if kind == "fall":
            add_fall(a, t, tc, rng); truth.append(tc)
        elif kind == "drop":
            add_drop(a, t, tc, rng)
        elif kind == "sit":
            add_sit(a, t, tc, rng)
    return t, a, truth


# =====================================================================
# ДЕТЕКТОР: копия трёхфазной FSM из прошивки
# =====================================================================
def run_fall_fsm(t, a, ff_thr, impact_thr, still_sigma):
    """
    Повторяет FallDetector::update(). Возвращает список (t_confirm, phase_log),
    где phase_log — массив номеров фаз (0..3) для окраски графика.
    """
    FP_IDLE, FP_FREEFALL, FP_STILL = 0, 1, 2
    phase = FP_IDLE
    ff_active = False
    ff_start = 0.0
    t_phase = 0.0
    ssum = ssum2 = 0.0
    n = 0
    phase_log = np.zeros(len(t), dtype=int)
    confirms = []

    for i, (ti, g) in enumerate(zip(t, a)):
        now_ms = ti * 1000.0
        if phase == FP_IDLE:
            if g < ff_thr:
                if not ff_active:
                    ff_active = True
                    ff_start = now_ms
                elif now_ms - ff_start >= FF_MS:
                    phase = FP_FREEFALL
                    t_phase = now_ms
            else:
                ff_active = False
        elif phase == FP_FREEFALL:
            if g > impact_thr:
                phase = FP_STILL
                t_phase = now_ms
                ssum = ssum2 = 0.0
                n = 0
            elif now_ms - t_phase > IMPACT_WIN_MS:
                phase = FP_IDLE
                ff_active = False
        elif phase == FP_STILL:
            dt = now_ms - t_phase
            if dt >= SETTLE_MS:
                ssum += g
                ssum2 += g * g
                n += 1
                if dt >= SETTLE_MS + STILL_MS:
                    mean = ssum / n
                    var = ssum2 / n - mean * mean
                    fall = (n > 100) and (var < still_sigma ** 2)
                    if fall:
                        confirms.append(ti)
                    phase = FP_IDLE
                    ff_active = False
        phase_log[i] = phase
    return confirms, phase_log


def score(confirms, truth, tol=10.0):
    """Падение засчитано, если подтверждение пришло в пределах tol c после старта."""
    hits = 0
    matched = [False] * len(confirms)
    for tm in truth:
        for k, tc in enumerate(confirms):
            if tm <= tc <= tm + tol:
                hits += 1
                matched[k] = True
                break
    false_alarms = sum(1 for m in matched if not m)
    misses = len(truth) - hits
    return hits, misses, false_alarms


# =====================================================================
# Визуализация
# =====================================================================
def main():
    st = {"seed": 1, "ff": FF_THR_G_DEF, "impact": IMPACT_THR_G_DEF,
          "sigma": STILL_SIGMA_DEF}

    fig = plt.figure(figsize=(12, 7))
    fig.suptitle("Маячок SOS: детектор падения — 3 фазы и настройка порогов",
                 fontsize=13, fontweight="bold")
    ax = fig.add_axes([0.08, 0.40, 0.88, 0.48])

    phase_colors = {1: ("#3b82f6", "невесомость"),
                    2: ("#f59e0b", "удар→неподвижность")}

    def redraw(_=None):
        t, a, truth = build_scene(st["seed"])
        confirms, plog = run_fall_fsm(t, a, st["ff"], st["impact"], st["sigma"])
        hits, misses, fa = score(confirms, truth)

        ax.clear()
        ax.plot(t, a, "-", lw=0.8, color="#334155", label="|a|, g")
        # пороги
        ax.axhline(st["ff"], color="#3b82f6", ls=":", lw=1.3)
        ax.text(0.2, st["ff"] + 0.1, f"невесомость <{st['ff']:.2f}g",
                color="#3b82f6", fontsize=8)
        ax.axhline(st["impact"], color="#ef4444", ls=":", lw=1.3)
        ax.text(0.2, st["impact"] + 0.1, f"удар >{st['impact']:.1f}g",
                color="#ef4444", fontsize=8)
        ax.axhline(1.0, color="#999", lw=0.6)
        # подсветка активных фаз FSM
        for ph, (col, _lab) in phase_colors.items():
            mask = plog == ph
            if mask.any():
                ax.fill_between(t, 0, a, where=mask, color=col, alpha=0.15, step="mid")
        # ground truth и подтверждения
        for tm in truth:
            ax.axvline(tm, color="#111", ls="--", lw=1.0, alpha=0.7)
        for tc in confirms:
            ax.axvline(tc, color="#22c55e", lw=2.0, alpha=0.8)
        ax.set_xlim(0, t[-1])
        ax.set_ylim(-0.3, max(7.0, st["impact"] + 1))
        ax.set_xlabel("время, c")
        ax.set_ylabel("|ускорение|, g")
        ax.set_title(
            f"Падений (правда): {len(truth)}  |  поймано: {hits}  |  "
            f"пропущено: {misses}  |  ложных: {fa}\n"
            "чёрный пунктир — реальное падение, зелёная линия — подтверждение FSM, "
            "провокации на t=20 (уронил) и t=30 (сел)",
            fontsize=9)
        ax.grid(alpha=0.2)
        ax.legend(loc="upper right", fontsize=8)
        fig.canvas.draw_idle()

    specs = [
        ("Порог невесомости, g", "ff", 0.1, 0.9, FF_THR_G_DEF, 0.05),
        ("Порог удара, g", "impact", 1.5, 5.0, IMPACT_THR_G_DEF, 0.1),
        ("Порог неподвижности σ, g", "sigma", 0.01, 0.30, STILL_SIGMA_DEF, 0.01),
    ]
    for i, (label, key, lo, hi, init, step) in enumerate(specs):
        axs = fig.add_axes([0.14, 0.26 - i * 0.05, 0.6, 0.03])
        s = Slider(axs, label, lo, hi, valinit=init, valstep=step)

        def make_cb(k):
            def cb(v):
                st[k] = v
                redraw()
            return cb
        s.on_changed(make_cb(key))

    ax_new = fig.add_axes([0.80, 0.13, 0.15, 0.05])
    b_new = Button(ax_new, "Новый шум")
    b_new.on_clicked(lambda _: (st.__setitem__("seed", st["seed"] + 1), redraw()))

    redraw()
    plt.show()


if __name__ == "__main__":
    main()

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597

/*
  ============================================================================
  MODULE 2 BEACON v3 — ШАГ 2: + НАСТОЯЩИЙ GPS (NEO-M8N, USART1 PA10/PA9, 9600)
  ============================================================================
  Радио, FSM, MPU6050, кнопка — без изменений (шаг 1). deepSleep пока НЕ включён —
  вместо него delay(), чтобы жила консоль (шаг 3 плана возврата).

  Железо:
    E77-900MBL (STM32WLE5CCU6), RadioLib 7.7.1, Arduino STM32 official core
    RF-ключ: PA6=TXEN, PA7=RXEN, только RFO_HP, OCP 140 мА
    MPU6050 (GY-521): I2C1 SCL=PB6 (конт.24), SDA=PB7 (конт.25), INT=PB4 (конт.22)
    Кнопка SOS: внешняя пушбаттон PA0 (конт.27) -> GND, INPUT_PULLUP
    Buzzer: PB5 (конт.23)

  ЗАПРЕТЫ (проверены болью):
    PB0 = VDDTCXO -> НЕ GPIO (err -707)
    PA2/PA3 = консоль -> GPS туда нельзя (GPS будет на шаге 2, USART1 PA9/PA10)

  FSM:
    S_ACQUIRE  — поиск фикса GPS (таймаут 60 с) + опрос детектора падения + кнопки
    S_TRANSMIT — сборка пакета, TX FPort 10
    S_PREALARM — 10 с прерывистый buzzer, короткое нажатие кнопки = отмена
    S_ALARM    — TX FPort 20 каждые 60 с, длинное нажатие (>=2 c) = отмена
    S_SLEEP    — пауза TX_PERIOD_S + джиттер radio.random(60) с (пока delay)
    СТАРТ = S_ACQUIRE (первый TX сразу после включения)

  Кнопка:
    в покое/ACQUIRE: короткое нажатие (>=50 мс дебаунс) = SOS -> S_PREALARM?
      нет: SOS — осознанное действие, поэтому SOS = сразу S_ALARM;
      S_PREALARM — только для АВТОдетекции (падение/неподвижность).
    в S_PREALARM: короткое нажатие = отмена («я в порядке»)
    в S_ALARM: удержание >=2 с = отмена тревоги

  Payload 12 Б LE — НЕ МЕНЯТЬ (декодер Node-RED):
    [0]=beacon_id [1..4]=lat*1e7 [5..8]=lon*1e7 [9]=status [10]=battery [11]=CRC8
    status: б0 SOS, б1 fall, б2 immobile, б3 gps_valid, б4 low_batt, б5..7 seq
  ============================================================================
*/

#include <RadioLib.h>
#include <Wire.h>
#include <TinyGPSPlus.h>

// ============================ GPS (шаг 2: настоящий NEO-M8N) ============================
// USART1: RX=PA10 <- GPS TX, TX=PA9 -> GPS RX. Консоль (PA2/PA3) НЕ трогаем.
HardwareSerial SerialGPS(PA10, PA9);   // RX, TX
TinyGPSPlus gps;

// ============================ ПИНЫ ============================
#define PIN_BTN     PA0    // конт.27, кнопка -> GND
#define PIN_BUZZER  PB5    // конт.23
#define PIN_MPU_INT PB4    // конт.22, INT MPU (active-low latched)
#define PIN_I2C_SCL PB6    // конт.24
#define PIN_I2C_SDA PB7    // конт.25

// ---------- ключи ABP (те же, что в диагностике) ----------
#define LORAWAN_DEV_ADDR  0x00120997
static uint8_t NWK_S_KEY[] = {
  0x9E, 0xB8, 0x21, 0xD3, 0x14, 0x04, 0x3B, 0xF0, 0x42, 0xD3, 0xBA, 0xD4, 0xD7, 0x6F, 0x1B, 0x4C
};
static uint8_t APP_S_KEY[] = {
  0x88, 0x16, 0x7E, 0xB1, 0x72, 0x3F, 0x8B, 0xEB, 0xDF, 0x3B, 0x4B, 0x1D, 0x5D, 0x2A, 0x61, 0x3B
};

// ---------- параметры ----------
#define LORAWAN_DR       2          // DR2 = SF10/BW125
#define FPORT_TELEMETRY 10
#define FPORT_ALARM     20
#define BEACON_ID     0x02
#define TX_PERIOD_S     600UL       // телеметрия раз в 10 мин (+джиттер)
#define ALARM_REPEAT_S   60UL       // повтор тревоги
#define PREALARM_MS   10000UL       // пред-тревога 10 с
#define LOW_BATT_PCT      20

// GPS НАСТОЯЩИЙ (шаг 2). Мок убран.
#define GPS_BAUD            9600
#define GPS_FIX_TIMEOUT_MS  60000UL   // S_ACQUIRE: ждём фикс не дольше 60 с
#define GPS_SETTLE_MS       12000UL   // дозревание после первого фикса (ранние решения врут на км)
#define GPS_VALID_AGE_MS    60000UL   // фикс свежее 60 c -> ST_GPS_OK=1 в телеметрии
#define ALARM_FRESH_MS      30000UL   // ТЗ: для тревоги фикс должен быть <=30 c
#define ALARM_FIX_RETRY_MS  10000UL   // если несвеж — быстрая попытка, тревогу дольше не держим

// Последние известные координаты (переживают потерю фикса; 0/0 = фикса не было вовсе)
int32_t  lastLatE7 = 0, lastLonE7 = 0;
uint32_t lastFixMs = 0;               // millis() момента последнего валидного фикса
bool     everFixed = false;

// ---------- статусные биты ----------
#define ST_SOS       (1 << 0)
#define ST_FALL      (1 << 1)
#define ST_IMMOBILE  (1 << 2)
#define ST_GPS_OK    (1 << 3)
#define ST_LOW_BATT  (1 << 4)

// ============================ РАДИО (1:1 из диаг-скетча) ============================
STM32WLx _radio = new STM32WLx_Module();

// ПУТЬ Б: неконстантная копия плана, каналы 868.3/868.5 гасим в setup()
LoRaWANBand_t Region = EU868;
LoRaWANNode node(&_radio, &Region);

static const uint32_t rfPins[Module::RFSWITCH_MAX_PINS] =
    {PA6, PA7, RADIOLIB_NC, RADIOLIB_NC, RADIOLIB_NC};
static const Module::RfSwitchMode_t rfTable[] = {
  {STM32WLx::MODE_IDLE,  {LOW,  LOW }},
  {STM32WLx::MODE_RX,    {LOW,  HIGH}},
  {STM32WLx::MODE_TX_HP, {HIGH, LOW }},
  END_OF_MODE_TABLE,
};

// ============================ PAYLOAD ============================
#pragma pack(push, 1)
struct BeaconPayload_t {
  uint8_t  beacon_id;
  int32_t  lat_e7;
  int32_t  lon_e7;
  uint8_t  status;
  uint8_t  battery;
  uint8_t  crc8;
};
#pragma pack(pop)
static_assert(sizeof(BeaconPayload_t) == 12, "payload must be 12 bytes");

uint8_t crc8(const uint8_t* d, size_t len) {
  uint8_t crc = 0;
  while (len--) {
    crc ^= *d++;
    for (uint8_t i = 0; i < 8; i++)
      crc = (crc & 0x80) ? (uint8_t)((crc << 1) ^ 0x07) : (uint8_t)(crc << 1);
  }
  return crc;
}

uint8_t readBatteryPct() {              // VREFINT (TODO: делитель VBAT)
  analogReadResolution(12);
  uint32_t vrefRaw = analogRead(AVREF);
  uint16_t mv = (uint16_t)__LL_ADC_CALC_VREFANALOG_VOLTAGE(
                    vrefRaw, LL_ADC_RESOLUTION_12B);
  int pct = ((int)mv - 3000) * 100 / (3600 - 3000);
  return (uint8_t)constrain(pct, 0, 100);
}

// ---------- GPS: скармливание NMEA и фиксация координат ----------
// Дёргать часто (внутри watchSensors) — буфер UART маленький, NMEA не ждёт.
void feedGPS() {
  while (SerialGPS.available()) gps.encode((char)SerialGPS.read());
  if (gps.location.isValid() && gps.location.isUpdated()) {
    lastLatE7 = (int32_t)llround(gps.location.lat() * 1e7);
    lastLonE7 = (int32_t)llround(gps.location.lng() * 1e7);
    lastFixMs = millis();
    everFixed = true;
  }
}
// возраст последнего фикса, мс (UINT32_MAX = фикса не было никогда)
uint32_t gpsFixAge() { return everFixed ? (millis() - lastFixMs) : UINT32_MAX; }
bool gpsFreshWithin(uint32_t ms) { return gpsFixAge() <= ms; }

void gpsPrintStatus(const char* tag) {   // образовательные логи — НЕ вычищать
  Serial.print(F("[GPS] ")); Serial.print(tag);
  Serial.print(F(": sats=")); Serial.print(gps.satellites.value());
  Serial.print(F(" HDOP=")); Serial.print(gps.hdop.hdop(), 1);
  Serial.print(F(" возраст фикса="));
  if (everFixed) { Serial.print(gpsFixAge() / 1000); Serial.println(F(" c")); }
  else Serial.println(F("нет фикса с включения"));
}

// ============================ BUZZER ============================
#define BUZZ_FREQ 2700              // пассивный зуммер: нужна частота, не уровень
void beep(uint16_t ms) {
  tone(PIN_BUZZER, BUZZ_FREQ);
  delay(ms);
  noTone(PIN_BUZZER);
}
void beepN(uint8_t n) {                 // чекпоинты init (работают без консоли)
  for (uint8_t i = 0; i < n; i++) { beep(80); delay(120); }
}

// ============================ КНОПКА ============================
// Схема: PA0 -> GND, INPUT_PULLUP. Прерывание только выставляет флаг,
// разбор (дебаунс, короткое/длинное) — в основном цикле.
volatile bool btnIrq = false;
void btnIsr() { btnIrq = true; }

// Блокирующе-короткий опрос: вернуть 0 = нет нажатия, 1 = короткое, 2 = длинное (>=2 c)
uint8_t pollButton() {
  if (!btnIrq && digitalRead(PIN_BTN) == HIGH) return 0;
  Serial.print(F("[BTN] irq=")); Serial.print(btnIrq);
  Serial.print(F(" pin=")); Serial.println(digitalRead(PIN_BTN));
  btnIrq = false;
  delay(50);                                    // дебаунс
  if (digitalRead(PIN_BTN) == HIGH) {
    Serial.println(F("[BTN] дребезг/ложное"));
    return 0;
  }
  uint32_t t0 = millis();
  while (digitalRead(PIN_BTN) == LOW) {
    if (millis() - t0 >= 2000) {                // длинное можно отдать сразу
      while (digitalRead(PIN_BTN) == LOW) delay(10); // дождаться отпускания
      Serial.println(F("[BTN] ДЛИННОЕ"));
      return 2;
    }
    delay(10);
  }
  Serial.println(F("[BTN] КОРОТКОЕ"));
  return 1;
}

// ============================ MPU6050 (по регистрам) ============================
#define MPU_ADDR        0x68
#define R_SMPLRT_DIV    0x19
#define R_CONFIG        0x1A
#define R_ACCEL_CONFIG  0x1C
#define R_MOT_THR       0x1F
#define R_MOT_DUR       0x20
#define R_INT_PIN_CFG   0x37
#define R_INT_ENABLE    0x38
#define R_INT_STATUS    0x3A
#define R_ACCEL_XOUT_H  0x3B
#define R_PWR_MGMT_1    0x6B
#define R_WHO_AM_I      0x75

bool mpuPresent = false;

// Все обмены с проверкой кода возврата — отвалившийся датчик не вешает прошивку
bool mpuWrite(uint8_t reg, uint8_t val) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg); Wire.write(val);
  return Wire.endTransmission() == 0;
}
bool mpuRead(uint8_t reg, uint8_t* buf, uint8_t len) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom((uint8_t)MPU_ADDR, len) != len) return false;
  for (uint8_t i = 0; i < len; i++) buf[i] = Wire.read();
  return true;
}

bool mpuInit() {
  uint8_t who;
  if (!mpuRead(R_WHO_AM_I, &who, 1)) {
    Serial.println(F("[MPU] чтение WHO_AM_I не удалось (requestFrom)"));
    return false;
  }
  Serial.print(F("[MPU] WHO_AM_I = 0x")); Serial.println(who, HEX);
  // стабильность шины: WHO_AM_I — константа, разнобой = плохой контакт/провода
  uint8_t w2, bad = 0;
  for (uint8_t i = 0; i < 20; i++) {
    if (!mpuRead(R_WHO_AM_I, &w2, 1) || w2 != who) bad++;
  }
  if (bad) {
    Serial.print(F("[MPU] !!! I2C НЕСТАБИЛЕН: "));
    Serial.print(bad); Serial.println(F("/20 чтений битые — проверь провода/контакты"));
  } else {
    Serial.println(F("[MPU] I2C стабилен (20/20)"));
  }
  if ((who & 0x7E) != 0x68 && who != 0x70 && who != 0x72 && who != 0x74) {
    Serial.println(F("[MPU] незнакомый WHO_AM_I — продолжаю на свой страх"));
    // не return false: чип отвечает на 0x68, пробуем работать
  }
  if (!mpuWrite(R_PWR_MGMT_1, 0x01)) return false;  // wake, clk = gyro X PLL
  delay(50);
  mpuWrite(R_SMPLRT_DIV, 9);                    // 1кГц/(1+9) = 100 Гц
  mpuWrite(R_CONFIG, 0x03);                     // DLPF 44 Гц
  mpuWrite(R_ACCEL_CONFIG, 0x10);               // ±8g
  mpuWrite(R_MOT_THR, 4);                       // motion-interrupt
  mpuWrite(R_MOT_DUR, 5);
  mpuWrite(R_INT_PIN_CFG, 0xB0);                // active-low, open-drain, latched
  mpuWrite(R_INT_ENABLE, 0x40);                 // motion int
  return true;
}

// |a| в единицах g (±8g -> 4096 LSB/g)
bool mpuReadAccelMag(float* mag) {
  uint8_t b[6];
  if (!mpuRead(R_ACCEL_XOUT_H, b, 6)) return false;
  int16_t ax = (b[0] << 8) | b[1];
  int16_t ay = (b[2] << 8) | b[3];
  int16_t az = (b[4] << 8) | b[5];
  float x = ax / 4096.0f, y = ay / 4096.0f, z = az / 4096.0f;
  *mag = sqrtf(x * x + y * y + z * z);
  return true;
}
void mpuClearInt() { uint8_t d; mpuRead(R_INT_STATUS, &d, 1); }

// ---------- детектор падения: 3 фазы по |a| ----------
// невесомость <0.4g >=60 мс -> удар >2.8g в окне 500 мс -> тишина sigma<0.05g 8 с
enum FallPhase : uint8_t { FP_IDLE, FP_FREEFALL, FP_IMPACT, FP_STILL };
struct FallDetector {
  FallPhase phase = FP_IDLE;
  uint32_t  tPhase = 0;       // старт текущей фазы
  uint32_t  ffStart = 0;      // старт невесомости
  bool      ffActive = false;
  // окно для sigma в фазе STILL
  float     sum = 0, sum2 = 0;
  uint16_t  n = 0;

  void reset() { phase = FP_IDLE; ffActive = false; sum = sum2 = 0; n = 0; }

  // вызывать ~100 Гц; true = падение подтверждено
  bool update(float g, uint32_t now) {
    switch (phase) {
      case FP_IDLE:
        if (g < 0.4f) {
          if (!ffActive) { ffActive = true; ffStart = now; }
          else if (now - ffStart >= 60) {
            phase = FP_FREEFALL; tPhase = now;
            Serial.print(F("[FALL] невесомость! |a|=")); Serial.println(g);
          }
        } else ffActive = false;
        break;
      case FP_FREEFALL:                       // ждём удар
        if (g > 2.8f) {
          phase = FP_STILL; tPhase = now; sum = sum2 = 0; n = 0;
          Serial.print(F("[FALL] удар! |a|=")); Serial.print(g);
          Serial.println(F(" — жду 8 c неподвижности"));
        }
        else if (now - tPhase > 500) {
          Serial.println(F("[FALL] удара не было за 500 мс — сброс"));
          reset();
        }
        break;
      case FP_STILL: {                        // успокоение 1.5 c, затем 8 с оцениваем
        uint32_t dt = now - tPhase;
        if (dt < 1500) break;                 // отскоки/звон после удара не считаем
        sum += g; sum2 += g * g; n++;
        if (dt >= 1500 + 8000) {
          float mean = sum / n;
          float var  = sum2 / n - mean * mean;
          bool fall = (n > 100) && (var < 0.05f * 0.05f);
          Serial.print(F("[FALL] итог: sigma^2=")); Serial.print(var, 6);
          Serial.println(fall ? F(" -> ПАДЕНИЕ") : F(" -> движение, сброс"));
          reset();
          return fall;
        }
        break;
      }
      default: reset();
    }
    return false;
  }
};
FallDetector fallDet;

// ============================ FSM ============================
enum State : uint8_t { S_ACQUIRE, S_TRANSMIT, S_PREALARM, S_ALARM, S_SLEEP };
State st = S_ACQUIRE;                  // первый TX сразу после старта

uint8_t alarmBits = 0;                 // ST_SOS / ST_FALL / ST_IMMOBILE
uint8_t seqCounter = 0;

void halt(int16_t s, const char* tag) {
  Serial.print(F("[HALT] ")); Serial.print(tag);
  Serial.print(F(" err=")); Serial.println(s);
  for (;;) { beepN(5); delay(3000); }   // SOS-паттерн и без консоли слышно
}

// ---------- сборка и отправка пакета ----------
int16_t sendBeacon(uint8_t fport) {
  BeaconPayload_t pkt;
  pkt.beacon_id = BEACON_ID;
  pkt.lat_e7    = lastLatE7;            // последние известные (0/0, если фикса не было)
  pkt.lon_e7    = lastLonE7;
  pkt.battery   = readBatteryPct();
  pkt.status    = (gpsFreshWithin(GPS_VALID_AGE_MS) ? ST_GPS_OK : 0)
                | alarmBits
                | (pkt.battery <= LOW_BATT_PCT ? ST_LOW_BATT : 0)
                | ((seqCounter & 0x07) << 5);
  pkt.crc8      = crc8((uint8_t*)&pkt, sizeof(pkt) - 1);
  seqCounter++;

  // duty cycle: подождать разрешения стека
  uint32_t wait = node.timeUntilUplink();
  if (wait > 0) { Serial.print(F("[DC] жду ")); Serial.print(wait);
                  Serial.println(F(" мс")); delay(wait); }

  Serial.print(F("[TX] port=")); Serial.print(fport);
  Serial.print(F(" status=0x")); Serial.print(pkt.status, HEX);
  Serial.print(F(" bat=")); Serial.print(pkt.battery); Serial.println('%');

  uint8_t dlBuf[16]; size_t dlSize = 0;
  int16_t s = node.sendReceive((uint8_t*)&pkt, sizeof(pkt), fport,
                               dlBuf, &dlSize, false);
  if (s < RADIOLIB_ERR_NONE) { Serial.print(F("[UP] ERR=")); Serial.println(s); }
  else { Serial.print(F("[UP] OK FCntUp=")); Serial.println(node.getFCntUp()); }
  return s;
}

// ---------- опрос датчиков в течение ms; выход раньше при событии ----------
// return: 0 ничего, 1 SOS-кнопка, 2 падение
uint8_t watchSensors(uint32_t ms) {
  uint32_t t0 = millis();
  uint32_t lastSample = 0;
  while (millis() - t0 < ms) {
    feedGPS();                          // NMEA течёт постоянно — не даём буферу переполниться
    uint8_t b = pollButton();
    if (b) return 1;                    // любое нажатие вне тревоги = SOS
    if (mpuPresent && millis() - lastSample >= 10) {   // ~100 Гц
      lastSample = millis();
      float g;
      if (mpuReadAccelMag(&g)) {
        if (fallDet.update(g, millis())) return 2;
      }
      if (digitalRead(PIN_MPU_INT) == LOW) mpuClearInt(); // сброс latched INT
    }
    delay(2);
  }
  return 0;
}

// ============================ SETUP ============================
void setup() {
  pinMode(PIN_BUZZER, OUTPUT); digitalWrite(PIN_BUZZER, LOW);
  pinMode(PIN_BTN, INPUT_PULLUP);
  pinMode(PIN_MPU_INT, INPUT_PULLUP);   // open-drain у MPU — подтяжка нужна
  attachInterrupt(digitalPinToInterrupt(PIN_BTN), btnIsr, FALLING);

  Serial.begin(115200);
  delay(1500);
  Serial.println(F("\n=== BEACON v3 step2: radio + FSM + MPU + НАСТОЯЩИЙ GPS ==="));
  beepN(1);                             // чекпоинт 1: живы

  // --- GPS UART (USART1 PA10/PA9). Главный риск: не убить консоль на PA2/PA3 ---
  SerialGPS.begin(GPS_BAUD);
  Serial.println(F("[GPS] SerialGPS.begin(9600). Эта строка видна => консоль жива."));

  // --- MPU6050 (до радио: если I2C стопорится — слышно по чекпоинтам) ---
  Wire.setSCL(PIN_I2C_SCL);
  Wire.setSDA(PIN_I2C_SDA);
  Wire.begin();
  Wire.setTimeout(25);                  // страховка от зависания шины

  // --- I2C-СКАНЕР: кто вообще есть на шине ---
  Serial.println(F("[I2C] сканирую 0x08..0x77..."));
  uint8_t found = 0;
  for (uint8_t a = 0x08; a <= 0x77; a++) {
    Wire.beginTransmission(a);
    uint8_t e = Wire.endTransmission();
    if (e == 0) {
      Serial.print(F("[I2C] найден 0x")); Serial.println(a, HEX);
      found++;
    }
  }
  if (!found) Serial.println(F("[I2C] пусто — проверь VCC/GND/SCL(24)/SDA(25)"));

  mpuPresent = mpuInit();
  Serial.print(F("[MPU] ")); Serial.println(mpuPresent ? F("OK") : F("НЕ НАЙДЕН — еду без него"));
  beepN(2);                             // чекпоинт 2: I2C пройден

  // --- радио: 1:1 из диаг-скетча ---
  _radio.setRfSwitchTable(rfPins, rfTable);
  ConfigLoRa_t config;
  config.frequency = 868;
  int16_t s = _radio.begin(config);
  if (s != RADIOLIB_ERR_NONE) halt(s, "radio.begin");
  _radio.setCurrentLimit(140.0f);

  Region.txFreqs[1] = RADIOLIB_LORAWAN_CHANNEL_NONE;   // ПУТЬ Б: только 868.1
  Region.txFreqs[2] = RADIOLIB_LORAWAN_CHANNEL_NONE;

  node.beginABP(LORAWAN_DEV_ADDR, nullptr, nullptr, NWK_S_KEY, APP_S_KEY);
  node.setADR(false);
  node.setDatarate(LORAWAN_DR);
  node.setDutyCycle(true, 0);
  s = node.activateABP();
  if (s != RADIOLIB_LORAWAN_NEW_SESSION && s != RADIOLIB_ERR_NONE)
    halt(s, "activateABP");
  Serial.println(F("[OK] радио готово"));
  beepN(3);                             // чекпоинт 3: радио живо

  // --- ОТЛАДКА КНОПКИ: 5 c печатаем изменения PA0 (нажми кнопку сейчас) ---
  Serial.println(F("[BTN-TEST] 5 c: жми кнопку, смотрю PA0 (1=отпущена, 0=нажата)"));
  { int last = -1; uint32_t t0 = millis();
    while (millis() - t0 < 5000) {
      int v = digitalRead(PIN_BTN);
      if (v != last) { Serial.print(F("[BTN-TEST] PA0=")); Serial.println(v); last = v; }
      delay(5);
    }
    Serial.print(F("[BTN-TEST] irq за тест: ")); Serial.println(btnIrq);
    btnIrq = false;
  }

  // TODO шаг 3: LowPower.begin(), wakeup-источники (кнопка, MPU INT, таймер)
  // TODO: EEPROM-сохранение ABP-сессии; confirmed uplink для SOS
}

// ============================ LOOP ============================
void loop() {
  switch (st) {

    case S_ACQUIRE: {
      // настоящий GPS: ждём СВЕЖИЙ фикс до GPS_FIX_TIMEOUT_MS,
      // датчики (кнопка+падение) крутятся параллельно внутри watchSensors — не ломать!
      Serial.println(F("[FSM] ACQUIRE (GPS, таймаут 60 c)"));
      uint32_t t0 = millis();
      uint32_t tFirstFix = 0;                   // момент ПЕРВОГО фикса в этом цикле
      uint8_t  ev = 0;
      while (millis() - t0 < GPS_FIX_TIMEOUT_MS) {
        ev = watchSensors(500);                 // ломтями по 0.5 c: между ними смотрим фикс
        if (ev) break;                          // событие важнее фикса
        if (gpsFreshWithin(2000)) {
          if (!tFirstFix) {
            tFirstFix = millis();               // первый фикс грязный — даём дозреть
            Serial.print(F("[GPS] первый фикс за ")); Serial.print((millis() - t0) / 1000);
            Serial.println(F(" c — дозревание 12 c (ранние решения врут на км)"));
          } else if (millis() - tFirstFix >= GPS_SETTLE_MS && gps.hdop.hdop() < 2.5) {
            Serial.print(F("[GPS] дозрел: lat=")); Serial.print(lastLatE7);
            Serial.print(F(" lon=")); Serial.print(lastLonE7);
            Serial.print(F(" HDOP=")); Serial.println(gps.hdop.hdop(), 1);
            break;
          }
        }
      }
      gpsPrintStatus("ACQUIRE итог");
      if (!gpsFreshWithin(2000) && !ev)
        Serial.println(F("[GPS] фикса нет за 60 c — шлю последние известные, gps_valid=0"));
      if (ev == 1) { alarmBits = ST_SOS; st = S_ALARM; }        // кнопка = сразу тревога
      else if (ev == 2) { alarmBits = ST_FALL; st = S_PREALARM; }
      else st = S_TRANSMIT;
      break;
    }

    case S_TRANSMIT:
      Serial.println(F("[FSM] TRANSMIT"));
      sendBeacon(FPORT_TELEMETRY);
      st = S_SLEEP;
      break;

    case S_PREALARM: {
      // 10 с прерывистый buzzer; короткое нажатие = отмена
      Serial.println(F("[FSM] PREALARM 10 c — кнопка = отмена"));
      uint32_t t0 = millis();
      bool cancelled = false;
      while (millis() - t0 < PREALARM_MS) {
        feedGPS();                              // к моменту тревоги фикс должен быть свежим
        if ((millis() / 250) & 1) tone(PIN_BUZZER, BUZZ_FREQ);
        else                      noTone(PIN_BUZZER);
        if (pollButton()) { cancelled = true; break; }
        delay(5);
      }
      noTone(PIN_BUZZER);
      if (cancelled) {
        Serial.println(F("[FSM] пред-тревога ОТМЕНЕНА"));
        alarmBits = 0; fallDet.reset();
        st = S_SLEEP;
      } else st = S_ALARM;
      break;
    }

    case S_ALARM: {
      Serial.println(F("[FSM] ALARM — TX FPort 20, повтор 60 c, удержание 2 c = отмена"));
      beep(600);
      // ТЗ: тревога с фиксом свежее 30 c. Несвеж — быстрая попытка обновить,
      // но тревогу не задерживаем дольше ALARM_FIX_RETRY_MS.
      if (!gpsFreshWithin(ALARM_FRESH_MS)) {
        Serial.println(F("[GPS] фикс несвежий (>30 c) — быстрая попытка до 10 c"));
        uint32_t tFix = millis();
        while (millis() - tFix < ALARM_FIX_RETRY_MS && !gpsFreshWithin(2000)) {
          feedGPS();
          delay(5);
        }
        gpsPrintStatus("ALARM pre-TX");
      }
      sendBeacon(FPORT_ALARM);
      // ждём ALARM_REPEAT_S, следя за кнопкой (длинное = отмена)
      uint32_t t0 = millis();
      while (millis() - t0 < ALARM_REPEAT_S * 1000UL) {
        feedGPS();                              // фикс не должен протухать между повторами
        if (pollButton() == 2) {
          Serial.println(F("[FSM] тревога СНЯТА"));
          alarmBits = 0; fallDet.reset();
          st = S_ACQUIRE;               // после снятия — сразу свежий цикл
          return;
        }
        delay(10);
      }
      break;                            // повтор тревожного TX
    }

    case S_SLEEP: {
      // TODO шаг 3: LowPower.deepSleep(sleepMs) вместо watchSensors/delay
      uint32_t sleepMs = TX_PERIOD_S * 1000UL + (radio_random60() * 1000UL);
      Serial.print(F("[FSM] SLEEP ")); Serial.print(sleepMs / 1000);
      Serial.println(F(" c (delay + датчики; deepSleep = шаг 3)"));
      uint8_t ev = watchSensors(sleepMs);
      if (ev == 1) { alarmBits = ST_SOS; st = S_ALARM; }
      else if (ev == 2) { alarmBits = ST_FALL; st = S_PREALARM; }
      else st = S_ACQUIRE;
      break;
    }
  }
}

// джиттер 0..59 с из радиошума (энтропия SX126x, сида не требует)
uint8_t radio_random60() {
  return (uint8_t)_radio.random(60);
}