Skip to content
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.

Commit 6f0102d

Browse files
andreagilardoniandreagilardoni
committedJul 29, 2024
Improving parse function in modem class
This commit aims to change the logic of the parse function from repeatedly calling a find on the buffer of incoming data to an approach based on FSM for parsing a stream of chars
1 parent c78fd2b commit 6f0102d

File tree

2 files changed

+295
-151
lines changed

2 files changed

+295
-151
lines changed
 

‎libraries/WiFiS3/src/Modem.cpp‎

Lines changed: 287 additions & 150 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,11 @@
11
#include "Modem.h"
22

3-
#define RESULT_OK "OK\r\n"
4-
#define RESULT_ERROR "ERROR\r\n"
5-
#define RESULT_DATA "DATA\r\n"
3+
#define OK_TOKEN "OK"
4+
#define ERROR_TOKEN "ERROR"
5+
#define TERM_TOKEN "\r\n"
6+
#define RESULT_OK OK_TOKEN TERM_TOKEN
7+
#define RESULT_ERROR OK_TOKEN TERM_TOKEN
8+
#define RESULT_DATA "DATA" TERM_TOKEN
69

710
using namespace std;
811

@@ -49,21 +52,24 @@ bool ModemClass::passthrough(const uint8_t *data, size_t size) {
4952
/* -------------------------------------------------------------------------- */
5053
_serial->write(data,size);
5154

52-
std::string tmp, data_res; // FIXME
53-
bool res = buf_read(tmp, data_res);
55+
std::string prompt = DO_NOT_CHECK_CMD, data_res;
56+
auto res = buf_read(prompt, data_res);
5457

55-
// if(_serial_debug && _debug_level >= 2) {
56-
// _serial_debug->print(" ANSWER (passthrough): ");
57-
// _serial_debug->println(data_res.c_str());
58-
// if(res) {
59-
// _serial_debug->println(" Result: OK");
60-
// }
61-
// else {
62-
// _serial_debug->println(" Result: FAILED");
63-
// }
64-
// }
58+
if(_serial_debug && _debug_level >= 2) {
59+
_serial_debug->print(" ANSWER (passthrough): ");
60+
_serial_debug->println(data_res.c_str());
61+
if(res == Ok) {
62+
_serial_debug->println(" Result: OK");
63+
} else if(res == Error) {
64+
_serial_debug->println(" Result: ERROR");
65+
} else if(res == Timeout) {
66+
_serial_debug->println(" Result: TIMEOUT");
67+
} else {
68+
_serial_debug->println(" Result: ParseError");
69+
}
70+
}
6571

66-
return res;
72+
return res == Ok;
6773
}
6874

6975
/* -------------------------------------------------------------------------- */
@@ -102,170 +108,301 @@ bool ModemClass::write(const string &prompt, string &data_res, const char * fmt,
102108
}
103109

104110
_serial->write(tx_buff,strlen((char *)tx_buff));
105-
return buf_read(prompt, data_res);;
106-
}
111+
auto res = buf_read(prompt, data_res);
107112

113+
if(_serial_debug) {
114+
_serial_debug->print(" ANSWER: ");
115+
_serial_debug->println(data_res.c_str());
116+
if(res == Ok) {
117+
_serial_debug->println(" Result: OK");
118+
} else if(res == Error) {
119+
_serial_debug->println(" Result: ERROR");
120+
} else if(res == Timeout) {
121+
_serial_debug->println(" Result: TIMEOUT");
122+
} else {
123+
_serial_debug->println(" Result: ParseError");
124+
}
125+
}
108126

109-
typedef enum {
110-
IDLE,
111-
WAIT_FOR_SIZE,
112-
WAIT_FOR_DATA
113-
} ReadBySizeSt_t;
127+
return res == Ok;
128+
}
114129

115130

