Here is code. A little messy since its more for testing random stuff not my final product.
bool arrayequal;
config mode
//uint8_t Message5[] = {0xFA,0xFF,0x10,0x00,0xF1}; // Message to make IMU go into config mode
int counter = 0;
uint8_t repeats = 0;
uint8_t test1[64];
uint8_t test2[64];
uint8_t test3[64];
uint8_t test4[64];
uint8_t test5[64];
int repeat = 0;
bool H = 0;
bool go = 0;
uint8_t data[42];
uint8_t test_1;
uint8_t test_2;
uint8_t test_3;
uint8_t test_4;
uint8_t test_5;
int counter1 = 0;
bool check;
int good = 0;
int bad = 0;
int z = 0;
uint8_t finalmessage[5];
void setup()
{
for(int i = 0; i<sizeof(test1); i++){
test1[i] = 0;
}
test1[0] = 0x00; test1[1] = 0x00; test1[2] = 0xFA; test1[3] = 0xFF; test1[4] = 0x31; test1[5] = 0x00; test1[6] = 0xD0;
for(int i = 0; i<sizeof(test2); i++){
test2[i] = 0;
}
test2[0] = 0x00; test2[1] = 0x00; test2[2] = 0xFA; test2[3] = 0x01; test2[4] = 0xD1; test2[5] = 0x00; test2[6] = 0x2E;
for(int i = 0; i<sizeof(test3); i++){
test3[i] = 0;
}
test3[0] = 0x00; test3[1] = 0x00; test3[2] = 0xFA; test3[3] = 0xFF; test3[4] = 0x11; test3[5] = 0x00; test3[6] = 0xF0;
for(int i = 0; i<sizeof(test4); i++){
test4[i] = 0;
}
test4[0] = 0x00; test4[1] = 0x00; test4[2] = 0xFA; test4[3] = 0xFF; test4[4] = 0x3E; test4[5] = 0x00; test4[6] = 0xC3;
for(int i = 0; i<sizeof(test5); i++){
test5[i] = 0;
}
test5[0] = 0x00; test5[1] = 0x00; test5[2] = 0xFA; test5[3] = 0xFF; test5[4] = 0x3E; test5[5] = 0x00; test5[6] = 0xC3;
Serial.begin( 250000 );
#if !defined(__MIPSEL__)
while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
Serial.println("Start");
if (Usb.Init() == -1)
Serial.println("OSC did not start.");
delay( 200 );
}
void loop()
{
Usb.Task();
if( Usb.getUsbTaskState() == USB_STATE_RUNNING )
{
uint8_t rcode;
//char strbuf[] = "The quick brown fox jumps over the lazy dog";
//char strbuf[] = "This string contains 61 character to demonstrate FTDI buffers"; //add one symbol to it to see some garbage
uint8_t buf[64];
uint16_t rcvd = 64;
if(counter == 0){
// for(int i = 0 ;i < 10; i++){
uint8_t message2[] = {0xFA,0xFF,0x30,0x00,0xD1};
rcode = Ftdi.SndData(sizeof(message2),(uint8_t*)message2);
delay(40);
}else if(counter == 1){
for(int i = 0 ;i < 10; i++){
uint8_t message3[] = {0xFA,0x01,0xD0,0x02,0x00,0x02,0x2B};
rcode = Ftdi.SndData(sizeof(message3),(uint8_t*)message3);
delay(40);
}
}else if(counter == 2){
uint8_t message4[] = {0xFA,0xFF,0x10,0x00,0xF1};
rcode = Ftdi.SndData(sizeof(message4),(uint8_t*)message4);
delay(40);
}else if(counter == 3){
counter = 5;
}else if(counter == 4){
uint8_t message6[] = {0xFA,0xFF,0x30,0x00,0xD1};
rcode = Ftdi.SndData(sizeof(message6),(uint8_t*)message6);
}
for(int i = 0; i<sizeof(buf); i++){
buf[i] = 0;
}
for(int i = 0; i<sizeof(data); i++){
data[i] = 0;
}
rcode = Ftdi.RcvData(&rcvd,buf); //At current baud rate it should take 3 miliseconds for the messsages to be recieved.
// delay(40);
if (rcode && rcode != hrNAK)
ErrorMessage<uint8_t>(PSTR("Ret"), rcode);
if( rcvd >2 ) { //more than zero bytes received
if(counter != 5){
for(uint16_t i=0; i < rcvd; i++ ) {
Serial.print(buf[i+2],HEX); //printing on the screen
Serial.print(" ");
}
}else{
repeats = 0;
repeats = Calibrationparsing1(buf, data);
if(repeats == 1){
repeat++;
}
for(uint16_t i=0; i < sizeof(data); i++ ) {
Serial.print(data[i],HEX); //printing on the screen
Serial.print(" ");
}
check = checksum(data,sizeof(data));
if(check == true){
counter1++;
good++;
}else{
counter1++;
bad++;
}
if(counter1 == 100){
Serial.print("Good Messages: ");
Serial.print(good);
Serial.println();
Serial.print("Bad Messages: ");
Serial.print(bad);
Serial.println();
Serial.print("Repeats: ");
Serial.print(repeat);
Serial.println();
delay(5000);
repeat = 0;
good = 0;
bad = 0;
counter1 = 0;
}
}
}
Serial.println();
if(counter ==0 || counter ==1 || counter ==2 || counter == 3){
FindMessage(sizeof(buf),buf,finalmessage);
if(finalmessage[0] == test1[2] && finalmessage[1] == test1[3] && finalmessage[2] == test1[4] && finalmessage[3] == test1[5] && finalmessage[4] == test1[6]){
counter = 1;
Serial.print('1');
}else if(finalmessage[0] == test2[2] && finalmessage[1] == test2[3] && finalmessage[2] == test2[4] && finalmessage[3] == test2[5] && finalmessage[4] == test2[6]){
counter = 2;
Serial.print('2');
}else if(finalmessage[0] == test3[2] && finalmessage[1] == test3[3] && finalmessage[2] == test3[4] && finalmessage[3] == test3[5] && finalmessage[4] == test3[6]){
counter = 3;
Serial.print('3');
}
}
}
}
uint8_t FindMessage(int Size, uint8_t *message,uint8_t *finalmessage){
bool startcounting = false;
bool stopcounting = true;
int count = 0;
uint8_t buf[Size];
bool preamblespotted = false;
int location = 0;
for(int i = 0; i<Size; i++){
buf[i] = message[i+2];
if(buf[i-2] == 0xFA && (buf[i-1] == 0xFF || buf[i-1] == 0x01) && (buf[i] != 0x3E && buf[i] != 0x32 && buf[i] != 0xD0)){
preamblespotted = true;
location = i-2;
startcounting = true;
stopcounting = false;
}
if(preamblespotted == true ){
stopcounting = true;
finalmessage[0] = buf[location];
finalmessage[1] = buf[location+1];
finalmessage[2] = buf[location+2];
finalmessage[3] = buf[location+3];
finalmessage[4] = buf[location+4];
}else{
finalmessage[0] = 0x00;
finalmessage[1] = 0x00;
finalmessage[2] = 0x00;
finalmessage[3] = 0x00;
finalmessage[4] = 0x00;
}
}
}
bool checksum(uint8_t *buf,int Size){
unsigned long value = 0;
bool checksumcomplete;
byte checksum;
for(int i = 0; i<Size; i++){
value = value + buf[i];
}
checksum = (value&0xFF);
if(checksum == 0x00){
checksumcomplete = true;
}else{
checksumcomplete = false;
}
return checksumcomplete;
}
uint8_t parsdata_for_test(uint8_t *Array,int startingpoint, uint8_t *tobefilled){ //when using often make sure to keep track of starting point.
for(int i = startingpoint, j = 0; i <startingpoint+42 && j<42; i++,j++){
tobefilled[j] = Array[i];
}
}
uint8_t Calibrationparsing1(uint8_t *buf, uint8_t *data){
uint8_t test_4;
uint8_t test_3;
uint8_t test_2;
uint8_t test_1;
bool H = false;
bool go = false;
int z = 0;
int c = 0;
uint8_t repeat = 0;
while(H == false){//checks whether preamble has been seen yet
test_4 = buf[z+3];
test_3 = buf[z+2];
test_2 = buf[z+1];
test_1 = buf[z];
if(test_1 == 0xFA && test_2 == 0xFF && test_3 == 0x32 && test_4 ==0x26){
H = true;
go = true;
}else if(z >64){
H = true;
go = false;
}else{
H = false;
go = false;
}
z++;
}
for(int i = z+4; i<z+30;i++){
test_4 = buf[i+3];
test_3 = buf[i+2];
test_2 = buf[i+1];
test_1 = buf[i];
if(test_1 == 0xFA && test_2 == 0xFF && test_3 == 0x32 && test_4 ==0x26){//Used if the message FA FF 32 26 repeats itself, was observed occuring.
z = i+1;
repeats = 1;
break;
}
}
if(go == true){
parsdata_for_test(buf,z,data);
}else{
Serial.print('1');
}
return repeats;
}