Add GPS, IMU, and barometer sensor suite to BLE bridge firmware
RYS352A GPS on UART2 (GPIO5/6) with PPS interrupt (GPIO7), MPU-9250 IMU and BMP388 barometer on shared I2C bus (GPIO8/9). Sensor data exposed via dedicated BLE service with binary notify characteristics alongside the existing NUS serial bridge. Sensors degrade gracefully if not wired.
This commit is contained in:
parent
e05edb92a0
commit
f218cd468b
4 changed files with 384 additions and 25 deletions
|
|
@ -1,31 +1,99 @@
|
|||
#include <Arduino.h>
|
||||
#include <NimBLEDevice.h>
|
||||
#include <Adafruit_NeoPixel.h>
|
||||
#include <Wire.h>
|
||||
#include <TinyGPSPlus.h>
|
||||
#include <MPU9250.h>
|
||||
#include <Adafruit_BMP3XX.h>
|
||||
#include "config.h"
|
||||
|
||||
// --- Globals ---
|
||||
// --- BLE Sensor Payloads (packed, little-endian) ---
|
||||
|
||||
struct __attribute__((packed)) GpsPayload {
|
||||
int32_t lat_1e7; // latitude × 10^7 (0.0000001° resolution)
|
||||
int32_t lon_1e7; // longitude × 10^7
|
||||
int16_t alt_dm; // altitude in decimeters
|
||||
uint8_t fix_type; // 0=none, 2=2D, 3=3D
|
||||
uint8_t satellites; // visible satellite count
|
||||
uint8_t hdop_10; // HDOP × 10
|
||||
uint8_t pad[3]; // alignment
|
||||
};
|
||||
|
||||
struct __attribute__((packed)) OrientPayload {
|
||||
int16_t heading_10; // magnetic heading × 10 (0-3599)
|
||||
int16_t elevation_10; // tilt from gravity × 10 (-900 to 900)
|
||||
int16_t roll_10; // roll × 10
|
||||
int16_t gyro_x_10; // angular rate × 10 (°/s)
|
||||
int16_t gyro_y_10;
|
||||
int16_t gyro_z_10;
|
||||
};
|
||||
|
||||
struct __attribute__((packed)) EnvPayload {
|
||||
uint32_t pressure_pa; // pressure in Pascals (e.g. 101325)
|
||||
int16_t temp_100; // temperature × 100 (e.g. 2150 = 21.50°C)
|
||||
uint16_t pad;
|
||||
};
|
||||
|
||||
struct __attribute__((packed)) PpsPayload {
|
||||
uint32_t pps_micros; // micros() at last PPS rising edge
|
||||
uint32_t pps_count; // cumulative PPS count since boot
|
||||
};
|
||||
|
||||
// --- Globals: BLE ---
|
||||
|
||||
static NimBLEServer *pServer = nullptr;
|
||||
static NimBLECharacteristic *pTxChar = nullptr; // ESP32 -> Client (notify)
|
||||
static NimBLECharacteristic *pRxChar = nullptr; // Client -> ESP32 (write)
|
||||
static NimBLECharacteristic *pTxChar = nullptr; // NUS: ESP32 -> Client (notify)
|
||||
static NimBLECharacteristic *pRxChar = nullptr; // NUS: Client -> ESP32 (write)
|
||||
static NimBLECharacteristic *pGpsChar = nullptr; // Sensor: GPS position
|
||||
static NimBLECharacteristic *pOrientChar = nullptr; // Sensor: heading/tilt/gyro
|
||||
static NimBLECharacteristic *pEnvChar = nullptr; // Sensor: pressure/temperature
|
||||
static NimBLECharacteristic *pPpsChar = nullptr; // Sensor: PPS timestamp
|
||||
|
||||
// --- Globals: Hardware ---
|
||||
|
||||
static Adafruit_NeoPixel led(LED_COUNT, PIN_LED, NEO_GRB + NEO_KHZ800);
|
||||
static HardwareSerial SerialGPS(2);
|
||||
static TinyGPSPlus gps;
|
||||
static MPU9250 imu(Wire, MPU9250_ADDR);
|
||||
static Adafruit_BMP3XX bmp;
|
||||
|
||||
// --- State ---
|
||||
|
||||
static bool deviceConnected = false;
|
||||
static uint32_t lastActivityMs = 0;
|
||||
static uint32_t lastLedUpdateMs = 0;
|
||||
|
||||
// Coalescing buffer for UART1 RX -> BLE TX
|
||||
// Serial bridge coalescing buffer
|
||||
static uint8_t coalBuf[UART_RX_BUF_SIZE];
|
||||
static size_t coalLen = 0;
|
||||
static uint32_t coalLastByteMs = 0;
|
||||
|
||||
// PPS interrupt state
|
||||
static volatile uint32_t ppsTimestamp = 0;
|
||||
static volatile uint32_t ppsCount = 0;
|
||||
static uint32_t lastPpsNotified = 0;
|
||||
|
||||
// Sensor timing
|
||||
static uint32_t lastGpsReportMs = 0;
|
||||
static uint32_t lastImuReportMs = 0;
|
||||
static uint32_t lastBaroReportMs = 0;
|
||||
static uint32_t lastStatusPrintMs = 0;
|
||||
|
||||
// Sensor availability (graceful degradation if not wired)
|
||||
static bool imuReady = false;
|
||||
static bool baroReady = false;
|
||||
|
||||
// Latest readings cached for USB status line
|
||||
static float snsHeading = 0, snsElevation = 0;
|
||||
static float snsPressureHpa = 0, snsTempC = 0;
|
||||
|
||||
// --- BLE Callbacks ---
|
||||
|
||||
class ServerCallbacks : public NimBLEServerCallbacks {
|
||||
void onConnect(NimBLEServer *server, NimBLEConnInfo &connInfo) override {
|
||||
deviceConnected = true;
|
||||
// Request fast connection interval (7.5ms-15ms) for low latency
|
||||
// params: min_interval, max_interval, latency, supervision_timeout
|
||||
// intervals are in 1.25ms units: 6 = 7.5ms, 12 = 15ms
|
||||
// intervals in 1.25ms units: 6 = 7.5ms, 12 = 15ms
|
||||
server->updateConnParams(connInfo.getConnHandle(), 6, 12, 0, 200);
|
||||
Serial.println("[BLE] Client connected");
|
||||
}
|
||||
|
|
@ -49,6 +117,13 @@ class RxCallbacks : public NimBLECharacteristicCallbacks {
|
|||
}
|
||||
};
|
||||
|
||||
// --- PPS Interrupt ---
|
||||
|
||||
void IRAM_ATTR gpsPpsISR() {
|
||||
ppsTimestamp = micros();
|
||||
ppsCount++;
|
||||
}
|
||||
|
||||
// --- LED Status ---
|
||||
|
||||
static void updateLed() {
|
||||
|
|
@ -71,48 +146,253 @@ static void updateLed() {
|
|||
led.show();
|
||||
}
|
||||
|
||||
// --- BLE Setup ---
|
||||
// --- BLE Setup (NUS only) ---
|
||||
|
||||
static void initBLE() {
|
||||
NimBLEDevice::init(BLE_DEVICE_NAME);
|
||||
NimBLEDevice::setMTU(BLE_MTU);
|
||||
// Set TX power to maximum for range through enclosure
|
||||
// Max TX power for range through enclosure
|
||||
NimBLEDevice::setPower(ESP_PWR_LVL_P9);
|
||||
|
||||
pServer = NimBLEDevice::createServer();
|
||||
pServer->setCallbacks(new ServerCallbacks());
|
||||
|
||||
NimBLEService *pService = pServer->createService(NUS_SERVICE_UUID);
|
||||
// NUS — serial passthrough (unchanged from bridge-only firmware)
|
||||
NimBLEService *pNus = pServer->createService(NUS_SERVICE_UUID);
|
||||
|
||||
// TX characteristic: ESP32 notifies client with data from G2
|
||||
pTxChar = pService->createCharacteristic(
|
||||
pTxChar = pNus->createCharacteristic(
|
||||
NUS_TX_UUID,
|
||||
NIMBLE_PROPERTY::NOTIFY
|
||||
);
|
||||
|
||||
// RX characteristic: client writes data destined for G2
|
||||
pRxChar = pService->createCharacteristic(
|
||||
pRxChar = pNus->createCharacteristic(
|
||||
NUS_RX_UUID,
|
||||
NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR
|
||||
);
|
||||
pRxChar->setCallbacks(new RxCallbacks());
|
||||
|
||||
pService->start();
|
||||
pNus->start();
|
||||
Serial.println("[BLE] NUS service started");
|
||||
}
|
||||
|
||||
// Advertising config
|
||||
// --- Sensor BLE Service ---
|
||||
|
||||
static void initSensorBLE() {
|
||||
NimBLEService *pSns = pServer->createService(SENSOR_SERVICE_UUID);
|
||||
|
||||
pGpsChar = pSns->createCharacteristic(
|
||||
SENSOR_GPS_UUID,
|
||||
NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY
|
||||
);
|
||||
pOrientChar = pSns->createCharacteristic(
|
||||
SENSOR_ORIENT_UUID,
|
||||
NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY
|
||||
);
|
||||
pEnvChar = pSns->createCharacteristic(
|
||||
SENSOR_ENV_UUID,
|
||||
NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY
|
||||
);
|
||||
pPpsChar = pSns->createCharacteristic(
|
||||
SENSOR_PPS_UUID,
|
||||
NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY
|
||||
);
|
||||
|
||||
pSns->start();
|
||||
Serial.println("[BLE] Sensor service started");
|
||||
}
|
||||
|
||||
// --- Start BLE Advertising ---
|
||||
|
||||
static void startAdvertising() {
|
||||
NimBLEAdvertising *pAdv = NimBLEDevice::getAdvertising();
|
||||
pAdv->addServiceUUID(NUS_SERVICE_UUID);
|
||||
// Sensor service discovered via GATT after connect (saves ad packet space)
|
||||
pAdv->setAppearance(0x0080); // Generic Computer
|
||||
NimBLEDevice::startAdvertising();
|
||||
|
||||
Serial.printf("[BLE] Advertising as \"%s\"\n", BLE_DEVICE_NAME);
|
||||
}
|
||||
|
||||
// --- Sensor Hardware Init ---
|
||||
|
||||
static void initSensors() {
|
||||
// I2C bus — call before any library that touches Wire
|
||||
// On ESP32, Wire.begin(sda, scl) locks the pin assignment;
|
||||
// subsequent Wire.begin() calls from libraries are a no-op.
|
||||
Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL);
|
||||
Wire.setClock(I2C_FREQ);
|
||||
Serial.printf("[I2C] Bus initialized on GPIO%d/GPIO%d at %dkHz\n",
|
||||
PIN_I2C_SDA, PIN_I2C_SCL, I2C_FREQ / 1000);
|
||||
|
||||
// MPU-9250 IMU
|
||||
int imuStatus = imu.begin();
|
||||
if (imuStatus < 0) {
|
||||
Serial.printf("[IMU] MPU-9250 not found at 0x%02x (err=%d)\n",
|
||||
MPU9250_ADDR, imuStatus);
|
||||
} else {
|
||||
imu.setAccelRange(MPU9250::ACCEL_RANGE_2G);
|
||||
imu.setGyroRange(MPU9250::GYRO_RANGE_250DPS);
|
||||
imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
|
||||
imu.setSrd(19); // 50Hz internal sample rate: 1000/(1+19)
|
||||
imuReady = true;
|
||||
Serial.printf("[IMU] MPU-9250 found at 0x%02x, calibrating...\n",
|
||||
MPU9250_ADDR);
|
||||
}
|
||||
|
||||
// BMP388 barometer
|
||||
if (!bmp.begin_I2C(BMP388_ADDR, &Wire)) {
|
||||
Serial.printf("[BARO] BMP388 not found at 0x%02x\n", BMP388_ADDR);
|
||||
} else {
|
||||
bmp.setTemperatureOversampling(BMP3_OVERSAMPLING_8X);
|
||||
bmp.setPressureOversampling(BMP3_OVERSAMPLING_4X);
|
||||
bmp.setIIRFilterCoeff(BMP3_IIR_FILTER_COEFF_3);
|
||||
bmp.setOutputDataRate(BMP3_ODR_50_HZ);
|
||||
baroReady = true;
|
||||
Serial.printf("[BARO] BMP388 found at 0x%02x\n", BMP388_ADDR);
|
||||
}
|
||||
|
||||
// GPS UART (UART2) — RYS352A
|
||||
SerialGPS.begin(GPS_BAUD, SERIAL_8N1, PIN_GPS_RX, PIN_GPS_TX);
|
||||
Serial.printf("[GPS] UART2 on GPIO%d/GPIO%d at %d baud, waiting for fix...\n",
|
||||
PIN_GPS_RX, PIN_GPS_TX, GPS_BAUD);
|
||||
|
||||
// PPS interrupt — captures micros() on rising edge
|
||||
pinMode(PIN_GPS_PPS, INPUT);
|
||||
attachInterrupt(digitalPinToInterrupt(PIN_GPS_PPS), gpsPpsISR, RISING);
|
||||
Serial.printf("[PPS] Interrupt attached on GPIO%d\n", PIN_GPS_PPS);
|
||||
}
|
||||
|
||||
// --- Sensor Read Loop ---
|
||||
|
||||
static void readSensors() {
|
||||
uint32_t now = millis();
|
||||
|
||||
// Feed GPS parser from UART2 (every iteration — NMEA bytes trickle in)
|
||||
while (SerialGPS.available()) {
|
||||
gps.encode(SerialGPS.read());
|
||||
}
|
||||
|
||||
// GPS report (1Hz)
|
||||
if (now - lastGpsReportMs >= GPS_REPORT_MS) {
|
||||
lastGpsReportMs = now;
|
||||
|
||||
if (gps.location.isValid()) {
|
||||
GpsPayload gp = {};
|
||||
gp.lat_1e7 = (int32_t)(gps.location.lat() * 1e7);
|
||||
gp.lon_1e7 = (int32_t)(gps.location.lng() * 1e7);
|
||||
gp.alt_dm = gps.altitude.isValid()
|
||||
? (int16_t)(gps.altitude.meters() * 10) : 0;
|
||||
gp.fix_type = gps.altitude.isValid() ? 3 : 2;
|
||||
gp.satellites = (uint8_t)min((unsigned long)gps.satellites.value(),
|
||||
(unsigned long)255);
|
||||
float hdop = gps.hdop.hdop();
|
||||
gp.hdop_10 = (uint8_t)min((int)(hdop * 10), 255);
|
||||
|
||||
if (deviceConnected && pGpsChar) {
|
||||
pGpsChar->setValue((uint8_t *)&gp, sizeof(gp));
|
||||
pGpsChar->notify();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// IMU report (10Hz)
|
||||
if (imuReady && (now - lastImuReportMs >= IMU_REPORT_MS)) {
|
||||
lastImuReportMs = now;
|
||||
imu.readSensor();
|
||||
|
||||
// Heading from magnetometer (uncalibrated — raw mag, no hard/soft iron)
|
||||
float mx = imu.getMagX_uT();
|
||||
float my = imu.getMagY_uT();
|
||||
snsHeading = atan2f(my, mx) * 180.0f / PI;
|
||||
if (snsHeading < 0) snsHeading += 360.0f;
|
||||
|
||||
// Elevation (pitch) and roll from accelerometer gravity vector
|
||||
float ax = imu.getAccelX_mss();
|
||||
float ay = imu.getAccelY_mss();
|
||||
float az = imu.getAccelZ_mss();
|
||||
snsElevation = atan2f(-ax, sqrtf(ay * ay + az * az)) * 180.0f / PI;
|
||||
float roll = atan2f(ay, az) * 180.0f / PI;
|
||||
|
||||
// Gyro rates (rad/s -> deg/s)
|
||||
float gx = imu.getGyroX_rads() * 180.0f / PI;
|
||||
float gy = imu.getGyroY_rads() * 180.0f / PI;
|
||||
float gz = imu.getGyroZ_rads() * 180.0f / PI;
|
||||
|
||||
OrientPayload op = {};
|
||||
op.heading_10 = (int16_t)(snsHeading * 10);
|
||||
op.elevation_10 = (int16_t)(snsElevation * 10);
|
||||
op.roll_10 = (int16_t)(roll * 10);
|
||||
op.gyro_x_10 = (int16_t)(gx * 10);
|
||||
op.gyro_y_10 = (int16_t)(gy * 10);
|
||||
op.gyro_z_10 = (int16_t)(gz * 10);
|
||||
|
||||
if (deviceConnected && pOrientChar) {
|
||||
pOrientChar->setValue((uint8_t *)&op, sizeof(op));
|
||||
pOrientChar->notify();
|
||||
}
|
||||
}
|
||||
|
||||
// Barometer report (1Hz)
|
||||
if (baroReady && (now - lastBaroReportMs >= BARO_REPORT_MS)) {
|
||||
lastBaroReportMs = now;
|
||||
|
||||
if (bmp.performReading()) {
|
||||
snsPressureHpa = bmp.pressure / 100.0f; // Pa -> hPa
|
||||
snsTempC = bmp.temperature;
|
||||
|
||||
EnvPayload ep = {};
|
||||
ep.pressure_pa = (uint32_t)bmp.pressure;
|
||||
ep.temp_100 = (int16_t)(bmp.temperature * 100);
|
||||
|
||||
if (deviceConnected && pEnvChar) {
|
||||
pEnvChar->setValue((uint8_t *)&ep, sizeof(ep));
|
||||
pEnvChar->notify();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// PPS notification (on each new pulse)
|
||||
uint32_t currentPps = ppsCount; // snapshot volatile
|
||||
if (currentPps != lastPpsNotified) {
|
||||
lastPpsNotified = currentPps;
|
||||
|
||||
PpsPayload pp = {};
|
||||
pp.pps_micros = ppsTimestamp;
|
||||
pp.pps_count = currentPps;
|
||||
|
||||
if (deviceConnected && pPpsChar) {
|
||||
pPpsChar->setValue((uint8_t *)&pp, sizeof(pp));
|
||||
pPpsChar->notify();
|
||||
}
|
||||
}
|
||||
|
||||
// USB serial status line (1Hz, human-readable)
|
||||
if (now - lastStatusPrintMs >= STATUS_PRINT_MS) {
|
||||
lastStatusPrintMs = now;
|
||||
|
||||
Serial.printf("[SNS] ");
|
||||
if (gps.location.isValid()) {
|
||||
Serial.printf("lat=%.4f lon=%.4f alt=%.1fm fix=%s sats=%d ",
|
||||
gps.location.lat(), gps.location.lng(),
|
||||
gps.altitude.meters(),
|
||||
gps.altitude.isValid() ? "3D" : "2D",
|
||||
gps.satellites.value());
|
||||
} else {
|
||||
Serial.printf("fix=none sats=%d ", gps.satellites.value());
|
||||
}
|
||||
if (imuReady) {
|
||||
Serial.printf("hdg=%.1f el=%.1f ", snsHeading, snsElevation);
|
||||
}
|
||||
if (baroReady) {
|
||||
Serial.printf("P=%.1fhPa T=%.1fC ", snsPressureHpa, snsTempC);
|
||||
}
|
||||
Serial.printf("pps=%u\n", (uint32_t)ppsCount);
|
||||
}
|
||||
}
|
||||
|
||||
// --- UART Bridge Loop ---
|
||||
|
||||
static void bridgeLoop() {
|
||||
uint32_t now = millis();
|
||||
|
||||
// --- UART1 RX (G2) -> coalesce -> BLE TX + USB echo ---
|
||||
while (Serial1.available()) {
|
||||
if (coalLen < UART_RX_BUF_SIZE) {
|
||||
|
|
@ -155,13 +435,13 @@ static void bridgeLoop() {
|
|||
// --- Arduino Entry Points ---
|
||||
|
||||
void setup() {
|
||||
// USB serial console (UART0 via CP2102N)
|
||||
// USB serial console (UART0 via CH343)
|
||||
Serial.begin(115200);
|
||||
delay(500); // Let USB enumerate
|
||||
Serial.println();
|
||||
Serial.println("=== Travler-G2 BLE Bridge ===");
|
||||
Serial.println("=== Travler-G2 BLE Bridge + Sensors ===");
|
||||
Serial.println("RS-422: 115200 8N1 on GPIO17(TX)/GPIO18(RX)");
|
||||
Serial.println("BLE: Nordic UART Service (NUS)");
|
||||
Serial.println("BLE: NUS (serial) + Sensor Service");
|
||||
Serial.println();
|
||||
|
||||
// RS-422 UART (UART1)
|
||||
|
|
@ -174,13 +454,23 @@ void setup() {
|
|||
led.setPixelColor(0, led.Color(0, 0, LED_BRIGHTNESS)); // Blue at boot
|
||||
led.show();
|
||||
|
||||
// BLE
|
||||
// BLE — NUS service (serial bridge)
|
||||
initBLE();
|
||||
|
||||
// Sensor hardware — I2C, GPS UART, PPS interrupt
|
||||
initSensors();
|
||||
|
||||
// BLE — Sensor service (requires pServer from initBLE)
|
||||
initSensorBLE();
|
||||
|
||||
// Start advertising both services
|
||||
startAdvertising();
|
||||
|
||||
Serial.println("[BOOT] Ready. Waiting for BLE client...");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
bridgeLoop();
|
||||
readSensors();
|
||||
updateLed();
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue