Skip to content

Commit

Permalink
Merge pull request #292 from sparkfun/pplFloatLockRestart
Browse files Browse the repository at this point in the history 
Reset PPL if RTK Float Lock is detected
  • Loading branch information
@nseidle
nseidle authored May 20, 2024
2 parents 0863a78 + 0b3194c commit b64c5c4
Show file tree
Hide file tree
Showing 9 changed files with 237 additions and 97 deletions.
64 changes: 41 additions & 23 deletions Firmware/RTK_Everywhere/Begin.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,7 @@ void beginBoard()

pin_GNSS_TimePulse = 39; // PPS on UM980

pin_muxA = 18; //Controls U12 switch between ESP UART1 to UM980 or LoRa
pin_muxA = 18; // Controls U12 switch between ESP UART1 to UM980 or LoRa
pin_usbSelect = 21;
pin_powerAdapterDetect = 36; // Goes low when USB cable is plugged in

Expand Down Expand Up @@ -227,7 +227,7 @@ void beginBoard()

else if (productVariant == RTK_EVK)
{
#ifdef EVKv1point1
#ifdef EVKv1point1
// Pin defs etc. for EVK v1.1
present.psram_4mb = true;
present.gnss_zedf9p = true;
Expand All @@ -237,9 +237,10 @@ void beginBoard()
present.microSd = true;
present.microSdCardDetectLow = true;
present.button_mode = true;
// Peripheral power controls the OLED, SD, ZED, NEO, USB Hub, LARA - if the SPWR & TPWR jumpers have been changed
// Peripheral power controls the OLED, SD, ZED, NEO, USB Hub, LARA - if the SPWR & TPWR jumpers have been
// changed
present.peripheralPowerControl = true;
present.laraPowerControl = true; // Tertiary power controls the LARA
present.laraPowerControl = true; // Tertiary power controls the LARA
present.antennaShortOpen = true;
present.timePulseInterrupt = true;
present.gnss_to_uart = true;
Expand Down Expand Up @@ -305,9 +306,10 @@ void beginBoard()
present.microSd = true;
present.microSdCardDetectLow = true;
present.button_mode = true;
// Peripheral power controls the OLED, SD, ZED, NEO, USB Hub, LARA - if the SPWR & TPWR jumpers have been changed
// Peripheral power controls the OLED, SD, ZED, NEO, USB Hub, LARA - if the SPWR & TPWR jumpers have been
// changed
present.peripheralPowerControl = true;
present.laraPowerControl = true; // Tertiary power controls the LARA
present.laraPowerControl = true; // Tertiary power controls the LARA
present.antennaShortOpen = true;
present.timePulseInterrupt = true;
present.i2c0BusSpeed_400 = true; // Run bus at higher speed
Expand Down Expand Up @@ -394,7 +396,8 @@ void beginBoard()
// Turn on power to the peripherals
DMW_if systemPrintf("pin_peripheralPowerControl: %d\r\n", pin_peripheralPowerControl);
pinMode(pin_peripheralPowerControl, OUTPUT);
peripheralsOn(); // Turn on power to OLED, SD, ZED, NEO, USB Hub, LARA - if SPWR & TPWR jumpers have been changed
peripheralsOn(); // Turn on power to OLED, SD, ZED, NEO, USB Hub, LARA - if SPWR & TPWR jumpers have been
// changed
}

else if (productVariant == RTK_FACET_V2)
Expand Down Expand Up @@ -741,10 +744,13 @@ void beginGnssUart()
length = settings.gnssHandlerBufferSize + (rbOffsetEntries * sizeof(RING_BUFFER_OFFSET));
ringBuffer = nullptr;

if (online.psram == true)
rbOffsetArray = (RING_BUFFER_OFFSET *)ps_malloc(length);
else
rbOffsetArray = (RING_BUFFER_OFFSET *)malloc(length);
if (rbOffsetArray == nullptr)
{
if (online.psram == true)
rbOffsetArray = (RING_BUFFER_OFFSET *)ps_malloc(length);
else
rbOffsetArray = (RING_BUFFER_OFFSET *)malloc(length);
}

