File content as of revision 0:d463d5c04541:

#define Ahrs_num 42
char checkhead[2], imu_cnt = 0, data_flag = 0, gcs_cnt = 0;
unsigned char data_que[Ahrs_num];


void IMU_Update(){
    fcc.roll = IMU.roll;
    fcc.pitch = IMU.pitch;
    fcc.yaw = IMU.yaw;
    fcc.g_roll = IMU.g_roll;
    fcc.g_pitch = IMU.g_pitch;
    fcc.g_yaw = IMU.g_yaw;
    fcc.acc_x = IMU.acc_x;
    fcc.acc_y = IMU.acc_y;
    fcc.acc_z = IMU.acc_z;// + (cos((fcc.roll*3.14)/180) * cos((fcc.pitch*3.14)/180));
    fcc.bat = (unsigned short)(bat * 12000);
    fcc.US = (unsigned short)(alti.read_u16() * 0.06);
    fcc.IR1 = (unsigned short)(IRb.read_u16() * 0.01) - 220;
    fcc.IR2 = (unsigned short)(IRl.read_u16() * 0.01) - 220;
    fcc.IR3 = (unsigned short)(IRf.read_u16() * 0.01) - 220;
    fcc.IR4 = (unsigned short)(IRr.read_u16() * 0.01) - 220;
    
    if(fcc.roll>180 || fcc.roll<-180) fcc.roll = last.roll;
    if(fcc.pitch>90 || fcc.pitch<-90) fcc.pitch = last.pitch;
    if(fcc.yaw>360 || fcc.yaw<0) fcc.yaw = last.yaw;
    if(fcc.g_roll>300 || fcc.g_roll<-300) fcc.g_roll = last.g_roll;
    if(fcc.g_pitch>300 || fcc.g_pitch<-300) fcc.g_pitch = last.g_pitch;
    if(fcc.g_yaw>300 || fcc.g_yaw<-300) fcc.g_yaw = last.g_yaw;
    if(fcc.acc_x>4 || fcc.acc_x<-4) fcc.acc_x = last.acc_x;
    if(fcc.acc_y>4 || fcc.acc_y<-4) fcc.acc_y = last.acc_y;
    if(fcc.acc_z>4 || fcc.acc_z<-4) fcc.acc_z = last.acc_z;
    
    last.roll = IMU.roll;
    last.pitch = IMU.pitch;
    last.yaw = IMU.yaw;
    last.g_roll = IMU.g_roll;
    last.g_pitch = IMU.g_pitch;
    last.g_yaw = IMU.g_yaw;
    last.acc_x = IMU.acc_x;
    last.acc_y = IMU.acc_y;
    last.acc_z = IMU.acc_z;
    
    if(gcs_cnt == 3){
        //IMUtoGCS();
        gcs_cnt = 0;
    }
    else toBHG();
    gcs_cnt++;    

}                 

void ahrs_parsing(){
    int i = 0;
    
    data.roll = (char *) &IMU.roll;
    for(i=0; i<4; i++) data.roll[i] = data_que[i+28];
    
    data.pitch = (char *) &IMU.pitch;
    for(i=0; i<4; i++) data.pitch[i] = data_que[i+32];
    
    data.yaw = (char *) &IMU.yaw;
    for(i=0; i<4; i++) data.yaw[i] = data_que[i+36];    
    
    data.g_roll = (char *) &IMU.g_roll;
    for(i=0; i<4; i++) data.g_roll[i] = data_que[i+4];
    
    data.g_pitch = (char *) &IMU.g_pitch;
    for(i=0; i<4; i++) data.g_pitch[i] = data_que[i+8];
    
    data.g_yaw = (char *) &IMU.g_yaw;
    for(i=0; i<4; i++) data.g_yaw[i] = data_que[i+12];        
            
    data.acc_x = (char *) &IMU.acc_x;
    for(i=0; i<4; i++) data.acc_x[i] = data_que[i+16];
    
    data.acc_y = (char *) &IMU.acc_y;
    for(i=0; i<4; i++) data.acc_y[i] = data_que[i+20];
    
    data.acc_z = (char *) &IMU.acc_z;
    for(i=0; i<4; i++) data.acc_z[i] = data_que[i+24]; 
    
    IMU_Update();
}

void Checksum(){
    char checksum = 0;
    for(char i=1; i<40; i++) checksum = checksum ^ data_que[i];
    if(checksum == data_que[40]) ahrs_parsing();
}    

void ahrs_rec(){      
    unsigned char data;
    data = ahrs.getc();
    checkhead[0] = checkhead[1];
    checkhead[1] = data;
    if(checkhead[0] == 0xFF && checkhead[1] == 0xFF) {
        imu_cnt = 0;
        data_flag = 1;
    }
    if(data_flag == 1){
        data_que[imu_cnt] = data;
        imu_cnt++;
    }
    if(imu_cnt == 41){
        Checksum();
        data_flag = 2;
    }    
}