Raharja Agie
/
Mini-X
Diff: ahrs.h
- Revision:
- 0:d463d5c04541
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ahrs.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,118 @@ +#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; + } +} + + + + + + + + + + \ No newline at end of file