116131
/* -------------------------------------------------------------------------- */
117-
bool ModemClass::read_by_size_finished(string &rx) {
132+
ModemClass::ParseResult ModemClass::buf_read(const string &prompt, string &data_res) {
118133
/* -------------------------------------------------------------------------- */
119-
bool rv = false;
120-
static bool first_call = true;
121-
static ReadBySizeSt_t st = IDLE;
122-
static int data_to_be_received = 0;
123-
static int data_received = 0;
124-
if(first_call) {
125-
first_call = false;
126-
st = WAIT_FOR_SIZE;
134+
/*
135+
* This function implements as FSM that parses basic AT command responses
136+
* The expected syntax should match the following regex
137+
* - (?:\+(\w+)[:=][ ]?(\w*))?(?:\r\n)?(ERROR\r\n|OK\r\n)
138+
* + "ERROR<CR><LF>" "OK<CR><LF>"
139+
* + "+COMMAND: <CR><LF>OK<CR><LF>"
140+
* + "+COMMAND: <CR><LF>ERROR<CR><LF>"
141+
* + "+COMMAND: 1231231<CR><LF>OK<CR><LF>" (NOTE only one parameter supported)
142+
* + "+COMMAND: 1231231<CR><LF>ERROR<CR><LF>" (NOTE only one parameter supported)
143+
* - custom sized response:
144+
* + "+COMMAND: 4| 123OK<CR><LF>"
145+
*/
146+
enum class at_parse_state_t {
147+
Begin = 0,
148+
Cmd = 1,
149+
Data = 2,
150+
Sized = 3,
151+
ResWaitLF = 4,
152+
Res = 5,
153+
Error = 6,
154+
ParseError = 7,
155+
Ok = 8,
156+
Completed = 9,
157+
};
158+
159+
at_parse_state_t state = at_parse_state_t::Begin;
160+
std::string commandName;
161+
162+
ModemClass::ParseResult res = Error;
163+
unsigned int sized_read_size = 0;
164+
unsigned int sized_read_count = 0;
165+
unsigned int result_parse = 0;
166+
bool restart = false,
167+
consume_char = true; // This flag is used to indicate to consume another character from the stream
168+
169+
char c;
170+
171+
// I expect the answer to be in this form: "ERROR<CR><LF>" "OK<CR><LF>"
172+
// if prompt == DO_NOT_CHECK_CMD
173+
const bool check_prompt = (prompt != DO_NOT_CHECK_CMD);
174+
175+
if(_serial_debug && _debug_level >= 1) {
176+
_serial_debug->print("RAW: ");
127177
}
128178

129-
switch(st) {
130-
case IDLE:
131-
132-
break;
133-
case WAIT_FOR_SIZE: {
134-
int pos = rx.find("|");
135-
int pos_space = rx.find(" ");
136-
if(pos != string::npos && pos_space != string::npos) {
137-
string n = rx.substr(pos_space,pos);
138-
int to_be_rx = atoi(n.c_str());
139-
if(to_be_rx <= 0) {
140-
while( _serial->available() ){
141-
_serial->read();
142-
}
143-
rv = true;
144-
first_call = true;
145-
st = IDLE;
146-
}
147-
else {
148-
/* add 4 because OK\r\n is always added at the end of data */
149-
data_to_be_received = to_be_rx + 4;
150-
data_received = 0;
151-
st = WAIT_FOR_DATA;
152-
}
153-
rx.clear();
179+
unsigned long start_time = millis();
180+
while(state != at_parse_state_t::Completed) {
181+
182+
if(millis() - start_time > _timeout) {
183+
res = Timeout;
184+
break;
185+
}
186+
187+
if(consume_char && !_serial->available()) {
188+
// if there is nothing available, go to the beginning of the cycle
189+
continue;
190+
} else if(consume_char) { // available is true
191+
c = _serial->read();
192+
} else if(!consume_char) {
193+
// reset consume_char to true
194+
consume_char = true;
195+
}
196+
197+
if(_serial_debug && _debug_level >= 1 && consume_char) {
198+
if(c == '\n') {
199+
_serial_debug->print("<LF>");
200+
} else if (c == '\r') {
201+
_serial_debug->print("<CR>");
202+
} else if (c == ' ') {
203+
_serial_debug->print("<SP>");
204+
} else if(c < ' ') {
205+
_serial_debug->print("<");
206+
_serial_debug->print((unsigned int)c);
207+
_serial_debug->print(">");
208+
} else {
209+
_serial_debug->print(c);
154210
}
155211
}
156-
break;
157-
158-
case WAIT_FOR_DATA:
159-
data_received++;
160-
if(data_received == data_to_be_received) {
161-
rv = true;
162-
first_call = true;
163-
st = IDLE;
212+
if(_serial_debug && _debug_level >= 3) {
213+
_serial_debug->print(" State ");
214+
_serial_debug->println((int)state);
215+
}
216+
217+
switch(state) {
218+
case at_parse_state_t::Begin:
219+
/*
220+
* In this state we wait for a '+' character, which will mark the beginning of a response
221+
* or the status response code "ERROR<CR><LF>" or "OK<CR><LF>"
222+
* we need to consume the available buffer if it doesn't match the expected response,
223+
* in order to avoiding dealing with previous responses which were not parsed successfully
224+
*/
225+
226+
if(c == '+') {
227+
// the answer doesn't match the expected form, we need to restart
228+
restart = !check_prompt;
229+
230+
commandName += c; // prompt includes also '+'
231+
state = at_parse_state_t::Cmd;
232+
} else if(c == RESULT_OK[result_parse]) {
233+
// the answer doesn't match the expected form, we need to restart
234+
restart = check_prompt;
235+
236+
state = at_parse_state_t::Ok;
237+
result_parse++;
238+
} else if(c == RESULT_ERROR[result_parse]) {
239+
// the answer doesn't match the expected form, we need to restart
240+
restart = check_prompt;
241+
242+
state = at_parse_state_t::Error;
243+
result_parse++;
164244
}
165-
break;
245+
// if we uncomment this we can force strict response matching
246+
// else {
247+
// state = at_parse_state_t::ParseError;
248+
// }
249+
250+
break;
251+
case at_parse_state_t::Cmd:
252+
/*
253+
* In this state we parse the command prompt and wait for either ':' or '=' characters
254+
* in order to go the next state
255+
*/
256+
257+
if(c == ':' || c == '=') {
258+
commandName += c; // prompt includes also ':'
259+
260+
if (check_prompt && commandName != prompt) {
261+
// the response we got is not the one we were expecting, parse the wrong response till the end
262+
// and start the parse of the next response
263+
restart = true;
264+
commandName = "";
265+
}
266+
state = at_parse_state_t::Data;
166267

167-
default:
168-
st = IDLE;
169-
break;
170-
}
171-
return rv;
172-
}
268+
data_res = "";
269+
// state = at_parse_state_t::Data;
270+
} else { // no space should be present in the prompt response
271+
commandName += c;
272+
}
173273

274+
break;
275+
case at_parse_state_t::Data:
276+
/*
277+
* In this state we parse response parameters and push them into data_res
278+
* in case multiple parameters separated by ',' are sent, they will be present in data_res
279+
* - if we encounter <CR> we need to wait for <LF>
280+
* - if we encounter <LF> we need to parse the response status
281+
* - if we encounter '|', the next token will contain binary sized data, the current value in
282+
* in data_res contains the length of the next token
283+
*/
284+
285+
if(c == '|') { // sized read, the previous parameter is the length
286+
state = at_parse_state_t::Sized;
287+
288+
sized_read_size = atoi(data_res.c_str());
289+
data_res.clear();
290+
} else if(c == '\r') {
291+
state = at_parse_state_t::ResWaitLF;
292+
} else if(c == '\n') {
293+
state = at_parse_state_t::Res;
294+
} else if(trim_results && c != ' ') {
295+
data_res += c; // in case trim_result is true, avoid adding spaces
296+
} else if(!trim_results) {
297+
data_res += c;
298+
}
174299

300+
break;
301+
case at_parse_state_t::Sized:
302+
/*
303+
* In this state we collect exactly sized_read_size characters into data_res
304+
* when we consume all of them we go into Result parse state, where we supposedly
305+
* wait for 'OK'
306+
*/
307+
data_res += c;
175308

176-
/* -------------------------------------------------------------------------- */
177-
bool ModemClass::buf_read(const string &prompt, string &data_res) {
178-
/* -------------------------------------------------------------------------- */
179-
bool res = false;
180-
bool found = false;
181-
182-
if(_serial_debug && _debug_level >= 1) {
183-
_serial_debug->print("RAW: ");
184-
}
309+
if(++sized_read_count == sized_read_size) {
310+
state = at_parse_state_t::Res;
311+
}
312+
break;
313+
case at_parse_state_t::ResWaitLF:
314+
if(c == '\n') {
315+
state = at_parse_state_t::Res;
316+
}
185317

186-
unsigned long start_time = millis();
187-
while((millis() - start_time < _timeout) && !found){
188-
while( _serial->available() ){
189-
char c = _serial->read();
190-
data_res += c;
191-
192-
if(_serial_debug && _debug_level >= 1) {
193-
_serial_debug->print(c);
318+
/*
319+
* break is volountary not present, to cover for cases where the response status is in the
320+
* following form: '...<CR>OK<CR><LF>' '<CR>ERROR<CR><LF>'
321+
*/
322+
case at_parse_state_t::Res:
323+
/*
324+
* In this state we wait for either an 'O' or an 'E', in order to get an 'OK<CR><LF>'
325+
* or 'ERROR<CR><LF>'
326+
* The first two cases is when there is no parameter in the response, but just the OK and ERROR tokens
327+
*/
328+
329+
if(data_res == OK_TOKEN) {
330+
res = Ok;
331+
state = at_parse_state_t::Completed;
332+
} else if(data_res == ERROR_TOKEN) {
333+
res = Error;
334+
state = at_parse_state_t::Completed;
335+
} if(c == RESULT_OK[0]) { // OK response
336+
state = at_parse_state_t::Ok;
337+
result_parse = 1;
338+
} else if(c == RESULT_ERROR[0]) { // Error response
339+
state = at_parse_state_t::Error;
340+
result_parse = 1;
194341
}
195-
196-
197-
if(read_by_size) {
198-
if(read_by_size_finished(data_res)) {
199-
found = true;
200-
read_by_size = false;
201-
res = true;
202-
if(data_res.size() > 0) {
203-
data_res = data_res.substr(0, data_res.length() - (sizeof(RESULT_OK) - 1));
204-
}
205-
else {
206-
break;
207-
}
208-
}
342+
// if we uncomment this we can force strict response matching
343+
// else {
344+
// state = at_parse_state_t::ParseError;
345+
// }
346+
break;
347+
case at_parse_state_t::Ok:
348+
/*
349+
* In this state we want to match the exact 'K<CR><LF>' response
350+
*/
351+
352+
if(c != RESULT_OK[result_parse++]) {
353+
state = at_parse_state_t::ParseError;
209354
}
210-
else {
211-
if(string::npos != data_res.rfind(RESULT_DATA)) {
212-
found = true;
213-
data_res = data_res.substr(0, data_res.length() - (sizeof(RESULT_DATA) - 1));
214-
if(prompt != DO_NOT_CHECK_CMD) {
215-
if(removeAtBegin(data_res, prompt)) {
216-
res = true;
217-
}
218-
}
219-
else {
220-
res = true;
221-
}
222-
break;
223-
}
224-
else if(string::npos != data_res.rfind(RESULT_OK)){
225-
found = true;
226-
data_res = data_res.substr(0, data_res.length() - (sizeof(RESULT_OK) - 1) );
227-
if(prompt != DO_NOT_CHECK_CMD) {
228-
if(removeAtBegin(data_res, prompt)) {
229-
res = true;
230-
}
231-
}
232-
else {
233-
res = true;
234-
}
235-
break;
236-
}
237-
else if (string::npos != data_res.rfind(RESULT_ERROR)) {
238-
found = true;
239-
data_res.substr(0, data_res.length() - (sizeof(RESULT_ERROR) - 1));
240-
res = false;
241-
break;
242-
}
355+
356+
if(result_parse == strlen(RESULT_OK)) {
357+
res = Ok;
358+
state = at_parse_state_t::Completed;
359+
}
360+
break;
361+
case at_parse_state_t::Error:
362+
/*
363+
* In this state we want to match the exact 'RROR<CR><LF>' response
364+
*/
365+
366+
if(c != RESULT_ERROR[result_parse++]) {
367+
state = at_parse_state_t::ParseError;
368+
}
369+
370+
if(result_parse == strlen(RESULT_ERROR)) {
371+
res = Error;
372+
state = at_parse_state_t::Completed;
243373
}
374+
break;
375+
case at_parse_state_t::ParseError:
376+
res = ParseError;
377+
// if we get a parseError, we go back from the beginning and try again to parse, unitl the timeout expires
378+
state = at_parse_state_t::Begin;
379+
restart = false;
380+
consume_char = false;
381+
break;
382+
case at_parse_state_t::Completed:
383+
break;
384+
}
385+
386+
if(restart && state == at_parse_state_t::Completed) {
387+
state = at_parse_state_t::Begin;
388+
restart = false;
244389
}
245390
}
246391

247-
if(trim_results) {
248-
trim(data_res);
392+
if(_serial_debug && _debug_level >= 3) {
393+
_serial_debug->print("Final State ");
394+
_serial_debug->print((int)state);
395+
_serial_debug->print(" res ");
396+
_serial_debug->println((int)res);
249397
}
398+
250399
trim_results = true;
251-
read_by_size = false;
252400

253401
if(_serial_debug && _debug_level >= 1) {
254402
_serial_debug->print("<-RAW END");
255403
_serial_debug->println();
256404
}
257405

258-
if(_serial_debug) {
259-
_serial_debug->print(" ANSWER: ");
260-
_serial_debug->println(data_res.c_str());
261-
if(res) {
262-
_serial_debug->println(" Result: OK");
263-
}
264-
else {
265-
_serial_debug->println(" Result: FAILED");
266-
}
267-
}
268-
269406
return res;
270407
}
271408

‎libraries/WiFiS3/src/Modem.h‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,14 @@ class ModemClass {
6666
void timeout(size_t timeout_ms) {_timeout = timeout_ms;}
6767

6868
private:
69-
bool buf_read(const std::string &cmd, std::string &data_res);
69+
enum ParseResult {
70+
Ok,
71+
Error,
72+
ParseError,
73+
Timeout
74+
};
75+
76+
ParseResult buf_read(const std::string &cmd, std::string &data_res);
7077
bool delete_serial;
7178
UART * _serial;
7279
unsigned long _timeout;

0 commit comments

Comments
 (0)
Please sign in to comment.