受信割り込みバージョン
Dependents: a_team_R1370 ashimawari_sample_test 2022NHK_A_ashimawari 2022A_NHK_kikou
R1370.cpp
- Committer:
- UCHITAKE
- Date:
- 2018-08-24
- Revision:
- 2:08ff7c46665f
- Parent:
- 0:243364135087
- Child:
- 3:2f00371553c1
File content as of revision 2:08ff7c46665f:
#include "R1370.h" 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()); } 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()); } } } } float R1370::getAngle() { return (float)(angle / 100.0); } float R1370::getRate() { return (float)(rate / 100.0); } 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]; }