All of this:
if ((data[0] == 's')){
if ((data[1] == 'n')){
if ((data[2] == 'p')) {
if (data[3] == (PT_HAS_DATA | PT_IS_BATCH | PT_BATCH_LEN)){
if (data[4] == REG_EULER_PHI_THETA){
for (byte j = 5; j <= 12; j++){
data_sum += data[j];
}
blank = data[11];//12
blank = data[12];//13
chksum1 = data[13];//14
chksum0 = data[14];//15
chksum = (chksum1 << 8) | chksum0;
if (chksum == ('s' + 'n' + 'p' + (PT_HAS_DATA | PT_IS_BATCH | PT_BATCH_LEN) + REG_EULER_PHI_THETA + data_sum)){ //If checksum is correct...
phi.data = (data[6] | (data[5] << 8));
theta.data = (data[8] | (data[7] << 8));
psi.data = (data[10] | (data[9] << 8));
IMU_flag = true;
return;
}//1
}//2
else{
IMU_flag = false;
return;
}
}//3
else{
IMU_flag = false;
return;
}
}//4
else{
IMU_flag = false;
return;
}
}//5
else{
IMU_flag = false;
return;
}
}//6
else{
IMU_flag = false;
return;
}
else {
IMU_flag = false;
return;
Could be simplified to:
if (data[0] == 's' && data[1] == 'n' && data[2] == 'p')
if (data[3] == (PT_HAS_DATA | PT_IS_BATCH | PT_BATCH_LEN)){
if (data[4] == REG_EULER_PHI_THETA){
for (byte j = 5; j <= 12; j++){
data_sum += data[j];
}
chksum1 = data[13];//14
chksum0 = data[14];//15
chksum = (chksum1 << 8) | chksum0;
if (chksum == ('s' + 'n' + 'p' + (PT_HAS_DATA | PT_IS_BATCH | PT_BATCH_LEN) + REG_EULER_PHI_THETA + data_sum)){ //If checksum is correct...
phi.data = (data[6] | (data[5] << 8));
theta.data = (data[8] | (data[7] << 8));
psi.data = (data[10] | (data[9] << 8));
IMU_flag = true;
return;
}//1
}//2
IMU_flag = false;
}
I might have left off a curly brace there, but you see the idea. Either it succeeds with IMU_flag as true, or you fall through with IMU_flag as false.