2021 NHK B
OmniPosition.cpp@1:0229fc98a26f, 2018-06-30 (annotated)
- Committer:
- takeuchi
- Date:
- Sat Jun 30 14:23:07 2018 +0900
- Revision:
- 1:0229fc98a26f
- Parent:
- 0:58910ef3f2b0
- Child:
- 2:edd33d3ad0fd
roughly
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 0:58910ef3f2b0 | 1 | #include "OmniPosition.h" |
UCHITAKE | 0:58910ef3f2b0 | 2 | |
takeuchi |
1:0229fc98a26f | 3 | OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) : |
takeuchi |
1:0229fc98a26f | 4 | RawSerial(serialTX, serialRX, DEFAULT_BAUD), |
takeuchi |
1:0229fc98a26f | 5 | readCounter(SERIAL_BUFFER_SIZE, 0), |
takeuchi |
1:0229fc98a26f | 6 | takeCounter(SERIAL_BUFFER_SIZE, 0) |
takeuchi |
1:0229fc98a26f | 7 | { |
takeuchi |
1:0229fc98a26f | 8 | buffer = new char[SERIAL_BUFFER_SIZE]; |
takeuchi |
1:0229fc98a26f | 9 | attach(callback(this, &OmniPosition::readData)); |
takeuchi |
1:0229fc98a26f | 10 | ticker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ); |
takeuchi |
1:0229fc98a26f | 11 | } |
takeuchi |
1:0229fc98a26f | 12 | |
takeuchi |
1:0229fc98a26f | 13 | void OmniPosition::readData() |
takeuchi |
1:0229fc98a26f | 14 | { |
takeuchi |
1:0229fc98a26f | 15 | buffer[(int)readCounter] = getc(); |
takeuchi |
1:0229fc98a26f | 16 | ++readCounter; |
takeuchi |
1:0229fc98a26f | 17 | } |
takeuchi |
1:0229fc98a26f | 18 | |
takeuchi |
1:0229fc98a26f | 19 | void OmniPosition::assemble() |
UCHITAKE | 0:58910ef3f2b0 | 20 | { |
takeuchi |
1:0229fc98a26f | 21 | //Find header |
takeuchi |
1:0229fc98a26f | 22 | headerCheck = false; |
takeuchi |
1:0229fc98a26f | 23 | headerPoint = 0xff; |
takeuchi |
1:0229fc98a26f | 24 | |
takeuchi |
1:0229fc98a26f | 25 | for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) { |
takeuchi |
1:0229fc98a26f | 26 | if(buffer[i] == HEADER_FIRST_BYTE) { |
takeuchi |
1:0229fc98a26f | 27 | takeCounter = i; |
takeuchi |
1:0229fc98a26f | 28 | ++takeCounter; |
takeuchi |
1:0229fc98a26f | 29 | if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) { |
takeuchi |
1:0229fc98a26f | 30 | headerCheck = true; |
takeuchi |
1:0229fc98a26f | 31 | headerPoint = i; |
takeuchi |
1:0229fc98a26f | 32 | } |
takeuchi |
1:0229fc98a26f | 33 | } |
takeuchi |
1:0229fc98a26f | 34 | } |
takeuchi |
1:0229fc98a26f | 35 | if(headerPoint == 0xff) { |
takeuchi |
1:0229fc98a26f | 36 | return; |
takeuchi |
1:0229fc98a26f | 37 | } |
takeuchi |
1:0229fc98a26f | 38 | |
takeuchi |
1:0229fc98a26f | 39 | //assemble |
takeuchi |
1:0229fc98a26f | 40 | takeCounter = headerPoint; //firstheader |
takeuchi |
1:0229fc98a26f | 41 | ++takeCounter; //secondheader |
takeuchi |
1:0229fc98a26f | 42 | } |