if (!rbOffsetArray)
{
Expand All @@ -755,7 +761,11 @@ void beginGnssUart()
{
ringBuffer = (uint8_t *)&rbOffsetArray[rbOffsetEntries];
rbOffsetArray[0] = 0;
if (!online.gnssUartPinned)

if (task.gnssUartPinnedTaskRunning == false)
{
task.gnssUartPinnedTaskRunning = true; // The xTaskCreate runs and completes nearly immediately. Mark start here and check for completion.

xTaskCreatePinnedToCore(
pinGnssUartTask,
"GnssUartStart", // Just for humans
Expand All @@ -764,8 +774,9 @@ void beginGnssUart()
0, // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest
&taskHandle, // Task handle
settings.gnssUartInterruptsCore); // Core where task should run, 0=core, 1=Arduino
}

while (!online.gnssUartPinned) // Wait for task to run once
while (task.gnssUartPinnedTaskRunning == true) // Wait for task to complete run
delay(1);
}
}
Expand All @@ -787,8 +798,7 @@ void pinGnssUartTask(void *pvParameters)
if (serialGNSS == nullptr)
serialGNSS = new HardwareSerial(2); // Use UART2 on the ESP32 for communication with the GNSS module

serialGNSS->setRxBufferSize(
settings.uartReceiveBufferSize);
serialGNSS->setRxBufferSize(settings.uartReceiveBufferSize);
serialGNSS->setTimeout(settings.serialTimeoutGNSS); // Requires serial traffic on the UART pins for detection

if (pin_GnssUart_RX == -1 || pin_GnssUart_TX == -1)
Expand All @@ -806,7 +816,7 @@ void pinGnssUartTask(void *pvParameters)
// Stop notification
if (settings.printTaskStartStop)
systemPrintln("Task pinGnssUartTask stopped");
online.gnssUartPinned = true;
task.gnssUartPinnedTaskRunning = false;
vTaskDelete(nullptr); // Delete task once it has run once
}

Expand Down Expand Up @@ -905,8 +915,8 @@ void tickerBegin()
{
ledcSetup(ledBtChannel, pwmFreq, pwmResolution);
ledcAttachPin(pin_bluetoothStatusLED, ledBtChannel);
ledcWrite(ledBtChannel, 255); // Turn on BT LED at startup
//Attach happens in bluetoothStart()
ledcWrite(ledBtChannel, 255); // Turn on BT LED at startup
// Attach happens in bluetoothStart()
}

if (pin_gnssStatusLED != PIN_UNDEFINED)
Expand Down Expand Up @@ -1100,7 +1110,7 @@ void beginButtons()
userBtn->begin();

// Starts task for monitoring button presses
if (!online.buttonCheckTaskRunning)
if (!task.buttonCheckTaskRunning)
xTaskCreate(buttonCheckTask,
"BtnCheck", // Just for humans
buttonTaskStackSize, // Stack Size
Expand Down Expand Up @@ -1236,7 +1246,8 @@ void beginI2C()
while (millis() < i2cPowerUpDelay)
;

if (!online.i2cPinned)
if (task.i2cPinnedTaskRunning == false)
{
xTaskCreatePinnedToCore(
pinI2CTask,
"I2CStart", // Just for humans
Expand All @@ -1246,14 +1257,21 @@ void beginI2C()
&taskHandle, // Task handle
settings.i2cInterruptsCore); // Core where task should run, 0=core, 1=Arduino

// Wait for task to run once
while (!online.i2cPinned)
// Wait for task to start running
while (task.i2cPinnedTaskRunning == false)
delay(1);
}

// Wait for task to complete run
while (task.i2cPinnedTaskRunning == true)
delay(1);
}

// Assign I2C interrupts to the core that started the task. See: https://github.com/espressif/arduino-esp32/issues/3386
void pinI2CTask(void *pvParameters)
{
task.i2cPinnedTaskRunning = true;

// Start notification
if (settings.printTaskStartStop)
systemPrintln("Task pinI2CTask started");
Expand Down Expand Up @@ -1285,7 +1303,7 @@ void pinI2CTask(void *pvParameters)
// Stop notification
if (settings.printTaskStartStop)
systemPrintln("Task pinI2CTask stopped");
online.i2cPinned = true;
task.i2cPinnedTaskRunning = false;
vTaskDelete(nullptr); // Delete task once it has run once
}

