LoRa Survivor Detection Beacon

#include <Arduino.h>
#include <SPI.h>
#include <LoRa.h>
#include <TinyGPS++.h>
#include "rescue_net_config.h"

#define LORA_SS 13
#define LORA_RST 14
#define LORA_DIO0 26
#define ULTRASONIC_TRIG 25
#define ULTRASONIC_ECHO 32
#define LED_PIN 2
#define BUZZER_PIN 27


// Forward declarations
uint8_t crc8(const uint8_t *data, size_t length);
void putU16LE(uint8_t *out, uint16_t value);
void putI32LE(uint8_t *out, int32_t value);
float measureCm();
void pruneEdges(uint32_t now);
void addEdge(uint32_t now);
uint8_t motionCount(uint32_t now);
void sensorsUpdate(uint32_t now);
bool hasMotion(uint32_t now);
bool newEdge(uint32_t now);
bool repeatedMotion(uint32_t now);
bool gpsBegin();
void gpsFeed();
void buildPacket(uint8_t type, uint8_t motion, bool motionNow, uint8_t frame[20]);
void sendFrame(uint8_t type, uint32_t now);
void chirp(uint32_t now);

constexpr int GPS_RX_PIN = 16;
constexpr int GPS_TX_PIN = 17;
constexpr uint8_t FRAME_LEN = 20;

TinyGPSPlus gps;
HardwareSerial gpsSerial(2);

bool inBand = false;
uint32_t lastSampleMs = 0;
uint32_t lastEdgeMs = 0;
uint32_t lastNewEdgeMs = 0;
uint32_t edgeHistory[MAX_EDGE_HISTORY] = {};
uint8_t edgeWriteIndex = 0;
uint32_t lastSendMs = 0;
uint32_t buzzerOffMs = 0;
bool radioReady = false;

uint8_t crc8(const uint8_t *data, size_t length) {
  uint8_t crc = 0x00;
  while (length--) {
    crc ^= *data++;
    for (uint8_t bit = 0; bit < 8; ++bit) {
      crc = (crc & 0x80) ? static_cast<uint8_t>((crc << 1) ^ 0x07) : static_cast<uint8_t>(crc << 1);
    }
  }
  return crc;
}

void putU16LE(uint8_t *out, uint16_t value) {
  out[0] = static_cast<uint8_t>(value);
  out[1] = static_cast<uint8_t>(value >> 8);
}

void putI32LE(uint8_t *out, int32_t value) {
  uint32_t raw = static_cast<uint32_t>(value);
  out[0] = static_cast<uint8_t>(raw);
  out[1] = static_cast<uint8_t>(raw >> 8);
  out[2] = static_cast<uint8_t>(raw >> 16);
  out[3] = static_cast<uint8_t>(raw >> 24);
}

