受信割り込みバージョン
Dependents: a_team_R1370 ashimawari_sample_test 2022NHK_A_ashimawari 2022A_NHK_kikou
Diff: R1370.cpp
- Revision:
- 3:2f00371553c1
- Parent:
- 2:08ff7c46665f
- Child:
- 4:ffc2a55269f9
--- a/R1370.cpp Fri Aug 24 05:39:24 2018 +0000 +++ b/R1370.cpp Wed Dec 19 07:44:20 2018 +0000 @@ -3,43 +3,57 @@ R1370::R1370(PinName tx, PinName rx) : RawSerial(tx, rx, 115200) { attach(callback(this, &R1370::receiveByte)); - thread.start(callback(this, &R1370::assembleLoop)); } void R1370::receiveByte() { - buf.push_back(getc()); + buffer[bufferPoint % R1370_BUFFER_SIZE] = getc(); + + if(bufferPoint != 0xff) { + ++bufferPoint; + } else { + bufferPoint = (255%R1370_BUFFER_SIZE)+1; + } + + ++receivedBytes; + + if(receivedBytes >= R1370_BUFFER_SIZE) checkData(); +} + +void R1370::checkData() +{ + for(int i = 0; i < R1370_BUFFER_SIZE; i++) { + if(buffer[i % R1370_BUFFER_SIZE] == R1370_HEADER0 && buffer[(i + 1) % R1370_BUFFER_SIZE] == R1370_HEADER1) { + + uint8_t checksum = 0x00; + for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) { + checksum += buffer[(i + 2 + j)% R1370_BUFFER_SIZE]; + + } + if(checksum == buffer[(i + R1370_BUFFER_SIZE - 1)% R1370_BUFFER_SIZE]) { + for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) { + data[j] = buffer[(i + 2 + j) % R1370_BUFFER_SIZE]; + } + receivedBytes = 0; + + assemble(); + return; + } + } + } } void R1370::assemble() { - char checksum = 0; - for(int i = 2; i <= 13; i++) { - checksum += buf[i]; - } - if(checksum == buf[14]) { - index = buf[2]; - angle = (buf[3] & 0xFF) | ((buf[4] << 8) & 0xFF00); - rate = (buf[5] & 0xFF) | ((buf[6] << 8) & 0xFF00); - acc[0] = (buf[7] & 0xFF) | ((buf[8] << 8) & 0xFF00); - acc[1] = (buf[9] & 0xFF) | ((buf[10] << 8) & 0xFF00); - acc[2] = (buf[11] & 0xFF) | ((buf[12] << 8) & 0xFF00); - reserved = buf[13]; - } -} - -void R1370::assembleLoop() -{ - while(true) { - if(buf.size() > R1370_BUFFER_SIZE) { - if(buf[0] == R1370_HEADER0 && buf[1] == R1370_HEADER1) { - assemble(); - buf.erase(buf.begin(), buf.begin() + (R1370_BUFFER_SIZE - 1)); - } else { - buf.erase(buf.begin()); - } - } - } + index = data[0]; + angle = (data[1] & 0xFF) | ((data[2] << 8) & 0xFF00); + rate = (data[3] & 0xFF) | ((data[4] << 8) & 0xFF00); + acc[0] = (data[5] & 0xFF) | ((data[6] << 8) & 0xFF00); + acc[1] = (data[7] & 0xFF) | ((data[8] << 8) & 0xFF00); + acc[2] = (data[9] & 0xFF) | ((data[10] << 8) & 0xFF00); + reserved = data[11]; + upbit_ = data[1]; + downbit_ = data[2]; } float R1370::getAngle() @@ -87,4 +101,14 @@ int16_t R1370::getAccZ() { return acc[2]; +} + +unsigned char R1370::upbit() +{ + return upbit_; +} + +unsigned char R1370::downbit() +{ + return downbit_; } \ No newline at end of file