Skip to content

Commit ee50be2

Browse files
nseidlenseidle
authored
Merge pull request #226 from LeeLeahy2/ring-buffer
Added ring buffer to F9PSerialReadTask
2 parents c2ecef5 + 19b49af commit ee50be2

File tree

2 files changed

+162
-20
lines changed

2 files changed

+162
-20
lines changed

‎Firmware/RTK_Surveyor/RTK_Surveyor.ino‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -233,7 +233,6 @@ char platformPrefix[55] = "Surveyor"; //Sets the prefix for broadcast names
233233
HardwareSerial serialGNSS(2); //TX on 17, RX on 16
234234

235235
#define SERIAL_SIZE_RX (1024 * 6) //Should match buffer size in BluetoothSerial.cpp. Reduced from 16384 to make room for WiFi/NTRIP server capabilities
236-
uint8_t rBuffer[SERIAL_SIZE_RX]; //Buffer for reading from F9P to SPP
237236
TaskHandle_t F9PSerialReadTaskHandle = NULL; //Store handles so that we can kill them if user goes into WiFi NTRIP Server mode
238237
const uint8_t F9PSerialReadTaskPriority = 1; //3 being the highest, and 0 being the lowest
239238

‎Firmware/RTK_Surveyor/Tasks.ino‎

