1
0
mirror of https://github.com/arduino/Arduino.git synced 2024-12-12 23:08:52 +01:00
Arduino/libraries/Robot_Control/EasyTransfer2.cpp
2013-05-15 10:47:17 +02:00

152 lines
3.5 KiB
C++

#include "EasyTransfer2.h"
//Captures address and size of struct
void EasyTransfer2::begin(HardwareSerial *theSerial){
_serial = theSerial;
//dynamic creation of rx parsing buffer in RAM
//rx_buffer = (uint8_t*) malloc(size);
resetData();
}
void EasyTransfer2::writeByte(uint8_t dat){
if(position<20)
data[position++]=dat;
size++;
}
void EasyTransfer2::writeInt(int dat){
if(position<19){
data[position++]=dat>>8;
data[position++]=dat;
size+=2;
}
}
uint8_t EasyTransfer2::readByte(){
if(position>=size)return 0;
return data[position++];
}
int EasyTransfer2::readInt(){
if(position+1>=size)return 0;
int dat_1=data[position++]<<8;
int dat_2=data[position++];
int dat= dat_1 | dat_2;
return dat;
}
void EasyTransfer2::resetData(){
for(int i=0;i<20;i++){
data[i]=0;
}
size=0;
position=0;
}
//Sends out struct in binary, with header, length info and checksum
void EasyTransfer2::sendData(){
uint8_t CS = size;
_serial->write(0x06);
_serial->write(0x85);
_serial->write(size);
for(int i = 0; i<size; i++){
CS^=*(data+i);
_serial->write(*(data+i));
//Serial.print(*(data+i));
//Serial.print(",");
}
//Serial.println("");
_serial->write(CS);
resetData();
}
boolean EasyTransfer2::receiveData(){
//start off by looking for the header bytes. If they were already found in a previous call, skip it.
if(rx_len == 0){
//this size check may be redundant due to the size check below, but for now I'll leave it the way it is.
if(_serial->available() >= 3){
//this will block until a 0x06 is found or buffer size becomes less then 3.
while(_serial->read() != 0x06) {
//This will trash any preamble junk in the serial buffer
//but we need to make sure there is enough in the buffer to process while we trash the rest
//if the buffer becomes too empty, we will escape and try again on the next call
if(_serial->available() < 3)
return false;
}
//Serial.println("head");
if (_serial->read() == 0x85){
rx_len = _serial->read();
//Serial.print("rx_len:");
//Serial.println(rx_len);
resetData();
//make sure the binary structs on both Arduinos are the same size.
/*if(rx_len != size){
rx_len = 0;
return false;
}*/
}
}
//Serial.println("nothing");
}
//we get here if we already found the header bytes, the struct size matched what we know, and now we are byte aligned.
if(rx_len != 0){
while(_serial->available() && rx_array_inx <= rx_len){
data[rx_array_inx++] = _serial->read();
}
if(rx_len == (rx_array_inx-1)){
//seem to have got whole message
//last uint8_t is CS
calc_CS = rx_len;
//Serial.print("len:");
//Serial.println(rx_len);
for (int i = 0; i<rx_len; i++){
calc_CS^=data[i];
//Serial.print("m");
//Serial.print(data[i]);
//Serial.print(",");
}
//Serial.println();
//Serial.print(data[rx_array_inx-1]);
//Serial.print(" ");
//Serial.println(calc_CS);
if(calc_CS == data[rx_array_inx-1]){//CS good
//resetData();
//memcpy(data,d,rx_len);
for(int i=0;i<20;i++){
//Serial.print(data[i]);
//Serial.print(",");
}
//Serial.println("");
size=rx_len;
rx_len = 0;
rx_array_inx = 0;
return true;
}
else{
//Serial.println("CS");
resetData();
//failed checksum, need to clear this out anyway
rx_len = 0;
rx_array_inx = 0;
return false;
}
}
}
//Serial.print(rx_len);
//Serial.print(" ");
//Serial.print(rx_array_inx);
//Serial.print(" ");
//Serial.println("Short");
return false;
}