Expand Down
13 changes: 10 additions & 3 deletions Firmware/RTK_Everywhere/MQTT_Client.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -315,7 +315,8 @@ void mqttClientReceiveMessage(int messageSize)
updateCorrectionsLastSeen(CORR_IP);
if (isHighestRegisteredCorrectionsSource(CORR_IP))
{
if (((settings.debugMqttClientData == true) || (settings.debugCorrections == true)) && !inMainMenu)
if (((settings.debugMqttClientData == true) || (settings.debugCorrections == true)) &&
!inMainMenu)
systemPrintf("Pushing %d bytes from %s topic to GNSS\r\n", mqttCount, topic);

updateZEDCorrectionsSource(0); // Set SOURCE to 0 (IP) if needed
Expand All @@ -325,8 +326,10 @@ void mqttClientReceiveMessage(int messageSize)
}
else
{
if (((settings.debugMqttClientData == true) || (settings.debugCorrections == true)) && !inMainMenu)
systemPrintf("NOT pushing %d bytes from %s topic to GNSS due to priority\r\n", mqttCount, topic);
if (((settings.debugMqttClientData == true) || (settings.debugCorrections == true)) &&
!inMainMenu)
systemPrintf("NOT pushing %d bytes from %s topic to GNSS due to priority\r\n", mqttCount,
topic);
}
}
// Always push KEYS and MGA to the ZED
Expand Down Expand Up @@ -423,6 +426,10 @@ void mqttClientStop(bool shutdown)
// Free the mqttClient resources
if (mqttClient)
{
// Disconnect from broker
if (mqttClient->connected() == true)
mqttClient->stop(); // Disconnects and stops client

if (settings.debugMqttClientState)
systemPrintln("Freeing mqttClient");

Expand Down
113 changes: 96 additions & 17 deletions Firmware/RTK_Everywhere/PointPerfectLibrary.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,13 @@
void updatePplTask(void *e)
{
// Start notification
online.updatePplTaskRunning = true;
task.updatePplTaskRunning = true;
if (settings.printTaskStartStop)
systemPrintln("updatePplTask started");

// Verify that the task is still running
while (online.updatePplTaskRunning)
// Run task until a request is raised
task.updatePplTaskStopRequest = false;
while (task.updatePplTaskStopRequest == false)
{
// Display an alive message
if (PERIODIC_DISPLAY(PD_TASK_UPDATE_PPL))
Expand Down Expand Up @@ -93,7 +94,7 @@ void updatePplTask(void *e)
}
}

online.updatePplTaskRunning = false; // Stop task either because new key failed or we need to apply new key
break; // Stop task either because new key failed or we need to apply new key
}

feedWdt();
Expand All @@ -103,7 +104,7 @@ void updatePplTask(void *e)
// Stop notification
if (settings.printTaskStartStop)
systemPrintln("Task updatePplTask stopped");
online.updatePplTaskRunning = false;
task.updatePplTaskRunning = false;
vTaskDelete(NULL);
}

Expand Down Expand Up @@ -131,10 +132,13 @@ void beginPPL()
reportHeapNow(false);

// PPL_MAX_RTCM_BUFFER is 3345 bytes so we create it on the heap
if (online.psram == true)
pplRtcmBuffer = (uint8_t *)ps_malloc(PPL_MAX_RTCM_BUFFER);
else
pplRtcmBuffer = (uint8_t *)malloc(PPL_MAX_RTCM_BUFFER);
if (pplRtcmBuffer == nullptr)
{
if (online.psram == true)
pplRtcmBuffer = (uint8_t *)ps_malloc(PPL_MAX_RTCM_BUFFER);
else
pplRtcmBuffer = (uint8_t *)malloc(PPL_MAX_RTCM_BUFFER);
}

