受信割り込みバージョン
Dependents: a_team_R1370 ashimawari_sample_test 2022NHK_A_ashimawari 2022A_NHK_kikou
R1370.cpp
- Committer:
- highfieldsnj
- Date:
- 2019-05-18
- Revision:
- 4:ffc2a55269f9
- Parent:
- 3:2f00371553c1
- Child:
- 5:553c02009ad0
File content as of revision 4:ffc2a55269f9:
#include "R1370.h" R1370::R1370(PinName tx, PinName rx) : RawSerial(tx, rx, 115200) { attach(callback(this, &R1370::receiveByte)); } void R1370::receiveByte() { 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() { 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() { return (float)(angle / 100.0); } float R1370::getRate() { return (float)(rate / 100.0); } float R1370::getRadian() { return (float)(angle / 100.0 / 180.0 * 3.141592653589793); } int16_t R1370::getAcc(char l) { if(l == 'x' || l == 'X') { return acc[0]; } else if(l == 'y' || l == 'Y') { return acc[1]; } else if(l == 'z' || l == 'Z') { return acc[2]; } else { return 0; } } int16_t R1370::getAcc(int i) { if(i >= 0 && i < 3) { return acc[i]; } else { return 0; } } int16_t R1370::getAccX() { return acc[0]; } int16_t R1370::getAccY() { return acc[1]; } int16_t R1370::getAccZ() { return acc[2]; } unsigned char R1370::upbit() { return upbit_; } unsigned char R1370::downbit() { return downbit_; }