R1370.cpp

00001 #include "R1370.h"
00002 
00003 R1370::R1370(PinName tx, PinName rx) : RawSerial(tx, rx, 115200)
00004 {
00005     attach(callback(this, &R1370::receiveByte));
00006 }
00007 
00008 void R1370::receiveByte()
00009 {
00010     buffer[bufferPoint % R1370_BUFFER_SIZE] = getc();
00011  
00012     if(bufferPoint != 0xff) {
00013         ++bufferPoint;
00014     } else {
00015         bufferPoint = (255%R1370_BUFFER_SIZE)+1;
00016     }
00017  
00018     ++receivedBytes;
00019  
00020     if(receivedBytes >= R1370_BUFFER_SIZE) checkData();
00021 }
00022 
00023 void R1370::checkData()
00024 {
00025     for(int i = 0; i < R1370_BUFFER_SIZE; i++) {
00026         if(buffer[i % R1370_BUFFER_SIZE] == R1370_HEADER0 && buffer[(i + 1) % R1370_BUFFER_SIZE] == R1370_HEADER1) {
00027 
00028             uint8_t checksum = 0x00;
00029             for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
00030                 checksum += buffer[(i + 2 + j)% R1370_BUFFER_SIZE];
00031 
00032             }
00033             if(checksum == buffer[(i + R1370_BUFFER_SIZE - 1)% R1370_BUFFER_SIZE]) {
00034                 for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
00035                     data[j] = buffer[(i + 2 + j) % R1370_BUFFER_SIZE];
00036                 }
00037                 receivedBytes = 0;
00038 
00039                 assemble();
00040                 return;
00041             }
00042         }
00043     }
00044 }
00045 
00046 void R1370::assemble()
00047 {
00048         index = data[0];
00049         angle = (data[1] & 0xFF) | ((data[2] << 8) & 0xFF00);
00050         rate = (data[3] & 0xFF) | ((data[4] << 8) & 0xFF00);
00051         acc[0] = (data[5] & 0xFF) | ((data[6] << 8) & 0xFF00);
00052         acc[1] = (data[7] & 0xFF) | ((data[8] << 8) & 0xFF00);
00053         acc[2] = (data[9] & 0xFF) | ((data[10] << 8) & 0xFF00);
00054         reserved = data[11];
00055         upbit_ = data[1];
00056         downbit_ = data[2];
00057 }
00058 
00059 float R1370::getAngle()
00060 {
00061     return (float)(angle / 100.0);
00062 }
00063 
00064 float R1370::getRate()
00065 {
00066     return (float)(rate / 100.0);
00067 }
00068 
00069 int16_t R1370::getAcc(char l)
00070 {
00071     if(l == 'x' || l == 'X') {
00072         return acc[0];
00073     } else if(l == 'y' || l == 'Y') {
00074         return acc[1];
00075     } else if(l == 'z' || l == 'Z') {
00076         return acc[2];
00077     } else {
00078         return 0;
00079     }
00080 }
00081 
00082 int16_t R1370::getAcc(int i)
00083 {
00084     if(i >= 0 && i < 3) {
00085         return acc[i];
00086     } else {
00087         return 0;
00088     }
00089 }
00090 
00091 int16_t R1370::getAccX()
00092 {
00093     return acc[0];
00094 }
00095 
00096 int16_t R1370::getAccY()
00097 {
00098     return acc[1];
00099 }
00100 
00101 int16_t R1370::getAccZ()
00102 {
00103     return acc[2];
00104 }
00105 
00106 unsigned char R1370::upbit()
00107 {
00108     return upbit_;
00109 }
00110 
00111 unsigned char R1370::downbit()
00112 {
00113     return downbit_;
00114 }