if (!pplRtcmBuffer)
{
Expand Down Expand Up @@ -174,7 +178,7 @@ void beginPPL()
TaskHandle_t taskHandle;

// Starts task for feeding NMEA+RTCM to PPL
if (online.updatePplTaskRunning == false)
if (task.updatePplTaskRunning == false)
xTaskCreate(updatePplTask,
"UpdatePpl", // Just for humans
updatePplTaskStackSize, // Stack Size
Expand All @@ -188,12 +192,42 @@ void beginPPL()
reportHeapNow(false);
}

// Stop PPL task and release resources
void stopPPL()
{
// Stop task if running
if (task.updatePplTaskRunning)
task.updatePplTaskStopRequest = true;

if (pplRtcmBuffer != nullptr)
{
free(pplRtcmBuffer);
pplRtcmBuffer = nullptr;
}

// Wait for task to stop running
do
delay(10);
while (task.updatePplTaskRunning);

online.ppl = false;
}

// Start the PPL if needed
// Because the key for the PPL expires every ~28 days, we use updatePPL to first apply keys, and
// restart the PPL when new keys need to be applied
void updatePPL()
{
if (online.ppl == false && (settings.enablePointPerfectCorrections))
static unsigned long pplReport = 0;

static unsigned long pplTimeFloatStarted; // Monitors when the PPL got first RTK Float.

// During float lock, the PPL has been seen to drop to 3D fix so once pplTimeFloatStarted
// is started, we do not reset it unless a 3D fix is lost.

static unsigned long pplTime3dFixStarted;

if (online.ppl == false && settings.enablePointPerfectCorrections && gnssIsFixed())
{
// Start PPL only after GNSS is outputting appropriate NMEA+RTCM, we have a key, and the MQTT broker is
// connected. Don't restart the PPL if we've already tried
Expand All @@ -212,11 +246,17 @@ void updatePPL()
{
if (settings.debugCorrections == true)
{
static unsigned long pplReport = 0;
if (millis() - pplReport > 5000)
{
pplReport = millis();


if (gnssIsRTKFloat() && pplTimeFloatStarted > 0)
{
systemPrintf("GNSS restarts: %d Time remaining before Float lock forced restart: %ds\r\n",
floatLockRestarts,
settings.pplFixTimeoutS - ((millis() - pplTimeFloatStarted) / 1000));
}

// Report which data source may be fouling the RTCM generation from the PPL
if ((millis() - lastMqttToPpl) > 5000)
systemPrintln("PPL MQTT Data is stale");
Expand All @@ -225,11 +265,50 @@ void updatePPL()
}
}

if (gnssIsRTKFix() && rtkTimeToFixMs == 0)
if (gnssIsRTKFloat())
{
rtkTimeToFixMs = millis();
if (settings.debugCorrections == true)
systemPrintf("Time to first PPL RTK Fix: %ds\r\n", rtkTimeToFixMs / 1000);
if (pplTimeFloatStarted == 0)
pplTimeFloatStarted = millis();

if (settings.pplFixTimeoutS > 0)
{
// If we don't get an RTK fix within Timeout, restart the PPL
if ((millis() - pplTimeFloatStarted) > (settings.pplFixTimeoutS * 1000L))
{
floatLockRestarts++;

pplTimeFloatStarted = millis(); // Restart timer for PPL monitoring.

stopPPL(); // Stop PPL and mark it offline. It will auto-restart at the next update().
pplAttemptedStart = false; // Reset to allow restart

if (settings.debugCorrections == true)
systemPrintf("Restarting PPL. Number of Float lock restarts: %d\r\n", floatLockRestarts);
}
}
}
else if (gnssIsRTKFix())
{
if (pplTimeFloatStarted != 0)
pplTimeFloatStarted = 0; // Reset pplTimeFloatStarted

if (rtkTimeToFixMs == 0)
rtkTimeToFixMs = millis();

if (millis() - pplReport > 5000)
{
pplReport = millis();

if (settings.debugCorrections == true)
systemPrintf("Time to first PPL RTK Fix: %ds\r\n", rtkTimeToFixMs / 1000);
}
}
else
{
// We are not in RTK Float or RTK Fix

if (gnssIsFixed() == false)
pplTimeFloatStarted = 0; // Reset pplTimeFloatStarted if we loose a 3D fix entirely
}
}

Expand Down
Loading

0 comments on commit b64c5c4

Please sign in to comment.