diff --git a/Firmware/RTK_Surveyor/AP-Config/src/main.js b/Firmware/RTK_Surveyor/AP-Config/src/main.js
index 8798dc22d..d190b2a22 100644
--- a/Firmware/RTK_Surveyor/AP-Config/src/main.js
+++ b/Firmware/RTK_Surveyor/AP-Config/src/main.js
@@ -10,6 +10,12 @@ function ge(e) {
}
var platformPrefix = "Surveyor";
+var geodeticLat = 40.01;
+var geodeticLon = -105.19;
+var geodeticAlt = 1500.1;
+var ecefX = 100;
+var ecefY = -100;
+var ecefZ = -200;
function parseIncoming(msg) {
//console.log("incoming message: " + msg);
@@ -92,6 +98,7 @@ function parseIncoming(msg) {
|| id.includes("profile5Name")
|| id.includes("profile6Name")
|| id.includes("profile7Name")
+ || id.includes("radioMAC")
) {
ge(id).innerHTML = val;
}
@@ -105,6 +112,41 @@ function parseIncoming(msg) {
else if (id.includes("firmwareUploadStatus")) {
firmwareUploadStatus(val);
}
+ else if (id.includes("geodeticLat")) {
+ geodeticLat = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("geodeticLon")) {
+ geodeticLon = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("geodeticAlt")) {
+ geodeticAlt = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("ecefX")) {
+ ecefX = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("ecefY")) {
+ ecefY = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("ecefZ")) {
+ ecefZ = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("bluetoothRadioType")) {
+ currentBtNumber = val;
+ $("input[name=bluetoothRadioType][value=" + currentBtNumber + "]").prop('checked', true);
+ }
+ else if (id.includes("espnowPeerCount")) {
+ if(val > 0)
+ ge("peerMACs").innerHTML = "";
+ }
+ else if (id.includes("peerMAC")) {
+ ge("peerMACs").innerHTML += val + "
";
+ }
//Check boxes / radio buttons
else if (val == "true") {
@@ -134,6 +176,7 @@ function parseIncoming(msg) {
//console.log("Settings loaded");
ge("profileChangeMessage").innerHTML = '';
+ ge("resetProfileMsg").innerHTML = '';
//Force element updates
ge("measurementRateHz").dispatchEvent(new CustomEvent('change'));
@@ -147,6 +190,7 @@ function parseIncoming(msg) {
ge("dataPortChannel").dispatchEvent(new CustomEvent('change'));
ge("enableExternalPulse").dispatchEvent(new CustomEvent('change'));
ge("enablePointPerfectCorrections").dispatchEvent(new CustomEvent('change'));
+ ge("radioType").dispatchEvent(new CustomEvent('change'));
}
function hide(id) {
@@ -215,6 +259,7 @@ function validateFields() {
collapseSection("collapseSensorConfig", "sensorCaret");
collapseSection("collapsePPConfig", "pointPerfectCaret");
collapseSection("collapsePortsConfig", "portsCaret");
+ collapseSection("collapseRadioConfig", "radioCaret");
collapseSection("collapseSystemConfig", "systemCaret");
errorCount = 0;
@@ -350,6 +395,8 @@ function validateFields() {
clearElement("fixedLat", 40.09029479);
clearElement("fixedLong", -105.18505761);
clearElement("fixedAltitude", 1560.089);
+ clearElement("antennaHeight", 0);
+ clearElement("antennaReferencePoint", 0);
}
else {
clearElement("observationSeconds", 60);
@@ -372,6 +419,9 @@ function validateFields() {
checkElementValue("fixedLat", -180, 180, "Must be -180 to 180", "collapseBaseConfig");
checkElementValue("fixedLong", -180, 180, "Must be -180 to 180", "collapseBaseConfig");
checkElementValue("fixedAltitude", -11034, 8849, "Must be -11034 to 8849", "collapseBaseConfig");
+
+ checkElementValue("antennaHeight", -15000, 15000, "Must be -15000 to 15000", "collapseBaseConfig");
+ checkElementValue("antennaReferencePoint", -200.0, 200.0, "Must be -200.0 to 200.0", "collapseBaseConfig");
}
}
@@ -553,7 +603,7 @@ function clearElement(id, value) {
}
function resetToFactoryDefaults() {
- ge("factoryDefaultsMsg").innerHTML = "Defaults Applied. Please wait for device reset..."
+ ge("factoryDefaultsMsg").innerHTML = "Defaults Applied. Please wait for device reset...";
ws.send("factoryDefaultReset,1,");
}
@@ -669,6 +719,16 @@ function resetToLoggingDefaults() {
ge("UBX_RXM_RAWX").value = 1;
ge("UBX_RXM_SFRBX").value = 1;
}
+function useECEFCoordinates() {
+ ge("fixedEcefX").value = ecefX;
+ ge("fixedEcefY").value = ecefY;
+ ge("fixedEcefZ").value = ecefZ;
+}
+function useGeodeticCoordinates() {
+ ge("fixedLat").value = geodeticLat;
+ ge("fixedLong").value = geodeticLon;
+ ge("fixedAltitude").value = geodeticAlt;
+}
function exitConfig() {
show("exitPage");
@@ -689,6 +749,17 @@ function firmwareUploadComplete() {
hide("mainPage");
}
+function forgetPairedRadios() {
+ ge("btnForgetRadiosMsg").innerHTML = "All radios forgotten.";
+ ge("peerMACs").innerHTML = "None";
+ ws.send("forgetEspNowPeers,1,");
+}
+
+function btnResetProfile() {
+ ge("resetProfileMsg").innerHTML = "Resetting profile.";
+ ws.send("resetProfile,1,");
+}
+
document.addEventListener("DOMContentLoaded", (event) => {
var radios = document.querySelectorAll('input[name=profileRadio]');
@@ -812,6 +883,24 @@ document.addEventListener("DOMContentLoaded", (event) => {
}
});
+ ge("radioType").addEventListener("change", function () {
+ if (ge("radioType").value == 0) {
+ hide("radioDetails");
+ }
+ else if (ge("radioType").value == 1){
+ show("radioDetails");
+ }
+ });
+
+ ge("enableForgetRadios").addEventListener("change", function () {
+ if (ge("enableForgetRadios").checked) {
+ ge("btnForgetRadios").disabled = false;
+ }
+ else {
+ ge("btnForgetRadios").disabled = true;
+ }
+ });
+
ge("enableLogging").addEventListener("change", function () {
if (ge("enableLogging").checked) {
show("enableLoggingDetails");
@@ -820,5 +909,4 @@ document.addEventListener("DOMContentLoaded", (event) => {
hide("enableLoggingDetails");
}
});
-
})
diff --git a/Firmware/RTK_Surveyor/Base.ino b/Firmware/RTK_Surveyor/Base.ino
index 73d65eb30..1c1f650f5 100644
--- a/Firmware/RTK_Surveyor/Base.ino
+++ b/Firmware/RTK_Surveyor/Base.ino
@@ -113,7 +113,7 @@ bool beginSurveyIn()
return (false);
}
- Serial.printf("Survey started. This will run until %d seconds have passed and less than %0.03f meter accuracy is achieved.\n\r",
+ Serial.printf("Survey started. This will run until %d seconds have passed and less than %0.03f meter accuracy is achieved.\r\n",
settings.observationSeconds,
settings.observationPositionAccuracy
);
@@ -173,9 +173,9 @@ bool startFixedBase()
long majorEcefZ = floor((settings.fixedEcefZ * 100) + 0.5);
long minorEcefZ = floor((((settings.fixedEcefZ * 100.0) - majorEcefZ) * 100.0) + 0.5);
- // Serial.printf("fixedEcefY (should be -4716808.5807): %0.04f\n\r", settings.fixedEcefY);
- // Serial.printf("major (should be -471680858): %ld\n\r", majorEcefY);
- // Serial.printf("minor (should be -7): %ld\n\r", minorEcefY);
+ // Serial.printf("fixedEcefY (should be -4716808.5807): %0.04f\r\n", settings.fixedEcefY);
+ // Serial.printf("major (should be -471680858): %ld\r\n", majorEcefY);
+ // Serial.printf("minor (should be -7): %ld\r\n", minorEcefY);
//Units are cm with a high precision extension so -1234.5678 should be called: (-123456, -78)
//-1280208.308,-4716803.847,4086665.811 is SparkFun HQ so...
@@ -188,26 +188,31 @@ bool startFixedBase()
}
else if (settings.fixedBaseCoordinateType == COORD_TYPE_GEODETIC)
{
+ //Add height of instrument (HI) to fixed altitude
+ //https://www.e-education.psu.edu/geog862/node/1853
+ //For example, if HAE is at 100.0m, + 2m stick + 73mm ARP = 102.073
+ float totalFixedAltitude = settings.fixedAltitude + (settings.antennaHeight / 1000.0) + (settings.antennaReferencePoint / 1000.0);
+
//Break coordinates into main and high precision parts
//The type casting should not effect rounding of original double cast coordinate
int64_t majorLat = settings.fixedLat * 10000000;
int64_t minorLat = ((settings.fixedLat * 10000000) - majorLat) * 100;
int64_t majorLong = settings.fixedLong * 10000000;
int64_t minorLong = ((settings.fixedLong * 10000000) - majorLong) * 100;
- int32_t majorAlt = settings.fixedAltitude * 100;
- int32_t minorAlt = ((settings.fixedAltitude * 100) - majorAlt) * 100;
+ int32_t majorAlt = totalFixedAltitude * 100;
+ int32_t minorAlt = ((totalFixedAltitude * 100) - majorAlt) * 100;
- // Serial.printf("fixedLong (should be -105.184774720): %0.09f\n\r", settings.fixedLong);
- // Serial.printf("major (should be -1051847747): %lld\n\r", majorLat);
- // Serial.printf("minor (should be -20): %lld\n\r", minorLat);
+ // Serial.printf("fixedLong (should be -105.184774720): %0.09f\r\n", settings.fixedLong);
+ // Serial.printf("major (should be -1051847747): %lld\r\n", majorLat);
+ // Serial.printf("minor (should be -20): %lld\r\n", minorLat);
//
- // Serial.printf("fixedLat (should be 40.090335429): %0.09f\n\r", settings.fixedLat);
- // Serial.printf("major (should be 400903354): %lld\n\r", majorLong);
- // Serial.printf("minor (should be 29): %lld\n\r", minorLong);
+ // Serial.printf("fixedLat (should be 40.090335429): %0.09f\r\n", settings.fixedLat);
+ // Serial.printf("major (should be 400903354): %lld\r\n", majorLong);
+ // Serial.printf("minor (should be 29): %lld\r\n", minorLong);
//
- // Serial.printf("fixedAlt (should be 1560.2284): %0.04f\n\r", settings.fixedAltitude);
- // Serial.printf("major (should be 156022): %ld\n\r", majorAlt);
- // Serial.printf("minor (should be 84): %ld\n\r", minorAlt);
+ // Serial.printf("fixedAlt (should be 1560.2284): %0.04f\r\n", settings.fixedAltitude);
+ // Serial.printf("major (should be 156022): %ld\r\n", majorAlt);
+ // Serial.printf("minor (should be 84): %ld\r\n", minorAlt);
response = i2cGNSS.setStaticPosition(
majorLat, minorLat,
@@ -230,13 +235,9 @@ void SFE_UBLOX_GNSS::processRTCM(uint8_t incoming)
{
if (rtcmPacketsSent > 99) rtcmPacketsSent = 1; //Trim to two digits to avoid overlap
}
- else if (logIncreasing == true)
- {
- if (rtcmPacketsSent > 999) rtcmPacketsSent = 1; //Trim to three digits to avoid log icon
- }
else
{
- if (rtcmPacketsSent > 9999) rtcmPacketsSent = 1;
+ if (rtcmPacketsSent > 999) rtcmPacketsSent = 1; //Trim to three digits to avoid log icon and increasing bar
}
//Determine if we should check this byte with the RTCM checker or simply pass it along
diff --git a/Firmware/RTK_Surveyor/Begin.ino b/Firmware/RTK_Surveyor/Begin.ino
index 822c6c9e5..87369a074 100644
--- a/Firmware/RTK_Surveyor/Begin.ino
+++ b/Firmware/RTK_Surveyor/Begin.ino
@@ -148,17 +148,29 @@ void beginBoard()
btMACAddress[5] += 2; //Convert MAC address to Bluetooth MAC (add 2): https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/system/system.html#mac-address
//For all boards, check reset reason. If reset was due to wdt or panic, append last log
- loadSettingsPartial(); //Get resetCount
+ loadSettingsPartial(); //Loads settings from LFS
if (esp_reset_reason() == ESP_RST_POWERON)
{
reuseLastLog = false; //Start new log
+
+ if (settings.enableResetDisplay == true)
+ {
+ settings.resetCount = 0;
+ recordSystemSettingsToFileLFS(settingsFileName); //Avoid overwriting LittleFS settings onto SD
+ }
settings.resetCount = 0;
}
else
{
reuseLastLog = true; //Attempt to reuse previous log
- settings.resetCount++;
+ if (settings.enableResetDisplay == true)
+ {
+ settings.resetCount++;
+ Serial.printf("resetCount: %d\r\n", settings.resetCount);
+ recordSystemSettingsToFileLFS(settingsFileName); //Avoid overwriting LittleFS settings onto SD
+ }
+
Serial.print("Reset reason: ");
switch (esp_reset_reason())
{
@@ -175,8 +187,6 @@ void beginBoard()
default : Serial.println("Unknown");
}
}
-
- recordSystemSettings(); //Record resetCount to NVM
}
void beginSD()
@@ -445,7 +455,7 @@ void beginGNSS()
zedFirmwareVersionInt = 132;
else
{
- Serial.printf("Unknown firmware version: %s\n\r", zedFirmwareVersion);
+ Serial.printf("Unknown firmware version: %s\r\n", zedFirmwareVersion);
zedFirmwareVersionInt = 99; //0.99 invalid firmware version
}
@@ -456,7 +466,7 @@ void beginGNSS()
zedModuleType = PLATFORM_F9R;
else
{
- Serial.printf("Unknown ZED module: %s\n\r", i2cGNSS.minfo.extension[3]);
+ Serial.printf("Unknown ZED module: %s\r\n", i2cGNSS.minfo.extension[3]);
zedModuleType = PLATFORM_F9P;
}
diff --git a/Firmware/RTK_Surveyor/Bluetooth.ino b/Firmware/RTK_Surveyor/Bluetooth.ino
index 0a1b80f44..c81d68b0a 100644
--- a/Firmware/RTK_Surveyor/Bluetooth.ino
+++ b/Firmware/RTK_Surveyor/Bluetooth.ino
@@ -27,7 +27,7 @@ Bluetooth States:
// Locals - compiled out
//----------------------------------------
-#ifdef COMPILE_BT
+#ifdef COMPILE_BT
BTSerialInterface *bluetoothSerial;
static volatile byte bluetoothState = BT_OFF;
diff --git a/Firmware/RTK_Surveyor/Display.ino b/Firmware/RTK_Surveyor/Display.ino
index d47e13a22..d45de065f 100644
--- a/Firmware/RTK_Surveyor/Display.ino
+++ b/Firmware/RTK_Surveyor/Display.ino
@@ -291,7 +291,7 @@ void updateDisplay()
break;
case (STATE_KEYS_STARTED):
- //Do nothing. Quick, fall through state.
+ paintRTCWait();
break;
case (STATE_KEYS_NEEDED):
//Do nothing. Quick, fall through state.
@@ -342,7 +342,7 @@ void updateDisplay()
displayShutdown();
break;
default:
- Serial.printf("Unknown display: %d\n\r", systemState);
+ Serial.printf("Unknown display: %d\r\n", systemState);
displayError("Display");
break;
}
@@ -2384,30 +2384,19 @@ void paintKeyWiFiFail(uint16_t displayTime)
oled.setFont(QW_FONT_8X16);
- int x = (oled.getWidth() / 2); //Center point for x coord
int y = 0;
int fontHeight = 13;
- int textX;
- textX = x - (oled.getStringWidth("PP") / 2); //Starting point of text
- oled.setCursor(textX, y);
- oled.print("PP");
+ printTextCenter("PP", y, QW_FONT_8X16, 1, false); //text, y, font type, kerning, inverted
y += fontHeight;
- textX = x - (oled.getStringWidth("Update") / 2);
- oled.setCursor(textX, y);
- oled.print("Update");
+ printTextCenter("Update", y, QW_FONT_8X16, 1, false); //text, y, font type, kerning, inverted
y += fontHeight;
- textX = x - (oled.getStringWidth("Failed") / 2);
- oled.setCursor(textX, y);
- oled.print("Failed");
+ printTextCenter("Failed", y, QW_FONT_8X16, 1, false); //text, y, font type, kerning, inverted
- oled.setFont(QW_FONT_5X7);
y += fontHeight + 1;
- textX = x - (oled.getStringWidth("No WiFi") / 2);
- oled.setCursor(textX, y);
- oled.print("No WiFi");
+ printTextCenter("No WiFi", y, QW_FONT_5X7, 1, false); //text, y, font type, kerning, inverted
oled.display();
@@ -2426,33 +2415,21 @@ void paintNtripWiFiFail(uint16_t displayTime, bool Client)
{
oled.erase();
- oled.setFont(QW_FONT_8X16);
-
- int x = (oled.getWidth() / 2); //Center point for x coord
int y = 0;
int fontHeight = 13;
- int textX;
+
const char * string = Client ? "Client" : "Server";
- textX = x - (oled.getStringWidth("NTRIP") / 2); //Starting point of text
- oled.setCursor(textX, y);
- oled.print("NTRIP");
+ printTextCenter("NTRIP", y, QW_FONT_8X16, 1, false); //text, y, font type, kerning, inverted
y += fontHeight;
- textX = x - (oled.getStringWidth(string) / 2);
- oled.setCursor(textX, y);
- oled.print(string);
+ printTextCenter(string, y, QW_FONT_8X16, 1, false); //text, y, font type, kerning, inverted
y += fontHeight;
- textX = x - (oled.getStringWidth("Failed") / 2);
- oled.setCursor(textX, y);
- oled.print("Failed");
+ printTextCenter("Failed", y, QW_FONT_8X16, 1, false); //text, y, font type, kerning, inverted
- oled.setFont(QW_FONT_5X7);
y += fontHeight + 1;
- textX = x - (oled.getStringWidth("No WiFi") / 2);
- oled.setCursor(textX, y);
- oled.print("No WiFi");
+ printTextCenter("No WiFi", y, QW_FONT_5X7, 1, false); //text, y, font type, kerning, inverted
oled.display();
@@ -2475,6 +2452,13 @@ void paintGettingKeys()
displayMessage("Getting Keys", 0);
}
+//If an L-Band is indoors without reception, we have a ~2s wait for the RTC to come online
+//Display something while we wait
+void paintRTCWait()
+{
+ displayMessage("RTC Wait", 0);
+}
+
void paintKeyProvisionFail(uint16_t displayTime)
{
//Whitelist Error
@@ -2490,43 +2474,28 @@ void paintKeyProvisionFail(uint16_t displayTime)
oled.setFont(QW_FONT_5X7);
- int x = (oled.getWidth() / 2); //Center point for x coord
int y = 0;
int fontHeight = 8;
- int textX;
- textX = x - (oled.getStringWidth("ZTP") / 2); //Starting point of text
- oled.setCursor(textX, y);
- oled.print("ZTP");
+ printTextCenter("ZTP", y, QW_FONT_5X7, 1, false); //text, y, font type, kerning, inverted
y += fontHeight;
- textX = x - (oled.getStringWidth("Failed") / 2);
- oled.setCursor(textX, y);
- oled.print("Failed");
+ printTextCenter("Failed", y, QW_FONT_5X7, 1, false); //text, y, font type, kerning, inverted
y += fontHeight;
- textX = x - (oled.getStringWidth("ID:") / 2);
- oled.setCursor(textX, y);
- oled.print("ID:");
+ printTextCenter("ID:", y, QW_FONT_5X7, 1, false); //text, y, font type, kerning, inverted
- //The MAC address is characters long so we have to split it onto two lines
+ //The MAC address is 12 characters long so we have to split it onto two lines
char hardwareID[13];
const uint8_t * rtkMacAddress = getMacAddress();
- sprintf(hardwareID, "%02X%02X%02X", rtkMacAddress[0], rtkMacAddress[1], rtkMacAddress[2]);
- String macAddress = String(hardwareID);
+ sprintf(hardwareID, "%02X%02X%02X", rtkMacAddress[0], rtkMacAddress[1], rtkMacAddress[2]);
y += fontHeight;
- textX = x - (oled.getStringWidth(macAddress) / 2);
- oled.setCursor(textX, y);
- oled.print(hardwareID);
+ printTextCenter(hardwareID, y, QW_FONT_5X7, 1, false); //text, y, font type, kerning, inverted
sprintf(hardwareID, "%02X%02X%02X", rtkMacAddress[3], rtkMacAddress[4], rtkMacAddress[5]);
- macAddress = String(hardwareID);
-
y += fontHeight;
- textX = x - (oled.getStringWidth(macAddress) / 2);
- oled.setCursor(textX, y);
- oled.print(hardwareID);
+ printTextCenter(hardwareID, y, QW_FONT_5X7, 1, false); //text, y, font type, kerning, inverted
oled.display();
@@ -2548,7 +2517,7 @@ const uint8_t * getMacAddress()
{
static const uint8_t zero[6] = {0, 0, 0, 0, 0, 0};
-#ifdef COMPILE_BT
+#ifdef COMPILE_BT
if (bluetoothState != BT_OFF)
return btMACAddress;
#ifdef COMPILE_WIFI
diff --git a/Firmware/RTK_Surveyor/ESPNOW.ino b/Firmware/RTK_Surveyor/ESPNOW.ino
index 0a21c294c..7cbec4dc8 100644
--- a/Firmware/RTK_Surveyor/ESPNOW.ino
+++ b/Firmware/RTK_Surveyor/ESPNOW.ino
@@ -89,25 +89,26 @@ void promiscuous_rx_cb(void *buf, wifi_promiscuous_pkt_type_t type)
//If the radio is off entirely, start the radio, turn on only the LR protocol
void espnowStart()
{
+
#ifdef COMPILE_ESPNOW
if (wifiState == WIFI_OFF && espnowState == ESPNOW_OFF)
{
+ WiFi.mode(WIFI_STA);
+
//Radio is off, turn it on
esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_LR); //Stops WiFi Station.
- WiFi.mode(WIFI_STA);
-
log_d("WiFi off, ESP-Now added to protocols");
}
//If WiFi is on but ESP NOW is off, then enable LR protocol
else if (wifiState > WIFI_OFF && espnowState == ESPNOW_OFF)
{
+ WiFi.mode(WIFI_STA);
+
//Enable WiFi + ESP-Now
// Enable long range, PHY rate of ESP32 will be 512Kbps or 256Kbps
esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR); //Stops WiFi Station.
- WiFi.mode(WIFI_STA);
-
log_d("WiFi on, ESP-Now added to protocols");
}
@@ -149,11 +150,26 @@ void espnowStart()
{
esp_err_t result = espnowAddPeer(settings.espnowPeers[x]);
if (result != ESP_OK)
- log_d("Failed to add peer #%d\n\r", x);
+ log_d("Failed to add peer #%d", x);
}
}
}
+ if (settings.espnowBroadcast == true)
+ {
+ //Add broadcast peer if override is turned on
+ uint8_t broadcastMac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+
+ if (esp_now_is_peer_exist(broadcastMac) == true)
+ log_d("Broadcast peer already exists");
+ else
+ {
+ esp_err_t result = espnowAddPeer(broadcastMac, false); //Encryption not support for broadcast MAC
+ if (result != ESP_OK)
+ log_d("Failed to add broadcast peer");
+ }
+ }
+
Serial.println("ESP-Now Started");
#endif
}
@@ -222,9 +238,13 @@ bool espnowIsPaired()
#ifdef COMPILE_ESPNOW
if (espnowState == ESPNOW_MAC_RECEIVED)
{
- //Remove broadcast peer
- uint8_t broadcastMac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
- espnowRemovePeer(broadcastMac);
+
+ if (settings.espnowBroadcast == false)
+ {
+ //Remove broadcast peer
+ uint8_t broadcastMac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+ espnowRemovePeer(broadcastMac);
+ }
if (esp_now_is_peer_exist(receivedMAC) == true)
log_d("Peer already exists");
@@ -291,7 +311,7 @@ esp_err_t espnowAddPeer(uint8_t *peerMac, bool encrypt)
peerInfo.channel = 0;
peerInfo.ifidx = WIFI_IF_STA;
//memcpy(peerInfo.lmk, "RTKProductsLMK56", 16);
- //peerInfo.encrypt = true;
+ //peerInfo.encrypt = encrypt;
peerInfo.encrypt = false;
esp_err_t result = esp_now_add_peer(&peerInfo);
@@ -360,7 +380,15 @@ void espnowProcessRTCM(byte incoming)
if (espnowOutgoingSpot == sizeof(espnowOutgoing))
{
espnowOutgoingSpot = 0; //Wrap
- esp_now_send(0, (uint8_t *) &espnowOutgoing, sizeof(espnowOutgoing)); //Send packet to all peers
+
+ if (settings.espnowBroadcast == false)
+ esp_now_send(0, (uint8_t *) &espnowOutgoing, sizeof(espnowOutgoing)); //Send packet to all peers
+ else
+ {
+ uint8_t broadcastMac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+ esp_now_send(broadcastMac, (uint8_t *) &espnowOutgoing, sizeof(espnowOutgoing)); //Send packet via broadcast
+ }
+
delay(10); //We need a small delay between sending multiple packets
espnowBytesSent += sizeof(espnowOutgoing);
@@ -370,3 +398,48 @@ void espnowProcessRTCM(byte incoming)
}
#endif
}
+
+//A blocking function that is used to pair two devices
+//either through the serial menu or AP config
+void espnowStaticPairing()
+{
+ Serial.println("Begin ESP NOW Pairing");
+
+ //Start ESP-Now if needed, put ESP-Now into broadcast state
+ espnowBeginPairing();
+
+ //Begin sending our MAC every 250ms until a remote device sends us there info
+ randomSeed(millis());
+
+ Serial.println("Begin pairing. Place other unit in pairing mode. Press any key to exit.");
+ while (Serial.available()) Serial.read();
+
+ uint8_t broadcastMac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+
+ bool exitPair = false;
+ while (exitPair == false)
+ {
+ if (Serial.available())
+ {
+ Serial.println("User pressed button. Pairing canceled.");
+ break;
+ }
+
+ int timeout = 1000 + random(0, 100); //Delay 1000 to 1100ms
+ for (int x = 0 ; x < timeout ; x++)
+ {
+ delay(1);
+
+ if (espnowIsPaired() == true) //Check if we've received a pairing message
+ {
+ Serial.println("Pairing compete");
+ exitPair = true;
+ break;
+ }
+ }
+
+ espnowSendPairMessage(broadcastMac); //Send unit's MAC address over broadcast, no ack, no encryption
+
+ Serial.println("Scanning for other radio...");
+ }
+}
diff --git a/Firmware/RTK_Surveyor/Form.ino b/Firmware/RTK_Surveyor/Form.ino
index a7c4ac227..7e4bcbbde 100644
--- a/Firmware/RTK_Surveyor/Form.ino
+++ b/Firmware/RTK_Surveyor/Form.ino
@@ -193,13 +193,13 @@ static void handleFirmwareFileUpload(AsyncWebServerRequest *request, String file
}
else
{
- Serial.printf("Unknown: %s\n\r", fname);
+ Serial.printf("Unknown: %s\r\n", fname);
return request->send(400, "text/html", "
Error: Unknown file type");
}
}
else
{
- Serial.printf("Unknown: %s\n\r", fname);
+ Serial.printf("Unknown: %s\r\n", fname);
return request->send(400, "text/html", "
Error: Unknown file type");
}
}
@@ -221,12 +221,12 @@ static void handleFirmwareFileUpload(AsyncWebServerRequest *request, String file
char bytesSentMsg[100];
sprintf(bytesSentMsg, "%'d bytes sent", binBytesSent);
- Serial.printf("bytesSentMsg: %s\n\r", bytesSentMsg);
+ Serial.printf("bytesSentMsg: %s\r\n", bytesSentMsg);
char statusMsg[200] = {'\0'};
stringRecord(statusMsg, "firmwareUploadStatus", bytesSentMsg); //Convert to "firmwareUploadMsg,11214 bytes sent,"
- Serial.printf("msg: %s\n\r", statusMsg);
+ Serial.printf("msg: %s\r\n", statusMsg);
ws.textAll(statusMsg);
}
@@ -255,15 +255,18 @@ static void handleFirmwareFileUpload(AsyncWebServerRequest *request, String file
//Events triggered by web sockets
#ifdef COMPILE_WIFI
#ifdef COMPILE_AP
+
void onWsEvent(AsyncWebSocket * server, AsyncWebSocketClient * client, AwsEventType type, void * arg, uint8_t *data, size_t len)
{
if (type == WS_EVT_CONNECT) {
- char settingsCSV[AP_CONFIG_SETTING_SIZE];
- memset(settingsCSV, 0, sizeof(settingsCSV));
+ settingsCSV = (char*)malloc(AP_CONFIG_SETTING_SIZE);
+ memset(settingsCSV, 0, AP_CONFIG_SETTING_SIZE); //Clear any garbage from settings array
+
createSettingsString(settingsCSV);
- log_d("Sending command: %s\n\r", settingsCSV);
+ log_d("Sending command: %s", settingsCSV);
client->text(settingsCSV);
wifiSetState(WIFI_CONNECTED);
+ free(settingsCSV);
}
else if (type == WS_EVT_DISCONNECT) {
log_d("Websocket client disconnected");
@@ -272,6 +275,7 @@ void onWsEvent(AsyncWebSocket * server, AsyncWebSocketClient * client, AwsEventT
else if (type == WS_EVT_DATA) {
for (int i = 0; i < len; i++) {
incomingSettings[incomingSettingsSpot++] = data[i];
+ incomingSettingsSpot %= AP_CONFIG_SETTING_SIZE;
}
timeSinceLastIncomingSetting = millis();
}
@@ -334,8 +338,8 @@ void createSettingsString(char* settingsCSV)
stringRecord(settingsCSV, "fixedEcefX", settings.fixedEcefX, 3);
stringRecord(settingsCSV, "fixedEcefY", settings.fixedEcefY, 3);
stringRecord(settingsCSV, "fixedEcefZ", settings.fixedEcefZ, 3);
- stringRecord(settingsCSV, "fixedLat", settings.fixedLat, 9);
- stringRecord(settingsCSV, "fixedLong", settings.fixedLong, 9);
+ stringRecord(settingsCSV, "fixedLat", settings.fixedLat, haeNumberOfDecimals);
+ stringRecord(settingsCSV, "fixedLong", settings.fixedLong, haeNumberOfDecimals);
stringRecord(settingsCSV, "fixedAltitude", settings.fixedAltitude, 4);
stringRecord(settingsCSV, "enableNtripServer", settings.enableNtripServer);
@@ -421,14 +425,64 @@ void createSettingsString(char* settingsCSV)
sprintf(nameText, "%d: %s", index + 1, profileNames[index]);
stringRecord(settingsCSV, tagText, nameText);
}
- stringRecord(settingsCSV, "activeProfiles", activeProfiles);
+ //stringRecord(settingsCSV, "activeProfiles", activeProfiles);
+
+ //Bluetooth radio type
+ stringRecord(settingsCSV, "bluetoothRadioType", settings.bluetoothRadioType);
+
+ //Current coordinates come from HPPOSLLH call back
+ stringRecord(settingsCSV, "geodeticLat", latitude, haeNumberOfDecimals);
+ stringRecord(settingsCSV, "geodeticLon", longitude, haeNumberOfDecimals);
+ stringRecord(settingsCSV, "geodeticAlt", altitude, 3);
+
+ double ecefX = 0;
+ double ecefY = 0;
+ double ecefZ = 0;
+
+ geodeticToEcef(latitude, longitude, altitude, &ecefX, &ecefY, &ecefZ);
+
+ stringRecord(settingsCSV, "ecefX", ecefX, 3);
+ stringRecord(settingsCSV, "ecefY", ecefY, 3);
+ stringRecord(settingsCSV, "ecefZ", ecefZ, 3);
+
+ //Antenna height and ARP
+ stringRecord(settingsCSV, "antennaHeight", settings.antennaHeight);
+ stringRecord(settingsCSV, "antennaReferencePoint", settings.antennaReferencePoint, 1);
+
+ //Radio / ESP-Now settings
+ char radioMAC[18]; //Send radio MAC
+ sprintf(radioMAC, "%02X:%02X:%02X:%02X:%02X:%02X",
+ wifiMACAddress[0],
+ wifiMACAddress[1],
+ wifiMACAddress[2],
+ wifiMACAddress[3],
+ wifiMACAddress[4],
+ wifiMACAddress[5]
+ );
+ stringRecord(settingsCSV, "radioMAC", radioMAC);
+ stringRecord(settingsCSV, "radioType", settings.radioType);
+ stringRecord(settingsCSV, "espnowPeerCount", settings.espnowPeerCount);
+ for (int index = 0; index < settings.espnowPeerCount; index++)
+ {
+ sprintf(tagText, "peerMAC%d", index);
+ sprintf(nameText, "%02X:%02X:%02X:%02X:%02X:%02X",
+ settings.espnowPeers[index][0],
+ settings.espnowPeers[index][1],
+ settings.espnowPeers[index][2],
+ settings.espnowPeers[index][3],
+ settings.espnowPeers[index][4],
+ settings.espnowPeers[index][5]
+ );
+ stringRecord(settingsCSV, tagText, nameText);
+ }
+ stringRecord(settingsCSV, "espnowBroadcast", settings.espnowBroadcast);
//New settings not yet integrated
//...
strcat(settingsCSV, "\0");
- Serial.printf("settingsCSV len: %d\n\r", strlen(settingsCSV));
- Serial.printf("settingsCSV: %s\n\r", settingsCSV);
+ Serial.printf("settingsCSV len: %d\r\n", strlen(settingsCSV));
+ Serial.printf("settingsCSV: %s\r\n", settingsCSV);
#endif
}
@@ -437,18 +491,13 @@ void updateSettingWithValue(const char *settingName, const char* settingValueStr
{
#ifdef COMPILE_AP
char* ptr;
- int newProfileNumber;
double settingValue = strtod(settingValueStr, &ptr);
bool settingValueBool = false;
if (strcmp(settingValueStr, "true") == 0) settingValueBool = true;
if (strcmp(settingName, "maxLogTime_minutes") == 0)
- {
- newAPSettings = true; //Mark settings as new to force record before reset
settings.maxLogTime_minutes = settingValue;
- }
-
else if (strcmp(settingName, "maxLogLength_minutes") == 0)
settings.maxLogLength_minutes = settingValue;
else if (strcmp(settingName, "measurementRateHz") == 0)
@@ -505,19 +554,6 @@ void updateSettingWithValue(const char *settingName, const char* settingValueStr
strcpy(settings.profileName, settingValueStr);
setProfileName(profileNumber);
}
- else if (strcmp(settingName, "profileNumber") == 0)
- {
- if ((sscanf(settingValueStr, "%d", &newProfileNumber) == 1)
- && (newProfileNumber >= 1) && (newProfileNumber <= MAX_PROFILE_COUNT)
- && (profileNumber != newProfileNumber))
- {
- profileNumber = newProfileNumber - 1;
-
- //Switch to a new profile
- setSettingsFileName();
- recordProfileNumber(profileNumber);
- }
- }
else if (strcmp(settingName, "enableNtripServer") == 0)
settings.enableNtripServer = settingValueBool;
else if (strcmp(settingName, "ntripServer_CasterHost") == 0)
@@ -569,6 +605,16 @@ void updateSettingWithValue(const char *settingName, const char* settingValueStr
strcpy(settings.home_wifiPW, settingValueStr);
else if (strcmp(settingName, "autoKeyRenewal") == 0)
settings.autoKeyRenewal = settingValueBool;
+ else if (strcmp(settingName, "antennaHeight") == 0)
+ settings.antennaHeight = settingValue;
+ else if (strcmp(settingName, "antennaReferencePoint") == 0)
+ settings.antennaReferencePoint = settingValue;
+ else if (strcmp(settingName, "bluetoothRadioType") == 0)
+ settings.bluetoothRadioType = (BluetoothRadioType_e)settingValue; //0 = SPP, 1 = BLE, 2 = Off
+ else if (strcmp(settingName, "espnowBroadcast") == 0)
+ settings.espnowBroadcast = settingValueBool;
+ else if (strcmp(settingName, "radioType") == 0)
+ settings.radioType = (RadioType_e)settingValue; //0 = Radio off, 1 = ESP-Now
//Unused variables - read to avoid errors
else if (strcmp(settingName, "measurementRateSec") == 0) {}
@@ -577,6 +623,7 @@ void updateSettingWithValue(const char *settingName, const char* settingValueStr
else if (strcmp(settingName, "saveToArduino") == 0) {}
else if (strcmp(settingName, "enableFactoryDefaults") == 0) {}
else if (strcmp(settingName, "enableFirmwareUpdate") == 0) {}
+ else if (strcmp(settingName, "enableForgetRadios") == 0) {}
//Special actions
else if (strcmp(settingName, "firmwareFileName") == 0)
@@ -591,11 +638,12 @@ void updateSettingWithValue(const char *settingName, const char* settingValueStr
factoryReset();
else if (strcmp(settingName, "exitAndReset") == 0)
{
- if (newAPSettings == true) recordSystemSettings(); //If we've received settings, record before restart
+ Serial.println("Reset after AP Config");
- //Reboot the machine
ESP.restart();
}
+
+
else if (strcmp(settingName, "setProfile") == 0)
{
//Change to new profile
@@ -604,12 +652,37 @@ void updateSettingWithValue(const char *settingName, const char* settingValueStr
//Load new profile into system
loadSettings();
- //Send settings to browser
- char settingsCSV[AP_CONFIG_SETTING_SIZE];
- memset(settingsCSV, 0, sizeof(settingsCSV));
+ //Send new settings to browser. Re-use settingsCSV to avoid stack.
+ settingsCSV = (char*)malloc(AP_CONFIG_SETTING_SIZE);
+ memset(settingsCSV, 0, AP_CONFIG_SETTING_SIZE); //Clear any garbage from settings array
+ createSettingsString(settingsCSV);
+ log_d("Sending command: %s", settingsCSV);
+ ws.textAll(String(settingsCSV));
+ free(settingsCSV);
+ }
+ else if (strcmp(settingName, "resetProfile") == 0)
+ {
+ settingsToDefaults(); //Overwrite our current settings with defaults
+
+ recordSystemSettings(); //Overwrite profile file and NVM with these settings
+
+ //Get bitmask of active profiles
+ activeProfiles = loadProfileNames();
+
+ //Send new settings to browser. Re-use settingsCSV to avoid stack.
+ settingsCSV = (char*)malloc(AP_CONFIG_SETTING_SIZE);
+ memset(settingsCSV, 0, AP_CONFIG_SETTING_SIZE); //Clear any garbage from settings array
createSettingsString(settingsCSV);
- log_d("Sending command: %s\n\r", settingsCSV);
+ log_d("Sending command: %s", settingsCSV);
ws.textAll(String(settingsCSV));
+ free(settingsCSV);
+ }
+ else if (strcmp(settingName, "forgetEspNowPeers") == 0)
+ {
+ //Forget all ESP-Now Peers
+ for (int x = 0 ; x < settings.espnowPeerCount ; x++)
+ espnowRemovePeer(settings.espnowPeers[x]);
+ settings.espnowPeerCount = 0;
}
//Check for bulk settings (constellations and message rates)
@@ -652,7 +725,7 @@ void updateSettingWithValue(const char *settingName, const char* settingValueStr
//Last catch
if (knownSetting == false)
{
- Serial.printf("Unknown '%s': %0.3lf\n\r", settingName, settingValue);
+ Serial.printf("Unknown '%s': %0.3lf\r\n", settingName, settingValue);
}
} //End last strcpy catch
#endif
diff --git a/Firmware/RTK_Surveyor/NVM.ino b/Firmware/RTK_Surveyor/NVM.ino
index 779689416..ba0ec0c84 100644
--- a/Firmware/RTK_Surveyor/NVM.ino
+++ b/Firmware/RTK_Surveyor/NVM.ino
@@ -20,7 +20,12 @@ void loadSettings()
{
//If we have a profile in both LFS and SD, the SD settings will overwrite LFS
loadSystemSettingsFromFileLFS(settingsFileName, &settings);
+
+ //Temp store any variables from LFS that should override SD
+ int resetCount = settings.resetCount;
+
loadSystemSettingsFromFileSD(settingsFileName, &settings);
+ settings.resetCount = resetCount;
//Change empty profile name to 'Profile1' etc
if (strlen(settings.profileName) == 0)
@@ -32,7 +37,7 @@ void loadSettings()
//Get bitmask of active profiles
activeProfiles = loadProfileNames();
- Serial.printf("Profile '%s' loaded\n\r", profileNames[profileNumber]);
+ Serial.printf("Profile '%s' loaded\r\n", profileNames[profileNumber]);
}
//Set the settingsFileName used many places
@@ -81,7 +86,10 @@ void recordSystemSettingsToFileSD(char *fileName)
{
gotSemaphore = true;
if (sd->exists(fileName))
+ {
+ log_d("Removing from SD: %s", fileName);
sd->remove(fileName);
+ }
SdFile settingsFile; //FAT32
if (settingsFile.open(fileName, O_CREAT | O_APPEND | O_WRITE) == false)
@@ -123,7 +131,10 @@ void recordSystemSettingsToFileLFS(char *fileName)
if (online.fs == true)
{
if (LittleFS.exists(fileName))
+ {
LittleFS.remove(fileName);
+ log_d("Removing LittleFS: %s", fileName);
+ }
File settingsFile = LittleFS.open(fileName, FILE_WRITE);
if (!settingsFile)
@@ -142,114 +153,115 @@ void recordSystemSettingsToFileLFS(char *fileName)
//Write the settings struct to a clear text file
void recordSystemSettingsToFile(File * settingsFile)
{
- settingsFile->printf("%s=%d\n\r", "sizeOfSettings", settings.sizeOfSettings);
- settingsFile->printf("%s=%d\n\r", "rtkIdentifier", settings.rtkIdentifier);
+ settingsFile->printf("%s=%d\r\n", "sizeOfSettings", settings.sizeOfSettings);
+ settingsFile->printf("%s=%d\r\n", "rtkIdentifier", settings.rtkIdentifier);
char firmwareVersion[30]; //v1.3 December 31 2021
sprintf(firmwareVersion, "v%d.%d-%s", FIRMWARE_VERSION_MAJOR, FIRMWARE_VERSION_MINOR, __DATE__);
- settingsFile->printf("%s=%s\n\r", "rtkFirmwareVersion", firmwareVersion);
+ settingsFile->printf("%s=%s\r\n", "rtkFirmwareVersion", firmwareVersion);
- settingsFile->printf("%s=%s\n\r", "zedFirmwareVersion", zedFirmwareVersion);
+ settingsFile->printf("%s=%s\r\n", "zedFirmwareVersion", zedFirmwareVersion);
if (productVariant == RTK_FACET_LBAND)
- settingsFile->printf("%s=%s\n\r", "neoFirmwareVersion", neoFirmwareVersion);
- settingsFile->printf("%s=%d\n\r", "printDebugMessages", settings.printDebugMessages);
- settingsFile->printf("%s=%d\n\r", "enableSD", settings.enableSD);
- settingsFile->printf("%s=%d\n\r", "enableDisplay", settings.enableDisplay);
- settingsFile->printf("%s=%d\n\r", "maxLogTime_minutes", settings.maxLogTime_minutes);
- settingsFile->printf("%s=%d\n\r", "maxLogLength_minutes", settings.maxLogLength_minutes);
- settingsFile->printf("%s=%d\n\r", "observationSeconds", settings.observationSeconds);
- settingsFile->printf("%s=%0.2f\n\r", "observationPositionAccuracy", settings.observationPositionAccuracy);
- settingsFile->printf("%s=%d\n\r", "fixedBase", settings.fixedBase);
- settingsFile->printf("%s=%d\n\r", "fixedBaseCoordinateType", settings.fixedBaseCoordinateType);
- settingsFile->printf("%s=%0.3f\n\r", "fixedEcefX", settings.fixedEcefX); //-1280206.568
- settingsFile->printf("%s=%0.3f\n\r", "fixedEcefY", settings.fixedEcefY);
- settingsFile->printf("%s=%0.3f\n\r", "fixedEcefZ", settings.fixedEcefZ);
- settingsFile->printf("%s=%0.9f\n\r", "fixedLat", settings.fixedLat); //40.09029479
- settingsFile->printf("%s=%0.9f\n\r", "fixedLong", settings.fixedLong);
- settingsFile->printf("%s=%0.4f\n\r", "fixedAltitude", settings.fixedAltitude);
- settingsFile->printf("%s=%d\n\r", "dataPortBaud", settings.dataPortBaud);
- settingsFile->printf("%s=%d\n\r", "radioPortBaud", settings.radioPortBaud);
- settingsFile->printf("%s=%0.1f\n\r", "surveyInStartingAccuracy", settings.surveyInStartingAccuracy);
- settingsFile->printf("%s=%d\n\r", "measurementRate", settings.measurementRate);
- settingsFile->printf("%s=%d\n\r", "navigationRate", settings.navigationRate);
- settingsFile->printf("%s=%d\n\r", "enableI2Cdebug", settings.enableI2Cdebug);
- settingsFile->printf("%s=%d\n\r", "enableHeapReport", settings.enableHeapReport);
- settingsFile->printf("%s=%d\n\r", "enableTaskReports", settings.enableTaskReports);
- settingsFile->printf("%s=%d\n\r", "dataPortChannel", (uint8_t)settings.dataPortChannel);
- settingsFile->printf("%s=%d\n\r", "spiFrequency", settings.spiFrequency);
- settingsFile->printf("%s=%d\n\r", "sppRxQueueSize", settings.sppRxQueueSize);
- settingsFile->printf("%s=%d\n\r", "sppTxQueueSize", settings.sppTxQueueSize);
- settingsFile->printf("%s=%d\n\r", "dynamicModel", settings.dynamicModel);
- settingsFile->printf("%s=%d\n\r", "lastState", settings.lastState);
- settingsFile->printf("%s=%d\n\r", "enableSensorFusion", settings.enableSensorFusion);
- settingsFile->printf("%s=%d\n\r", "autoIMUmountAlignment", settings.autoIMUmountAlignment);
- settingsFile->printf("%s=%d\n\r", "enableResetDisplay", settings.enableResetDisplay);
- settingsFile->printf("%s=%d\n\r", "enableExternalPulse", settings.enableExternalPulse);
- settingsFile->printf("%s=%d\n\r", "externalPulseTimeBetweenPulse_us", settings.externalPulseTimeBetweenPulse_us);
- settingsFile->printf("%s=%d\n\r", "externalPulseLength_us", settings.externalPulseLength_us);
- settingsFile->printf("%s=%d\n\r", "externalPulsePolarity", settings.externalPulsePolarity);
- settingsFile->printf("%s=%d\n\r", "enableExternalHardwareEventLogging", settings.enableExternalHardwareEventLogging);
- settingsFile->printf("%s=%s\n\r", "profileName", settings.profileName);
- settingsFile->printf("%s=%d\n\r", "enableNtripServer", settings.enableNtripServer);
- settingsFile->printf("%s=%d\n\r", "ntripServer_StartAtSurveyIn", settings.ntripServer_StartAtSurveyIn);
- settingsFile->printf("%s=%s\n\r", "ntripServer_CasterHost", settings.ntripServer_CasterHost);
- settingsFile->printf("%s=%d\n\r", "ntripServer_CasterPort", settings.ntripServer_CasterPort);
- settingsFile->printf("%s=%s\n\r", "ntripServer_CasterUser", settings.ntripServer_CasterUser);
- settingsFile->printf("%s=%s\n\r", "ntripServer_CasterUserPW", settings.ntripServer_CasterUserPW);
- settingsFile->printf("%s=%s\n\r", "ntripServer_MountPoint", settings.ntripServer_MountPoint);
- settingsFile->printf("%s=%s\n\r", "ntripServer_MountPointPW", settings.ntripServer_MountPointPW);
- settingsFile->printf("%s=%s\n\r", "ntripServer_wifiSSID", settings.ntripServer_wifiSSID);
- settingsFile->printf("%s=%s\n\r", "ntripServer_wifiPW", settings.ntripServer_wifiPW);
- settingsFile->printf("%s=%d\n\r", "enableNtripClient", settings.enableNtripClient);
- settingsFile->printf("%s=%s\n\r", "ntripClient_CasterHost", settings.ntripClient_CasterHost);
- settingsFile->printf("%s=%d\n\r", "ntripClient_CasterPort", settings.ntripClient_CasterPort);
- settingsFile->printf("%s=%s\n\r", "ntripClient_CasterUser", settings.ntripClient_CasterUser);
- settingsFile->printf("%s=%s\n\r", "ntripClient_CasterUserPW", settings.ntripClient_CasterUserPW);
- settingsFile->printf("%s=%s\n\r", "ntripClient_MountPoint", settings.ntripClient_MountPoint);
- settingsFile->printf("%s=%s\n\r", "ntripClient_MountPointPW", settings.ntripClient_MountPointPW);
- settingsFile->printf("%s=%s\n\r", "ntripClient_wifiSSID", settings.ntripClient_wifiSSID);
- settingsFile->printf("%s=%s\n\r", "ntripClient_wifiPW", settings.ntripClient_wifiPW);
- settingsFile->printf("%s=%d\n\r", "ntripClient_TransmitGGA", settings.ntripClient_TransmitGGA);
- settingsFile->printf("%s=%d\n\r", "serialTimeoutGNSS", settings.serialTimeoutGNSS);
- settingsFile->printf("%s=%s\n\r", "pointPerfectDeviceProfileToken", settings.pointPerfectDeviceProfileToken);
- settingsFile->printf("%s=%d\n\r", "enablePointPerfectCorrections", settings.enablePointPerfectCorrections);
- settingsFile->printf("%s=%s\n\r", "home_wifiSSID", settings.home_wifiSSID);
- settingsFile->printf("%s=%s\n\r", "home_wifiPW", settings.home_wifiPW);
- settingsFile->printf("%s=%d\n\r", "autoKeyRenewal", settings.autoKeyRenewal);
- settingsFile->printf("%s=%s\n\r", "pointPerfectClientID", settings.pointPerfectClientID);
- settingsFile->printf("%s=%s\n\r", "pointPerfectBrokerHost", settings.pointPerfectBrokerHost);
- settingsFile->printf("%s=%s\n\r", "pointPerfectLBandTopic", settings.pointPerfectLBandTopic);
- settingsFile->printf("%s=%s\n\r", "pointPerfectCurrentKey", settings.pointPerfectCurrentKey);
- settingsFile->printf("%s=%llu\n\r", "pointPerfectCurrentKeyDuration", settings.pointPerfectCurrentKeyDuration);
- settingsFile->printf("%s=%llu\n\r", "pointPerfectCurrentKeyStart", settings.pointPerfectCurrentKeyStart);
- settingsFile->printf("%s=%s\n\r", "pointPerfectNextKey", settings.pointPerfectNextKey);
- settingsFile->printf("%s=%llu\n\r", "pointPerfectNextKeyDuration", settings.pointPerfectNextKeyDuration);
- settingsFile->printf("%s=%llu\n\r", "pointPerfectNextKeyStart", settings.pointPerfectNextKeyStart);
- settingsFile->printf("%s=%llu\n\r", "lastKeyAttempt", settings.lastKeyAttempt);
- settingsFile->printf("%s=%d\n\r", "updateZEDSettings", settings.updateZEDSettings);
- settingsFile->printf("%s=%d\n\r", "LBandFreq", settings.LBandFreq);
- settingsFile->printf("%s=%d\n\r", "enableLogging", settings.enableLogging);
- settingsFile->printf("%s=%d\n\r", "timeZoneHours", settings.timeZoneHours);
- settingsFile->printf("%s=%d\n\r", "timeZoneMinutes", settings.timeZoneMinutes);
- settingsFile->printf("%s=%d\n\r", "timeZoneSeconds", settings.timeZoneSeconds);
- settingsFile->printf("%s=%d\n\r", "enablePrintWifiIpAddress", settings.enablePrintWifiIpAddress);
- settingsFile->printf("%s=%d\n\r", "enablePrintState", settings.enablePrintState);
- settingsFile->printf("%s=%d\n\r", "enablePrintWifiState", settings.enablePrintWifiState);
- settingsFile->printf("%s=%d\n\r", "enablePrintNtripClientState", settings.enablePrintNtripClientState);
- settingsFile->printf("%s=%d\n\r", "enablePrintNtripServerState", settings.enablePrintNtripServerState);
- settingsFile->printf("%s=%d\n\r", "enablePrintPosition", settings.enablePrintPosition);
- settingsFile->printf("%s=%d\n\r", "enableMarksFile", settings.enableMarksFile);
- settingsFile->printf("%s=%d\n\r", "enablePrintBatteryMessages", settings.enablePrintBatteryMessages);
- settingsFile->printf("%s=%d\n\r", "enablePrintRoverAccuracy", settings.enablePrintRoverAccuracy);
- settingsFile->printf("%s=%d\n\r", "enablePrintBadMessages", settings.enablePrintBadMessages);
- settingsFile->printf("%s=%d\n\r", "enablePrintLogFileMessages", settings.enablePrintLogFileMessages);
- settingsFile->printf("%s=%d\n\r", "enablePrintLogFileStatus", settings.enablePrintLogFileStatus);
- settingsFile->printf("%s=%d\n\r", "enablePrintRingBufferOffsets", settings.enablePrintRingBufferOffsets);
- settingsFile->printf("%s=%d\n\r", "enablePrintNtripServerRtcm", settings.enablePrintNtripServerRtcm);
- settingsFile->printf("%s=%d\n\r", "enablePrintNtripClientRtcm", settings.enablePrintNtripClientRtcm);
- settingsFile->printf("%s=%d\n\r", "enablePrintStates", settings.enablePrintStates);
- settingsFile->printf("%s=%d\n\r", "enablePrintDuplicateStates", settings.enablePrintDuplicateStates);
- settingsFile->printf("%s=%d\n\r", "radioType", settings.radioType);
+ settingsFile->printf("%s=%s\r\n", "neoFirmwareVersion", neoFirmwareVersion);
+ settingsFile->printf("%s=%d\r\n", "printDebugMessages", settings.printDebugMessages);
+ settingsFile->printf("%s=%d\r\n", "enableSD", settings.enableSD);
+ settingsFile->printf("%s=%d\r\n", "enableDisplay", settings.enableDisplay);
+ settingsFile->printf("%s=%d\r\n", "maxLogTime_minutes", settings.maxLogTime_minutes);
+ settingsFile->printf("%s=%d\r\n", "maxLogLength_minutes", settings.maxLogLength_minutes);
+ settingsFile->printf("%s=%d\r\n", "observationSeconds", settings.observationSeconds);
+ settingsFile->printf("%s=%0.2f\r\n", "observationPositionAccuracy", settings.observationPositionAccuracy);
+ settingsFile->printf("%s=%d\r\n", "fixedBase", settings.fixedBase);
+ settingsFile->printf("%s=%d\r\n", "fixedBaseCoordinateType", settings.fixedBaseCoordinateType);
+ settingsFile->printf("%s=%0.3f\r\n", "fixedEcefX", settings.fixedEcefX); //-1280206.568
+ settingsFile->printf("%s=%0.3f\r\n", "fixedEcefY", settings.fixedEcefY);
+ settingsFile->printf("%s=%0.3f\r\n", "fixedEcefZ", settings.fixedEcefZ);
+ settingsFile->printf("%s=%0.9f\r\n", "fixedLat", settings.fixedLat); //40.09029479
+ settingsFile->printf("%s=%0.9f\r\n", "fixedLong", settings.fixedLong);
+ settingsFile->printf("%s=%0.4f\r\n", "fixedAltitude", settings.fixedAltitude);
+ settingsFile->printf("%s=%d\r\n", "dataPortBaud", settings.dataPortBaud);
+ settingsFile->printf("%s=%d\r\n", "radioPortBaud", settings.radioPortBaud);
+ settingsFile->printf("%s=%0.1f\r\n", "surveyInStartingAccuracy", settings.surveyInStartingAccuracy);
+ settingsFile->printf("%s=%d\r\n", "measurementRate", settings.measurementRate);
+ settingsFile->printf("%s=%d\r\n", "navigationRate", settings.navigationRate);
+ settingsFile->printf("%s=%d\r\n", "enableI2Cdebug", settings.enableI2Cdebug);
+ settingsFile->printf("%s=%d\r\n", "enableHeapReport", settings.enableHeapReport);
+ settingsFile->printf("%s=%d\r\n", "enableTaskReports", settings.enableTaskReports);
+ settingsFile->printf("%s=%d\r\n", "dataPortChannel", (uint8_t)settings.dataPortChannel);
+ settingsFile->printf("%s=%d\r\n", "spiFrequency", settings.spiFrequency);
+ settingsFile->printf("%s=%d\r\n", "sppRxQueueSize", settings.sppRxQueueSize);
+ settingsFile->printf("%s=%d\r\n", "sppTxQueueSize", settings.sppTxQueueSize);
+ settingsFile->printf("%s=%d\r\n", "dynamicModel", settings.dynamicModel);
+ settingsFile->printf("%s=%d\r\n", "lastState", settings.lastState);
+ settingsFile->printf("%s=%d\r\n", "enableSensorFusion", settings.enableSensorFusion);
+ settingsFile->printf("%s=%d\r\n", "autoIMUmountAlignment", settings.autoIMUmountAlignment);
+ settingsFile->printf("%s=%d\r\n", "enableResetDisplay", settings.enableResetDisplay);
+ settingsFile->printf("%s=%d\r\n", "enableExternalPulse", settings.enableExternalPulse);
+ settingsFile->printf("%s=%d\r\n", "externalPulseTimeBetweenPulse_us", settings.externalPulseTimeBetweenPulse_us);
+ settingsFile->printf("%s=%d\r\n", "externalPulseLength_us", settings.externalPulseLength_us);
+ settingsFile->printf("%s=%d\r\n", "externalPulsePolarity", settings.externalPulsePolarity);
+ settingsFile->printf("%s=%d\r\n", "enableExternalHardwareEventLogging", settings.enableExternalHardwareEventLogging);
+ settingsFile->printf("%s=%s\r\n", "profileName", settings.profileName);
+ settingsFile->printf("%s=%d\r\n", "enableNtripServer", settings.enableNtripServer);
+ settingsFile->printf("%s=%d\r\n", "ntripServer_StartAtSurveyIn", settings.ntripServer_StartAtSurveyIn);
+ settingsFile->printf("%s=%s\r\n", "ntripServer_CasterHost", settings.ntripServer_CasterHost);
+ settingsFile->printf("%s=%d\r\n", "ntripServer_CasterPort", settings.ntripServer_CasterPort);
+ settingsFile->printf("%s=%s\r\n", "ntripServer_CasterUser", settings.ntripServer_CasterUser);
+ settingsFile->printf("%s=%s\r\n", "ntripServer_CasterUserPW", settings.ntripServer_CasterUserPW);
+ settingsFile->printf("%s=%s\r\n", "ntripServer_MountPoint", settings.ntripServer_MountPoint);
+ settingsFile->printf("%s=%s\r\n", "ntripServer_MountPointPW", settings.ntripServer_MountPointPW);
+ settingsFile->printf("%s=%s\r\n", "ntripServer_wifiSSID", settings.ntripServer_wifiSSID);
+ settingsFile->printf("%s=%s\r\n", "ntripServer_wifiPW", settings.ntripServer_wifiPW);
+ settingsFile->printf("%s=%d\r\n", "enableNtripClient", settings.enableNtripClient);
+ settingsFile->printf("%s=%s\r\n", "ntripClient_CasterHost", settings.ntripClient_CasterHost);
+ settingsFile->printf("%s=%d\r\n", "ntripClient_CasterPort", settings.ntripClient_CasterPort);
+ settingsFile->printf("%s=%s\r\n", "ntripClient_CasterUser", settings.ntripClient_CasterUser);
+ settingsFile->printf("%s=%s\r\n", "ntripClient_CasterUserPW", settings.ntripClient_CasterUserPW);
+ settingsFile->printf("%s=%s\r\n", "ntripClient_MountPoint", settings.ntripClient_MountPoint);
+ settingsFile->printf("%s=%s\r\n", "ntripClient_MountPointPW", settings.ntripClient_MountPointPW);
+ settingsFile->printf("%s=%s\r\n", "ntripClient_wifiSSID", settings.ntripClient_wifiSSID);
+ settingsFile->printf("%s=%s\r\n", "ntripClient_wifiPW", settings.ntripClient_wifiPW);
+ settingsFile->printf("%s=%d\r\n", "ntripClient_TransmitGGA", settings.ntripClient_TransmitGGA);
+ settingsFile->printf("%s=%d\r\n", "serialTimeoutGNSS", settings.serialTimeoutGNSS);
+ settingsFile->printf("%s=%s\r\n", "pointPerfectDeviceProfileToken", settings.pointPerfectDeviceProfileToken);
+ settingsFile->printf("%s=%d\r\n", "enablePointPerfectCorrections", settings.enablePointPerfectCorrections);
+ settingsFile->printf("%s=%s\r\n", "home_wifiSSID", settings.home_wifiSSID);
+ settingsFile->printf("%s=%s\r\n", "home_wifiPW", settings.home_wifiPW);
+ settingsFile->printf("%s=%d\r\n", "autoKeyRenewal", settings.autoKeyRenewal);
+ settingsFile->printf("%s=%s\r\n", "pointPerfectClientID", settings.pointPerfectClientID);
+ settingsFile->printf("%s=%s\r\n", "pointPerfectBrokerHost", settings.pointPerfectBrokerHost);
+ settingsFile->printf("%s=%s\r\n", "pointPerfectLBandTopic", settings.pointPerfectLBandTopic);
+ settingsFile->printf("%s=%s\r\n", "pointPerfectCurrentKey", settings.pointPerfectCurrentKey);
+ settingsFile->printf("%s=%llu\r\n", "pointPerfectCurrentKeyDuration", settings.pointPerfectCurrentKeyDuration);
+ settingsFile->printf("%s=%llu\r\n", "pointPerfectCurrentKeyStart", settings.pointPerfectCurrentKeyStart);
+ settingsFile->printf("%s=%s\r\n", "pointPerfectNextKey", settings.pointPerfectNextKey);
+ settingsFile->printf("%s=%llu\r\n", "pointPerfectNextKeyDuration", settings.pointPerfectNextKeyDuration);
+ settingsFile->printf("%s=%llu\r\n", "pointPerfectNextKeyStart", settings.pointPerfectNextKeyStart);
+ settingsFile->printf("%s=%llu\r\n", "lastKeyAttempt", settings.lastKeyAttempt);
+ settingsFile->printf("%s=%d\r\n", "updateZEDSettings", settings.updateZEDSettings);
+ settingsFile->printf("%s=%d\r\n", "LBandFreq", settings.LBandFreq);
+ settingsFile->printf("%s=%d\r\n", "enableLogging", settings.enableLogging);
+ settingsFile->printf("%s=%d\r\n", "timeZoneHours", settings.timeZoneHours);
+ settingsFile->printf("%s=%d\r\n", "timeZoneMinutes", settings.timeZoneMinutes);
+ settingsFile->printf("%s=%d\r\n", "timeZoneSeconds", settings.timeZoneSeconds);
+ settingsFile->printf("%s=%d\r\n", "enablePrintWifiIpAddress", settings.enablePrintWifiIpAddress);
+ settingsFile->printf("%s=%d\r\n", "enablePrintState", settings.enablePrintState);
+ settingsFile->printf("%s=%d\r\n", "enablePrintWifiState", settings.enablePrintWifiState);
+ settingsFile->printf("%s=%d\r\n", "enablePrintNtripClientState", settings.enablePrintNtripClientState);
+ settingsFile->printf("%s=%d\r\n", "enablePrintNtripServerState", settings.enablePrintNtripServerState);
+ settingsFile->printf("%s=%d\r\n", "enablePrintPosition", settings.enablePrintPosition);
+ settingsFile->printf("%s=%d\r\n", "enableMarksFile", settings.enableMarksFile);
+ settingsFile->printf("%s=%d\r\n", "enablePrintBatteryMessages", settings.enablePrintBatteryMessages);
+ settingsFile->printf("%s=%d\r\n", "enablePrintRoverAccuracy", settings.enablePrintRoverAccuracy);
+ settingsFile->printf("%s=%d\r\n", "enablePrintBadMessages", settings.enablePrintBadMessages);
+ settingsFile->printf("%s=%d\r\n", "enablePrintLogFileMessages", settings.enablePrintLogFileMessages);
+ settingsFile->printf("%s=%d\r\n", "enablePrintLogFileStatus", settings.enablePrintLogFileStatus);
+ settingsFile->printf("%s=%d\r\n", "enablePrintRingBufferOffsets", settings.enablePrintRingBufferOffsets);
+ settingsFile->printf("%s=%d\r\n", "enablePrintNtripServerRtcm", settings.enablePrintNtripServerRtcm);
+ settingsFile->printf("%s=%d\r\n", "enablePrintNtripClientRtcm", settings.enablePrintNtripClientRtcm);
+ settingsFile->printf("%s=%d\r\n", "enablePrintStates", settings.enablePrintStates);
+ settingsFile->printf("%s=%d\r\n", "enablePrintDuplicateStates", settings.enablePrintDuplicateStates);
+ settingsFile->printf("%s=%d\r\n", "radioType", settings.radioType);
+
//Record peer MAC addresses
for (int x = 0 ; x < settings.espnowPeerCount ; x++)
{
@@ -264,9 +276,14 @@ void recordSystemSettingsToFile(File * settingsFile)
);
settingsFile->println(tempString);
}
- settingsFile->printf("%s=%d\n\r", "espnowPeerCount", settings.espnowPeerCount);
- settingsFile->printf("%s=%d\n\r", "enableRtcmMessageChecking", settings.enableRtcmMessageChecking);
- settingsFile->printf("%s=%d\n\r", "bluetoothRadioType", settings.bluetoothRadioType);
+ settingsFile->printf("%s=%d\r\n", "espnowPeerCount", settings.espnowPeerCount);
+ settingsFile->printf("%s=%d\r\n", "enableRtcmMessageChecking", settings.enableRtcmMessageChecking);
+ settingsFile->printf("%s=%d\r\n", "bluetoothRadioType", settings.bluetoothRadioType);
+ settingsFile->printf("%s=%d\r\n", "enableNmeaClient", settings.enableNmeaClient);
+ settingsFile->printf("%s=%d\r\n", "enableNmeaServer", settings.enableNmeaServer);
+ settingsFile->printf("%s=%d\r\n", "espnowBroadcast", settings.espnowBroadcast);
+ settingsFile->printf("%s=%d\r\n", "antennaHeight", settings.antennaHeight);
+ settingsFile->printf("%s=%0.2f\r\n", "antennaReferencePoint", settings.antennaReferencePoint);
//Record constellation settings
for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
@@ -457,7 +474,8 @@ bool parseLine(char* str, Settings *settings)
else
{
//if (strcmp(settingName, "ntripServer_CasterHost") == 0) //Debug
- // Serial.printf("Found problem spot raw: %s\n\r", str);
+ //if (strcmp(settingName, "profileName") == 0) //Debug
+ // Serial.printf("Found problem spot raw: %s\r\n", str);
//Assume the value is a string such as 8d8a48b. The leading number causes skipSpace to fail.
//If settingValue has a mix of letters and numbers, just convert to string
@@ -483,7 +501,7 @@ bool parseLine(char* str, Settings *settings)
//It's a mix. Skip strtod.
//if (strcmp(settingName, "ntripServer_CasterHost") == 0) //Debug
- // Serial.printf("Skipping strtod - settingValue: %s\n\r", settingValue);
+ // Serial.printf("Skipping strtod - settingValue: %s\r\n", settingValue);
}
else
{
@@ -877,6 +895,16 @@ bool parseLine(char* str, Settings *settings)
settings->radioType = (RadioType_e)d;
else if (strcmp(settingName, "bluetoothRadioType") == 0)
settings->bluetoothRadioType = (BluetoothRadioType_e)d;
+ else if (strcmp(settingName, "enableNmeaClient") == 0)
+ settings->enableNmeaClient = d;
+ else if (strcmp(settingName, "enableNmeaServer") == 0)
+ settings->enableNmeaServer = d;
+ else if (strcmp(settingName, "espnowBroadcast") == 0)
+ settings->espnowBroadcast = d;
+ else if (strcmp(settingName, "antennaHeight") == 0)
+ settings->antennaHeight = d;
+ else if (strcmp(settingName, "antennaReferencePoint") == 0)
+ settings->antennaReferencePoint = d;
//Check for bulk settings (constellations, message rates, ESPNOW Peers)
//Must be last on else list
@@ -1150,7 +1178,10 @@ void recordFile(const char* fileID, char* fileContents, uint32_t fileSize)
sprintf(fileName, "/%s_%s_%d.txt", platformFilePrefix, fileID, profileNumber);
if (LittleFS.exists(fileName))
+ {
LittleFS.remove(fileName);
+ log_d("Removing LittleFS: %s", fileName);
+ }
File fileToWrite = LittleFS.open(fileName, FILE_WRITE);
if (!fileToWrite)
diff --git a/Firmware/RTK_Surveyor/NtripClient.ino b/Firmware/RTK_Surveyor/NtripClient.ino
index 963a17356..b52bd50b4 100644
--- a/Firmware/RTK_Surveyor/NtripClient.ino
+++ b/Firmware/RTK_Surveyor/NtripClient.ino
@@ -35,13 +35,15 @@
// Constants - compiled out
//----------------------------------------
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
//Size of the credentials buffer in bytes
static const int CREDENTIALS_BUFFER_SIZE = 512;
//Give up connecting after this number of attempts
-static const int MAX_NTRIP_CLIENT_CONNECTION_ATTEMPTS = 3;
+//Connection attempts are throttled to increase the time between attempts
+//30 attempts with 15 second increases will take almost two hours
+static const int MAX_NTRIP_CLIENT_CONNECTION_ATTEMPTS = 30;
//NTRIP caster response timeout
static const uint32_t NTRIP_CLIENT_RESPONSE_TIMEOUT = 10 * 1000; //Milliseconds
@@ -55,6 +57,8 @@ static const int RTCM_DATA_SIZE = 512 * 4;
//NTRIP client server request buffer size
static const int SERVER_BUFFER_SIZE = CREDENTIALS_BUFFER_SIZE + 3;
+static const int NTRIPCLIENT_MS_BETWEEN_GGA = 5000; //5s between transmission of GGA messages, if enabled
+
//----------------------------------------
// Locals - compiled out
//----------------------------------------
@@ -62,13 +66,10 @@ static const int SERVER_BUFFER_SIZE = CREDENTIALS_BUFFER_SIZE + 3;
//The WiFi connection to the NTRIP caster to obtain RTCM data.
static WiFiClient * ntripClient;
-//Count the number of connection attempts
-static int ntripClientConnectionAttempts;
-
-//NTRIP client timer usage:
-// * Measure the connection response time
-// * Receive NTRIP data timeout
-static uint32_t ntripClientTimer;
+//Throttle the time between connection attempts
+static int ntripClientConnectionAttemptTimeout = 0;
+static uint32_t ntripClientLastConnectionAttempt = 0;
+static uint32_t ntripClientTimeoutPrint = 0;
//Last time the NTRIP client state was displayed
static uint32_t lastNtripClientState = 0;
@@ -80,17 +81,30 @@ unsigned long lastGGAPush = 0;
// NTRIP Client Routines - compiled out
//----------------------------------------
-void ntripClientAllowMoreConnections()
-{
- ntripClientConnectionAttempts = 0;
-}
-
bool ntripClientConnect()
{
- if ((!ntripClient)
- || (!ntripClient->connect(settings.ntripClient_CasterHost, settings.ntripClient_CasterPort)))
+ if (!ntripClient)
+ return false;
+
+ //Remove any http:// or https:// prefix from host name
+ char hostname[50];
+ strncpy(hostname, settings.ntripClient_CasterHost, 50); //strtok modifies string to be parsed so we create a copy
+ char *token = strtok(hostname, "//");
+ if (token != NULL)
+ {
+ token = strtok(NULL, "//"); //Advance to data after //
+ if (token != NULL)
+ strcpy(settings.ntripClient_CasterHost, token);
+ }
+
+ Serial.printf("NTRIP Client connecting to %s:%d\r\n", settings.ntripClient_CasterHost, settings.ntripClient_CasterPort);
+
+ int connectResponse = ntripClient->connect(settings.ntripClient_CasterHost, settings.ntripClient_CasterPort);
+
+ if (connectResponse < 1)
return false;
- Serial.printf("NTRIP Client connected to %s:%d\n\r", settings.ntripClient_CasterHost, settings.ntripClient_CasterPort);
+
+ Serial.println("NTRIP Client connected");
// Set up the server request (GET)
char serverRequest[SERVER_BUFFER_SIZE];
@@ -145,20 +159,29 @@ bool ntripClientConnect()
bool ntripClientConnectLimitReached()
{
//Shutdown the NTRIP client
- ntripClientStop(false);
+ ntripClientStop(false); //Allocate new wifiClient
//Retry the connection a few times
- bool limitReached = (ntripClientConnectionAttempts++ >= MAX_NTRIP_CLIENT_CONNECTION_ATTEMPTS);
- if (!limitReached)
- //Display the heap state
+ bool limitReached = false;
+ if (ntripClientConnectionAttempts++ >= MAX_NTRIP_CLIENT_CONNECTION_ATTEMPTS) limitReached = true;
+
+ ntripClientConnectionAttemptsTotal++;
+
+ if (limitReached == false)
+ {
+ ntripClientConnectionAttemptTimeout = ntripClientConnectionAttempts * 15 * 1000L; //Wait 15, 30, 45, etc seconds between attempts
+
+ log_d("ntripClientConnectionAttemptTimeout increased to %d minutes", ntripClientConnectionAttemptTimeout / (60 * 1000L));
+
reportHeapNow();
+ }
else
{
//No more connection attempts, switching to Bluetooth
Serial.println("NTRIP Client connection attempts exceeded!");
//Stop WiFi operations
- ntripClientStop(true);
+ ntripClientStop(true); //Do not allocate new wifiClient
}
return limitReached;
}
@@ -225,9 +248,9 @@ void ntripClientSetState(byte newState)
void ntripClientStart()
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
//Stop NTRIP client and WiFi
- ntripClientStop(true);
+ ntripClientStop(true); //Do not allocate new wifiClient
//Start the NTRIP client if enabled
if (settings.enableNtripClient == true)
@@ -244,16 +267,14 @@ void ntripClientStart()
ntripClientSetState(NTRIP_CLIENT_ON);
}
- //Only fallback to Bluetooth once, then try WiFi again. This enables changes
- //to the WiFi SSID and password to properly restart the WiFi.
- ntripClientAllowMoreConnections();
+ ntripClientConnectionAttempts = 0;
#endif //COMPILE_WIFI
}
//Stop the NTRIP client
-void ntripClientStop(bool done)
+void ntripClientStop(bool wifiClientAllocated)
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
if (ntripClient)
{
//Break the NTRIP client connection if necessary
@@ -265,20 +286,25 @@ void ntripClientStop(bool done)
ntripClient = NULL;
//Allocate the NTRIP client structure if not done
- if (!done)
+ if (wifiClientAllocated == false)
ntripClient = new WiFiClient();
}
//Stop WiFi if in use
if (ntripClientState > NTRIP_CLIENT_ON)
+ {
wifiStop();
+ ntripClientLastConnectionAttempt = millis(); //Mark the Client stop so that we don't immediately attempt re-connect to Caster
+ ntripClientConnectionAttemptTimeout = 15 * 1000L; //Wait 15s between stopping and the first re-connection attempt.
+ }
+
// Return the Main Talker ID to "GN".
i2cGNSS.setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GN);
i2cGNSS.setNMEAGPGGAcallbackPtr(NULL); // Remove callback
//Determine the next NTRIP client state
- ntripClientSetState((ntripClient && (!done)) ? NTRIP_CLIENT_ON : NTRIP_CLIENT_OFF);
+ ntripClientSetState((ntripClient && (wifiClientAllocated == false)) ? NTRIP_CLIENT_ON : NTRIP_CLIENT_OFF);
online.ntripClient = false;
wifiIncomingRTCM = false;
#endif //COMPILE_WIFI
@@ -288,7 +314,7 @@ void ntripClientStop(bool done)
//Stop task if the connection has dropped or if we receive no data for maxTimeBeforeHangup_ms
void ntripClientUpdate()
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
//Periodically display the NTRIP client state
if (settings.enablePrintNtripClientState && ((millis() - lastNtripClientState) > 15000))
{
@@ -304,18 +330,45 @@ void ntripClientUpdate()
//Start WiFi
case NTRIP_CLIENT_ON:
- wifiStart(settings.ntripClient_wifiSSID, settings.ntripClient_wifiPW);
- ntripClientSetState(NTRIP_CLIENT_WIFI_CONNECTING);
+ if (strlen(settings.ntripClient_wifiSSID) == 0)
+ {
+ Serial.println("Error: Please enter SSID before starting NTRIP Client");
+ ntripClientSetState(NTRIP_CLIENT_OFF);
+ }
+ else
+ {
+ //Pause until connection timeout has passed
+ if (millis() - ntripClientLastConnectionAttempt > ntripClientConnectionAttemptTimeout)
+ {
+ ntripClientLastConnectionAttempt = millis();
+ wifiStart(settings.ntripClient_wifiSSID, settings.ntripClient_wifiPW);
+ ntripClientSetState(NTRIP_CLIENT_WIFI_CONNECTING);
+ }
+ else
+ {
+ if (millis() - ntripClientTimeoutPrint > 1000)
+ {
+ ntripClientTimeoutPrint = millis();
+ Serial.printf("NTRIP Client connection timeout wait: %ld of %d seconds \r\n",
+ (millis() - ntripClientLastConnectionAttempt) / 1000,
+ ntripClientConnectionAttemptTimeout / 1000
+ );
+ }
+ }
+
+ }
break;
case NTRIP_CLIENT_WIFI_CONNECTING:
if (!wifiIsConnected())
{
- if (wifiConnectionTimeout())
+ //Throttle if SSID is not detected
+ if (wifiConnectionTimeout() || wifiGetStatus() == WL_NO_SSID_AVAIL)
{
- //Assume AP weak signal, the AP is unable to respond successfully
- if (ntripClientConnectLimitReached())
- //Display the WiFi failure
+ if (wifiGetStatus() == WL_NO_SSID_AVAIL)
+ Serial.printf("WiFi network '%s' not found\r\n", settings.ntripClient_wifiSSID);
+
+ if (ntripClientConnectLimitReached()) //Stop WiFi, give up
paintNtripWiFiFail(4000, true);
}
}
@@ -371,37 +424,53 @@ void ntripClientUpdate()
char response[512];
ntripClientResponse(&response[0], sizeof(response));
- //Serial.printf("Response: %s\n\r", response);
+ log_d("Caster Response: %s", response);
- //Look for '200 OK'
- if (strstr(response, "200") == NULL)
+ //Look for various responses
+ if (strstr(response, "401") != NULL)
{
//Look for '401 Unauthorized'
- Serial.printf("NTRIP Client caster responded with bad news: %s. Are you sure your caster credentials are correct?\n\r", response);
+ Serial.printf("NTRIP Caster responded with bad news: %s. Are you sure your caster credentials are correct?\r\n", response);
//Stop WiFi operations
- ntripClientStop(true);
+ ntripClientStop(true); //Do not allocate new wifiClient
}
- else
+ else if (strstr(response, "banned") != NULL)
+ {
+ //Look for 'HTTP/1.1 200 OK' and banned IP information
+ Serial.printf("NTRIP Client connected to caster but caster reponded with problem: %s", response);
+
+ //Stop WiFi operations
+ ntripClientStop(true); //Do not allocate new wifiClient
+ }
+ else if (strstr(response, "200") != NULL)
{
log_d("NTRIP Client connected to caster");
//Connection is now open, start the NTRIP receive data timer
ntripClientTimer = millis();
- // Set the Main Talker ID to "GP". The NMEA GGA messages will be GPGGA instead of GNGGA
- i2cGNSS.setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
- i2cGNSS.setNMEAGPGGAcallbackPtr(&pushGPGGA); // Set up the callback for GPGGA
+ if (settings.ntripClient_TransmitGGA == true)
+ {
+ // Set the Main Talker ID to "GP". The NMEA GGA messages will be GPGGA instead of GNGGA
+ i2cGNSS.setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
+ i2cGNSS.setNMEAGPGGAcallbackPtr(&pushGPGGA); // Set up the callback for GPGGA
+
+ float measurementFrequency = (1000.0 / settings.measurementRate) / settings.navigationRate;
+ if (measurementFrequency < 0.2) measurementFrequency = 0.2; //0.2Hz * 5 = 1 measurement every 5 seconds
+ i2cGNSS.enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_I2C, measurementFrequency * 5); // Enable GGA over I2C. Tell the module to output GGA every 5 seconds
- float measurementFrequency = (1000.0 / settings.measurementRate) / settings.navigationRate;
- if (measurementFrequency < 0.2) measurementFrequency = 0.2; //0.2Hz * 5 = 1 measurement every 5 seconds
- i2cGNSS.enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_I2C, measurementFrequency * 5); // Enable GGA over I2C. Tell the module to output GGA every 5 seconds
+ log_d("Adjusting GGA setting to %f", measurementFrequency);
- lastGGAPush = millis();
+ i2cGNSS.enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_I2C, measurementFrequency); // Enable GGA over I2C. Tell the module to output GGA every second
+
+ lastGGAPush = millis() - NTRIPCLIENT_MS_BETWEEN_GGA; //Force immediate transmission of GGA message
+ }
//We don't use a task because we use I2C hardware (and don't have a semphore).
online.ntripClient = true;
- ntripClientAllowMoreConnections();
+ ntripClientStartTime = millis();
+ ntripClientConnectionAttempts = 0;
ntripClientSetState(NTRIP_CLIENT_CONNECTED);
}
}
@@ -413,7 +482,7 @@ void ntripClientUpdate()
{
//Broken connection, retry the NTRIP client connection
Serial.println("NTRIP Client connection dropped");
- ntripClientStop(false);
+ ntripClientStop(false); //Allocate new wifiClient
}
else
{
@@ -424,7 +493,7 @@ void ntripClientUpdate()
{
//Timeout receiving NTRIP data, retry the NTRIP client connection
Serial.println("NTRIP Client: No data received timeout");
- ntripClientStop(false);
+ ntripClientStop(false); //Allocate new wifiClient
}
}
else
@@ -459,15 +528,15 @@ void ntripClientUpdate()
void pushGPGGA(NMEA_GGA_data_t *nmeaData)
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
//Provide the caster with our current position as needed
- if ((ntripClient->connected() == true) && (settings.ntripClient_TransmitGGA == true))
+ if (ntripClient->connected() == true && settings.ntripClient_TransmitGGA == true)
{
- if (millis() - lastGGAPush > 5000)
+ if (millis() - lastGGAPush > NTRIPCLIENT_MS_BETWEEN_GGA)
{
lastGGAPush = millis();
- //Serial.print(F("Pushing GGA to server: "));
- //Serial.print((const char *)nmeaData->nmea); // .nmea is printable (NULL-terminated) and already has \r\n on the end
+
+ log_d("Pushing GGA to server: %s", (const char *)nmeaData->nmea); // .nmea is printable (NULL-terminated) and already has \r\n on the end
//Push our current GGA sentence to caster
ntripClient->print((const char *)nmeaData->nmea);
diff --git a/Firmware/RTK_Surveyor/NtripServer.ino b/Firmware/RTK_Surveyor/NtripServer.ino
index 42ab6825b..5363f6173 100644
--- a/Firmware/RTK_Surveyor/NtripServer.ino
+++ b/Firmware/RTK_Surveyor/NtripServer.ino
@@ -50,7 +50,7 @@
// Constants - compiled out
//----------------------------------------
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
//Give up connecting after this number of attempts
//Connection attempts are throttled to increase the time between attempts
@@ -67,9 +67,6 @@ static WiFiClient * ntripServer;
//Count of bytes sent by the NTRIP server to the NTRIP caster
uint32_t ntripServerBytesSent = 0;
-//Count the number of connection attempts
-static int ntripServerConnectionAttempts;
-
//Throttle the time between connection attempts
static int ntripServerConnectionAttemptTimeout = 0;
static uint32_t ntripServerLastConnectionAttempt = 0;
@@ -78,12 +75,6 @@ static uint32_t ntripServerTimeoutPrint = 0;
//Last time the NTRIP server state was displayed
static uint32_t ntripServerStateLastDisplayed = 0;
-//NTRIP server timer usage:
-// * Measure the connection response time
-// * Receive RTCM correction data timeout
-// * Monitor last RTCM byte received for frame counting
-static uint32_t ntripServerTimer;
-
//----------------------------------------
// NTRIP Server Routines - compiled out
//----------------------------------------
@@ -94,14 +85,26 @@ bool ntripServerConnectCaster()
const int SERVER_BUFFER_SIZE = 512;
char serverBuffer[SERVER_BUFFER_SIZE];
+ //Remove any http:// or https:// prefix from host name
+ char hostname[50];
+ strncpy(hostname, settings.ntripServer_CasterHost, 50); //strtok modifies string to be parsed so we create a copy
+ char *token = strtok(hostname, "//");
+ if (token != NULL)
+ {
+ token = strtok(NULL, "//"); //Advance to data after //
+ if (token != NULL)
+ strcpy(settings.ntripServer_CasterHost, token);
+ }
+
+ Serial.printf("NTRIP Server connecting to %s:%d\r\n", settings.ntripServer_CasterHost,
+ settings.ntripServer_CasterPort);
+
//Attempt a connection to the NTRIP caster
if (!ntripServer->connect(settings.ntripServer_CasterHost,
settings.ntripServer_CasterPort))
return false;
- //Note the connection to the NTRIP caster
- Serial.printf("Connected to %s:%d\n\r", settings.ntripServer_CasterHost,
- settings.ntripServer_CasterPort);
+ Serial.println("NTRIP Server connected");
//Build the authorization credentials message
// * Mount point
@@ -130,6 +133,8 @@ bool ntripServerConnectLimitReached()
bool limitReached = false;
if (ntripServerConnectionAttempts++ >= MAX_NTRIP_SERVER_CONNECTION_ATTEMPTS) limitReached = true;
+ ntripServerConnectionAttemptsTotal++;
+
if (limitReached == false)
{
ntripServerConnectionAttemptTimeout = ntripServerConnectionAttempts * 5 * 60 * 1000L; //Wait 5, 10, 15, etc minutes between attempts
@@ -209,7 +214,7 @@ void ntripServerSetState(byte newState)
//This function gets called as each RTCM byte comes in
void ntripServerProcessRTCM(uint8_t incoming)
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
if (ntripServerState == NTRIP_SERVER_CASTING)
{
@@ -244,10 +249,13 @@ void ntripServerProcessRTCM(uint8_t incoming)
ntripServerBytesSent = 0;
}
- ntripServer->write(incoming); //Send this byte to socket
- ntripServerBytesSent++;
- ntripServerTimer = millis();
- wifiOutgoingRTCM = true;
+ if (ntripServer->connected())
+ {
+ ntripServer->write(incoming); //Send this byte to socket
+ ntripServerBytesSent++;
+ ntripServerTimer = millis();
+ wifiOutgoingRTCM = true;
+ }
}
//Indicate that the GNSS is providing correction data
@@ -262,7 +270,7 @@ void ntripServerProcessRTCM(uint8_t incoming)
//Start the NTRIP server
void ntripServerStart()
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
//Stop NTRIP server and WiFi
ntripServerStop(true); //Don't allocate new wifiClient
@@ -288,7 +296,7 @@ void ntripServerStart()
//Stop the NTRIP server
void ntripServerStop(bool wifiClientAllocated)
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
if (ntripServer)
{
//Break the NTRIP server connection if necessary
@@ -323,7 +331,7 @@ void ntripServerStop(bool wifiClientAllocated)
//Update the NTRIP server state machine
void ntripServerUpdate()
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
//Periodically display the NTRIP server state
if (settings.enablePrintNtripServerState && ((millis() - ntripServerStateLastDisplayed) > 15000))
{
@@ -343,22 +351,30 @@ void ntripServerUpdate()
//Start WiFi
case NTRIP_SERVER_ON:
- //Pause until connection timeout has passed
- if (millis() - ntripServerLastConnectionAttempt > ntripServerConnectionAttemptTimeout)
+ if (strlen(settings.ntripServer_wifiSSID) == 0)
{
- ntripServerLastConnectionAttempt = millis();
- wifiStart(settings.ntripServer_wifiSSID, settings.ntripServer_wifiPW);
- ntripServerSetState(NTRIP_SERVER_WIFI_CONNECTING);
+ Serial.println("Error: Please enter SSID before starting NTRIP Server");
+ ntripServerSetState(NTRIP_SERVER_OFF);
}
else
{
- if (millis() - ntripServerTimeoutPrint > 1000)
+ //Pause until connection timeout has passed
+ if (millis() - ntripServerLastConnectionAttempt > ntripServerConnectionAttemptTimeout)
+ {
+ ntripServerLastConnectionAttempt = millis();
+ wifiStart(settings.ntripServer_wifiSSID, settings.ntripServer_wifiPW);
+ ntripServerSetState(NTRIP_SERVER_WIFI_CONNECTING);
+ }
+ else
{
- ntripServerTimeoutPrint = millis();
- Serial.printf("NTRIP Server connection timeout wait: %d of %d seconds \n\r",
- (millis() - ntripServerLastConnectionAttempt) / 1000,
- ntripServerConnectionAttemptTimeout / 1000
- );
+ if (millis() - ntripServerTimeoutPrint > 1000)
+ {
+ ntripServerTimeoutPrint = millis();
+ Serial.printf("NTRIP Server connection timeout wait: %ld of %d seconds \r\n",
+ (millis() - ntripServerLastConnectionAttempt) / 1000,
+ ntripServerConnectionAttemptTimeout / 1000
+ );
+ }
}
}
break;
@@ -367,9 +383,12 @@ void ntripServerUpdate()
case NTRIP_SERVER_WIFI_CONNECTING:
if (!wifiIsConnected())
{
- if (wifiConnectionTimeout())
+ //Throttle if SSID is not detected
+ if (wifiConnectionTimeout() || wifiGetStatus() == WL_NO_SSID_AVAIL)
{
- //Assume AP weak signal, the AP is unable to respond successfully
+ if (wifiGetStatus() == WL_NO_SSID_AVAIL)
+ Serial.printf("WiFi network '%s' not found\r\n", settings.ntripServer_wifiSSID);
+
if (ntripServerConnectLimitReached())
{
Serial.println("NTRIP Server failed to get WiFi. Are your WiFi credentials correct?");
@@ -382,6 +401,7 @@ void ntripServerUpdate()
else
{
//WiFi connection established
+ ntripServerStartTime = millis();
ntripServerSetState(NTRIP_SERVER_WIFI_CONNECTED);
// Start the SD card server
@@ -433,9 +453,10 @@ void ntripServerUpdate()
if (ntripServer->available() < strlen("ICY 200 OK")) //Wait until at least a few bytes have arrived
{
//Check for response timeout
- if (millis() - ntripServerTimer > 5000)
+ if (millis() - ntripServerTimer > 10000)
{
- //NTRIP web service did not respone
+ Serial.println("Caster failed to respond in time.");
+
if (ntripServerConnectLimitReached())
{
Serial.println("Caster failed to respond. Do you have your caster address and port correct?");
@@ -448,15 +469,36 @@ void ntripServerUpdate()
char response[512];
ntripServerResponse(response, sizeof(response));
- //Look for '200 OK'
- if (strstr(response, "200") == NULL)
+ //Look for various responses
+ if (strstr(response, "401") != NULL)
{
//Look for '401 Unauthorized'
- Serial.printf("NTRIP Server caster responded with bad news: %s. Are you sure your caster credentials are correct?\n\r", response);
+ Serial.printf("NTRIP Caster responded with bad news: %s. Are you sure your caster credentials are correct?\r\n", response);
- ntripServerStop(true); //Don't allocate new wifiClient
+ //Give up - Stop WiFi operations
+ ntripServerStop(true); //Do not allocate new wifiClient
}
- else
+ else if (strstr(response, "banned") != NULL) //'Banned' found
+ {
+ //Look for 'HTTP/1.1 200 OK' and banned IP information
+ Serial.printf("NTRIP Server connected to caster but caster reponded with problem: %s", response);
+
+ //Give up - Stop WiFi operations
+ ntripServerStop(true); //Do not allocate new wifiClient
+ }
+ else if (strstr(response, "200") == NULL) //'200' not found
+ {
+ //Look for 'ERROR - Mountpoint taken' from Emlid.
+ Serial.printf("NTRIP Server connected but caster reponded with problem: %s", response);
+
+ //Attempt to reconnect after throttle controlled timeout
+ if (ntripServerConnectLimitReached())
+ {
+ Serial.println("Caster failed to respond. Do you have your caster address and port correct?");
+ }
+ }
+ else if (strstr(response, "200") != NULL) //'200' found
+
{
Serial.printf("NTRIP Server connected to %s:%d %s\r\n",
settings.ntripServer_CasterHost,
@@ -479,14 +521,14 @@ void ntripServerUpdate()
//Check for a broken connection
if (!ntripServer->connected())
{
- //Broken connection, retry the NTRIP server connection
- Serial.println("Connection to NTRIP Server was closed");
+ //Broken connection, retry the NTRIP connection
+ Serial.println("Connection to NTRIP Caster was lost");
ntripServerStop(false); //Allocate new wifiClient
}
else if ((millis() - ntripServerTimer) > (3 * 1000))
{
//GNSS stopped sending RTCM correction data
- Serial.println("NTRIP Server breaking caster connection due to lack of RTCM data!");
+ Serial.println("NTRIP Server breaking connection to caster due to lack of RTCM data!");
ntripServerStop(false); //Allocate new wifiClient
}
else
diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
index 10f777de8..33892836b 100644
--- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino
+++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
@@ -23,7 +23,7 @@
*/
const int FIRMWARE_VERSION_MAJOR = 2;
-const int FIRMWARE_VERSION_MINOR = 4;
+const int FIRMWARE_VERSION_MINOR = 5;
#define COMPILE_WIFI //Comment out to remove WiFi functionality
#define COMPILE_AP //Requires WiFi. Comment out to remove Access Point functionality
@@ -85,6 +85,8 @@ int pin_radio_rts;
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
+#include "esp_ota_ops.h" //Needed for partition counting and updateFromSD
+
//I2C for GNSS, battery gauge, display, accelerometer
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
#include
@@ -125,7 +127,7 @@ int startCurrentLogTime_minutes = 0; //Mark when we start this specific log file
//System crashes if two tasks access a file at the same time
//So we use a semaphore to see if file system is available
SemaphoreHandle_t sdCardSemaphore;
-TickType_t loggingSemaphore_shortWait_ms = 10 / portTICK_PERIOD_MS;
+TickType_t loggingSemaphoreWait_ms = 10 / portTICK_PERIOD_MS;
const TickType_t fatSemaphore_shortWait_ms = 10 / portTICK_PERIOD_MS;
const TickType_t fatSemaphore_longWait_ms = 200 / portTICK_PERIOD_MS;
@@ -159,10 +161,25 @@ LoggingType loggingType = LOGGING_UNKNOWN;
#endif
-//char *certificateContents; //Holds the contents of the keys prior to MQTT connection
-//char *keyContents;
-char certificateContents[2000]; //Holds the contents of the keys prior to MQTT connection
-char keyContents[2000];
+volatile uint8_t wifiNmeaConnected;
+
+//NTRIP client timer usage:
+// * Measure the connection response time
+// * Receive NTRIP data timeout
+static uint32_t ntripClientTimer;
+static uint32_t ntripClientStartTime; //For calculating uptime
+static int ntripClientConnectionAttempts; //Count the number of connection attempts between restarts
+static int ntripClientConnectionAttemptsTotal; //Count the number of connection attempts absolutely
+
+//NTRIP server timer usage:
+// * Measure the connection response time
+// * Receive RTCM correction data timeout
+// * Monitor last RTCM byte received for frame counting
+static uint32_t ntripServerTimer;
+static uint32_t ntripServerStartTime;
+static int ntripServerConnectionAttempts; //Count the number of connection attempts between restarts
+static int ntripServerConnectionAttemptsTotal; //Count the number of connection attempts absolutely
+
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
//GNSS configuration
@@ -219,6 +236,8 @@ uint16_t mseconds;
uint8_t numSV;
uint8_t fixType;
uint8_t carrSoln;
+
+const byte haeNumberOfDecimals = 8; //Used for printing and transitting lat/lon
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
//Battery fuel gauge and PWM LEDs
@@ -332,6 +351,9 @@ unsigned long lastRockerSwitchChange = 0; //If quick toggle is detected (less th
AsyncWebServer server(80);
AsyncWebSocket ws("/ws");
+
+char *settingsCSV; //Push large array onto heap
+
#endif
#endif
@@ -345,6 +367,10 @@ unsigned long timeSinceLastIncomingSetting = 0;
//PointPerfect Corrections
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
+#if __has_include("tokens.h")
+#include "tokens.h"
+#endif
+
float lBandEBNO = 0.0; //Used on system status menu
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
@@ -374,7 +400,7 @@ const uint8_t ESPNOW_MAX_PEERS = 5; //Maximum of 5 rovers
uint8_t wifiMACAddress[6]; //Display this address in the system menu
uint8_t btMACAddress[6]; //Display this address when Bluetooth is enabled, otherwise display wifiMACAddress
char deviceName[70]; //The serial string that is broadcast. Ex: 'Surveyor Base-BC61'
-const byte menuTimeout = 15; //Menus will exit/timeout after this number of seconds
+const uint16_t menuTimeout = 60 * 10; //Menus will exit/timeout after this number of seconds
int systemTime_minutes = 0; //Used to test if logging is less than max minutes
uint32_t powerPressedStartTime = 0; //Times how long user has been holding power button, used for power down
bool inMainMenu = false; //Set true when in the serial config menu system.
@@ -425,8 +451,6 @@ uint32_t triggerCount = 0; //Global copy - TM2 event counter
uint32_t towMsR = 0; //Global copy - Time Of Week of rising edge (ms)
uint32_t towSubMsR = 0; //Global copy - Millisecond fraction of Time Of Week of rising edge in nanoseconds
-bool newAPSettings = false; //Goes true when new setting is received via AP config. Allows us to record settings when combined with a reset.
-
unsigned int binBytesSent = 0; //Tracks firmware bytes sent over WiFi OTA update via AP config.
int binBytesLastUpdate = 0; //Allows websocket notification to be sent every 100k bytes
bool firstPowerOn = true; //After boot, apply new settings to ZED if user switches between base or rover
@@ -437,7 +461,7 @@ bool restartRover = false; //If user modifies any NTRIP Client settings, we need
unsigned long startTime = 0; //Used for checking longest running functions
bool lbandCorrectionsReceived = false; //Used to display L-Band SIV icon when corrections are successfully decrypted
unsigned long lastLBandDecryption = 0; //Timestamp of last successfully decrypted PMP message
-bool mqttMessageReceived = false; //Goes true when the subscribed MQTT channel reports back
+volatile bool mqttMessageReceived = false; //Goes true when the subscribed MQTT channel reports back
uint8_t leapSeconds = 0; //Gets set if GNSS is online
unsigned long systemTestDisplayTime = 0; //Timestamp for swapping the graphic during testing
uint8_t systemTestDisplayNumber = 0; //Tracks which test screen we're looking at
@@ -852,7 +876,14 @@ void updateRadio()
//then we've reached the end of the RTCM stream. Send partial buffer.
if (espnowOutgoingSpot > 0 && (millis() - espnowLastAdd) > 50)
{
- esp_now_send(0, (uint8_t *) &espnowOutgoing, espnowOutgoingSpot); //Send partial packet to all peers
+
+ if (settings.espnowBroadcast == false)
+ esp_now_send(0, (uint8_t *) &espnowOutgoing, espnowOutgoingSpot); //Send partial packet to all peers
+ else
+ {
+ uint8_t broadcastMac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+ esp_now_send(broadcastMac, (uint8_t *) &espnowOutgoing, espnowOutgoingSpot); //Send packet via broadcast
+ }
if (!inMainMenu) log_d("ESPNOW transmitted %d RTCM bytes", espnowBytesSent + espnowOutgoingSpot);
espnowBytesSent = 0;
diff --git a/Firmware/RTK_Surveyor/Rover.ino b/Firmware/RTK_Surveyor/Rover.ino
index 99dd6a4cb..ad3202b90 100644
--- a/Firmware/RTK_Surveyor/Rover.ino
+++ b/Firmware/RTK_Surveyor/Rover.ino
@@ -80,16 +80,12 @@ bool configureUbloxModuleRover()
//The last thing we do is set output rate.
response = true; //Reset
- if (i2cGNSS.getMeasurementRate() != settings.measurementRate)
- {
- response &= i2cGNSS.setMeasurementRate(settings.measurementRate);
- }
- if (i2cGNSS.getNavigationRate() != settings.navigationRate)
+
+ if (i2cGNSS.getMeasurementRate() != settings.measurementRate || i2cGNSS.getNavigationRate() != settings.navigationRate)
{
- response &= i2cGNSS.setNavigationRate(settings.navigationRate);
+ float secondsBetweenSolutions = (settings.measurementRate * settings.navigationRate) / 1000.0;
+ setMeasurementRates(secondsBetweenSolutions); //This will set settings.measurementRate, settings.navigationRate, and GSV message
}
- if (response == false)
- Serial.println("Set Nav Rate failed");
response &= i2cGNSS.saveConfiguration(); //Save the current settings to flash and BBR
if (response == false)
diff --git a/Firmware/RTK_Surveyor/States.ino b/Firmware/RTK_Surveyor/States.ino
index 60723dcf7..4b5e22a3a 100644
--- a/Firmware/RTK_Surveyor/States.ino
+++ b/Firmware/RTK_Surveyor/States.ino
@@ -276,7 +276,7 @@ void updateSystemState()
}
//Check for <1m horz accuracy before starting surveyIn
- Serial.printf("Waiting for Horz Accuracy < %0.2f meters: %0.2f\n\r", settings.surveyInStartingAccuracy, horizontalAccuracy);
+ Serial.printf("Waiting for Horz Accuracy < %0.2f meters: %0.2f\r\n", settings.surveyInStartingAccuracy, horizontalAccuracy);
if (horizontalAccuracy > 0.0 && horizontalAccuracy < settings.surveyInStartingAccuracy)
{
@@ -310,7 +310,7 @@ void updateSystemState()
if (i2cGNSS.getSurveyInValid(50) == true) //Survey in complete
{
- Serial.printf("Observation Time: %d\n\r", svinObservationTime);
+ Serial.printf("Observation Time: %d\r\n", svinObservationTime);
Serial.println("Base survey complete! RTCM now broadcasting.");
if (productVariant == RTK_SURVEYOR)
@@ -337,7 +337,7 @@ void updateSystemState()
if (svinObservationTime > maxSurveyInWait_s)
{
- Serial.printf("Survey-In took more than %d minutes. Returning to rover mode.\n\r", maxSurveyInWait_s / 60);
+ Serial.printf("Survey-In took more than %d minutes. Returning to rover mode.\r\n", maxSurveyInWait_s / 60);
resetSurvey();
@@ -564,7 +564,7 @@ void updateSystemState()
else
{
//Enable retry by not changing states
- log_d("sdCardSemaphore failed to yield in STATE_MARK_EVENT\r\n");
+ log_d("sdCardSemaphore failed to yield in STATE_MARK_EVENT");
}
}
break;
@@ -606,6 +606,7 @@ void updateSystemState()
break;
case (STATE_WIFI_CONFIG):
{
+
if (incomingSettingsSpot > 0)
{
//Allow for 750ms before we parse buffer for all data to arrive
@@ -653,7 +654,7 @@ void updateSystemState()
}
break;
-#ifdef COMPILE_L_BAND
+#ifdef COMPILE_L_BAND
case (STATE_KEYS_STARTED):
{
if (rtcWaitTime == 0) rtcWaitTime = millis();
@@ -697,7 +698,7 @@ void updateSystemState()
log_d("Days until keys expire: %d", daysRemaining);
if (daysRemaining >= 28 && daysRemaining <= 56)
- changeState(STATE_KEYS_LBAND_CONFIGURE);
+ changeState(STATE_KEYS_DAYS_REMAINING);
else
changeState(STATE_KEYS_NEEDED);
}
@@ -726,7 +727,7 @@ void updateSystemState()
else
{
log_d("Already tried to obtain keys for today");
- changeState(STATE_KEYS_LBAND_CONFIGURE); //We have valid keys, we've already tried today. No need to try again.
+ changeState(STATE_KEYS_DAYS_REMAINING); //We have valid keys, we've already tried today. No need to try again.
}
}
break;
@@ -760,7 +761,7 @@ void updateSystemState()
case (STATE_KEYS_WIFI_CONNECTED):
{
- if (updatePointPerfectKeys() == true) //Connect to ThingStream MQTT and get PointPerfect key UBX packet
+ if (pointperfectUpdateKeys() == true) //Connect to ThingStream MQTT and get PointPerfect key UBX packet
{
displayKeysUpdated();
}
@@ -779,10 +780,11 @@ void updateSystemState()
if (settings.pointPerfectNextKeyStart > 0)
{
uint8_t daysRemaining = daysFromEpoch(settings.pointPerfectNextKeyStart + settings.pointPerfectNextKeyDuration + 1);
- Serial.printf("Days until PointPerfect keys expire: %d\n\r", daysRemaining);
+ Serial.printf("Days until PointPerfect keys expire: %d\r\n", daysRemaining);
paintKeyDaysRemaining(daysRemaining, 2000);
}
}
+ paintLBandConfigure();
forceSystemStateUpdate = true; //Imediately go to this new state
changeState(STATE_KEYS_LBAND_CONFIGURE);
@@ -794,7 +796,7 @@ void updateSystemState()
//Be sure we ignore any external RTCM sources
i2cGNSS.setPortInput(COM_PORT_UART2, COM_TYPE_UBX); //Set the UART2 to input UBX (no RTCM)
- applyLBandKeys(); //Send current keys, if available, to ZED-F9P
+ pointperfectApplyKeys(); //Send current keys, if available, to ZED-F9P
forceSystemStateUpdate = true; //Imediately go to this new state
changeState(settings.lastState); //Go to either rover or base
@@ -872,7 +874,7 @@ void updateSystemState()
{
forceSystemStateUpdate = true; //Imediately go to this new state
- if (provisionDevice() == true)
+ if (pointperfectProvisionDevice() == true)
{
displayKeysUpdated();
changeState(STATE_KEYS_DAYS_REMAINING);
@@ -920,7 +922,7 @@ void updateSystemState()
if (espnowIsPaired() == true)
{
paintEspNowPaired();
-
+
// Return to the previous state
changeState(lastSystemState);
}
@@ -942,7 +944,7 @@ void updateSystemState()
default:
{
- Serial.printf("Unknown state: %d\n\r", systemState);
+ Serial.printf("Unknown state: %d\r\n", systemState);
}
break;
}
@@ -1036,7 +1038,7 @@ void changeState(SystemState newState)
case (STATE_PROFILE):
Serial.print("State: Profile");
break;
-#ifdef COMPILE_L_BAND
+#ifdef COMPILE_L_BAND
case (STATE_KEYS_STARTED):
Serial.print("State: Keys Started ");
break;
@@ -1099,7 +1101,9 @@ void changeState(SystemState newState)
struct tm timeinfo = rtc.getTimeStruct();
char s[30];
strftime(s, sizeof(s), "%Y-%m-%d %H:%M:%S", &timeinfo);
- Serial.printf(", %s.%03ld\r\n", s, rtc.getMillis());
+ Serial.printf(", %s.%03ld", s, rtc.getMillis());
}
+
+ Serial.println();
}
}
diff --git a/Firmware/RTK_Surveyor/System.ino b/Firmware/RTK_Surveyor/System.ino
index 718eedb88..24fe3a372 100644
--- a/Firmware/RTK_Surveyor/System.ino
+++ b/Firmware/RTK_Surveyor/System.ino
@@ -6,9 +6,10 @@ bool configureUbloxModule()
if (online.gnss == false) return (false);
boolean response = true;
- int maxWait = 2000;
//Wait for initial report from module
+ int maxWait = 2000;
+ startTime = millis();
while (pvtUpdated == false)
{
i2cGNSS.checkUblox(); //Regularly poll to get latest data and any RTCM
@@ -376,7 +377,7 @@ void checkBatteryLevels()
else
sprintf(tempStr, "No batt");
- Serial.printf("%s\n\r", tempStr);
+ Serial.printf("%s\r\n", tempStr);
}
if (productVariant == RTK_SURVEYOR)
@@ -436,7 +437,7 @@ void reportHeapNow()
if (settings.enableHeapReport == true)
{
lastHeapReport = millis();
- Serial.printf("FreeHeap: %d / HeapLowestPoint: %d / LargestBlock: %d\n\r", ESP.getFreeHeap(), xPortGetMinimumEverFreeHeapSize(), heap_caps_get_largest_free_block(MALLOC_CAP_8BIT));
+ Serial.printf("FreeHeap: %d / HeapLowestPoint: %d / LargestBlock: %d\r\n", ESP.getFreeHeap(), xPortGetMinimumEverFreeHeapSize(), heap_caps_get_largest_free_block(MALLOC_CAP_8BIT));
}
}
@@ -589,3 +590,9 @@ void createNMEASentence(customNmeaType_e textID, char *nmeaMessage, char *textMe
sprintf(nmeaMessage, "%s%02X", nmeaTxt, CRC);
}
+
+//Reset settings struct to default initializers
+void settingsToDefaults()
+{
+ settings = defaultSettings;
+}
diff --git a/Firmware/RTK_Surveyor/Tasks.ino b/Firmware/RTK_Surveyor/Tasks.ino
index e25d4bd5b..5c7d8e5c7 100644
--- a/Firmware/RTK_Surveyor/Tasks.ino
+++ b/Firmware/RTK_Surveyor/Tasks.ino
@@ -21,7 +21,7 @@ void F9PSerialWriteTask(void *e)
if (!inMainMenu) log_d("Bluetooth received %d RTCM bytes, sent to ZED", s);
if (settings.enableTaskReports == true)
- Serial.printf("SerialWriteTask High watermark: %d\n\r", uxTaskGetStackHighWaterMark(NULL));
+ Serial.printf("SerialWriteTask High watermark: %d\r\n", uxTaskGetStackHighWaterMark(NULL));
}
delay(1); //Poor man's way of feeding WDT. Required to prevent Priority 1 tasks from causing WDT reset
taskYIELD();
@@ -39,9 +39,11 @@ void F9PSerialReadTask(void *e)
static uint8_t rBuffer[1024 * 6]; //Buffer for reading from F9P to SPP
static uint16_t dataHead = 0; //Head advances as data comes in from GNSS's UART
static uint16_t btTail = 0; //BT Tail advances as it is sent over BT
+ static uint16_t nmeaTail = 0; //NMEA TCP client tail
static uint16_t sdTail = 0; //SD Tail advances as it is recorded to SD
int btBytesToSend; //Amount of buffered Bluetooth data
+ int nmeaBytesToSend; //Amount of buffered NMEA TCP data
int sdBytesToRecord; //Amount of buffered microSD card logging data
int availableBufferSpace; //Distance between head and furthest away tail
@@ -50,191 +52,223 @@ void F9PSerialReadTask(void *e)
while (true)
{
- while (serialGNSS.available())
+ if (settings.enableTaskReports == true)
+ Serial.printf("SerialReadTask High watermark: %d\r\n", uxTaskGetStackHighWaterMark(NULL));
+
+ //----------------------------------------------------------------------
+ //The ESP32<->ZED-F9P serial connection is default 460,800bps to facilitate
+ //10Hz fix rate with PPP Logging Defaults (NMEAx5 + RXMx2) messages enabled.
+ //ESP32 UART2 is begun with SERIAL_SIZE_RX size buffer. The circular buffer
+ //is 1024*6. At approximately 46.1K characters/second, a 6144 * 2
+ //byte buffer should hold 267ms worth of serial data. Assuming SD writes are
+ //250ms worst case, we should record incoming all data. Bluetooth congestion
+ //or conflicts with the SD card semaphore should clear within this time.
+ //
+ //Ring buffer empty when (dataHead == btTail) and (dataHead == sdTail)
+ //
+ // +---------+
+ // | |
+ // | |
+ // | |
+ // | |
+ // +---------+ <-- dataHead, btTail, sdTail
+ //
+ //Ring buffer contains data when (dataHead != btTail) or (dataHead != sdTail)
+ //
+ // +---------+
+ // | |
+ // | |
+ // | yyyyyyy | <-- dataHead
+ // | xxxxxxx | <-- btTail (1 byte in buffer)
+ // +---------+ <-- sdTail (2 bytes in buffer)
+ //
+ // +---------+
+ // | yyyyyyy | <-- btTail (1 byte in buffer)
+ // | xxxxxxx | <-- sdTail (2 bytes in buffer)
+ // | |
+ // | |
+ // +---------+ <-- dataHead
+ //
+ //Maximum ring buffer fill is sizeof(rBuffer) - 1
+ //----------------------------------------------------------------------
+
+ //Determine BT connection state
+ btConnected = (bluetoothGetState() == BT_CONNECTED)
+ && (systemState != STATE_BASE_TEMP_SETTLE)
+ && (systemState != STATE_BASE_TEMP_SURVEY_STARTED);
+
+ availableBufferSpace = sizeof(rBuffer);
+
+ //Determine the amount of Bluetooth data in the buffer
+ btBytesToSend = 0;
+ if (btConnected)
{
- if (settings.enableTaskReports == true)
- Serial.printf("SerialReadTask High watermark: %d\n\r", uxTaskGetStackHighWaterMark(NULL));
-
- //----------------------------------------------------------------------
- //The ESP32<->ZED-F9P serial connection is default 460,800bps to facilitate
- //10Hz fix rate with PPP Logging Defaults (NMEAx5 + RXMx2) messages enabled.
- //ESP32 UART2 is begun with SERIAL_SIZE_RX size buffer. The circular buffer
- //is 1024*6. At approximately 46.1K characters/second, a 6144 * 2
- //byte buffer should hold 267ms worth of serial data. Assuming SD writes are
- //250ms worst case, we should record incoming all data. Bluetooth congestion
- //or conflicts with the SD card semaphore should clear within this time.
- //
- //Ring buffer empty when (dataHead == btTail) and (dataHead == sdTail)
- //
- // +---------+
- // | |
- // | |
- // | |
- // | |
- // +---------+ <-- dataHead, btTail, sdTail
- //
- //Ring buffer contains data when (dataHead != btTail) or (dataHead != sdTail)
- //
- // +---------+
- // | |
- // | |
- // | yyyyyyy | <-- dataHead
- // | xxxxxxx | <-- btTail (1 byte in buffer)
- // +---------+ <-- sdTail (2 bytes in buffer)
- //
- // +---------+
- // | yyyyyyy | <-- btTail (1 byte in buffer)
- // | xxxxxxx | <-- sdTail (2 bytes in buffer)
- // | |
- // | |
- // +---------+ <-- dataHead
- //
- //Maximum ring buffer fill is sizeof(rBuffer) - 1
- //----------------------------------------------------------------------
-
- availableBufferSpace = sizeof(rBuffer);
-
- //Determine BT connection state
- btConnected = (bluetoothGetState() == BT_CONNECTED)
- && (systemState != STATE_BASE_TEMP_SETTLE)
- && (systemState != STATE_BASE_TEMP_SURVEY_STARTED);
-
- //Determine the amount of Bluetooth data in the buffer
- btBytesToSend = 0;
- if (btConnected)
- {
- btBytesToSend = dataHead - btTail;
- if (btBytesToSend < 0)
- btBytesToSend += sizeof(rBuffer);
- }
+ btBytesToSend = dataHead - btTail;
+ if (btBytesToSend < 0)
+ btBytesToSend += sizeof(rBuffer);
+ }
- //Determine the amount of microSD card logging data in the buffer
- sdBytesToRecord = 0;
- if (online.logging)
- {
- sdBytesToRecord = dataHead - sdTail;
- if (sdBytesToRecord < 0)
- sdBytesToRecord += sizeof(rBuffer);
- }
+ //Determine the amount of NMEA TCP data in the buffer
+ nmeaBytesToSend = 0;
+ if (settings.enableNmeaServer || settings.enableNmeaClient)
+ {
+ nmeaBytesToSend = dataHead - nmeaTail;
+ if (nmeaBytesToSend < 0)
+ nmeaBytesToSend += sizeof(rBuffer);
+ }
- //Determine the free bytes in the buffer
- if (btBytesToSend >= sdBytesToRecord)
- availableBufferSpace = sizeof(rBuffer) - btBytesToSend;
- else
- availableBufferSpace = sizeof(rBuffer) - sdBytesToRecord;
+ //Determine the amount of microSD card logging data in the buffer
+ sdBytesToRecord = 0;
+ if (online.logging)
+ {
+ sdBytesToRecord = dataHead - sdTail;
+ if (sdBytesToRecord < 0)
+ sdBytesToRecord += sizeof(rBuffer);
+ }
+
+ //Determine the free bytes in the buffer
+ if (btBytesToSend >= sdBytesToRecord)
+ availableBufferSpace = sizeof(rBuffer) - btBytesToSend;
+ else
+ availableBufferSpace = sizeof(rBuffer) - sdBytesToRecord;
- //Don't fill the last byte to prevent buffer overflow
- if (availableBufferSpace)
- availableBufferSpace -= 1;
+ //Don't fill the last byte to prevent buffer overflow
+ if (availableBufferSpace)
+ availableBufferSpace -= 1;
+ //While there is free buffer space and UART2 has at least one RX byte
+ while (availableBufferSpace && serialGNSS.available())
+ {
//Fill the buffer to the end and then start at the beginning
if ((dataHead + availableBufferSpace) >= sizeof(rBuffer))
availableBufferSpace = sizeof(rBuffer) - dataHead;
- //If we have buffer space, read data from the GNSS into the buffer
- newBytesToRecord = 0;
- if (availableBufferSpace)
- {
- //Add new data into circular buffer in front of the head
- //availableBufferSpace is already reduced to avoid buffer overflow
- newBytesToRecord = serialGNSS.readBytes(&rBuffer[dataHead], availableBufferSpace);
- }
+ //Add new data into circular buffer in front of the head
+ //availableBufferSpace is already reduced to avoid buffer overflow
+ newBytesToRecord = serialGNSS.readBytes(&rBuffer[dataHead], availableBufferSpace);
- //Account for the byte read
- if (newBytesToRecord > 0)
- {
- //Set the next fill offset
- dataHead += newBytesToRecord;
- if (dataHead >= sizeof(rBuffer))
- dataHead -= sizeof(rBuffer);
-
- //Account for the new data
- if (btConnected)
- btBytesToSend += newBytesToRecord;
- if (online.logging)
- sdBytesToRecord += newBytesToRecord;
- }
+ //Account for the bytes read
+ if (newBytesToRecord <= 0)
+ break;
- //----------------------------------------------------------------------
- //Send data over Bluetooth
- //----------------------------------------------------------------------
+ //Set the next fill offset
+ dataHead += newBytesToRecord;
+ if (dataHead >= sizeof(rBuffer))
+ dataHead -= sizeof(rBuffer);
- //If we are actively survey-in then do not pass NMEA data from ZED to phone
- if (!btConnected)
- //Discard the data
- btTail = dataHead;
- else
- {
- //Reduce bytes to send if we have more to send then the end of the buffer
- //We'll wrap next loop
- if ((btTail + btBytesToSend) >= sizeof(rBuffer))
- btBytesToSend = sizeof(rBuffer) - btTail;
-
- //Push new data to BT SPP if not congested or not throttling
- btBytesToSend = bluetoothWriteBytes(&rBuffer[btTail], btBytesToSend);
- if (btBytesToSend > 0)
- {
- //If we are in base mode, assume part of the outgoing data is RTCM
- if (systemState >= STATE_BASE_NOT_STARTED && systemState <= STATE_BASE_FIXED_TRANSMITTING)
- bluetoothOutgoingRTCM = true;
- }
- else
- log_w("BT failed to send");
+ //Account for the new data
+ if (btConnected)
+ btBytesToSend += newBytesToRecord;
+ if (online.logging)
+ sdBytesToRecord += newBytesToRecord;
+ if ((online.nmeaServer || online.nmeaClient) && wifiNmeaConnected)
+ nmeaBytesToSend += newBytesToRecord;
+ } //End Serial.available()
+
+ //----------------------------------------------------------------------
+ //Send data over Bluetooth
+ //----------------------------------------------------------------------
+
+ //If we are actively survey-in then do not pass NMEA data from ZED to phone
+ if (!btConnected)
+ //Discard the data
+ btTail = dataHead;
+ else if (btBytesToSend > 0)
+ {
+ //Reduce bytes to send if we have more to send then the end of the buffer
+ //We'll wrap next loop
+ if ((btTail + btBytesToSend) > sizeof(rBuffer))
+ btBytesToSend = sizeof(rBuffer) - btTail;
+
+ //Push new data to BT SPP
+ btBytesToSend = bluetoothWriteBytes(&rBuffer[btTail], btBytesToSend);
+ if (btBytesToSend > 0)
+ {
+ //If we are in base mode, assume part of the outgoing data is RTCM
+ if (systemState >= STATE_BASE_NOT_STARTED && systemState <= STATE_BASE_FIXED_TRANSMITTING)
+ bluetoothOutgoingRTCM = true;
//Account for the sent or dropped data
btTail += btBytesToSend;
if (btTail >= sizeof(rBuffer))
btTail -= sizeof(rBuffer);
}
+ else
+ log_w("BT failed to send");
+ }
- //----------------------------------------------------------------------
- //Log data to the SD card
- //----------------------------------------------------------------------
+ //----------------------------------------------------------------------
+ //Send data to the NMEA clients
+ //----------------------------------------------------------------------
- //If user wants to log, record to SD
- if (!online.logging)
- //Discard the data
- sdTail = dataHead;
- else
+ if ((!settings.enableNmeaServer) && (!settings.enableNmeaClient) && (!wifiNmeaConnected))
+ nmeaTail = dataHead;
+ else if (nmeaBytesToSend > 0)
+ {
+ //Reduce bytes to send if we have more to send then the end of the buffer
+ //We'll wrap next loop
+ if ((nmeaTail + nmeaBytesToSend) > sizeof(rBuffer))
+ nmeaBytesToSend = sizeof(rBuffer) - nmeaTail;
+
+ //Send the data to the NMEA TCP clients
+ wifiNmeaData (&rBuffer[nmeaTail], nmeaBytesToSend);
+
+ //Assume all data was sent, wrap the buffer pointer
+ nmeaTail += nmeaBytesToSend;
+ if (nmeaTail >= sizeof(rBuffer))
+ nmeaTail -= sizeof(rBuffer);
+ }
+
+ //----------------------------------------------------------------------
+ //Log data to the SD card
+ //----------------------------------------------------------------------
+
+ //If user wants to log, record to SD
+ if (!online.logging)
+ //Discard the data
+ sdTail = dataHead;
+ else if (sdBytesToRecord > 0)
+ {
+ //Check if we are inside the max time window for logging
+ if ((systemTime_minutes - startLogTime_minutes) < settings.maxLogTime_minutes)
{
- //Check if we are inside the max time window for logging
- if ((systemTime_minutes - startLogTime_minutes) < settings.maxLogTime_minutes)
+ //Attempt to gain access to the SD card, avoids collisions with file
+ //writing from other functions like recordSystemSettingsToFile()
+ if (xSemaphoreTake(sdCardSemaphore, loggingSemaphoreWait_ms) == pdPASS)
{
- //Attempt to gain access to the SD card, avoids collisions with file
- //writing from other functions like recordSystemSettingsToFile()
- if (xSemaphoreTake(sdCardSemaphore, loggingSemaphore_shortWait_ms) == pdPASS)
- {
- //Reduce bytes to send if we have more to send then the end of the buffer
- //We'll wrap next loop
- if ((sdTail + sdBytesToRecord) >= sizeof(rBuffer))
- sdBytesToRecord = sizeof(rBuffer) - sdTail;
-
- //Write the data to the file
- sdBytesToRecord = ubxFile->write(&rBuffer[sdTail], sdBytesToRecord);
- xSemaphoreGive(sdCardSemaphore);
-
- //Account for the sent data or dropped
- sdTail += sdBytesToRecord;
- if (sdTail >= sizeof(rBuffer))
- sdTail -= sizeof(rBuffer);
- } //End sdCardSemaphore
- else
+ //Reduce bytes to send if we have more to send then the end of the buffer
+ //We'll wrap next loop
+ if ((sdTail + sdBytesToRecord) > sizeof(rBuffer))
+ sdBytesToRecord = sizeof(rBuffer) - sdTail;
+
+ //Write the data to the file
+ sdBytesToRecord = ubxFile->write(&rBuffer[sdTail], sdBytesToRecord);
+ xSemaphoreGive(sdCardSemaphore);
+
+ //Account for the sent data or dropped
+ sdTail += sdBytesToRecord;
+ if (sdTail >= sizeof(rBuffer))
+ sdTail -= sizeof(rBuffer);
+ } //End sdCardSemaphore
+ else
+ {
+ //Retry the semaphore a little later if possible
+ if (sdBytesToRecord >= (sizeof(rBuffer) - 1))
{
- //Retry the semaphore a little later if possible
- if (sdBytesToRecord >= (sizeof(rBuffer) - 1))
- {
- //Error - no more room in the buffer, drop a buffer's worth of data
- sdTail = dataHead;
- log_e("ERROR - sdCardSemaphore failed to yield, Tasks.ino line %d", __LINE__);
- Serial.printf("ERROR - Dropped %d bytes: GNSS --> log file\r\n", sdBytesToRecord);
- }
- else
- log_w("sdCardSemaphore failed to yield, Tasks.ino line %d", __LINE__);
+ //Error - no more room in the buffer, drop a buffer's worth of data
+ sdTail = dataHead;
+ log_e("ERROR - sdCardSemaphore failed to yield, Tasks.ino line %d", __LINE__);
+ Serial.printf("ERROR - Dropped %d bytes: GNSS --> log file\r\n", sdBytesToRecord);
}
- } //End maxLogTime
- } //End logging
- } //End Serial.available()
+
+ //Only complain when over half the buffer needs to be written
+ //to the microSD card
+ else if (sdBytesToRecord > (sizeof(rBuffer) / 2))
+ log_w("sdCardSemaphore failed to yield, Tasks.ino line %d", __LINE__);
+ }
+ } //End maxLogTime
+ } //End logging
//----------------------------------------------------------------------
//Let other tasks run, prevent watch dog timer (WDT) resets
@@ -466,14 +500,14 @@ void ButtonCheckTask(void *e)
setupState = STATE_MARK_EVENT;
break;
default:
- Serial.printf("ButtonCheckTask unknown setup state: %d\n\r", setupState);
+ Serial.printf("ButtonCheckTask unknown setup state: %d\r\n", setupState);
setupState = STATE_MARK_EVENT;
break;
}
break;
default:
- Serial.printf("ButtonCheckTask unknown system state: %d\n\r", systemState);
+ Serial.printf("ButtonCheckTask unknown system state: %d\r\n", systemState);
requestChangeState(STATE_ROVER_NOT_STARTED);
break;
}
@@ -586,14 +620,14 @@ void ButtonCheckTask(void *e)
setupState = STATE_MARK_EVENT;
break;
default:
- Serial.printf("ButtonCheckTask unknown setup state: %d\n\r", setupState);
+ Serial.printf("ButtonCheckTask unknown setup state: %d\r\n", setupState);
setupState = STATE_MARK_EVENT;
break;
}
break;
default:
- Serial.printf("ButtonCheckTask unknown system state: %d\n\r", systemState);
+ Serial.printf("ButtonCheckTask unknown system state: %d\r\n", systemState);
requestChangeState(STATE_ROVER_NOT_STARTED);
break;
}
@@ -646,7 +680,7 @@ void idleTask(void *e)
&& ((millis() - lastStackPrintTime) >= (IDLE_TIME_DISPLAY_SECONDS * 1000)))
{
lastStackPrintTime = millis();
- Serial.printf("idleTask %d High watermark: %d\n\r",
+ Serial.printf("idleTask %d High watermark: %d\r\n",
xPortGetCoreID(), uxTaskGetStackHighWaterMark(NULL));
}
diff --git a/Firmware/RTK_Surveyor/WiFi.ino b/Firmware/RTK_Surveyor/WiFi.ino
index 961cdaaa9..683f1414b 100644
--- a/Firmware/RTK_Surveyor/WiFi.ino
+++ b/Firmware/RTK_Surveyor/WiFi.ino
@@ -50,11 +50,14 @@ static const int WIFI_CONNECTION_TIMEOUT = 10 * 1000; //Milliseconds
//Interval to use when displaying the IP address
static const int WIFI_IP_ADDRESS_DISPLAY_INTERVAL = 12 * 1000; //Milliseconds
+#define WIFI_MAX_NMEA_CLIENTS 4
+#define WIFI_NMEA_TCP_PORT 1958
+
//----------------------------------------
// Locals - compiled out
//----------------------------------------
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
//WiFi Timer usage:
// * Measure connection time to access point
@@ -64,6 +67,10 @@ static unsigned long wifiTimer = 0;
//Last time the WiFi state was displayed
static uint32_t lastWifiState = 0;
+//NMEA TCP server
+static WiFiServer wifiNmeaServer(WIFI_NMEA_TCP_PORT);
+static WiFiClient wifiNmeaClient[WIFI_MAX_NMEA_CLIENTS];
+
//----------------------------------------
// WiFi Routines - compiled out
//----------------------------------------
@@ -72,7 +79,7 @@ void wifiDisplayIpAddress()
{
Serial.print("WiFi IP address: ");
Serial.print(WiFi.localIP());
- Serial.printf(" RSSI: %d\n\r", WiFi.RSSI());
+ Serial.printf(" RSSI: %d\r\n", WiFi.RSSI());
wifiTimer = millis();
}
@@ -108,7 +115,7 @@ void wifiPeriodicallyDisplayIpAddress()
}
//Update the state of the WiFi state machine
-void wifiSetState (byte newState)
+void wifiSetState(byte newState)
{
if (wifiState == newState)
Serial.print("*");
@@ -141,18 +148,18 @@ void wifiStartAP()
{
//When testing, operate on local WiFi instead of AP
//#define LOCAL_WIFI_TESTING 1
-#ifdef LOCAL_WIFI_TESTING
+#ifdef LOCAL_WIFI_TESTING
//Connect to local router
#define WIFI_SSID "TRex"
#define WIFI_PASSWORD "parachutes"
+ WiFi.mode(WIFI_STA);
+
#ifdef COMPILE_ESPNOW
// Return protocol to default settings (no WIFI_PROTOCOL_LR for ESP NOW)
esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N); //Stops WiFi Station
#endif
- WiFi.mode(WIFI_STA);
-
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
Serial.print("WiFi connecting to");
while (wifiGetStatus() != WL_CONNECTED)
@@ -165,15 +172,15 @@ void wifiStartAP()
#else //End LOCAL_WIFI_TESTING
//Start in AP mode
+ WiFi.mode(WIFI_AP);
+
#ifdef COMPILE_ESPNOW
// Return protocol to default settings (no WIFI_PROTOCOL_LR for ESP NOW)
esp_wifi_set_protocol(WIFI_IF_AP, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N); //Stops WiFi AP.
#endif
- WiFi.mode(WIFI_AP);
-
IPAddress local_IP(192, 168, 4, 1);
- IPAddress gateway(192, 168, 1, 1);
+ IPAddress gateway(192, 168, 4, 1);
IPAddress subnet(255, 255, 255, 0);
WiFi.softAPConfig(local_IP, gateway, subnet);
@@ -196,7 +203,7 @@ void wifiStartAP()
//Determine if the WiFi connection has timed out
bool wifiConnectionTimeout()
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
if ((millis() - wifiTimer) <= WIFI_CONNECTION_TIMEOUT)
return false;
Serial.println("WiFi connection timeout!");
@@ -204,12 +211,122 @@ bool wifiConnectionTimeout()
return true;
}
+//Send NMEA data to the NMEA clients
+void wifiNmeaData(uint8_t * data, uint16_t length)
+{
+#ifdef COMPILE_WIFI
+ static IPAddress ipAddress[WIFI_MAX_NMEA_CLIENTS];
+ int index = 0;
+ static uint32_t lastNmeaConnectAttempt;
+
+ if (online.nmeaClient)
+ {
+ //Start the NMEA client if enabled
+ if (((!wifiNmeaClient[0]) || (!wifiNmeaClient[0].connected()))
+ && ((millis() - lastNmeaConnectAttempt) >= 1000))
+ {
+ lastNmeaConnectAttempt = millis();
+ ipAddress[0] = WiFi.gatewayIP();
+ if (settings.enablePrintNmeaTcpStatus)
+ {
+ Serial.print("Trying to connect NMEA client to ");
+ Serial.println(ipAddress[0]);
+ }
+ if (wifiNmeaClient[0].connect(ipAddress[0], WIFI_NMEA_TCP_PORT))
+ {
+ online.nmeaClient = true;
+ Serial.print("NMEA client connected to ");
+ Serial.println(ipAddress[0]);
+ wifiNmeaConnected |= 1 << index;
+ }
+ else
+ {
+ //Release any allocated resources
+ //if (wifiNmeaClient[0])
+ wifiNmeaClient[0].stop();
+ }
+ }
+ }
+
+ if (online.nmeaServer)
+ {
+ //Check for another client
+ for (index = 0; index < WIFI_MAX_NMEA_CLIENTS; index++)
+ if (!(wifiNmeaConnected & (1 << index)))
+ {
+ if ((!wifiNmeaClient[index]) || (!wifiNmeaClient[index].connected()))
+ {
+ wifiNmeaClient[index] = wifiNmeaServer.available();
+ if (!wifiNmeaClient[index])
+ break;
+ ipAddress[index] = wifiNmeaClient[index].remoteIP();
+ Serial.printf("Connected NMEA client %d to ", index);
+ Serial.println(ipAddress[index]);
+ wifiNmeaConnected |= 1 << index;
+ }
+ }
+ }
+
+ //Walk the list of NMEA TCP clients
+ for (index = 0; index < WIFI_MAX_NMEA_CLIENTS; index++)
+ {
+ if (wifiNmeaConnected & (1 << index))
+ {
+ //Check for a broken connection
+ if ((!wifiNmeaClient[index]) || (!wifiNmeaClient[index].connected()))
+ Serial.printf("Disconnected NMEA client %d from ", index);
+
+ //Send the NMEA data to the connected clients
+ else if (((settings.enableNmeaServer && online.nmeaServer)
+ || (settings.enableNmeaClient && online.nmeaClient))
+ && ((!length) || (wifiNmeaClient[index].write(data, length) == length)))
+ {
+ if (settings.enablePrintNmeaTcpStatus && length)
+ Serial.printf("NMEA %d bytes written\r\n", length);
+ continue;
+ }
+
+ //Failed to write the data
+ else
+ Serial.printf("Breaking NMEA client %d connection to ", index);
+
+ //Done with this client connection
+ Serial.println(ipAddress[index]);
+ wifiNmeaClient[index].stop();
+ wifiNmeaConnected &= ~(1 << index);
+
+ //Shutdown the NMEA server if necessary
+ if (settings.enableNmeaServer || online.nmeaServer)
+ wifiNmeaTcpServerActive();
+ }
+ }
+#endif //COMPILE_WIFI
+}
+
+//Check for NMEA TCP server active
+bool wifiNmeaTcpServerActive()
+{
+#ifdef COMPILE_WIFI
+ if ((settings.enableNmeaServer && online.nmeaServer) || wifiNmeaConnected)
+ return true;
+
+ //Shutdown the NMEA server
+ online.nmeaServer = false;
+
+ //Stop the NMEA server
+ wifiNmeaServer.stop();
+#endif //COMPILE_WIFI
+ return false;
+}
+
//If radio is off entirely, start WiFi
//If ESP-Now is active, only add the LR protocol
void wifiStart(char* ssid, char* pw)
{
#ifdef COMPILE_WIFI
+#ifdef COMPILE_ESPNOW
bool restartESPNow = false;
+#endif
if ((wifiState == WIFI_OFF) || (wifiState == WIFI_ON))
{
@@ -241,7 +358,16 @@ void wifiStart(char* ssid, char* pw)
if (restartESPNow == true)
espnowStart();
#endif
-
+
+ //Verify WIFI_MAX_NMEA_CLIENTS
+ if ((sizeof(wifiNmeaConnected) * 8) < WIFI_MAX_NMEA_CLIENTS)
+ {
+ Serial.printf("Please set WIFI_MAX_NMEA_CLIENTS <= %d or increase size of wifiNmeaConnected\r\n", sizeof(wifiNmeaConnected) * 8);
+ while (true)
+ {
+ }
+ }
+
//Display the heap state
reportHeapNow();
}
@@ -253,9 +379,30 @@ void wifiStart(char* ssid, char* pw)
//If ESP NOW is active, leave WiFi on enough for ESP NOW
void wifiStop()
{
-#ifdef COMPILE_WIFI
+#ifdef COMPILE_WIFI
stopWebServer();
+ //Shutdown the NMEA client
+ if (online.nmeaClient)
+ {
+ //Tell the UART2 tasks that the NMEA client is shutting down
+ online.nmeaClient = false;
+ delay(5);
+ Serial.println("NMEA TCP client offline");
+ }
+
+ //Shutdown the NMEA server connection
+ if (online.nmeaServer)
+ {
+ //Tell the UART2 tasks that the NMEA server is shutting down
+ online.nmeaServer = false;
+
+ //Wait for the UART2 tasks to close the NMEA client connections
+ while (wifiNmeaTcpServerActive())
+ delay(5);
+ Serial.println("NMEA Server offline");
+ }
+
if (wifiState == WIFI_OFF)
{
//Do nothing
@@ -295,7 +442,8 @@ void wifiStop()
void wifiUpdate()
{
-#ifdef COMPILE_WIFI
+
+#ifdef COMPILE_WIFI
//Periodically display the WiFi state
if (settings.enablePrintWifiState && ((millis() - lastWifiState) > 15000))
{
@@ -306,6 +454,27 @@ void wifiUpdate()
//Periodically display the IP address
wifiPeriodicallyDisplayIpAddress();
+ //Start the NMEA client if enabled
+ if (settings.enableNmeaClient && (!online.nmeaClient) && (!settings.enableNmeaServer)
+ && (wifiState == WIFI_CONNECTED))
+ {
+ online.nmeaClient = true;
+ Serial.print("NMEA TCP client online, local IP ");
+ Serial.print(WiFi.localIP());
+ Serial.print(", gateway IP ");
+ Serial.println(WiFi.gatewayIP());
+ }
+
+ //Start the NMEA server if enabled
+ if ((!wifiNmeaServer) && (!settings.enableNmeaClient) && settings.enableNmeaServer
+ && (wifiState == WIFI_CONNECTED))
+ {
+ wifiNmeaServer.begin();
+ online.nmeaServer = true;
+ Serial.print("NMEA Server online, IP Address ");
+ Serial.println(WiFi.localIP());
+ }
+
//Support NTRIP client during Rover operation
if (systemState < STATE_BASE_NOT_STARTED)
ntripClientUpdate();
diff --git a/Firmware/RTK_Surveyor/form.h b/Firmware/RTK_Surveyor/form.h
index f0de15f0c..b85b4c97d 100644
--- a/Firmware/RTK_Surveyor/form.h
+++ b/Firmware/RTK_Surveyor/form.h
@@ -33,6 +33,12 @@ function ge(e) {
}
var platformPrefix = "Surveyor";
+var geodeticLat = 40.01;
+var geodeticLon = -105.19;
+var geodeticAlt = 1500.1;
+var ecefX = 100;
+var ecefY = -100;
+var ecefZ = -200;
function parseIncoming(msg) {
//console.log("incoming message: " + msg);
@@ -115,6 +121,7 @@ function parseIncoming(msg) {
|| id.includes("profile5Name")
|| id.includes("profile6Name")
|| id.includes("profile7Name")
+ || id.includes("radioMAC")
) {
ge(id).innerHTML = val;
}
@@ -128,6 +135,41 @@ function parseIncoming(msg) {
else if (id.includes("firmwareUploadStatus")) {
firmwareUploadStatus(val);
}
+ else if (id.includes("geodeticLat")) {
+ geodeticLat = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("geodeticLon")) {
+ geodeticLon = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("geodeticAlt")) {
+ geodeticAlt = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("ecefX")) {
+ ecefX = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("ecefY")) {
+ ecefY = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("ecefZ")) {
+ ecefZ = val;
+ ge(id).innerHTML = val;
+ }
+ else if (id.includes("bluetoothRadioType")) {
+ currentBtNumber = val;
+ $("input[name=bluetoothRadioType][value=" + currentBtNumber + "]").prop('checked', true);
+ }
+ else if (id.includes("espnowPeerCount")) {
+ if(val > 0)
+ ge("peerMACs").innerHTML = "";
+ }
+ else if (id.includes("peerMAC")) {
+ ge("peerMACs").innerHTML += val + "
";
+ }
//Check boxes / radio buttons
else if (val == "true") {
@@ -157,6 +199,7 @@ function parseIncoming(msg) {
//console.log("Settings loaded");
ge("profileChangeMessage").innerHTML = '';
+ ge("resetProfileMsg").innerHTML = '';
//Force element updates
ge("measurementRateHz").dispatchEvent(new CustomEvent('change'));
@@ -170,6 +213,7 @@ function parseIncoming(msg) {
ge("dataPortChannel").dispatchEvent(new CustomEvent('change'));
ge("enableExternalPulse").dispatchEvent(new CustomEvent('change'));
ge("enablePointPerfectCorrections").dispatchEvent(new CustomEvent('change'));
+ ge("radioType").dispatchEvent(new CustomEvent('change'));
}
function hide(id) {
@@ -238,6 +282,7 @@ function validateFields() {
collapseSection("collapseSensorConfig", "sensorCaret");
collapseSection("collapsePPConfig", "pointPerfectCaret");
collapseSection("collapsePortsConfig", "portsCaret");
+ collapseSection("collapseRadioConfig", "radioCaret");
collapseSection("collapseSystemConfig", "systemCaret");
errorCount = 0;
@@ -373,6 +418,8 @@ function validateFields() {
clearElement("fixedLat", 40.09029479);
clearElement("fixedLong", -105.18505761);
clearElement("fixedAltitude", 1560.089);
+ clearElement("antennaHeight", 0);
+ clearElement("antennaReferencePoint", 0);
}
else {
clearElement("observationSeconds", 60);
@@ -395,6 +442,9 @@ function validateFields() {
checkElementValue("fixedLat", -180, 180, "Must be -180 to 180", "collapseBaseConfig");
checkElementValue("fixedLong", -180, 180, "Must be -180 to 180", "collapseBaseConfig");
checkElementValue("fixedAltitude", -11034, 8849, "Must be -11034 to 8849", "collapseBaseConfig");
+
+ checkElementValue("antennaHeight", -15000, 15000, "Must be -15000 to 15000", "collapseBaseConfig");
+ checkElementValue("antennaReferencePoint", -200.0, 200.0, "Must be -200.0 to 200.0", "collapseBaseConfig");
}
}
@@ -576,7 +626,7 @@ function clearElement(id, value) {
}
function resetToFactoryDefaults() {
- ge("factoryDefaultsMsg").innerHTML = "Defaults Applied. Please wait for device reset..."
+ ge("factoryDefaultsMsg").innerHTML = "Defaults Applied. Please wait for device reset...";
ws.send("factoryDefaultReset,1,");
}
@@ -692,6 +742,16 @@ function resetToLoggingDefaults() {
ge("UBX_RXM_RAWX").value = 1;
ge("UBX_RXM_SFRBX").value = 1;
}
+function useECEFCoordinates() {
+ ge("fixedEcefX").value = ecefX;
+ ge("fixedEcefY").value = ecefY;
+ ge("fixedEcefZ").value = ecefZ;
+}
+function useGeodeticCoordinates() {
+ ge("fixedLat").value = geodeticLat;
+ ge("fixedLong").value = geodeticLon;
+ ge("fixedAltitude").value = geodeticAlt;
+}
function exitConfig() {
show("exitPage");
@@ -712,6 +772,17 @@ function firmwareUploadComplete() {
hide("mainPage");
}
+function forgetPairedRadios() {
+ ge("btnForgetRadiosMsg").innerHTML = "All radios forgotten.";
+ ge("peerMACs").innerHTML = "None";
+ ws.send("forgetEspNowPeers,1,");
+}
+
+function btnResetProfile() {
+ ge("resetProfileMsg").innerHTML = "Resetting profile.";
+ ws.send("resetProfile,1,");
+}
+
document.addEventListener("DOMContentLoaded", (event) => {
var radios = document.querySelectorAll('input[name=profileRadio]');
@@ -835,6 +906,24 @@ document.addEventListener("DOMContentLoaded", (event) => {
}
});
+ ge("radioType").addEventListener("change", function () {
+ if (ge("radioType").value == 0) {
+ hide("radioDetails");
+ }
+ else if (ge("radioType").value == 1){
+ show("radioDetails");
+ }
+ });
+
+ ge("enableForgetRadios").addEventListener("change", function () {
+ if (ge("enableForgetRadios").checked) {
+ ge("btnForgetRadios").disabled = false;
+ }
+ else {
+ ge("btnForgetRadios").disabled = true;
+ }
+ });
+
ge("enableLogging").addEventListener("change", function () {
if (ge("enableLogging").checked) {
show("enableLoggingDetails");
@@ -843,7 +932,6 @@ document.addEventListener("DOMContentLoaded", (event) => {
hide("enableLoggingDetails");
}
});
-
})
)====="; //End main.js
@@ -938,6 +1026,16 @@ static const char *index_html = R"=====(
RTK Firmware: v0.0
ZED-F9P Firmware: v0.0
+ LLh:
+ 40.09029479,
+ -105.18505761,
+ 1560.089 (APC)
+
+ ECEF:
+ -1280206.568,
+ -4716804.403,
+ 4086665.484
+
@@ -1020,6 +1118,18 @@ static const char *index_html = R"=====(