/*
* ard-blueTEcho-SA by Marco Zonca 05/2025, make bluetooth connection with LilyGo T-Echo / T-Beam + SoftRF Moshe Braner version,
* tested on MB158; this is the SA=Stand Alone generic version (not phisically attached with T-Echo);
*
* the sketch, with the LED Indicator and display circuit, connects LilyGO for receiving NMEA collision warnings sentences
* via bluetooth-LE, evaluates the warning level and produces "bip" sounds with a buzzer (3 levels); additionally
* it shows on 1 of 8 LED the relative direction and on 1 of 5 LEDs the relative altitude of the potentially
* collitioning aircraft; there is also a status LED to show NMEA activity;
* this Stand Alone version includes a 128x64 Oled display to show relative distance and type of potentially collisioning aircraft,
* a recharging circuit for the battery with warning alarm in case of battery level below 3.4V.
*
* Arduino Nano ESP32 MCU with onboard bluetooth-LE module, active buzzer, JST-HX connector, SOIC-24 version of the
* 74HC4067 3.3V 16 channel multiplexer, 8+5+1 = 14 x 3mm LEDs, self resettable fuse, mini switch to on/off, 128x64 OLED dual color
* display, a few other SMD resistors and capacitors 1206 size; to program the MCU I used the free on-line Arduino Cloud;
* the display would be nice to have a dual color type: they sell 128x64 displays with top 16 rows yellow, remaining 48 rows cyan;
*
* About the LEDs let choose 3mm high brightness ones producing very good light at low power consumption
* around 2-6mA max; I suggest blue color for the direction, red/orange/green for altitude, green for NMEA receiving;
* I tested at 3.4V on VIN pin: the power consumption is around 110mA during BLE communication, one LED blinking and data on display,
* this suggest to use a battery with a good capacity one as per Li-Ion 3.7V 1350mA/h model CT3650 or similar;
*
* LED status (without active alarms): all off = circuit off, nmea on = LilyGO connected, nmea 1" blinking = nmea receiving ok,
* red 5" blinking = searching LilyGO but not found yet (10 tries), red 0.5" blinking = LilyGO not found after 10 tries
* (to try searching again do a restart switching off/on);
*
* On LilyGO settings you have to activate bluetooth NMEA output, then use a computer or a phone to see his broacasting name
* that should be something like "SoftRF8605a6-LE" so put his name in the code instead of mine one: deviceName[] = "SoftRF8605a6-LE";
*
*/
#include <ArduinoBLE.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display = Adafruit_SSD1306 (128, 64, &Wire, -1);
///////////////////////////
const boolean isDebug=false;
///////////////////////////
typedef struct {
String dID;
String dDescr;
long dTime;
} record_type;
record_type datatable[16];
const char serUUID[] = "ffe0";
const char charUUID[] = "ffe1";
const char deviceName[] = "SoftRF8605a6-LE";
const byte pinTestBattery=A1;
const byte pinNmea=A2;
const byte pinMux0=A3;
// A4 used by i2c SDA
// A5 used by i2c SCL
const byte pinMux2=A6;
const byte pinBuzzer=A7;
const byte pinMux3=D6;
const byte pinMux1=D7;
const byte pinIntLEDRed=LED_RED;
const byte pinIntLEDGreen=LED_GREEN;
const byte pinIntLEDBlue=LED_BLUE;
const float adc_res=4095.00; // 4095=12bit 1023=10bit, for battery charge level check, depending of your MCU
const int inactivityTimeout=10000;
const byte schemaLeds[16]={12,3,2,1,0,10,9,8,7,12,12,15,14,5,6,11}; // 0,9,10=off, 1-8 direction (in 45) clockwise,
// 11-15 altitude lowest->low->same->high->highest
boolean isDeviceFound=false;
boolean isConnected=false;
boolean isNmeaComplete=false;
boolean ret = false;
boolean isLedToSwitch=false;
boolean isBeepToSwitch=false;
boolean isDisplayClear=false;
boolean isValueDisplayed=false;
byte tryConnect=0;
byte exitStatus=0;
String inputString = "";
String inputStringNext = "";
unsigned long prevNmeaUpdate=0;
byte ALARM_beeps=0;
unsigned long ALARM_timeout=0;
unsigned long ALARM_ledtimeout=0;
unsigned long ALARM_beeptimeout=0;
unsigned long prevMillisService = 0;
int ALARM_period=0;
int ALARM_ledswitchperiod=0;
int ALARM_beepsperiod=0;
byte schemaAlt=0;
byte schemaDir=0;
int previous_alarm=0;
String distm="9999";
String descr="";
unsigned long prevTest=0;
int testFase=0;
float Volt=0.0;
int nmi_gps=0;
int nmi_alarm=0;
int nmi_relBearing=0;
int nmi_alarmType=0;
int nmi_relVertical=0;
long nml_relHorizontal=0;
String nms_ID_LAU="";
String nms_aircraftType="";
String nms_ID_LAA="";
String nms_AcftType="";
int nmi_AcftType=0;
BLEDevice peripheral;
BLEService DataService;
BLECharacteristic DataCharacteristic;
void setup() {
pinMode(pinNmea, OUTPUT);
pinMode(pinBuzzer, OUTPUT);
pinMode(pinMux0, OUTPUT);
pinMode(pinMux1, OUTPUT);
pinMode(pinMux2, OUTPUT);
pinMode(pinMux3, OUTPUT);
pinMode(pinIntLEDRed, OUTPUT);
pinMode(pinIntLEDGreen, OUTPUT);
pinMode(pinIntLEDBlue, OUTPUT);
pinMode(pinTestBattery, INPUT);
digitalWrite(pinNmea,LOW);
digitalWrite(pinBuzzer,LOW);
digitalWrite(pinMux3,HIGH); //4 bit mux=12 as starting point (direction and altitude leds off)
digitalWrite(pinMux2,HIGH);
digitalWrite(pinMux1,LOW);
digitalWrite(pinMux0,LOW);
digitalWrite(pinIntLEDRed,HIGH); //off
digitalWrite(pinIntLEDGreen,HIGH); //off
digitalWrite(pinIntLEDBlue,HIGH); //off
if (isDebug==true) {Serial.begin(9600);}
if (isDebug==true) {delay(5000);}
if (!BLE.begin()) {
dprint("Starting BLE failed! Sketch STOPPED!",true);
while (1);
}
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Display Address 0x3C
inputString.reserve(129);
inputStringNext.reserve(129);
nms_aircraftType.reserve(21);
nms_ID_LAA.reserve(21);
nms_ID_LAU.reserve(21);
nms_AcftType.reserve(3);
distm.reserve(11);
descr.reserve(21);
dprint("BLE started",true);
dprint("Checking VIn",true);
Volt=checkVIn();
initialShow();
dprint("Sketch initialized",true);
delay(100);
} //setup()
void loop() {
if (isDeviceFound==false){ // ------------------------------search device/peripheral and check everything
if (tryConnect<10) {
tryConnect++;
dprint("Searching for peripheral/Device, try nr. ",false);
if (isDebug==true) {Serial.println(tryConnect);}
setLed(schemaLeds[13]); // one time red=on (error & retry)
delay(200);
setLed(schemaLeds[0]); // all off
deviceConnect();
}else{
dprint("Device not available/unknown after 10 tries! Sketch STOPPED!",true);
while (1){
setLed(schemaLeds[0]); // all off
delay(200);
setLed(schemaLeds[13]); // blinking red=on (error & halt)
delay(200);
}//while(1)
}//if(tryConnect<10)
}//if(isDeviceFound==false)
if (isDeviceFound==true){ // ------------------------------device ok, reading data
if (prevNmeaUpdate==0) {prevNmeaUpdate=millis();}
if (DataCharacteristic.valueUpdated()) {
unsigned char data[129] = {};
if (inputStringNext.length()!=0) {inputString=inputStringNext; inputStringNext="";} // retrieve previous remained data after '\n'
DataCharacteristic.readValue(data, 128);
for (int i = 0; i < 128; i++) {
if (data[i] != 0) {
//if (isDebug==true) {Serial.print(char(data[i]));}
if (isNmeaComplete==false){
inputString += char(data[i]); // collect data until '\n'
if (char(data[i]) == '\n') {isNmeaComplete=true;}
}else{
inputStringNext += char(data[i]); // collect remaining data after '\n'
}
}else{
break;
}
}//for(int i...
prevNmeaUpdate=millis();
}//if(DataChar...)
if (millis() > (prevNmeaUpdate+inactivityTimeout)){ //-------check inactivity
dprint("Nmea inactivity timeout.",true);
tryConnect=0;
devDisconnect();
}
}//if(isDeviceFound==true)
if (isNmeaComplete==true){ // ----------------------manage nmea sentences
ret = nmeaExtractData();
if ((ret==true) && (inputString.substring(0,6)=="$PFLAU")) { // *********************************** $PFLAU
dprint("Good : ",false);
if (isDebug==true) {Serial.print(inputString);}
isValueDisplayed=false; // show values on display ones per sentence $PFLAU with alarm
/*
int nmi_gps=0; // GPS 0=no GPS fix, 1=3D fix on ground, 2=GPS fix when airborn
int nmi_alarm=0; // ALARM level 0=none, 1=15-20" to impact, 2=10-15" to impact, 3=0-10" to impact
int nmi_relBearing=0; // RELATIVE BEARING of impact, 0=ahead, 180/-180=behind, -45=left, 45=right, etc.
int nmi_alarmType=0; // ALARM type 0=none, 2=aircraft, 3=obstacle/zone, 4=traffic advisory, 10-FF other ignored
int nmi_relVertical=0; // relative VERTICAL distance in meters, positive=OVER, negative=BELOW
long nml_relHorizontal=0; // relative HORIZONTAL distance in meters
String nms_ID_LAU=""; // aircraft ID from $PFLAU sentence
*/
//if (nms_ID_LAU != "") {nmi_alarm=1;} //*********** TEST TEST TEST ************
/* // simulation of alarm, uncomment to test
switch (testFase) {
case 0:
nmi_alarm=0;
nmi_relBearing=0;
nmi_relVertical=0;
nml_relHorizontal=0;
nms_ID_LAU="";
break;
case 1:
nmi_alarm=1;
nmi_relBearing=-65;
nmi_relVertical=250;
nml_relHorizontal=1234;
nms_ID_LAU="456789";
break;
case 2:
nmi_alarm=2;
nmi_relBearing=-135;
nmi_relVertical=120;
nml_relHorizontal=234;
nms_ID_LAU="456789";
break;
case 3:
nmi_alarm=1;
nmi_relBearing=-65;
nmi_relVertical=250;
nml_relHorizontal=4;
nms_ID_LAU="456789";
break;
case 4:
nmi_alarm=3;
nmi_relBearing=0;
nmi_relVertical=50;
nml_relHorizontal=34;
nms_ID_LAU="456789";
break;
case 5:
nmi_alarm=2;
nmi_relBearing=50;
nmi_relVertical=150;
nml_relHorizontal=234;
nms_ID_LAU="456789";
break;
case 6:
nmi_alarm=1;
nmi_relBearing=150;
nmi_relVertical=350;
nml_relHorizontal=0;
nms_ID_LAU="456789";
break;
case 7:
nmi_alarm=3;
nmi_relBearing=150;
nmi_relVertical=350;
nml_relHorizontal=34;
nms_ID_LAU="456789";
break;
}
if (millis()>prevTest) {
prevTest=millis()+10000;
testFase++;
if (testFase==8) {testFase=0;}
}
*/
switch (nmi_alarm){ // ALARM level
case 0: // 0=none
ALARM_beeps=0; // how many beeps (series) for the alarm level
ALARM_period=0; // duration+pause between series of beeps
ALARM_ledswitchperiod=0; // duration of alternating dir/alt LEDs
ALARM_beepsperiod=0; // duration or pause of a beep
break;
case 1: // 1=15-20" to impact
ALARM_beeps=1;
ALARM_period=4000;
ALARM_ledswitchperiod=300;
ALARM_beepsperiod=300;
break;
case 2: // 2=10-15" to impact
ALARM_beeps=2;
ALARM_period=3000;
ALARM_ledswitchperiod=200;
ALARM_beepsperiod=200;
break;
case 3: // 3=0-10" to impact
ALARM_beeps=3;
ALARM_period=2000;
ALARM_ledswitchperiod=100;
ALARM_beepsperiod=100;
break;
default:
break;
}
digitalWrite(pinNmea,!digitalRead(pinNmea)); // blink nmea led
if ((millis() < ALARM_timeout) && ( nmi_alarm <= previous_alarm)) { // no more beeps if no timeout and same or less alarm
ALARM_beeps=0;
}
if ((nmi_alarm <= previous_alarm) && (nmi_alarm!=0) && (millis() > ALARM_timeout)) { // renew timeout
ALARM_timeout=millis()+ALARM_period;
}
if ((nmi_alarm > previous_alarm) || (nmi_alarm==0)) {
ALARM_timeout=millis()+ALARM_period;
ALARM_ledtimeout=millis()+ALARM_ledswitchperiod;
ALARM_beeptimeout=millis()+ALARM_beepsperiod;
goLeds();
goBeeps();
}
previous_alarm=nmi_alarm;
}else if ((ret==true) && (inputString.substring(0,6)=="$PFLAA")) { // *********************************** $PFLAA
dprint("Good : ",false);
if (isDebug==true) {Serial.print(inputString);}
byte q=-1;
unsigned long qt=999999999;
boolean isQfound=false;
for (byte h=0;h<=15;h++){ // update Acft in data_table
if (datatable[h].dTime < qt) {q=h; qt=datatable[h].dTime;} // a free record or oldest
if (datatable[h].dID==nms_ID_LAA){
datatable[h].dDescr=nms_aircraftType;
datatable[h].dTime=millis();
dprint("Update aircraft ID: ",false);
if (isDebug==true) {Serial.print(datatable[h].dID);}
dprint(" Descr: ",false);
if (isDebug==true) {Serial.print(datatable[h].dDescr);}
dprint(" Time: ",false);
if (isDebug==true) {Serial.println(datatable[h].dTime);}
isQfound=true;
break;
}
}
if (isQfound==false) { // new ID so new record or overwrite oldest
datatable[q].dID=nms_ID_LAA;
datatable[q].dDescr=nms_aircraftType;
datatable[q].dTime=millis();
dprint("New aircraft ID: ",false);
if (isDebug==true) {Serial.print(datatable[q].dID);}
dprint(" Descr: ",false);
if (isDebug==true) {Serial.print(datatable[q].dDescr);}
dprint(" Time: ",false);
if (isDebug==true) {Serial.println(datatable[q].dTime);}
if (isDebug==true){ // print all datatable(h)
Serial.println("---------------------------------------------------");
for (byte h=0;h<=15;h++){
dprint("Rec: ",false);
Serial.print(h);
dprint(" ID: ",false);
Serial.print(datatable[h].dID);
dprint(" Descr: ",false);
Serial.print(datatable[h].dDescr);
dprint(" Time: ",false);
Serial.println(datatable[h].dTime);
}
Serial.println("---------------------------------------------------");
}
}
}else{
dprint("Discard: ",false);
if (isDebug==true) {Serial.print(inputString);}
}
inputString="";
isNmeaComplete=false;
}
// ----------------------------------ALARM MANAGER between NMEA sentences
if ((nmi_alarm > 0) && (millis() > ALARM_ledtimeout)){ // if led timeout
ALARM_ledtimeout=millis()+ALARM_ledswitchperiod; // renew timeout
goLeds();
}
if ((nmi_alarm > 0) && (millis() > ALARM_beeptimeout)){ // if beep timeout
ALARM_beeptimeout=millis()+ALARM_beepsperiod; // renew timeout
goBeeps();
}
if ((nmi_alarm > 0) && (isValueDisplayed==false)) { //display distance & aircraft type ones per sentence $PFLAU with alarm
showVal();
}else if ((nmi_alarm == 0) && (isDisplayClear==false)) {
display.clearDisplay(); // display nothing
display.display();
isDisplayClear=true;
}
// ----------------------------------
if ((millis() > (prevMillisService+30000)) && (nmi_alarm==0)) { Volt=checkVIn(); }
}//loop()
void showVal() {
//
// display size of 128x64 should have 21 columns of 6 pixels each, and 8 rows of 8 pixels each (at TextSize=1)
// dual color display has 16 single pixel lines as yellow color and 48 single pixel lines as cyan (viceversa if
// rotation=2 as in this sketch)
//
// display: 21 cols x 8 rows [setcursor(x,y)] x=(col-1)*6 y=(row-1)*8
// x=0,6,12,18,24,30,36,42,48,54,60,66,72,78,84,90,96,102,108,114,120
// y=0,8,16,24,32,40,48,56
//
display.clearDisplay(); // output to display
display.setRotation(2); //rotates text on OLED 1=90 degrees, 2=180 degrees
display.setTextColor(SSD1306_WHITE, SSD1306_BLACK); // Set text to white, background to black
display.setTextSize(1);
display.setCursor(0,0); // x, y
display.print(" Distance in meters:");
display.setTextSize(4);
display.setCursor(24,16); // x, y
if (nml_relHorizontal>9999) {nml_relHorizontal=9999;}
distm=" "+String(nml_relHorizontal);
display.print(distm.substring(distm.length()-4)); // a space + number, right alignment
nms_aircraftType=" -unknown-";
if (nms_ID_LAU != ""){
for (byte h=0;h<=15;h++){ // search Acft in data_table
if (datatable[h].dID==nms_ID_LAU){
nms_aircraftType=datatable[h].dDescr;
break;
}
}
}
display.setTextSize(2);
display.setCursor(0,48); // x, y
display.print(nms_aircraftType);
display.display();
isDisplayClear=false;
isValueDisplayed=true; // show values on display ones per sentence $PFLAU with alarm
dprint("Alarm=",false);
if (isDebug==true) {Serial.print(nmi_alarm);}
dprint(" ID=",false);
if (isDebug==true) {Serial.print(nms_ID_LAU);}
dprint(" Distance=",false);
if (isDebug==true) {Serial.print(nml_relHorizontal);}
dprint(" RelAlt=",false);
if (isDebug==true) {Serial.print(nmi_relVertical);}
dprint(" Bearing=",false);
if (isDebug==true) {Serial.print(nmi_relBearing);}
dprint(" Type=",false);
if (isDebug==true) {Serial.println(nms_aircraftType);}
}//showVal()
float checkVIn() {
int n1 = analogRead(pinTestBattery);
float corr=0.0; // %
float n2=(((3.30 * n1 * 2) / adc_res)); // (3.3 / voltage divider) * 2 = 3.3v ref. voltage, 12bit ADC
float v=(n2 + ((n2 * corr) /100)); // arbitrary correction in % if needed (not active if 0.0)
Volt=round(v*100)/100.0; // two decimals rounding
if (v < 3.4) {
tone (pinBuzzer,800,500);
dprint("Low Voltage=",false);
}else{
dprint("Battery V=",false);
if (isDebug==true) {Serial.print(v);}
dprint(" AnalogRead=",false);
if (isDebug==true) {Serial.print(n1);}
dprint(" %correction=",false);
if (isDebug==true) {Serial.println(corr);}
}
prevMillisService=millis();
return Volt;
}//checkVIn()
void goLeds() {
if (nmi_relVertical >= 150){ // set altitude leds
schemaAlt=15;
}
if ((nmi_relVertical > 50) && (nmi_relVertical < 150)){
schemaAlt=14;
}
if ((nmi_relVertical >= -50) && (nmi_relVertical <= 50)){
schemaAlt=13;
}
if ((nmi_relVertical < -50) && (nmi_relVertical > -150)){
schemaAlt=12;
}
if (nmi_relVertical <= -150){
schemaAlt=11;
}
if ((nmi_relBearing >= 22.5) && (nmi_relBearing < 67.5)){ // set direction leds
schemaDir=1;
}
if ((nmi_relBearing >= 67.5) && (nmi_relBearing < 112.5)){
schemaDir=2;
}
if ((nmi_relBearing >= 112.5) && (nmi_relBearing < 157.5)){
schemaDir=3;
}
if ((nmi_relBearing >= 157.5) || (nmi_relBearing <= -157.5)){
schemaDir=4;
}
if ((nmi_relBearing > -157.5) && (nmi_relBearing <= -112.5)){
schemaDir=5;
}
if ((nmi_relBearing > -112.5) && (nmi_relBearing <= -67.5)){
schemaDir=6;
}
if ((nmi_relBearing > -67.5) && (nmi_relBearing <= -22.5)){
schemaDir=7;
}
if ((nmi_relBearing > -22.5) && (nmi_relBearing < 22.5)){
schemaDir=8;
}
if (nmi_alarm == 0){
setLed(schemaLeds[0]); //off
isLedToSwitch=false;
}
if (nmi_alarm >= 1){
if (isLedToSwitch == false){
setLed(schemaLeds[schemaDir]); //direction
}else{
setLed(schemaLeds[schemaAlt]); //altitude
}
}
isLedToSwitch = !isLedToSwitch; // alternating
}//goLeds()
void goBeeps() {
if (nmi_alarm == 0){
isBeepToSwitch=false;
digitalWrite(pinBuzzer,LOW);
}
if ((nmi_alarm > 0) && (ALARM_beeps > 0)){
if (isBeepToSwitch == false){
digitalWrite(pinBuzzer,HIGH); //play
}else{
ALARM_beeps--;
digitalWrite(pinBuzzer,LOW); //silent
}
isBeepToSwitch = !isBeepToSwitch; // alternating
}
}//goBeeps()
void deviceConnect() {
exitStatus=0;
BLE.scanForName(deviceName); // ------------scan device/peripheral
delay(5000);
peripheral = BLE.available();
while (exitStatus==0){
if (peripheral) { // ----------------------------use device/peripheral
isDeviceFound=true;
dprint("Found",true);
dprint("Device Address: ",false);
if (isDebug==true) {Serial.println(peripheral.address());}
dprint("Device RSSI: ",false);
if (isDebug==true) {Serial.println(peripheral.rssi());}
dprint("Connecting...",true);
if (peripheral.connect()) { // ----------------------------connect
isConnected=true;
digitalWrite(pinNmea,HIGH);
dprint("Connected.",true);
dprint("Discovering attributes...",true);
if (peripheral.discoverAttributes()==0) { // ------------check attributes
dprint("Done",true);
int serviceCount = peripheral.serviceCount(); // -------------count services
dprint("Services discovered nr = ",false);
if (isDebug==true) {Serial.println(serviceCount);}
dprint("Verifying service: ",false);
if (isDebug==true) {Serial.println(serUUID);}
if (peripheral.hasService(serUUID)) { // ----------------check needed service
DataService = peripheral.service(serUUID);
if (DataService) { // ----------------------------------- use service, count characteristics
dprint("Service is available",true);
int characteristicCount = DataService.characteristicCount();
dprint("Characteristics discovered nr = ",false);
if (isDebug==true) {Serial.println(characteristicCount);}
dprint("Verifying Characteristic ",false); // ---------------------check needed characteristic
if (isDebug==true) {Serial.println(charUUID);}
if (DataService.hasCharacteristic(charUUID)) {
DataCharacteristic = DataService.characteristic(charUUID);
if (DataCharacteristic) { // -------------------------use characteristic, check read & cansubscribe
dprint("Characteristic is available, properties mask is: ",false);
if (isDebug==true) {Serial.println(DataCharacteristic.properties(),BIN);}
dprint("(as bit masked: Broadcast|Read|WriteWithoutResponse|Write|Notify|Indicate)",true);
if (DataCharacteristic.canRead()) {dprint("Characteristic is readable",true);}
if (DataCharacteristic.canSubscribe()) {dprint("Characteristic can be subscribed",true);}
if (DataCharacteristic.subscribe()==true) { // ------------subscribe to data changing notification
dprint("Subscribed.",true);
dprint("Ready for data changing notifications...",true);
exitStatus=99; // ok
break;
} else {
dprint("Subscribtion to data changing notifications FAILED!.",true);
exitStatus=8;
break;
}//if(DataCharacteristic.subscribe()
} else {
dprint("Specified characteristic NOT AVAILABLE!",true);
exitStatus=7;
break;
}//if(DataCharacteristic)
}else{
dprint("Device/Peripheral DOES NOT HAVE needed characteristic!",true);
exitStatus=6;
break;
}//if(Dataservice.has.characteristic...)
} else {
dprint("Device/Peripheral service NOT AVAILABLE!",true);
exitStatus=5;
break;
}//if(DataService)
}else{
dprint("Device/Peripheral DOES NOT HAVE the necessary service!",true);
exitStatus=4;
break;
}//if(peripheral.has.service...
}else{
dprint("Attributes discovering failed, try again.",true);
exitStatus=3;
break;
}//if(peripheral.discoverAttributes()
}else{
dprint("Peripheral/Device does NOT WANT TO CONNECT, try again.",true);
exitStatus=2;
break;
}//if(peripheral.connect()
}else{
dprint("Peripheral/Device NOT FOUND, try again.",true);
exitStatus=1;
break;
}//if(peripheral)
}//while(exitStatus==0)
if (exitStatus!=99) { //============ exit with error
devDisconnect();
}//if(exitStatus!=99)
if (exitStatus==99) { //============ exit ok
isDeviceFound=true;
}
}//deviceConnect()
void devDisconnect(){
if (isConnected==true) {peripheral.disconnect();dprint("Disconnected",true);}
digitalWrite(pinNmea,LOW);
setLed(schemaLeds[0]);
isLedToSwitch=false;
prevNmeaUpdate=0;
isConnected=false;
isDeviceFound=false;
delay(2000);
}//devDisconnect()
void dprint(const char dtext[], boolean dnewline){
if (isDebug==true && dnewline==true) {Serial.println(dtext);}
if (isDebug==true && dnewline==false) {Serial.print(dtext);}
}//dprint()
void initialShow(){ // leds + display show
digitalWrite(pinBuzzer,HIGH);
display.clearDisplay();
display.setRotation(2); //rotates text on OLED 1=90 degrees, 2=180 degrees
display.setTextColor(SSD1306_WHITE, SSD1306_BLACK); // Set text to white, background to black
display.setTextSize(1);
display.setCursor(12,0); // x, y
display.print("Battery level:");
display.setTextSize(2);
display.setCursor(24,24);
display.print(Volt);
display.print("v");
display.setTextSize(2);
display.setCursor(0,48); // x, y
display.print("initialize");
display.display();
isDisplayClear=false;
for (int L=1;L<=8;L++){
setLed(schemaLeds[L]);
delay(200);
}
for (int L=11;L<=15;L++){
setLed(schemaLeds[L]);
delay(200);
}
setLed(schemaLeds[0]);
digitalWrite(pinNmea,HIGH);
delay(250);
digitalWrite(pinNmea,LOW);
delay(250);
digitalWrite(pinNmea,HIGH);
delay(250);
digitalWrite(pinNmea,LOW);
digitalWrite(pinBuzzer,LOW);
display.clearDisplay();
display.display();
isDisplayClear=true;
delay(1000);
}//initialShow()
void setLed (byte led){
byte L=led;
if (L>=8) {L=L-8;digitalWrite(pinMux3,HIGH);}else{digitalWrite(pinMux3,LOW);}
if (L>=4) {L=L-4;digitalWrite(pinMux2,HIGH);}else{digitalWrite(pinMux2,LOW);}
if (L>=2) {L=L-2;digitalWrite(pinMux1,HIGH);}else{digitalWrite(pinMux1,LOW);}
if (L>=1) {L=L-1;digitalWrite(pinMux0,HIGH);}else{digitalWrite(pinMux0,LOW);}
}//setLed()
String getAircraftType(int type){
descr=" -unknown-";
switch (type){
case 1:
descr=" glider";
break;
case 2:
descr=" tow plane";
break;
case 3:
descr="helicopter";
break;
case 4:
descr=" parachute";
break;
case 5:
descr="drop plane";
break;
case 6:
descr=" hanglider";
break;
case 7:
descr="paraglider";
break;
case 8:
descr=" aircraft";
break;
case 9:
descr=" aircraft";
break;
case 11:
descr=" balloon";
break;
case 12:
descr=" zeppelin";
break;
case 13:
descr=" UAV drone";
break;
case 15:
descr=" obstacle";
break;
default:
descr=" -unknown-";
break;
}
return descr;
}//getAircraftType(byte type)
String nmea0183_checksum(String nmea_data) {
int crc = 0;
String chSumString = "";
int i;
// ignore the first $ sign, checksum in sentence
for (i = 1; i < (nmea_data.length()-5); i ++) { // remove the - 5 if no "*" + cksum + cr + lf are present
crc ^= nmea_data[i];
}
chSumString = String(crc,HEX);
if (chSumString.length()==1) {
chSumString="0"+chSumString.substring(0,1);
}
chSumString.toUpperCase();
return chSumString;
} // nmea0183_checksum(String nmea_data)
bool nmeaExtractData() {
int d=0;
int s=0;
int y=0;
int z=0;
int j=0;
float t=0;
bool ret = false; //true if nmea sentence = $PFLAU or $PFLAA and valid CHKSUM
//$PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,<RelativeVertical>,<RelativeDistance>,<ID>
if ((inputString.substring(0,6)=="$PFLAU") && (inputString.substring(inputString.length()-4,inputString.length()-2)==nmea0183_checksum(inputString))) {
y=0;
for (s = 1; s <= 10; s ++) {
y=inputString.indexOf(",",y);
switch (s) {
case 1: //-----------------------RX n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 2: //-----------------------TX n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 3: //-----------------------GPS 0=no GPS fix, 1=3D fix on ground, 2=GPS fix when airborn
nmi_gps=0;
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nmi_gps=inputString.substring(y+1,z).toInt();
}
y=z;
break;
case 4: //-----------------------POWER n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 5: //-----------------------ALARM level 0=none, 1=15-20" to impact, 2=10-15" to impact, 3=0-10" to impact
nmi_alarm=0;
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nmi_alarm=inputString.substring(y+1,z).toInt();
}
y=z;
break;
case 6: //-----------------------RELATIVE BEARING of impact, 0=ahead, 180/-180=behind, -45=left, 45=right, etc.
nmi_relBearing=0;
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nmi_relBearing=inputString.substring(y+1,z).toInt();
}
y=z;
break;
case 7: //-----------------------ALARM type 0=none, 2=aircraft, 3=obstacle/zone, 4=traffic advisory, 10-FF other ignored
nmi_alarmType=0;
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nmi_alarmType=inputString.substring(y+1,z).toInt();
}
y=z;
break;
case 8: //-----------------------relative VERTICAL distance in meters, positive=OVER, negative=BELOW
nmi_relVertical=0;
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nmi_relVertical=inputString.substring(y+1,z).toInt();
}
y=z;
break;
case 9: //-----------------------relative HORIZONTAL distance in meters
nml_relHorizontal=0;
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nml_relHorizontal=inputString.substring(y+1,z).toInt();
}
y=z;
break;
case 10: //-----------------------ID=NNNNNN
nms_ID_LAU="";
z=inputString.indexOf("*",y+1);
if (z>(y+1)) {
nms_ID_LAU=inputString.substring(y+1,z);
if (nms_ID_LAU==",") {nms_ID_LAU="";}
}
y=z;
break;
default:
break;
}
}
ret=true;
}//if"$PFLAU"
//$PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,<IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>,<NoTrack>,<Source>,<RSSI>
if ((inputString.substring(0,6)=="$PFLAA") && (inputString.substring(inputString.length()-4,inputString.length()-2)==nmea0183_checksum(inputString))) {
y=0;
for (s = 1; s <= 11; s ++) {
y=inputString.indexOf(",",y);
switch (s) {
case 1: //-----------------------AlarmLevel n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 2: //-----------------------RelativeNorth n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 3: //-----------------------RelativeEast n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 4: //-----------------------RelativeVertical n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 5: //-----------------------IDType n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 6: //-----------------------ID=composite as per NNNNNN!FLR_NNNNNNN
nms_ID_LAA="";
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
j=inputString.indexOf("!",y+1);
if (j!=-1){
nms_ID_LAA=inputString.substring(y+1,j); // normalized ID=NNNNNN
}else{
nms_ID_LAA=inputString.substring(y+1,z); // full ID if not present "!"
}
}
y=z;
break;
case 7: //-----------------------Track n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 8: //-----------------------TurnRate n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 9: //-----------------------GroundSpeed n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 10: //-----------------------ClimbRate n.u.
z=inputString.indexOf(",",y+1);
y=z;
break;
case 11: //-----------------------AcftType
nmi_AcftType=0;
nms_AcftType="";
z=inputString.indexOf(",",y+1);
if (z>(y+1)) {
nms_AcftType=inputString.substring(y+1,z);
nmi_AcftType=inputString.substring(y+1,z).toInt();
if (nms_AcftType=="A") {nmi_AcftType=10;}
if (nms_AcftType=="B") {nmi_AcftType=11;}
if (nms_AcftType=="C") {nmi_AcftType=12;}
if (nms_AcftType=="D") {nmi_AcftType=13;}
if (nms_AcftType=="E") {nmi_AcftType=14;}
if (nms_AcftType=="F") {nmi_AcftType=15;}
}
nms_aircraftType=getAircraftType(nmi_AcftType);
y=z;
break;
default:
break;
}
}
ret=true;
}//if"$PFLAA"
return ret;
}//bool nmeaExtractData()
Comments