float measureCm() {
  digitalWrite(ULTRASONIC_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(ULTRASONIC_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRASONIC_TRIG, LOW);
  const unsigned long duration = pulseIn(ULTRASONIC_ECHO, HIGH, 20000UL);
  return duration == 0 ? -1.0f : static_cast<float>(duration) * 0.0343f / 2.0f;
}

void pruneEdges(uint32_t now) {
  for (uint8_t i = 0; i < MAX_EDGE_HISTORY; ++i) {
    if (edgeHistory[i] != 0 && now - edgeHistory[i] > MOTION_WINDOW_MS) edgeHistory[i] = 0;
  }
}

void addEdge(uint32_t now) {
  edgeHistory[edgeWriteIndex] = now;
  edgeWriteIndex = static_cast<uint8_t>((edgeWriteIndex + 1) % MAX_EDGE_HISTORY);
  lastEdgeMs = now;
  lastNewEdgeMs = now;
}

uint8_t motionCount(uint32_t now) {
  pruneEdges(now);
  uint8_t count = 0;
  for (uint8_t i = 0; i < MAX_EDGE_HISTORY; ++i) if (edgeHistory[i] != 0) ++count;
  return count;
}

void sensorsUpdate(uint32_t now) {
  if (now - lastSampleMs < ULTRASONIC_SAMPLE_MS) return;
  lastSampleMs = now;
  const float cm = measureCm();
  const bool nowInBand = cm >= ULTRASONIC_MIN_CM && cm <= ULTRASONIC_MAX_CM;
  if (nowInBand && !inBand && (lastEdgeMs == 0 || now - lastEdgeMs >= EDGE_DEBOUNCE_MS)) addEdge(now);
  inBand = nowInBand;
}

bool hasMotion(uint32_t now) { return inBand || (lastEdgeMs != 0 && now - lastEdgeMs <= MOTION_WINDOW_MS); }
bool newEdge(uint32_t now) { return lastNewEdgeMs != 0 && now - lastNewEdgeMs <= EDGE_ACTIVE_MS; }
bool repeatedMotion(uint32_t now) { return motionCount(now) >= REPEAT_EDGE_MIN; }

bool gpsBegin() {
  const uint32_t bauds[] = {9600, 115200, 38400, 57600};
  for (uint32_t baud : bauds) {
    gpsSerial.end();
    gpsSerial.begin(baud, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
    const uint32_t started = millis();
    uint8_t dollarCount = 0;
    while (millis() - started < 900) {
      while (gpsSerial.available()) {
        const char c = static_cast<char>(gpsSerial.read());
        gps.encode(c);
        if (c == '$') ++dollarCount;
      }
      if (dollarCount >= 3) return true;
      delay(1);
    }
  }
  return false;
}

void gpsFeed() { while (gpsSerial.available()) gps.encode(static_cast<char>(gpsSerial.read())); }

void buildPacket(uint8_t type, uint8_t motion, bool motionNow, uint8_t frame[FRAME_LEN]) {
  const bool fixed = gps.location.isValid() && gps.location.age() < 10000;
  uint8_t flags = 0;
  if (motionNow) flags |= 0x01;
  if (fixed) flags |= 0x10;
  frame[0] = 0xAA;
  frame[1] = MODULE_ID;
  frame[2] = type;
  frame[3] = flags;
  frame[4] = motion;
  putU16LE(&frame[5], 0); // MIC_ENABLED = 0
  putU16LE(&frame[7], 0); // BATTERY_PIN = -1
  putI32LE(&frame[9], fixed ? static_cast<int32_t>(gps.location.lat() * 10000000.0) : 0);
  putI32LE(&frame[13], fixed ? static_cast<int32_t>(gps.location.lng() * 10000000.0) : 0);
  putU16LE(&frame[17], fixed && gps.altitude.isValid() ? static_cast<uint16_t>(max(0.0, gps.altitude.meters())) : 0);
  frame[19] = crc8(frame, 19);
}

void sendFrame(uint8_t type, uint32_t now) {
  uint8_t frame[FRAME_LEN];
  buildPacket(type, motionCount(now), hasMotion(now), frame);
  LoRa.beginPacket();
  LoRa.write(frame, FRAME_LEN);
  LoRa.endPacket();
  Serial.print("[FRAME] ");
  for (uint8_t i = 0; i < FRAME_LEN; ++i) {
    if (frame[i] < 16) Serial.print('0');
    Serial.print(frame[i], HEX);
    Serial.print(i == FRAME_LEN - 1 ? '\n' : ' ');
  }
}

void chirp(uint32_t now) {
  digitalWrite(BUZZER_PIN, HIGH);
  buzzerOffMs = now + 120;
}

void setup() {
  Serial.begin(115200);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(ULTRASONIC_TRIG, OUTPUT);
  pinMode(ULTRASONIC_ECHO, INPUT);
  digitalWrite(ULTRASONIC_TRIG, LOW);
  digitalWrite(BUZZER_PIN, LOW);
  gpsBegin();

  SPI.begin(18, 19, 23, LORA_SS);
  LoRa.setPins(LORA_SS, LORA_RST, LORA_DIO0);
  radioReady = LoRa.begin(LORA_FREQ);
  if (radioReady) {
    LoRa.setSpreadingFactor(9);
    LoRa.setCodingRate4(5);
    LoRa.setSyncWord(0x12);
    LoRa.setTxPower(20);
  }

  const uint32_t settleUntil = millis() + 3000;
  while (millis() < settleUntil) {
    digitalWrite(LED_PIN, (millis() / 250) % 2);
    delay(10);
  }
  digitalWrite(LED_PIN, LOW);
  lastEdgeMs = 0;
  lastNewEdgeMs = 0;
  memset(edgeHistory, 0, sizeof(edgeHistory));
}

void loop() {
  const uint32_t now = millis();
  if (!radioReady) {
    digitalWrite(LED_PIN, (now / 150) % 2);
    return;
  }
  gpsFeed();
  sensorsUpdate(now);
  if (buzzerOffMs != 0 && static_cast<int32_t>(now - buzzerOffMs) >= 0) {
    digitalWrite(BUZZER_PIN, LOW);
    buzzerOffMs = 0;
  }

  const bool alert = newEdge(now) || repeatedMotion(now);
  const uint32_t interval = alert ? SEND_INTERVAL_MS_HIGH : SEND_INTERVAL_MS;
  if (now - lastSendMs >= interval) {
    sendFrame(alert ? 1 : 0, now);
    lastSendMs = now;
    if (alert) chirp(now);
  }
  digitalWrite(LED_PIN, (now / 500) % 2);
}