You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
662 lines
21 KiB
662 lines
21 KiB
// Tracker for LoRA APRS |
|
// from OE1ACM and OE3CJB redesigned by SQ9MDD |
|
// KISS ans Bluetooth by SQ5RWU |
|
// TTGO T-Beam v1.0 only |
|
// |
|
// licensed under CC BY-NC-SA |
|
|
|
// Includes |
|
#include <TTGO_T-Beam_LoRa_APRS_config.h> // to config user parameters |
|
#include <Arduino.h> |
|
#include <SPI.h> |
|
#include <BG_RF95.h> // library from OE1ACM |
|
#include <math.h> |
|
#include <driver/adc.h> |
|
#include <Wire.h> |
|
#include <Adafruit_I2CDevice.h> |
|
#include <Adafruit_SSD1306.h> |
|
#include <splash.h> |
|
#include <Adafruit_GFX.h> |
|
#include <Adafruit_SPITFT.h> |
|
#include <Adafruit_SPITFT_Macros.h> |
|
#include <gfxfont.h> |
|
#include <axp20x.h> |
|
#include "taskGPS.h" |
|
#ifdef KISS_PROTOCOL |
|
#include "taskTNC.h" |
|
#endif |
|
|
|
|
|
// I2C LINES |
|
#define I2C_SDA 21 |
|
#define I2C_SCL 22 |
|
|
|
// DISPLAY address |
|
#define SSD1306_ADDRESS 0x3C |
|
|
|
//other global Variables |
|
|
|
// LED for signalling |
|
#ifdef T_BEAM_V1_0 |
|
const byte TXLED = 4; //pin number for LED on TX Tracker |
|
#else |
|
const byte TXLED = 14; //pin number for LED on TX Tracker |
|
#endif |
|
|
|
// Button of TTGO T-Beam |
|
#ifdef T_BEAM_V1_0 |
|
// const byte BUTTON = 38; //pin number for Button on TTGO T-Beam |
|
#define BUTTON 38 //pin number for Button on TTGO T-Beam |
|
#else |
|
#define BUTTON 39 //pin number for Button on TTGO T-Beam |
|
#endif |
|
|
|
// Pins for LoRa module |
|
const byte lora_PReset = 23; //pin where LoRa device reset line is connected |
|
const byte lora_PNSS = 18; //pin number where the NSS line for the LoRa device is connected. |
|
|
|
// Variables for APRS packaging |
|
String Tcall; //your Call Sign for normal position reports |
|
String sTable="/"; //Primer |
|
String relay_path; |
|
boolean gps_state = true; |
|
boolean key_up = true; |
|
boolean t_lock = false; |
|
|
|
// Variables and Constants |
|
String loraReceivedFrameString = ""; //data on buff is copied to this string |
|
String Outputstring = ""; |
|
String outString=""; //The new Output String with GPS Conversion RAW |
|
String LongShown=""; |
|
String LatShown=""; |
|
String LongFixed=""; |
|
String LatFixed=""; |
|
|
|
#if defined(ENABLE_TNC_SELF_TELEMETRY) && defined(KISS_PROTOCOL) |
|
time_t nextTelemetryFrame; |
|
#endif |
|
|
|
//byte arrays |
|
byte lora_TXBUFF[BG_RF95_MAX_MESSAGE_LEN]; //buffer for packet to send |
|
byte lora_RXBUFF[BG_RF95_MAX_MESSAGE_LEN]; //buffer for packet to send |
|
|
|
//byte Variables |
|
byte lora_TXStart; //start of packet data in TXbuff |
|
byte lora_TXEnd; //end of packet data in TXbuff |
|
byte lora_FTXOK; //flag, set to 1 if TX OK |
|
byte lora_TXPacketType; //type number of packet to send |
|
byte lora_TXDestination; //destination address of packet to send |
|
byte lora_TXSource; //source address of packet received |
|
byte lora_FDeviceError; //flag, set to 1 if RFM98 device error |
|
byte lora_TXPacketL; //length of packet to send, includes source, destination and packet type. |
|
|
|
unsigned long lastTX = 0L; |
|
float BattVolts; |
|
|
|
// variables for smart beaconing |
|
float average_speed[5] = {0,0,0,0,0}, average_speed_final=0, max_speed=30, min_speed=0; |
|
float old_course = 0, new_course = 0; |
|
int point_avg_speed = 0, point_avg_course = 0; |
|
ulong min_time_to_nextTX=60000L; // minimum time period between TX = 60000ms = 60secs = 1min |
|
ulong max_time_to_nextTX= MAX_TIME_TO_NEXT_TX; |
|
ulong nextTX=60000L; // preset time period between TX = 60000ms = 60secs = 1min |
|
ulong time_to_refresh = 0; |
|
ulong next_fixed_beacon = 0; |
|
ulong fix_beacon_interval = FIX_BEACON_INTERVAL; |
|
ulong time_delay = 0; |
|
#define ANGLE 60 // angle to send packet at smart beaconing |
|
#define ANGLE_AVGS 3 // angle averaging - x times |
|
float average_course[ANGLE_AVGS]; |
|
float avg_c_y, avg_c_x; |
|
|
|
static const adc_atten_t atten = ADC_ATTEN_DB_6; |
|
static const adc_unit_t unit = ADC_UNIT_1; |
|
|
|
#ifdef T_BEAM_V1_0 |
|
AXP20X_Class axp; |
|
#endif |
|
|
|
// checkRX |
|
uint8_t loraReceivedLength = sizeof(lora_RXBUFF); |
|
|
|
// Singleton instance of the radio driver |
|
BG_RF95 rf95(18, 26); // TTGO T-Beam has NSS @ Pin 18 and Interrupt IO @ Pin26 |
|
|
|
// initialize OLED display |
|
#define OLED_RESET 16 // not used |
|
Adafruit_SSD1306 display(128, 64, &Wire, OLED_RESET); |
|
|
|
// + FUNCTIONS-----------------------------------------------------------+// |
|
|
|
char *ax25_base91enc(char *s, uint8_t n, uint32_t v){ |
|
/* Creates a Base-91 representation of the value in v in the string */ |
|
/* pointed to by s, n-characters long. String length should be n+1. */ |
|
for(s += n, *s = '\0'; n; n--) |
|
{ |
|
*(--s) = v % 91 + 33; |
|
v /= 91; |
|
} |
|
return(s); |
|
} |
|
|
|
void prepareAPRSFrame(){ |
|
String Ns, Ew, helper; |
|
char helper_base91[] = {"0000\0"}; |
|
double Tlat=52.0000, Tlon=20.0000; |
|
uint32_t aprs_lat, aprs_lon; |
|
double Tspeed=0, Tcourse=0; |
|
String Speedx, Coursex; |
|
int i; |
|
#ifdef SHOW_ALT |
|
String Altx; |
|
int Talt; |
|
#endif |
|
|
|
Tlat=gps.location.lat(); |
|
Tlon=gps.location.lng(); |
|
|
|
Tcourse=gps.course.deg(); |
|
Tspeed=gps.speed.knots(); |
|
if(Tlat<0) { Ns = "S"; } else { Ns = "N"; } |
|
if(Tlon<0) { Ew = "W"; } else { Ew = "E"; } |
|
if(Tlat < 0) { Tlat= -Tlat; } |
|
|
|
if(Tlon < 0) { Tlon= -Tlon; } |
|
aprs_lat = 900000000 - Tlat * 10000000; |
|
aprs_lat = aprs_lat / 26 - aprs_lat / 2710 + aprs_lat / 15384615; |
|
aprs_lon = 900000000 + Tlon * 10000000 / 2; |
|
aprs_lon = aprs_lon / 26 - aprs_lon / 2710 + aprs_lon / 15384615; |
|
//} |
|
|
|
outString = ""; |
|
outString += Tcall; |
|
|
|
#ifdef DIGI_PATH |
|
outString += ">APLM0," + relay_path + ":!"; |
|
#elif |
|
outString += ">APLM0:!"; |
|
#endif |
|
|
|
if(gps_state && gps.location.isValid()){ |
|
outString += APRS_SYMBOL_TABLE; |
|
ax25_base91enc(helper_base91, 4, aprs_lat); |
|
for (i=0; i<4; i++) { |
|
outString += helper_base91[i]; |
|
} |
|
ax25_base91enc(helper_base91, 4, aprs_lon); |
|
for (i=0; i<4; i++) { |
|
outString += helper_base91[i]; |
|
} |
|
outString += APRS_SYMBOL; |
|
ax25_base91enc(helper_base91, 1, (uint32_t) Tcourse/4 ); |
|
outString += helper_base91[0]; |
|
ax25_base91enc(helper_base91, 1, (uint32_t) (log1p(Tspeed)/0.07696)); |
|
outString += helper_base91[0]; |
|
outString += "H"; |
|
#ifdef SHOW_ALT |
|
Talt=gps.altitude.meters() * 3.28d; |
|
Altx = Talt; |
|
outString += "/A="; |
|
for (i = 0; i < (6-Altx.length()); ++i) { |
|
outString += "0"; |
|
} |
|
outString += Talt; |
|
#endif |
|
}else{ |
|
outString += LATIDUDE_PRESET; |
|
outString += APRS_SYMBOL_TABLE; |
|
outString += LONGITUDE_PRESET; |
|
outString += APRS_SYMBOL; |
|
} |
|
|
|
outString += MY_COMMENT; |
|
|
|
#ifdef SHOW_BATT // battery is not frame part move after comment |
|
outString += " Batt="; |
|
outString += String(BattVolts, 2); |
|
outString += ("V"); |
|
#endif |
|
|
|
#ifdef KISS_PROTOCOL |
|
sendToTNC(outString); |
|
#else |
|
Serial.println(outString); |
|
#endif |
|
} |
|
|
|
void sendpacket(){ |
|
batt_read(); |
|
|
|
prepareAPRSFrame(); |
|
loraSend(TXdbmW, TXFREQ, outString); //send the packet, data is in TXbuff from lora_TXStart to lora_TXEnd |
|
} |
|
|
|
/** |
|
* Send message as APRS LoRa packet |
|
* @param lora_LTXPower |
|
* @param lora_FREQ |
|
* @param message |
|
*/ |
|
void loraSend(byte lora_LTXPower, float lora_FREQ, const String &message) { |
|
digitalWrite(TXLED, LOW); |
|
lastTX = millis(); |
|
|
|
int messageSize = min(message.length(), sizeof(lora_TXBUFF) - 1); |
|
message.toCharArray((char*)lora_TXBUFF, messageSize + 1, 0); |
|
rf95.setModemConfig(BG_RF95::Bw125Cr45Sf4096); |
|
rf95.setFrequency(lora_FREQ); |
|
rf95.setTxPower(lora_LTXPower); |
|
rf95.sendAPRS(lora_TXBUFF, messageSize); |
|
rf95.waitPacketSent(); |
|
digitalWrite(TXLED, HIGH); |
|
} |
|
|
|
void batt_read(){ |
|
#ifdef T_BEAM_V1_0 |
|
BattVolts = axp.getBattVoltage()/1000; |
|
#else |
|
BattVolts = analogRead(35)*7.221/4096; |
|
#endif |
|
} |
|
|
|
void writedisplaytext(String HeaderTxt, String Line1, String Line2, String Line3, String Line4, String Line5, int warten) { |
|
batt_read(); |
|
if (BattVolts < 3.5){ |
|
#ifdef T_BEAM_V1_0 |
|
axp.setChgLEDMode(AXP20X_LED_BLINK_4HZ); |
|
#endif |
|
} |
|
|
|
display.clearDisplay(); |
|
display.setTextColor(WHITE); |
|
display.setTextSize(2); |
|
display.setCursor(0,0); |
|
display.println(HeaderTxt); |
|
display.setTextSize(1); |
|
display.setCursor(0,16); |
|
display.println(Line1); |
|
display.setCursor(0,26); |
|
display.println(Line2); |
|
display.setCursor(0,36); |
|
display.println(Line3); |
|
display.setCursor(0,46); |
|
display.println(Line4); |
|
display.setCursor(0,56); |
|
display.println(Line5); |
|
display.display(); |
|
time_to_refresh = millis() + SHOW_RX_TIME; |
|
} |
|
|
|
|
|
String getSatAndBatInfo() { |
|
String line5; |
|
if(gps_state == true){ |
|
line5 = "SAT: " + String(gps.satellites.value()) + " BAT: " + String(BattVolts, 1) + "V"; |
|
}else{ |
|
line5 = "SAT: X BAT: " + String(BattVolts, 1) + "V"; |
|
} |
|
#if defined(ENABLE_BLUETOOTH) && defined(KISS_PROTOCOL) |
|
if (SerialBT.hasClient()){ |
|
line5 += "BT"; |
|
} |
|
#endif |
|
return line5; |
|
} |
|
|
|
void displayInvalidGPS() { |
|
writedisplaytext(" " + Tcall, "(TX) at valid GPS", "LAT: not valid", "LON: not valid", "SPD: --- CRS: ---", getSatAndBatInfo(), 1); |
|
#ifdef SHOW_GPS_DATA |
|
Serial.print("(TX) at valid GPS / LAT: not valid / Lon: not valid / SPD: --- / CRS: ---"); |
|
Serial.print(" / SAT: "); |
|
Serial.print(String(gps.satellites.value())); |
|
Serial.print(" / BAT: "); |
|
Serial.println(String(BattVolts,1)); |
|
#endif |
|
} |
|
|
|
#if defined(KISS_PROTOCOL) |
|
/** |
|
* |
|
* @param TNC2FormatedFrame |
|
*/ |
|
void sendToTNC(const String& TNC2FormatedFrame) { |
|
if (tncToSendQueue){ |
|
auto *buffer = new String(); |
|
buffer->concat(TNC2FormatedFrame); |
|
if (xQueueSend(tncReceivedQueue, &buffer, (1000 / portTICK_PERIOD_MS)) != pdPASS){ |
|
delete buffer; |
|
} |
|
} |
|
} |
|
#endif |
|
|
|
String prepareCallsign(const String& callsign){ |
|
String tmpString = ""; |
|
for (int i=0; i<callsign.length();++i){ // remove unneeded "spaces" from callsign field |
|
if (callsign.charAt(i) != ' ') { |
|
tmpString += callsign.charAt(i); |
|
} |
|
} |
|
return tmpString; |
|
} |
|
|
|
#if defined(ENABLE_TNC_SELF_TELEMETRY) && defined(KISS_PROTOCOL) |
|
void sendTelemetryFrame() { |
|
#ifdef T_BEAM_V1_0 |
|
uint8_t b_volt = (axp.getBattVoltage() - 3000) / 5.1; |
|
uint8_t b_in_c = (axp.getBattChargeCurrent()) / 10; |
|
uint8_t b_out_c = (axp.getBattDischargeCurrent()) / 10; |
|
uint8_t ac_volt = (axp.getVbusVoltage() - 3000) / 28; |
|
uint8_t ac_c = (axp.getVbusCurrent()) / 10; |
|
|
|
String telemetryParamsNames = String(":") + Tcall + ":PARM.B Volt,B In,B Out,AC V,AC C"; |
|
String telemetryUnitNames = String(":") + Tcall + ":UNIT.mV,mA,mA,mV,mA"; |
|
String telemetryEquations = String(":") + Tcall + ":EQNS.0,5.1,3000,0,10,0,0,10,0,0,28,3000,0,10,0"; |
|
String telemetryData = String("T#MIC") + String(b_volt) + ","+ String(b_in_c) + ","+ String(b_out_c) + ","+ String(ac_volt) + ","+ String(ac_c) + ",00000000"; |
|
String telemetryBase = ""; |
|
telemetryBase += Tcall + ">APLM0" + ":"; |
|
sendToTNC(telemetryBase + telemetryParamsNames); |
|
sendToTNC(telemetryBase + telemetryUnitNames); |
|
sendToTNC(telemetryBase + telemetryEquations); |
|
sendToTNC(telemetryBase + telemetryData); |
|
#else |
|
#endif |
|
} |
|
#endif |
|
|
|
// + SETUP --------------------------------------------------------------+// |
|
|
|
void setup(){ |
|
for (int i=0;i<ANGLE_AVGS;i++) { // set average_course to "0" |
|
average_course[i]=0; |
|
} |
|
|
|
pinMode(TXLED, OUTPUT); |
|
pinMode(BUTTON, INPUT); |
|
digitalWrite(TXLED, LOW); // turn blue LED off |
|
Serial.begin(115200); |
|
Wire.begin(I2C_SDA, I2C_SCL); |
|
|
|
#ifdef T_BEAM_V1_0 |
|
if (!axp.begin(Wire, AXP192_SLAVE_ADDRESS)) { |
|
} |
|
axp.setPowerOutPut(AXP192_LDO2, AXP202_ON); |
|
axp.setPowerOutPut(AXP192_LDO3, AXP202_ON); // switch on GPS |
|
axp.setPowerOutPut(AXP192_DCDC2, AXP202_ON); |
|
axp.setPowerOutPut(AXP192_EXTEN, AXP202_ON); |
|
axp.setPowerOutPut(AXP192_DCDC1, AXP202_ON); |
|
axp.setDCDC1Voltage(3300); |
|
// Enable ADC to measure battery current, USB voltage etc. |
|
axp.adc1Enable(0xfe, true); |
|
axp.adc2Enable(0x80, true); |
|
#endif |
|
|
|
if(!display.begin(SSD1306_SWITCHCAPVCC, SSD1306_ADDRESS)) { |
|
for(;;); // Don't proceed, loop forever |
|
} |
|
|
|
writedisplaytext("LoRa-APRS","","Init:","Display OK!","","",1000); |
|
Tcall = prepareCallsign(String(CALLSIGN)); |
|
relay_path = DIGI_PATH; |
|
|
|
if (!rf95.init()) { |
|
writedisplaytext("LoRa-APRS","","Init:","RF95 FAILED!",":-(","",250); |
|
for(;;); // Don't proceed, loop forever |
|
} |
|
|
|
if (max_time_to_nextTX < nextTX){ |
|
max_time_to_nextTX=nextTX; |
|
} |
|
writedisplaytext("LoRa-APRS","","Init:","RF95 OK!","","",250); |
|
writedisplaytext(" "+Tcall,"","Init:","Waiting for GPS","","",250); |
|
xTaskCreate(taskGPS, "taskGPS", 10000, nullptr, 1, nullptr); |
|
writedisplaytext(" "+Tcall,"","Init:","GPS Task Created!","","",250); |
|
#ifndef T_BEAM_V1_0 |
|
adc1_config_width(ADC_WIDTH_BIT_12); |
|
adc1_config_channel_atten(ADC1_CHANNEL_7,ADC_ATTEN_DB_6); |
|
#endif |
|
batt_read(); |
|
writedisplaytext("LoRa-APRS","","Init:","ADC OK!","BAT: "+String(BattVolts,1),"",250); |
|
rf95.setFrequency(433.775); |
|
rf95.setModemConfig(BG_RF95::Bw125Cr45Sf4096); // hard coded because of double definition |
|
rf95.setTxPower(TXdbmW); |
|
delay(250); |
|
#ifdef KISS_PROTOCOL |
|
xTaskCreatePinnedToCore(taskTNC, "taskTNC", 10000, nullptr, 1, nullptr, xPortGetCoreID()); |
|
#endif |
|
|
|
#if defined(ENABLE_BLUETOOTH) && defined(KISS_PROTOCOL) |
|
#ifdef BLUETOOTH_PIN |
|
SerialBT.setPin(BLUETOOTH_PIN); |
|
#endif |
|
SerialBT.begin(String("TTGO LORA APRS ") + CALLSIGN); |
|
writedisplaytext("LoRa-APRS","","Init:","BT OK!","","",250); |
|
#endif |
|
writedisplaytext("LoRa-APRS","","Init:","FINISHED OK!"," =:-) ","",250); |
|
writedisplaytext("","","","","","",0); |
|
time_to_refresh = millis() + SHOW_RX_TIME; |
|
displayInvalidGPS(); |
|
digitalWrite(TXLED, HIGH); |
|
#ifdef T_BEAM_V1_0 |
|
axp.setChgLEDMode(AXP20X_LED_OFF); |
|
#endif |
|
} |
|
|
|
// +---------------------------------------------------------------------+// |
|
// + MAINLOOP -----------------------------------------------------------+// |
|
// +---------------------------------------------------------------------+// |
|
|
|
void loop() { |
|
if(digitalRead(BUTTON)==LOW && key_up == true){ |
|
key_up = false; |
|
delay(50); |
|
if(digitalRead(BUTTON)==LOW){ |
|
delay(300); |
|
time_delay = millis() + 1500; |
|
if(digitalRead(BUTTON)==HIGH){ |
|
if(gps_state == true && gps.location.isValid()){ |
|
writedisplaytext("((MAN TX))","","","","","",1); |
|
sendpacket(); |
|
}else{ |
|
writedisplaytext("((FIX TX))","","","","","",1); |
|
sendpacket(); |
|
} |
|
key_up = true; |
|
} |
|
} |
|
} |
|
//delay(1500); |
|
if(digitalRead(BUTTON)==LOW && key_up == false && millis() >= time_delay && t_lock == false){ |
|
t_lock = true; |
|
if(gps_state == true){ |
|
gps_state = false; |
|
#ifdef T_BEAM_V1_0 |
|
axp.setPowerOutPut(AXP192_LDO3, AXP202_OFF); // GPS OFF |
|
#endif |
|
writedisplaytext("((GPSOFF))","","","","","",1); |
|
next_fixed_beacon = millis() + fix_beacon_interval; |
|
|
|
}else{ |
|
gps_state = true; |
|
#ifdef T_BEAM_V1_0 |
|
axp.setPowerOutPut(AXP192_LDO3, AXP202_ON); |
|
#endif |
|
writedisplaytext("((GPS ON))","","","","","",1); // GPS ON |
|
} |
|
} |
|
|
|
if(digitalRead(BUTTON)==HIGH && key_up == false){ |
|
key_up = true; |
|
t_lock = false; |
|
} |
|
|
|
#ifdef FIXED_BEACON_EN |
|
if(millis() >= next_fixed_beacon && gps_state == false){ |
|
next_fixed_beacon = millis() + fix_beacon_interval; |
|
writedisplaytext("((AUT TX))","","","","","",1); |
|
sendpacket(); |
|
} |
|
#endif |
|
|
|
#ifdef KISS_PROTOCOL |
|
String *TNC2DataFrame = nullptr; |
|
if (tncToSendQueue) { |
|
if (xQueueReceive(tncToSendQueue, &TNC2DataFrame, (1 / portTICK_PERIOD_MS)) == pdPASS) { |
|
writedisplaytext("((KISSTX))","","","","","",1); |
|
time_to_refresh = millis() + SHOW_RX_TIME; |
|
loraSend(TXdbmW, TXFREQ, *TNC2DataFrame); |
|
delete TNC2DataFrame; |
|
} |
|
} |
|
#endif |
|
|
|
|
|
if (rf95.waitAvailableTimeout(100)) { |
|
#ifdef T_BEAM_V1_0 |
|
axp.setChgLEDMode(AXP20X_LED_LOW_LEVEL); |
|
#endif |
|
#ifdef SHOW_RX_PACKET // only show RX packets when activitated in config |
|
loraReceivedLength = sizeof(lora_RXBUFF); // reset max length before receiving! |
|
if (rf95.recvAPRS(lora_RXBUFF, &loraReceivedLength)) { |
|
loraReceivedFrameString = ""; |
|
for (int i=0 ; i < loraReceivedLength ; i++) { |
|
loraReceivedFrameString += (char) lora_RXBUFF[i]; |
|
} |
|
writedisplaytext(" ((RX))", "", loraReceivedFrameString, "", "", "", SHOW_RX_TIME); |
|
#ifdef KISS_PROTOCOL |
|
sendToTNC(loraReceivedFrameString); |
|
#endif |
|
} |
|
#endif |
|
#ifdef T_BEAM_V1_0 |
|
axp.setChgLEDMode(AXP20X_LED_OFF); |
|
#endif |
|
} |
|
|
|
LatShown = String(gps.location.lat(),5); |
|
LongShown = String(gps.location.lng(),5); |
|
average_speed[point_avg_speed] = gps.speed.kmph(); // calculate smart beaconing |
|
++point_avg_speed; |
|
if (point_avg_speed>4) { |
|
point_avg_speed=0; |
|
} |
|
average_speed_final = (average_speed[0]+average_speed[1]+average_speed[2]+average_speed[3]+average_speed[4])/5; |
|
nextTX = (max_time_to_nextTX-min_time_to_nextTX)/(max_speed-min_speed)*(max_speed-average_speed_final)+min_time_to_nextTX; |
|
|
|
if (nextTX < min_time_to_nextTX) {nextTX=min_time_to_nextTX;} |
|
if (nextTX > max_time_to_nextTX) {nextTX=max_time_to_nextTX;} |
|
|
|
average_course[point_avg_course] = gps.course.deg(); // calculate smart beaconing course |
|
|
|
++point_avg_course; |
|
if (point_avg_course>(ANGLE_AVGS-1)) { |
|
point_avg_course=0; |
|
avg_c_y = 0; |
|
avg_c_x = 0; |
|
|
|
for (int i=0;i<ANGLE_AVGS;i++) { |
|
avg_c_y += sin(average_course[i]/180*3.1415); |
|
avg_c_x += cos(average_course[i]/180*3.1415); |
|
} |
|
|
|
new_course = atan2f(avg_c_y,avg_c_x)*180/3.1415; |
|
if (new_course < 0) { |
|
new_course=360+new_course; |
|
} |
|
|
|
if ((old_course < ANGLE) && (new_course > (360-ANGLE))) { |
|
if (abs(new_course-old_course-360)>=ANGLE) { |
|
nextTX = 0; |
|
// lastTX = min_time_to_nextTX |
|
} |
|
} else { |
|
if ((old_course > (360-ANGLE)) && (new_course < ANGLE)) { |
|
if (abs(new_course-old_course+360)>=ANGLE) { |
|
nextTX = 0; |
|
} |
|
} else { |
|
if (abs(new_course-old_course)>=ANGLE) { |
|
nextTX = 0; |
|
} |
|
} |
|
} |
|
old_course = new_course; |
|
} |
|
|
|
if ((millis()<max_time_to_nextTX)&&(lastTX == 0)) { |
|
nextTX = 0; |
|
} |
|
|
|
if ( (lastTX+nextTX) <= millis() ) { |
|
if (gps.location.age() < 2000) { |
|
writedisplaytext(" ((TX))","","LAT: "+LatShown,"LON: "+LongShown,"SPD: "+String(gps.speed.kmph(),1)+" CRS: "+String(gps.course.deg(),1),getSatAndBatInfo(),1); |
|
sendpacket(); |
|
#ifdef SHOW_GPS_DATA |
|
Serial.print("((TX)) / LAT: "); |
|
Serial.print(LatShown); |
|
Serial.print(" / LON: "); |
|
Serial.print(LongShown); |
|
Serial.print(" / SPD: "); |
|
Serial.print(String(gps.speed.kmph(),1)); |
|
Serial.print(" / CRS: "); |
|
Serial.print(String(gps.course.deg(),1)); |
|
Serial.print(" / SAT: "); |
|
Serial.print(String(gps.satellites.value())); |
|
Serial.print(" / BAT: "); |
|
Serial.print(String(BattVolts,1)); |
|
//digitalWrite(TXLED, LOW); |
|
#endif |
|
} else { |
|
if (millis() > time_to_refresh){ |
|
displayInvalidGPS(); |
|
} |
|
} |
|
}else{ |
|
if (millis() > time_to_refresh){ |
|
if (gps.location.age() < 2000) { |
|
writedisplaytext(" "+Tcall,"Time to TX: "+String(((lastTX+nextTX)-millis())/1000)+"sec","LAT: "+LatShown,"LON: "+LongShown,"SPD: "+String(gps.speed.kmph(),1)+" CRS: "+String(gps.course.deg(),1),getSatAndBatInfo() ,1); |
|
} else { |
|
displayInvalidGPS(); |
|
} |
|
} |
|
} |
|
#if defined(ENABLE_TNC_SELF_TELEMETRY) && defined(KISS_PROTOCOL) |
|
if (nextTelemetryFrame < millis()){ |
|
nextTelemetryFrame = millis() + TNC_SELF_TELEMETRY_INTERVAL; |
|
sendTelemetryFrame(); |
|
} |
|
#endif |
|
#ifdef KISS_PROTOCOL |
|
#ifdef KISS_DEBUG |
|
static auto last_debug_send_time = millis(); |
|
if (millis() - last_debug_send_time > 1000*5) { |
|
last_debug_send_time = millis(); |
|
String debug_message = ""; |
|
#ifdef T_BEAM_V1_0 |
|
debug_message += "Bat V: " + String(axp.getBattVoltage()); |
|
debug_message += ", "; |
|
debug_message += "Bat IN A: " + String(axp.getBattChargeCurrent()); |
|
debug_message += ", "; |
|
debug_message += "Bat OUT A: " + String(axp.getBattDischargeCurrent()); |
|
debug_message += ", "; |
|
debug_message += "USB Plugged: " + String(axp.isVBUSPlug()); |
|
debug_message += ", "; |
|
debug_message += "USB V: " + String(axp.getVbusVoltage()); |
|
debug_message += ", "; |
|
debug_message += "USB A: " + String(axp.getVbusCurrent()); |
|
debug_message += ", "; |
|
debug_message += "Temp C: " + String(axp.getTemp()); |
|
#else |
|
debug_message += "Bat V: " + String(BattVolts); |
|
#endif |
|
|
|
Serial.print(encapsulateKISS(debug_message, CMD_HARDWARE)); |
|
#ifdef ENABLE_BLUETOOTH |
|
SerialBT.print(encapsulateKISS(debug_message, CMD_HARDWARE)); |
|
#endif |
|
} |
|
#endif |
|
#endif |
|
vTaskDelay(1); |
|
} |
|
// end of main loop
|
|
|