Skip to content

Commit 45e5d75

Browse files
nseidlenseidle
committed
Partial fix for issue #237.
readBytes() was dumping into [0] spot of array instead.
1 parent fed9547 commit 45e5d75

File tree

1 file changed

+99
-78
lines changed

1 file changed

+99
-78
lines changed

‎Firmware/RTK_Surveyor/Tasks.ino‎

Lines changed: 99 additions & 78 deletions
Original file line numberDiff line numberDiff line change
@@ -33,15 +33,17 @@ 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;
3936
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
37+
static uint16_t dataHead = 0; //Head advances as data comes in from GNSS's UART
38+
static uint16_t btTail = 0; //BT Tail advances as it is sent over BT
39+
static uint16_t sdTail = 0; //SD Tail advances as it is recorded to SD
40+
41+
int btBytesToSend; //Amount of buffered Bluetooth data
42+
int sdBytesToRecord; //Amount of buffered microSD card logging data
43+
int availableBufferSpace; //Distance between head and furthest away tail
44+
45+
int btConnected; //Is the RTK in a state to send Bluetooth data?
46+
int newBytesToRecord; //Size of data from GNSS
4547

4648
while (true)
4749
{
@@ -55,128 +57,147 @@ void F9PSerialReadTask(void *e)
5557
//approximately 2 seconds worth of data. Bluetooth congestion or conflicts
5658
//with the SD card semaphore should clear within this time. At 57600 baud
5759
//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
60+
//mSec delay this routine runs approximately 100 times per second providing
5961
//multiple chances to empty the buffer.
6062
//
61-
//Ring buffer empty when (rBufferFillOffset == rBufferBtOffset) and
62-
//(rBufferFillOffset == rBufferSdOffset)
63+
//Ring buffer empty when (dataHead == btTail) and (dataHead == sdTail)
6364
//
6465
// +---------+
6566
// | |
6667
// | |
6768
// | |
6869
// | |
69-
// +---------+ <-- rBufferFillOffset, rBufferBtOffset, rBufferSdOffset
70+
// +---------+ <-- dataHead, btTail, sdTail
7071
//
71-
//Ring buffer contains data when (rBufferFillOffset != rBufferBtOffset) or
72-
//(rBufferFillOffset != rBufferSdOffset)
72+
//Ring buffer contains data when (dataHead != btTail) or (dataHead != sdTail)
7373
//
7474
// +---------+
7575
// | |
7676
// | |
77-
// | yyyyyyy | <-- rBufferFillOffset
78-
// | xxxxxxx | <-- rBufferBtOffset (1 byte in buffer)
79-
// +---------+ <-- rBufferSdOffset (2 bytes in buffer)
77+
// | yyyyyyy | <-- dataHead
78+
// | xxxxxxx | <-- btTail (1 byte in buffer)
79+
// +---------+ <-- sdTail (2 bytes in buffer)
8080
//
8181
// +---------+
82-
// | yyyyyyy | <-- rBufferBtOffset (1 byte in buffer)
83-
// | xxxxxxx | <-- rBufferSdOffset (2 bytes in buffer)
82+
// | yyyyyyy | <-- btTail (1 byte in buffer)
83+
// | xxxxxxx | <-- sdTail (2 bytes in buffer)
8484
// | |
8585
// | |
86-
// +---------+ <-- rBufferFillOffset
86+
// +---------+ <-- dataHead
8787
//
8888
//Maximum ring buffer fill is sizeof(rBuffer) - 1
8989
//----------------------------------------------------------------------
9090

91-
//Determine the amount of Bluetooth data in the buffer
92-
length = sizeof(rBuffer);
93-
btData = 0;
91+
availableBufferSpace = sizeof(rBuffer);
92+
93+
//Determine BT connection state
9494
btConnected = (bluetoothGetState() == BT_CONNECTED)
95-
&& (systemState != STATE_BASE_TEMP_SETTLE)
96-
&& (systemState != STATE_BASE_TEMP_SURVEY_STARTED);
95+
&& (systemState != STATE_BASE_TEMP_SETTLE)
96+
&& (systemState != STATE_BASE_TEMP_SURVEY_STARTED);
97+
98+
//Determine the amount of Bluetooth data in the buffer
99+
btBytesToSend = 0;
97100
if (btConnected)
98101
{
99-
btData = rBufferFillOffset - rBufferBtOffset;
100-
if (btData < 0)
101-
btData += sizeof(rBuffer);
102-
length = sizeof(rBuffer) - btData;
102+
btBytesToSend = dataHead - btTail;
103+
if (btBytesToSend < 0)
104+
btBytesToSend += sizeof(rBuffer);
103105
}
106+
Serial.printf("btBytesToSend: %d ", btBytesToSend);
104107

105108
//Determine the amount of microSD card logging data in the buffer
106-
sdData = 0;
109+
sdBytesToRecord = 0;
107110
if (online.logging)
108111
{
109-
sdData = rBufferFillOffset - rBufferSdOffset;
110-
if (sdData < 0)
111-
sdData += sizeof(rBuffer);
112-
if (length > (sizeof(rBuffer) - sdData))
113-
length = sizeof(rBuffer) - sdData;
112+
sdBytesToRecord = dataHead - sdTail;
113+
if (sdBytesToRecord < 0)
114+
sdBytesToRecord += sizeof(rBuffer);
114115
}
116+
Serial.printf("sdBytesToRecord: %d ", sdBytesToRecord);
115117

116118
//Determine the free bytes in the buffer
119+
if (btBytesToSend >= sdBytesToRecord)
120+
availableBufferSpace = sizeof(rBuffer) - btBytesToSend;
121+
else
122+
availableBufferSpace = sizeof(rBuffer) - sdBytesToRecord;
123+
124+
Serial.printf("pure: %d ", availableBufferSpace);
125+
117126
//Don't fill the last byte to prevent buffer overflow
118-
if (length)
119-
length -= 1;
127+
if (availableBufferSpace)
128+
availableBufferSpace -= 1;
129+
130+
Serial.printf("protected: %d ", availableBufferSpace);
120131

121132
//Fill the buffer to the end and then start at the beginning
122-
if ((rBufferFillOffset + length) > sizeof(rBuffer))
123-
length = sizeof(rBuffer) - rBufferFillOffset;
133+
if ((dataHead + availableBufferSpace) > sizeof(rBuffer))
134+
availableBufferSpace = sizeof(rBuffer) - dataHead;
124135

125-
//Read more data from the GNSS into the buffer
126-
s = 0;
127-
if (length)
128-
s = serialGNSS.readBytes(rBuffer, length);
136+
Serial.printf("trimmed: %d ", availableBufferSpace);
129137

130-
//Account for the bytes read
131-
if (s > 0)
138+
//If we have buffer space, read data from the GNSS into the buffer
139+
newBytesToRecord = 0;
140+
if (availableBufferSpace)
141+
{
142+
//Add new data into circular buffer in front of the head
143+
//availableBufferSpace is already reduced to avoid buffer overflow
144+
newBytesToRecord = serialGNSS.readBytes(&rBuffer[dataHead], availableBufferSpace);
145+
}
146+
147+
//Account for the byte read
148+
if (newBytesToRecord > 0)
132149
{
133150
//Set the next fill offset
134-
rBufferFillOffset += s;
135-
if (rBufferFillOffset >= sizeof(rBuffer))
136-
rBufferFillOffset -= sizeof(rBuffer);
151+
dataHead += newBytesToRecord;
152+
if (dataHead >= sizeof(rBuffer))
153+
dataHead -= sizeof(rBuffer);
137154

138155
//Account for the new data
139156
if (btConnected)
140-
btData += s;
157+
btBytesToSend += newBytesToRecord;
141158
if (online.logging)
142-
sdData += s;
159+
sdBytesToRecord += newBytesToRecord;
143160
}
144161

162+
Serial.printf("btBytesToSend: %d ", btBytesToSend);
163+
Serial.printf("sdBytesToRecord: %d ", sdBytesToRecord);
164+
165+
Serial.println();
166+
145167
//----------------------------------------------------------------------
146168
//Send data over Bluetooth
147169
//----------------------------------------------------------------------
148170

149171
//If we are actively survey-in then do not pass NMEA data from ZED to phone
150-
if (!btData)
172+
if (!btConnected)
151173
//Discard the data
152-
rBufferBtOffset = rBufferFillOffset;
174+
btTail = dataHead;
153175
else
154176
{
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;
177+
//Reduce bytes to send if we have more to send then the end of the buffer
178+
//We'll wrap next loop
179+
if ((btTail + btBytesToSend) > sizeof(rBuffer))
180+
btBytesToSend = sizeof(rBuffer) - btTail;
159181

160182
if ((bluetoothIsCongested() == false) || (settings.throttleDuringSPPCongestion == false))
161183
{
162184
//Push new data to BT SPP if not congested or not throttling
163-
length = bluetoothWriteBytes(&rBuffer[rBufferBtOffset], length);
185+
btBytesToSend = bluetoothWriteBytes(&rBuffer[btTail], btBytesToSend);
164186
online.txNtripDataCasting = true;
165187
}
166188
else
167189
{
168190
//Don't push data to BT SPP if there is congestion to prevent heap hits.
169-
if (btData < (sizeof(rBuffer) - 1))
170-
length = 0;
191+
if (btBytesToSend < (sizeof(rBuffer) - 1))
192+
btBytesToSend = 0;
171193
else
172-
Serial.printf("ERROR - Congestion, dropped %d bytes: GNSS --> Bluetooth\r\n", length);
194+
Serial.printf("ERROR - Congestion, dropped %d bytes: GNSS --> Bluetooth\r\n", btBytesToSend);
173195
}
174196

175197
//Account for the sent data or dropped
176-
//Set the next removal offset
177-
rBufferBtOffset += length;
178-
if (rBufferBtOffset >= sizeof(rBuffer))
179-
rBufferBtOffset -= sizeof(rBuffer);
198+
btTail += btBytesToSend;
199+
if (btTail >= sizeof(rBuffer))
200+
btTail -= sizeof(rBuffer);
180201
}
181202

182203
//----------------------------------------------------------------------
@@ -186,7 +207,7 @@ void F9PSerialReadTask(void *e)
186207
//If user wants to log, record to SD
187208
if (!online.logging)
188209
//Discard the data
189-
rBufferSdOffset = rBufferFillOffset;
210+
sdTail = dataHead;
190211
else
191212
{
192213
//Check if we are inside the max time window for logging
@@ -196,29 +217,29 @@ void F9PSerialReadTask(void *e)
196217
//writing from other functions like recordSystemSettingsToFile()
197218
if (xSemaphoreTake(sdCardSemaphore, fatSemaphore_shortWait_ms) == pdPASS)
198219
{
199-
//Fill the buffer to the end and then start at the beginning
200-
length = sdData;
201-
if ((rBufferSdOffset + length) > sizeof(rBuffer))
202-
length = sizeof(rBuffer) - rBufferSdOffset;
220+
//Reduce bytes to send if we have more to send then the end of the buffer
221+
//We'll wrap next loop
222+
if ((sdTail + sdBytesToRecord) > sizeof(rBuffer))
223+
sdBytesToRecord = sizeof(rBuffer) - sdTail;
203224

204225
//Write the data to the file
205-
length = ubxFile->write(rBuffer, length);
226+
sdBytesToRecord = ubxFile->write(rBuffer, sdBytesToRecord);
206227
xSemaphoreGive(sdCardSemaphore);
207228

208229
//Account for the sent data or dropped
209-
rBufferSdOffset += length;
210-
if (rBufferSdOffset >= sizeof(rBuffer))
211-
rBufferSdOffset -= sizeof(rBuffer);
230+
sdTail += sdBytesToRecord;
231+
if (sdTail >= sizeof(rBuffer))
232+
sdTail -= sizeof(rBuffer);
212233
} //End sdCardSemaphore
213234
else
214235
{
215236
//Retry the semaphore a little later if possible
216-
if (sdData == (sizeof(rBuffer) - 1))
237+
if (sdBytesToRecord == (sizeof(rBuffer) - 1))
217238
{
218239
//Error - no more room in the buffer, drop a buffer's worth of data
219-
rBufferSdOffset = rBufferFillOffset;
240+
sdTail = dataHead;
220241
log_e("ERROR - sdCardSemaphore failed to yield, Tasks.ino line %d\r\n", __LINE__);
221-
Serial.printf("ERROR - Dropped %d bytes: GNSS --> log file\r\n", sdData);
242+
Serial.printf("ERROR - Dropped %d bytes: GNSS --> log file\r\n", sdBytesToRecord);
222243
}
223244
else
224245
log_w("WARNING - sdCardSemaphore failed to yield, Tasks.ino line %d\r\n", __LINE__);
@@ -231,7 +252,7 @@ void F9PSerialReadTask(void *e)
231252
//Let other tasks run, prevent watch dog timer (WDT) resets
232253
//----------------------------------------------------------------------
233254

234-
delay(10);
255+
delay(50);
235256
}
236257
}
237258

@@ -623,7 +644,7 @@ void idleTask(void *e)
623644

624645
//Display the high water mark if requested
625646
if ((settings.enableTaskReports == true)
626-
&& ((millis() - lastStackPrintTime) >= (IDLE_TIME_DISPLAY_SECONDS * 1000)))
647+
&& ((millis() - lastStackPrintTime) >= (IDLE_TIME_DISPLAY_SECONDS * 1000)))
627648
{
628649
lastStackPrintTime = millis();
629650
Serial.printf("idleTask %d High watermark: %d\n\r",

0 commit comments

Comments
 (0)