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
@ -15,6 +15,7 @@ with optional IMU and barometric sensors for orientation and refraction correcti
|
||||
**Sensors (optional):**
|
||||
- 1× GY-9250 (MPU-9250) — 9-axis IMU (accelerometer + gyroscope + magnetometer)
|
||||
- 1× BMP388 — barometric pressure + temperature
|
||||
- 1× RYS352A GPS module — observer location + PPS timing
|
||||
|
||||
## Schematic
|
||||
|
||||
@ -204,14 +205,50 @@ Where R is refraction in arcminutes, el is apparent elevation in degrees,
|
||||
P is pressure in hPa, T is temperature in °C. At el=15°, P=1013, T=20°C:
|
||||
R ≈ 3.4 arcmin ≈ 0.057°. Small but meaningful for narrow-beam antennas.
|
||||
|
||||
## GPS — RYS352A
|
||||
|
||||
The RYS352A is a compact GPS module with PPS output. It connects via UART2 and
|
||||
provides observer location for satellite pass prediction and a 1Hz PPS pulse
|
||||
for precise UTC time synchronization.
|
||||
|
||||
```
|
||||
ESP32 GPIO5 (UART2 RX) ◄── RYS352A TX (NMEA sentences out)
|
||||
ESP32 GPIO6 (UART2 TX) ──► RYS352A RX (config commands in, optional)
|
||||
ESP32 GPIO7 ◄── RYS352A PPS (1Hz rising edge, ~100ns jitter)
|
||||
ESP32 3V3 ──► RYS352A VCC
|
||||
ESP32 GND ──► RYS352A GND
|
||||
```
|
||||
|
||||
| Module Pin | ESP32 Pin | Function |
|
||||
|------------|-----------|----------|
|
||||
| VCC | 3V3 | 3.3V power (onboard LDO on most breakouts) |
|
||||
| GND | GND | Ground |
|
||||
| TX | GPIO5 (UART2 RX) | NMEA sentence output at 9600 baud |
|
||||
| RX | GPIO6 (UART2 TX) | UBX/NMEA config input (optional) |
|
||||
| PPS | GPIO7 | 1Hz pulse synchronized to GPS time |
|
||||
|
||||
**PPS (Pulse Per Second):** The RYS352A outputs a precise 1Hz pulse on the
|
||||
rising edge, synchronized to UTC via GPS constellation. The firmware captures
|
||||
this edge via interrupt (`micros()` timestamp) for correlating satellite events
|
||||
with sub-microsecond precision relative to the GPS epoch. The module's RTC
|
||||
battery backup enables warm starts (~5s) after initial cold start fix (~30-60s).
|
||||
|
||||
**UART notes:** The RYS352A defaults to 9600 baud NMEA output. The TX line
|
||||
(GPIO6) is optional — only needed if you want to send UBX configuration
|
||||
commands to change update rate, constellation selection, or enable additional
|
||||
NMEA sentences. The firmware uses TinyGPS++ to parse standard GGA/RMC sentences.
|
||||
|
||||
## Full GPIO Map
|
||||
|
||||
| GPIO | Function | Interface | Notes |
|
||||
|------|----------|-----------|-------|
|
||||
| 17 | RS-422 TX | UART1 TX | → Level shifter → MAX485₁ DI |
|
||||
| 18 | RS-422 RX | UART1 RX | ← Level shifter ← MAX485₂ RO |
|
||||
| 5 | GPS RX | UART2 RX | ← RYS352A TX (NMEA out) |
|
||||
| 6 | GPS TX | UART2 TX | → RYS352A RX (config in) |
|
||||
| 7 | GPS PPS | GPIO interrupt | 1Hz rising edge |
|
||||
| 8 | I2C SDA | I2C | MPU-9250 + BMP388 (shared bus) |
|
||||
| 9 | I2C SCL | I2C | MPU-9250 + BMP388 (shared bus) |
|
||||
| 17 | RS-422 TX | UART1 TX | → Level shifter → MAX485₁ DI |
|
||||
| 18 | RS-422 RX | UART1 RX | ← Level shifter ← MAX485₂ RO |
|
||||
| 38 | RGB LED | WS2812 | Onboard NeoPixel (DevKitC V1.1) |
|
||||
| 43 | USB Console TX | UART0 | CH343 USB-serial (untouched) |
|
||||
| 44 | USB Console RX | UART0 | CH343 USB-serial (untouched) |
|
||||
|
||||
@ -2,8 +2,23 @@
|
||||
|
||||
// --- GPIO Pin Assignments ---
|
||||
// UART1 to RS-422 module via 3.3V<->5V level shifter
|
||||
#define PIN_RS422_TX 17 // ESP32 TX -> level shifter -> MAX490 RXD
|
||||
#define PIN_RS422_RX 18 // MAX490 TXD -> level shifter -> ESP32 RX
|
||||
#define PIN_RS422_TX 17 // ESP32 TX -> level shifter -> MAX485₁ DI
|
||||
#define PIN_RS422_RX 18 // MAX485₂ RO -> level shifter -> ESP32 RX
|
||||
|
||||
// GPS UART (UART2) — RYS352A
|
||||
#define PIN_GPS_RX 5 // ESP32 RX <- GPS TX (NMEA out)
|
||||
#define PIN_GPS_TX 6 // ESP32 TX -> GPS RX (config in, optional)
|
||||
#define PIN_GPS_PPS 7 // 1Hz PPS rising edge (interrupt)
|
||||
#define GPS_BAUD 9600
|
||||
|
||||
// I2C Sensor Bus
|
||||
#define PIN_I2C_SDA 8
|
||||
#define PIN_I2C_SCL 9
|
||||
#define I2C_FREQ 400000 // 400kHz
|
||||
|
||||
// Sensor I2C addresses
|
||||
#define MPU9250_ADDR 0x68
|
||||
#define BMP388_ADDR 0x76
|
||||
|
||||
// Onboard RGB LED (WS2812, DevKitC-1 V1.1)
|
||||
#define PIN_LED 38
|
||||
@ -23,6 +38,13 @@
|
||||
#define NUS_RX_UUID "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" // Client writes here
|
||||
#define NUS_TX_UUID "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" // ESP32 notifies here
|
||||
|
||||
// Sensor Service UUIDs (custom, A0E7xxxx block)
|
||||
#define SENSOR_SERVICE_UUID "A0E70001-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
#define SENSOR_GPS_UUID "A0E70002-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
#define SENSOR_ORIENT_UUID "A0E70003-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
#define SENSOR_ENV_UUID "A0E70004-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
#define SENSOR_PPS_UUID "A0E70005-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
|
||||
// --- Timing ---
|
||||
// Inter-byte coalescing window: collect bytes arriving within this
|
||||
// gap into one BLE notification instead of sending byte-by-byte
|
||||
@ -31,6 +53,12 @@
|
||||
// LED refresh interval
|
||||
#define LED_UPDATE_MS 50
|
||||
|
||||
// Sensor read/report intervals
|
||||
#define GPS_REPORT_MS 1000 // 1Hz GPS position reports
|
||||
#define IMU_REPORT_MS 100 // 10Hz orientation updates
|
||||
#define BARO_REPORT_MS 1000 // 1Hz pressure/temperature
|
||||
#define STATUS_PRINT_MS 1000 // 1Hz USB serial status line
|
||||
|
||||
// --- LED ---
|
||||
#define LED_BRIGHTNESS 30 // 0-255, keep low to avoid blinding in enclosure
|
||||
#define LED_COUNT 1
|
||||
|
||||
@ -7,9 +7,13 @@ board = esp32-s3-devkitc1-n16r8
|
||||
framework = arduino
|
||||
|
||||
; NimBLE for BLE (lighter than BlueDroid), NeoPixel for status LED
|
||||
; TinyGPSPlus for NMEA parsing, MPU9250 for IMU, BMP3XX for barometer
|
||||
lib_deps =
|
||||
h2zero/NimBLE-Arduino@^2.1
|
||||
adafruit/Adafruit NeoPixel@^1.12
|
||||
mikalhart/TinyGPSPlus@^1.1
|
||||
bolderflight/Bolder Flight Systems MPU9250@^1.0
|
||||
adafruit/Adafruit BMP3XX Library@^2.1
|
||||
|
||||
build_flags =
|
||||
; Serial via CH343 UART port, not USB-CDC
|
||||
|
||||
@ -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…
x
Reference in New Issue
Block a user