Football Player GPS Tracker

#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <TinyGPSPlus.h>
#include <SparkFun_BMI270_Arduino_Library.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

constexpr int GNSS_RX_PIN = 16;
constexpr int GNSS_TX_PIN = 17;
constexpr int IMU_SDA_PIN = 21;
constexpr int IMU_SCL_PIN = 22;
constexpr int SD_CS_PIN = 4;
constexpr int SD_MISO_PIN = 19;
constexpr int SD_MOSI_PIN = 23;
constexpr int SD_SCK_PIN = 18;
constexpr uint32_t LOG_INTERVAL_MS = 200; // 5 records per second
constexpr float SPRINT_THRESHOLD_KMH = 20.0f;
constexpr float ACCEL_EVENT_THRESHOLD_MPS2 = 2.5f;
constexpr uint8_t OLED_ADDRESS = 0x3C;
constexpr int OLED_WIDTH = 128;
constexpr int OLED_HEIGHT = 64;

HardwareSerial gnssSerial(2);
TinyGPSPlus gps;
BMI270 imu;
BLECharacteristic *liveCharacteristic = nullptr;
Adafruit_SSD1306 display(OLED_WIDTH, OLED_HEIGHT, &Wire, -1);

bool sdReady = false;
bool imuReady = false;
bool haveLastPoint = false;
bool sprintActive = false;
float lastLatitude = 0.0f;
float lastLongitude = 0.0f;
float lastSpeedMps = 0.0f;
uint32_t lastSpeedMs = 0;
uint32_t lastLogMs = 0;
uint32_t lastBleMs = 0;
float distanceM = 0.0f;
float maxSpeedKmh = 0.0f;
float lastAccelerationMps2 = 0.0f;
uint32_t sprintCount = 0;
uint32_t accelerationCount = 0;
bool displayReady = false;
char lastDisplayText[128] = "";

void sendUbx(const uint8_t *payload, size_t payloadLength) {
  uint8_t ckA = 0;
  uint8_t ckB = 0;
  for (size_t i = 0; i < payloadLength; ++i) {
    ckA += payload[i];
    ckB += ckA;
  }
  gnssSerial.write(0xB5);
  gnssSerial.write(0x62);
  gnssSerial.write(payload, payloadLength);
  gnssSerial.write(ckA);
  gnssSerial.write(ckB);
}

void configureGnssFor5Hz() {
  // UBX-CFG-VALSET: RAM layer, CFG-RATE-MEAS (0x30210001) = 200 ms.
  const uint8_t setRate5Hz[] = {
    0x06, 0x8A, 0x0C, 0x00,
    0x00, 0x01, 0x00, 0x00,
    0x01, 0x00, 0x21, 0x30,
    0xC8, 0x00, 0x00, 0x00
  };
  sendUbx(setRate5Hz, sizeof(setRate5Hz));
}

void appendHeader() {
  if (!sdReady || SD.exists("/track.csv")) return;
  File file = SD.open("/track.csv", FILE_WRITE);
  if (file) {
    file.println("ms,lat,lon,speed_kmh,distance_m,max_speed_kmh,sprints,accel_mps2,accel_events,ax,ay,az,gx,gy,gz");
    file.close();
  }
}

void updateDisplay() {
  if (!displayReady) return;

  const bool fixValid = gps.location.isValid() && gps.location.age() < 1500;
  char status[128];
  snprintf(status, sizeof(status),
           "GPS: %s\nSPD: %5.1f km/h\nDST: %6.0f m\nMAX: %5.1f km/h\nSPRINTS: %lu\nSD: %s  BLE: ON",
           fixValid ? "FIX" : "SEARCHING", gps.speed.isValid() ? gps.speed.kmph() : 0.0f,
           distanceM, maxSpeedKmh, static_cast<unsigned long>(sprintCount), sdReady ? "OK" : "NO");
  if (strcmp(status, lastDisplayText) == 0) return;

  strncpy(lastDisplayText, status, sizeof(lastDisplayText) - 1);
  lastDisplayText[sizeof(lastDisplayText) - 1] = '\0';
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print(status);
  display.display();
}

void updateBle() {
  if (liveCharacteristic == nullptr) return;
  char message[160];
  snprintf(message, sizeof(message),
           "distance_m=%.1f;speed_kmh=%.1f;max_kmh=%.1f;sprints=%lu;accel_mps2=%.2f;accel_events=%lu",
           distanceM, gps.speed.isValid() ? gps.speed.kmph() : 0.0f, maxSpeedKmh,
           static_cast<unsigned long>(sprintCount), lastAccelerationMps2,
           static_cast<unsigned long>(accelerationCount));
  liveCharacteristic->setValue(reinterpret_cast<uint8_t *>(message), strlen(message));
  liveCharacteristic->notify();
}

void setupBle() {
  BLEDevice::init("FootballTracker");
  BLEServer *server = BLEDevice::createServer();
  BLEService *service = server->createService("7f770001-8b3d-4ca9-8c64-7d9d21a4c001");
  liveCharacteristic = service->createCharacteristic(
      "7f770002-8b3d-4ca9-8c64-7d9d21a4c001",
      BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
  liveCharacteristic->addDescriptor(new BLE2902());
  liveCharacteristic->setValue("waiting for GNSS fix");
  service->start();
  BLEAdvertising *advertising = BLEDevice::getAdvertising();
  advertising->addServiceUUID(service->getUUID());
  advertising->start();
}

void setup() {
  Serial.begin(115200);
  Wire.begin(IMU_SDA_PIN, IMU_SCL_PIN);
  gnssSerial.begin(9600, SERIAL_8N1, GNSS_RX_PIN, GNSS_TX_PIN);
  delay(300);
  configureGnssFor5Hz();

  int imuStatus = imu.beginI2C();
  imuReady = (imuStatus == BMI2_OK);

  SPI.begin(SD_SCK_PIN, SD_MISO_PIN, SD_MOSI_PIN, SD_CS_PIN);
  sdReady = SD.begin(SD_CS_PIN, SPI);
  appendHeader();
  displayReady = display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS);
  if (displayReady) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("Football Tracker");
    display.println("Starting GNSS...");
    display.display();
  }
  setupBle();
}

void loop() {
  while (gnssSerial.available() > 0) {
    gps.encode(gnssSerial.read());
  }

  const uint32_t now = millis();
  if (now - lastLogMs >= LOG_INTERVAL_MS) {
    lastLogMs = now;
    float ax = NAN, ay = NAN, az = NAN, gx = NAN, gy = NAN, gz = NAN;
    if (imuReady) {
      imu.getSensorData();
      ax = imu.data.accelX;
      ay = imu.data.accelY;
      az = imu.data.accelZ;
      gx = imu.data.gyroX;
      gy = imu.data.gyroY;
      gz = imu.data.gyroZ;
    }

    float speedKmh = gps.speed.isValid() ? gps.speed.kmph() : 0.0f;
    if (gps.location.isValid() && gps.location.age() < 1500) {
      const float latitude = gps.location.lat();
      const float longitude = gps.location.lng();
      if (haveLastPoint) {
        const float stepM = TinyGPSPlus::distanceBetween(lastLatitude, lastLongitude, latitude, longitude);
        // Reject impossible 5 Hz jumps caused by a weak GNSS fix.
        if (stepM < 25.0f) distanceM += stepM;
      }
      lastLatitude = latitude;
      lastLongitude = longitude;
      haveLastPoint = true;
    }

    if (speedKmh > maxSpeedKmh) maxSpeedKmh = speedKmh;
    if (speedKmh >= SPRINT_THRESHOLD_KMH && !sprintActive) {
      ++sprintCount;
      sprintActive = true;
    } else if (speedKmh < (SPRINT_THRESHOLD_KMH - 1.0f)) {
      sprintActive = false;
    }

    if (lastSpeedMs != 0 && now > lastSpeedMs) {
      const float speedMps = speedKmh / 3.6f;
      lastAccelerationMps2 = (speedMps - lastSpeedMps) * 1000.0f / static_cast<float>(now - lastSpeedMs);
      if (lastAccelerationMps2 >= ACCEL_EVENT_THRESHOLD_MPS2) ++accelerationCount;
      lastSpeedMps = speedMps;
    } else {
      lastSpeedMps = speedKmh / 3.6f;
    }
    lastSpeedMs = now;

    if (sdReady) {
      File file = SD.open("/track.csv", FILE_APPEND);
      if (file) {
        file.printf("%lu,%.7f,%.7f,%.2f,%.2f,%.2f,%lu,%.3f,%lu,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",
                    static_cast<unsigned long>(now),
                    gps.location.isValid() ? gps.location.lat() : NAN,
                    gps.location.isValid() ? gps.location.lng() : NAN,
                    speedKmh, distanceM, maxSpeedKmh, static_cast<unsigned long>(sprintCount),
                    lastAccelerationMps2, static_cast<unsigned long>(accelerationCount),
                    ax, ay, az, gx, gy, gz);
        file.close();
      }
    }
  }

  if (now - lastBleMs >= 1000) {
    lastBleMs = now;
    updateBle();
    updateDisplay();
  }
  delay(1);
}