| 12 | |
| 13 | template<typename T> |
| 14 | void ProcessNextByte(byte b, T&& frameCompleted) |
| 15 | { |
| 16 | static std::string frame{}; |
| 17 | static bool inHeader{false}; |
| 18 | static bool wasESC{false}; |
| 19 | static bool lookingForSOF{false}; |
| 20 | |
| 21 | if(inHeader) { |
| 22 | if((ESC == b) && not wasESC) { |
| 23 | wasESC = true; |
| 24 | return; |
| 25 | } else if(wasESC) { |
| 26 | wasESC = false; |
| 27 | |
| 28 | if((SOF == b) || (ESC != b)) { |
| 29 | // if b is not SOF discard the frame |
| 30 | if(SOF == b) { frameCompleted(frame); } |
| 31 | |
| 32 | frame.clear(); |
| 33 | inHeader = false; |
| 34 | return; |
| 35 | } |
| 36 | } |
| 37 | |
| 38 | frame += static_cast<char>(b); |
| 39 | |
| 40 | } else if((ESC == b) && !lookingForSOF) { |
| 41 | lookingForSOF = true; |
| 42 | } else if((SOF == b) && lookingForSOF) { |
| 43 | inHeader = true; |
| 44 | lookingForSOF = false; |
| 45 | } else { |
| 46 | lookingForSOF = false; |
| 47 | } |
| 48 | } |
| 49 | |
| 50 | int main() |
| 51 | { |