Lines changed: 162 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -33,59 +33,202 @@ void F9PSerialWriteTask(void *e)
3333
//Task for reading data from the GNSS receiver.
3434
void F9PSerialReadTask(void *e)
3535
{
36+
int btConnected; //Is the RTK in a state to send Bluetooth data?
37+
int btData; //Amount of buffered Bluetooth data
38+
int length;
39+
static uint8_t rBuffer[SERIAL_SIZE_RX]; //Buffer for reading from F9P to SPP
40+
static uint16_t rBufferBtOffset; //Removal offset for Bluetooth
41+
static uint16_t rBufferFillOffset; //Fill offset for serial data
42+
static uint16_t rBufferSdOffset; //Removal offset for microSD card logging
43+
int s;
44+
int sdData; //Amount of buffered microSD card logging data
45+
3646
while (true)
3747
{
3848
while (serialGNSS.available())
3949
{
40-
int s = serialGNSS.readBytes(rBuffer, sizeof(rBuffer));
50+
if (settings.enableTaskReports == true)
51+
Serial.printf("SerialReadTask High watermark: %d\n\r", uxTaskGetStackHighWaterMark(NULL));
4152

42-
//If we are actively survey-in then do not pass NMEA data from ZED to phone
43-
if (systemState == STATE_BASE_TEMP_SETTLE || systemState == STATE_BASE_TEMP_SURVEY_STARTED)
53+
//----------------------------------------------------------------------
54+
//At approximately 3.3K characters/second, a 6K byte buffer should hold
55+
//approximately 2 seconds worth of data. Bluetooth congestion or conflicts
56+
//with the SD card semaphore should clear within this time. At 57600 baud
57+
//the Bluetooth UART is able to send 7200 characters a second. With a 10
58+
//mSec delay this rouitne runs approximately 100 times per second providing
59+
//multiple chances to empty the buffer.
60+
//
61+
//Ring buffer empty when (rBufferFillOffset == rBufferBtOffset) and
62+
//(rBufferFillOffset == rBufferSdOffset)
63+
//
64+
// +---------+
65+
// | |
66+
// | |
67+
// | |
68+
// | |
69+
// +---------+ <-- rBufferFillOffset, rBufferBtOffset, rBufferSdOffset
70+
//
71+
//Ring buffer contains data when (rBufferFillOffset != rBufferBtOffset) or
72+
//(rBufferFillOffset != rBufferSdOffset)
73+
//
74+
// +---------+
75+
// | |
76+
// | |
77+
// | yyyyyyy | <-- rBufferFillOffset
78+
// | xxxxxxx | <-- rBufferBtOffset (1 byte in buffer)
79+
// +---------+ <-- rBufferSdOffset (2 bytes in buffer)
80+
//
81+
// +---------+
82+
// | yyyyyyy | <-- rBufferBtOffset (1 byte in buffer)
83+
// | xxxxxxx | <-- rBufferSdOffset (2 bytes in buffer)
84+
// | |
85+
// | |
86+
// +---------+ <-- rBufferFillOffset
87+
//
88+
//Maximum ring buffer fill is sizeof(rBuffer) - 1
89+
//----------------------------------------------------------------------
90+
91+
//Determine the amount of Bluetooth data in the buffer
92+
length = sizeof(rBuffer);
93+
btData = 0;
94+
btConnected = (bluetoothGetState() == BT_CONNECTED)
95+
&& (systemState != STATE_BASE_TEMP_SETTLE)
96+
&& (systemState != STATE_BASE_TEMP_SURVEY_STARTED);
97+
if (btConnected)
4498
{
45-
//Do nothing
46-
taskYIELD();
99+
btData = rBufferFillOffset - rBufferBtOffset;
100+
if (btData < 0)
101+
btData += sizeof(rBuffer);
102+
length = sizeof(rBuffer) - btData;
47103
}
48-
else if (bluetoothGetState() == BT_CONNECTED)
104+
105+
//Determine the amount of microSD card logging data in the buffer
106+
sdData = 0;
107+
if (online.logging)
108+
{
109+
sdData = rBufferFillOffset - rBufferSdOffset;
110+
if (sdData < 0)
111+
sdData += sizeof(rBuffer);
112+
if (length > (sizeof(rBuffer) - sdData))
113+
length = sizeof(rBuffer) - sdData;
114+
}
115+
116+
//Determine the free bytes in the buffer
117+
//Don't fill the last byte to prevent buffer overflow
118+
if (length)
119+
length -= 1;
120+
121+
//Fill the buffer to the end and then start at the beginning
122+
if ((rBufferFillOffset + length) > sizeof(rBuffer))
123+
length = sizeof(rBuffer) - rBufferFillOffset;
124+
125+
//Read more data from the GNSS into the buffer
126+
s = 0;
127+
if (length)
128+
s = serialGNSS.readBytes(rBuffer, length);
129+
130+
//Account for the byte read
131+
if (s > 0)
132+
{
133+
//Set the next fill offset
134+
rBufferFillOffset += s;
135+
if (rBufferFillOffset >= sizeof(rBuffer))
136+
rBufferFillOffset -= sizeof(rBuffer);
137+
138+
//Account for the new data
139+
if (btConnected)
140+
btData += s;
141+
if (online.logging)
142+
sdData += s;
143+
}
144+
145+
//----------------------------------------------------------------------
146+
//Send data over Bluetooth
147+
//----------------------------------------------------------------------
148+
149+
//If we are actively survey-in then do not pass NMEA data from ZED to phone
150+
if (!btData)
151+
//Discard the data
152+
rBufferBtOffset = rBufferFillOffset;
153+
else
49154
{
155+
//Fill the buffer to the end and then start at the beginning
156+
length = btData;
157+
if ((rBufferBtOffset + length) > sizeof(rBuffer))
158+
length = sizeof(rBuffer) - rBufferBtOffset;
159+
50160
if ((bluetoothIsCongested() == false) || (settings.throttleDuringSPPCongestion == false))
51-
{
52161
//Push new data to BT SPP if not congested or not throttling
53-
bluetoothWriteBytes(rBuffer, s);
54-
}
162+
length = bluetoothWriteBytes(&rBuffer[rBufferBtOffset], length);
55163
else
56164
{
57165
//Don't push data to BT SPP if there is congestion to prevent heap hits.
58-
log_d("Dropped SPP Bytes: %d", s);
166+
if (btData < (sizeof(rBuffer) - 1))
167+
length = 0;
168+
else
169+
Serial.printf("ERROR - Congestion, dropped %d bytes: GNSS --> Bluetooth\r\n", length);
59170
}
171+
172+
//Account for the sent data or dropped
173+
//Set the next removal offset
174+
rBufferBtOffset += length;
175+
if (rBufferBtOffset >= sizeof(rBuffer))
176+
rBufferBtOffset -= sizeof(rBuffer);
60177
}
61178

62-
if (settings.enableTaskReports == true)
63-
Serial.printf("SerialReadTask High watermark: %d\n\r", uxTaskGetStackHighWaterMark(NULL));
179+
//----------------------------------------------------------------------
180+
//Log data to the SD card
181+
//----------------------------------------------------------------------
64182

65183
//If user wants to log, record to SD
66-
if (online.logging == true)
184+
if (!online.logging)
185+
//Discard the data
186+
rBufferSdOffset = rBufferFillOffset;
187+
else
67188
{
68189
//Check if we are inside the max time window for logging
69190
if ((systemTime_minutes - startLogTime_minutes) < settings.maxLogTime_minutes)
70191
{
71-
//Attempt to write to file system. This avoids collisions with file writing from other functions like recordSystemSettingsToFile()
192+
//Attempt to gain access to the SD card, avoids collisions with file
193+
//writing from other functions like recordSystemSettingsToFile()
72194
if (xSemaphoreTake(sdCardSemaphore, fatSemaphore_shortWait_ms) == pdPASS)
73195
{
74-
ubxFile->write(rBuffer, s);
196+
//Fill the buffer to the end and then start at the beginning
197+
length = sdData;
198+
if ((rBufferSdOffset + length) > sizeof(rBuffer))
199+
length = sizeof(rBuffer) - rBufferSdOffset;
75200

201+
//Write the data to the file
202+
length = ubxFile->write(rBuffer, length);
76203
xSemaphoreGive(sdCardSemaphore);
204+
205+
//Account for the sent data or dropped
206+
rBufferSdOffset += length;
207+
if (rBufferSdOffset >= sizeof(rBuffer))
208+
rBufferSdOffset -= sizeof(rBuffer);
77209
} //End sdCardSemaphore
78210
else
79211
{
80-
//Error causing dropped bytes in the log file
81-
Serial.printf("sdCardSemaphore failed to yield, Tasks.ino line %d\r\n", __LINE__);
212+
//Retry the semaphore a little later if possible
213+
if (sdData == (sizeof(rBuffer) - 1))
214+
{
215+
//Error - no more room in the buffer, drop a buffer's worth of data
216+
rBufferSdOffset = rBufferFillOffset;
217+
log_e("ERROR - sdCardSemaphore failed to yield, Tasks.ino line %d\r\n", __LINE__);
218+
Serial.printf("ERROR - Dropped %d bytes: GNSS --> log file\r\n", sdData);
219+
}
220+
else
221+
log_w("WARNING - sdCardSemaphore failed to yield, Tasks.ino line %d\r\n", __LINE__);
82222
}
83223
} //End maxLogTime
84224
} //End logging
85225
} //End Serial.available()
86226

87-
delay(1); //Poor man's way of feeding WDT. Required to prevent Priority 1 tasks from causing WDT reset
88-
taskYIELD();
227+
//----------------------------------------------------------------------
228+
//Let other tasks run, prevent watch dog timer (WDT) resets
229+
//----------------------------------------------------------------------
230+
231+
delay(10);
89232
}
90233
}
91234

0 commit comments

Comments
 (0)