Raharja Agie
/
Mini-X
Revision 0:d463d5c04541, committed 2011-08-16
- Comitter:
- agiembed
- Date:
- Tue Aug 16 05:32:33 2011 +0000
- Commit message:
Changed in this revision
diff -r 000000000000 -r d463d5c04541 2fcs.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/2fcs.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,73 @@ +char gud = 0, flag_com = 1, check_error = start; + +void Get_GPS(char *data) //F_Mode[F_Mode_num] +{ + int i; + char *pTemp = (char *) &gps; //Fligth Mode Data + + for(i=0;i<GPS_num;i++) *(pTemp+i) = *(data+i); //F_Mode[F_Mode_num] + gpscal(); +} + +void Get_Mode(char *data) //F_Mode[F_Mode_num] +{ + int i; + char *pTemp = (char *) &FMD; //Fligth Mode Data + + for(i=0;i<2;i++) *(pTemp+i) = *(data+i); //F_Mode[F_Mode_num] + //toGCS(); +} + +void GCS_data(char data) +{ + if(check_error == start) + { + if(data == head1) check_error = check1; + else check_error = start; + } + + else if(check_error == check1) + { + if(data == head2) check_error = check2; + else if(data == Flight_Start_bit2) check_error = check3; + else check_error = start; + } + + else if((check_error == check2)&&(data != End_bit)) + { + GPS[data_cnt] = data; + if(data_cnt<(GPS_num-1)) data_cnt++; + else + { + Get_GPS(GPS); + data_cnt = 0; + check_error = start; + } + } + + else if((check_error == check3)&&(data != End_bit)) + { + Mode[data_cnt] = data; + if(data_cnt<(2-1)) data_cnt++; + else + { + Get_Mode(Mode); + data_cnt = 0; + check_error = start; + } + } + + else + { + data_cnt = 0; + check_error = start; + } +} + + +void getGCS(){ + unsigned char data; + data = radio.getc(); + //GCS_data(data); + radio.putc(radio.getc()); +}
diff -r 000000000000 -r d463d5c04541 2gcs.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/2gcs.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,137 @@ +#define FCC_DATA_SIZE 52 //39 Direct from IMU +#define GPS_DATA_SIZE 20 + +char lati[6], altig[6]; + +float llt = 127.1234; //POSRoll;//dlat;// gps.lat; gps.lat = +float lng = 37.4321; //POSPitch; //dlon;// gps.lon; gps.lon = + +void IMUtoGCS(){ + + char *FtoG = (char *)&fcc, i; //&IMU + + radio.putc(0xFF); + radio.putc(0xFF); + + fcc.acc_y = roll_con; // llt; + fcc.acc_z = pitch_con;// lng; + + if(gps.lat>0) fcc.acc_x = 1; else fcc.acc_x = 0; + + // fcc.IR2 = (unsigned short)F_range; + //fcc.IR1 = F_error; + + llt += 0.0001; + if(llt > 128.00) llt = 127.1234; + lng += 0.0001; + if(lng > 38.00) lng = 37.4321; + + + + for (i=0;i<FCC_DATA_SIZE;i++) radio.putc(*FtoG++); + radio.putc(0x0D); + return; +} + +void GPStoGCS(){ + + char *FtoG = (char *)&gps, i; //&IMU + + radio.putc(0xFE); + radio.putc(0xFE); + + for (i=0;i<GPS_DATA_SIZE;i++) radio.putc(*FtoG++); + radio.putc(0x0D); + return; +} + +void GPSASCII(){ + + radio.printf("%f ", dlat); + radio.printf("%f ", dlon); + //radio.printf("%d ", FMD.lock); + radio.printf("%f ", gps.lat); + // radio.printf("%d\n", FMD.mode); + radio.printf("%f ", gps.lon); + /// radio.printf("%d\n", FMD.mission); + radio.printf("%f\n ", gps.alti); + + + +//radio.printf("%f ", IMU.roll); +//radio.printf("%f ", IMU.pitch); +//radio.printf("%f\n", IMU.yaw); + + // radio.printf("%f", gps.dist); + //radio.printf(" "); + //radio.printf("%f", gps.bear); + //radio.printf("\n"); +} + +void toBHG(){ + + char *FtoG = (char *)&fcc, i; //&IMU + + pc.putc(0xFF); + pc.putc(0xFF); + for (i=0;i<FCC_DATA_SIZE;i++) pc.putc(*FtoG++); + pc.putc(0x0D); +} + +void rcprint(){ + + printf("%d\t", RC.rolls); + printf("%d\t", RC.throttles); + printf("%d\t", RC.pitchs); + printf("%d\t", RC.yaws); + printf("%d\n", RC.sws); + return; +} + +void imuprint(){ + printf("%d\t", (int)fcc.roll); + printf("%d\t", (int)fcc.pitch); + printf("%d\n", (int)fcc.yaw); + return; +} + +/*void motprint(){ + printf("%d\t", bl_F[1]); + printf("%d\t", bl_B[1]); + printf("%d\t", bl_R[1]); + printf("%d\n", bl_L[1]); + return; +} + +void xmotprint(){ + printf("%d\t", front_bl); + printf("%d\t", back_bl); + printf("%d\t", right_bl); + printf("%d\n", left_bl); + return; +}*/ + +void pidprint(){ + printf("%d\t", alti_con); + printf("%d\t", roll_con); + printf("%d\t", pitch_con); + printf("%d\n", yaw_con); + return; +} + +void gainprint(){ + printf("%d\t", (int)gainRoll.p); + printf("%d\t", (int)gainRoll.i); + printf("%d\t", (int)gainRoll.d); + printf("%d\n", (int)gainRoll.r); + return; +} + + +void yawprint(){ + + printf("%d\t", RC.yaws); + printf("%d\t", (short)fcc.yaw); + printf("%d\n", (short)Yaw_ref); + return; +}
diff -r 000000000000 -r d463d5c04541 ahrs.h --- /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
diff -r 000000000000 -r d463d5c04541 define.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/define.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,40 @@ +#define Roll_INDEX 0x01 +#define Pitch_INDEX 0x02 +#define Yaw_INDEX 0x03 +#define Dis_INDEX 0x05 +#define Alt_INDEX 0x04 +#define TEMP_INDEX 0x06 + +#define Gain_Start_bit1 0xFE //Gain stick +#define Gain_Start_bit2 0xFE +#define Gain_End_bit 0x0D + +#define Flight_Start_bit1 0xFF //Flight Mode Data +#define Flight_Start_bit2 0xFE +#define Flight_End_bit 0x0D + +#define Gain_num 20 // +#define GPS_num 12 +#define Mode_num 3 //14 + +#define head1 0xFF +#define head2 0xFF +#define End_bit 0x0D + + +char data_cnt = 0; +char Gain[Gain_num]; +char GPS[GPS_num]; +char Mode[Mode_num]; + +typedef enum +{ + start, check1, check2, check3, check4, check5, check6, check7, check8 +} Check; + +float latt, lont; //Target point +float latp, lonp; //Current point +float d_lat, d_lon; +float a, b, c, d, ld; +float lats, lons; +float dlat, dlon; \ No newline at end of file
diff -r 000000000000 -r d463d5c04541 gps.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/gps.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,51 @@ +int gpscnt = 0; + + + +void gpscal() +{ + + if(gpscnt<10){ + lats = gps.lat; //latt=37.541230; //37.540553; + lons = gps.lon; // lont=127.079819; // 127.078038; + gpscnt++; + } + + latt = lats; //37.5409023;// + lont = lons; //127.0800591;// + + latp = gps.lat; + lonp = gps.lon; + + dlat = latp - latt; + dlon = lonp - lont; + + //GPSASCII(); + + /* + //jarak ke tujuan..... + latp=latp*3.14/180; //ftoa(latp, 1, dst); putstr3(dst); putchar3('\n'); + latt=latt*3.14/180; //ftoa(latt, 1, dst); putstr3(dst); putchar3('\n'); + lonp=lonp*3.14/180; //ftoa(lonp, 1, dst); putstr3(dst); putchar3('\n'); + lont=lont*3.14/180; //ftoa(lont, 1, dst); putstr3(dst); putchar3('\n'); + d_lat=latp-latt; + d_lon=lonp-lont; + a=(sin(d_lat/2))*(sin(d_lat/2))+cos(latt)*cos(latp)*(sin(d_lon/2))*(sin(d_lon/2)); + //c=2*atan2(sqrt(a),sqrt(1-a)); + //d=r*c; + c=2*asin(sqrt(a)); + d=c*180*60*1000/3.14*1.852; // d = distance to target + // if(d > 5000 || d < -5000) d = ld; + gps.dist = d; + + //sudut hadap ke tujuan.... + //b=atan2(sin(d_lon)*cos(latp), cos(latt)*sin(latp)-sin(latt)*cos(latp)*cos(d_lon)); + b=(acos((sin(latt)-sin(latp)*cos(c))/(sin(c)*cos(latp))))*180/3.14; + if (b==360)b=0; + gps.bear = b; + + ld = d; + GPSASCII(); + */ + +} \ No newline at end of file
diff -r 000000000000 -r d463d5c04541 i2c.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/i2c.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,76 @@ + + +#define ON 8 +#define OFF 0 +#define ESCMax 254 +#define ESCMin 0 + +const int addr = 0x50; // define the I2C Address +char bl_FR[2], bl_BR[2], bl_BL[2], bl_FL[2], all, engine, key = 0, key_st = 0; +short front_r = 0, back_r = 0, front_l = 0, back_l = 0; + +void securinit(){ + + i2c.write(addr+2, 0, 2); // Send command string to Motor Left + i2c.write(addr+4, 0, 2); // Send command string to Motor Back + i2c.write(addr+6, 0, 2); // Send command string to Motor Front + i2c.write(addr+8, 0, 2); // Send command string to Motor Right + return; +} + +void updatei2c(){ + /* + bl_F[0]= all; + bl_B[0]= all; + bl_R[0]= all; + bl_L[0]= all; + */ + + bl_FR[1]= front_r; + bl_BR[1]= back_r; + bl_FL[1]= front_l; + bl_BL[1]= back_l; + + i2c.write(addr+2, bl_FR, 2); // Send command string to Motor Left + i2c.write(addr+4, bl_BR, 2); // Send command string to Motor Back + i2c.write(addr+6, bl_BL, 2); // Send command string to Motor Front + i2c.write(addr+8, bl_FL, 2); // Send command string to Motor Right + return; +} + +void moteq(){ // motor equation + all = 0; + + if(RC.throttles < 5 && RC.yaws < -200){ + if(key_st == 0)key = !key; + key_st = 1; + } + else key_st = 0; + if(key == 1) engine = ON; + else engine = OFF; + + + + front_r = engine + alti_con + yaw_con - pitch_con - roll_con; + + if(front_r>ESCMax) front_r = ESCMax; + else if(front_r<ESCMin+engine) front_r = engine; + + back_r = engine + alti_con - yaw_con + pitch_con - roll_con; + + if(back_r>ESCMax) back_r = ESCMax; + else if(back_r<ESCMin+engine) back_r = engine; + + back_l= engine + alti_con + yaw_con + pitch_con + roll_con; + + if(back_l>ESCMax) back_l = ESCMax; + else if(back_l<ESCMin+engine) back_l = engine; + + front_l = engine + alti_con - yaw_con - pitch_con + roll_con; + + if(front_l>ESCMax) front_l = ESCMax; + else if(front_l<ESCMin+engine) front_l = engine; + + if(engine == OFF) front_r = back_r = front_l = back_l = 0; + +} \ No newline at end of file
diff -r 000000000000 -r d463d5c04541 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,48 @@ +#include "mbed.h" +#include "define.h" +#include "setup.h" +#include "struct.h" + +int loop_cnt = 0, init_cnt = 0; + +#include "pid.h" +#include "i2c.h" +#include "rc.h" + +#include "2gcs.h" +#include "gps.h" +#include "2fcs.h" + +#include "ahrs.h" + +void init(){ + Yaw_ref = fcc.yaw; + init_cnt++; +} + +int main() { + setup(); + securinit(); + rc.rise(&PPM_rise); // attach the address of the PPM_rise function to the rising edge + wait(0.5); + + while(1) { // wait around, interrupts will interrupt this! + if(ahrs.readable())ahrs_rec(); + + if(radio.readable())radio.putc(radio.getc());//getGCS(); + if(loop_cnt>10000){ + getrc(); + pid(); + moteq(); + updatei2c(); + loop_cnt = 0; + //rcprint(); + if(bat > 0.8) myled = 1; + else myled = !myled; + } + loop_cnt++; + + } +} + + \ No newline at end of file
diff -r 000000000000 -r d463d5c04541 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da
diff -r 000000000000 -r d463d5c04541 pid.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pid.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,130 @@ +short alti_con, roll_con, yaw_con, pitch_con; +char yaw_state1 = 0, yaw_state2 = 0, yaw_cnt = 0; +#define PID_max 80 +#define Int_max 100 +#define Int_min -100 + +float PIDRoll, PIDPitch, PIDYaw, PIDAlti; +float Int_Roll, Int_Pitch; +float Roll_error, Roll_prev_error, Roll_ref = 0, Roll_int_error = 0; +float Pitch_error, Pitch_prev_error, Pitch_ref = 0, Pitch_int_error = 0; +float Yaw_error, Yaw_prev_error, Yaw_ref, Yaw_prev_ref; +float Alti_error, Alti_prev_error, Z_rate_error, Alti_ref, Alti_I_error; + + +void PID_Roll() { + + gainRoll.p = 0.65;//28; //1.05; + gainRoll.d = 0.5; + gainRoll.i = 0.035; + gainRoll.r = 0.175;//9; + //if(fcc.US < 1000){ + //gainRoll.p = 1.05; //1.05 Take-off Gain + //gainRoll.r = 0.275; //0.275 Take-off Gain + //} + + Roll_ref = (RC.rolls / 7); + if (Roll_ref>30) Roll_ref = 30; + else if (Roll_ref<-30) Roll_ref = -30; + + Roll_error = fcc.roll - Roll_ref; + Roll_int_error += Roll_error; + + if (Roll_int_error > Int_max) Roll_int_error = Int_max; + else if (Roll_int_error < Int_min) Roll_int_error = Int_min; + Int_Roll = gainRoll.i * Roll_int_error; + + PIDRoll = gainRoll.p * Roll_error; + PIDRoll += Int_Roll; + PIDRoll += (gainRoll.d * (Roll_error - Roll_prev_error)); + PIDRoll += (gainRoll.r * fcc.g_roll); + + if (PIDRoll > PID_max) PIDRoll = PID_max; + else if (PIDRoll < -PID_max) PIDRoll = -PID_max; + + Roll_prev_error = Roll_error; + return; +} + +void PID_Pitch() { + + gainPitch.p = gainRoll.p;//1.075; //1.35 //1.05; + gainPitch.d = gainRoll.d;//0.9635; + gainPitch.i = gainRoll.i;//0.021; + gainPitch.r = gainRoll.r;// 0.175; //0.29 + //if(fcc.US < 1000){ + // gainPitch.p = 1.05; //1.05 Take-off Gain + // gainPitch.r = 0.275; //0.275 Take-off Gain + //} + + Pitch_ref = (-RC.pitchs / 6); + if (Pitch_ref>30) Pitch_ref = 30; + else if (Pitch_ref<-30) Pitch_ref = -30; + + Pitch_error = fcc.pitch - Pitch_ref; + Pitch_int_error += Pitch_error; + + if (Pitch_int_error > Int_max) Pitch_int_error = Int_max; + else if (Pitch_int_error < Int_min) Pitch_int_error = Int_min; + Int_Pitch = gainPitch.i * Pitch_int_error; + + PIDPitch = (gainPitch.p * Pitch_error); + PIDPitch += Int_Pitch; + PIDPitch += (gainPitch.d * (Pitch_error - Pitch_prev_error)); + PIDPitch += (gainPitch.r * fcc.g_pitch); + + if (PIDPitch > PID_max) PIDPitch = PID_max; + else if (PIDPitch < -PID_max) PIDPitch = -PID_max; + + Pitch_prev_error = Pitch_error; + return; +} + + +void PID_Yaw() { + + gainYaw.p = 1.35; //1.35 Take-off Gain //1.15; + //gainYaw.d = 0; + gainYaw.r = 0.25; //0.35 Take-off Gain + + if (RC.yaws>=50 || RC.yaws<=-50) yaw_state1 = 1; + if (yaw_state1 == 1)if (RC.yaws>50 || RC.yaws<-50) yaw_state2 = 1; + if (yaw_state1 == 1 && yaw_state2 == 1) yaw_cnt++; + if (yaw_cnt == 10) { + Yaw_ref = fcc.yaw; + yaw_state1 = yaw_state2 = yaw_cnt = 0; + } + Yaw_error = Yaw_ref - fcc.yaw; + if (Yaw_error>180) Yaw_error -= 360; + if (Yaw_error<-180) Yaw_error += 360; + + PIDYaw = (gainYaw.p * Yaw_error); + //PIDYaw += (gainYaw.i * Yaw_error); + //PIDYaw += (gainYaw.d * (Yaw_error - Yaw_prev_error)); + PIDYaw -= (gainYaw.r * fcc.g_yaw); + + if (PIDYaw > PID_max) PIDYaw = PID_max; + if (PIDYaw < -PID_max) PIDYaw = -PID_max; + + Yaw_prev_error = Yaw_error; + return; +} + + + +void pid() { + + alti_con = RC.throttles; + + + PID_Roll(); + roll_con = - PIDRoll; //RC.rolls * 0.14 ;// + + PID_Pitch(); + pitch_con = PIDPitch; //RC.pitchs * 0.14; // + + PID_Yaw(); + yaw_con = (RC.yaws * 0.3) + PIDYaw; + if (alti_con < 5) yaw_con = 0; + +}
diff -r 000000000000 -r d463d5c04541 rc.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rc.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,111 @@ +Timer tick; +InterruptIn rc(p11); +short buf_ppm[6]; +char i = 0, idx, cnt = 0; + +void get_PPM(){ + switch (idx){ + case 1: + RC.roll = buf_ppm[2]; + RC.throttle = buf_ppm[3]; + RC.pitch = buf_ppm[4]; + RC.yaw = buf_ppm[5]; + RC.sw = buf_ppm[0]; + break; + + case 2: + RC.roll = buf_ppm[3]; + RC.throttle = buf_ppm[4]; + RC.pitch = buf_ppm[5]; + RC.yaw = buf_ppm[0]; + RC.sw = buf_ppm[1]; + break; + + case 3: + RC.roll = buf_ppm[4]; + RC.throttle = buf_ppm[5]; + RC.pitch = buf_ppm[0]; + RC.yaw = buf_ppm[1]; + RC.sw = buf_ppm[2]; + break; + + case 4: + RC.roll = buf_ppm[5]; + RC.throttle = buf_ppm[0]; + RC.pitch = buf_ppm[1]; + RC.yaw = buf_ppm[2]; + RC.sw = buf_ppm[3]; + break; + + case 5: + RC.roll = buf_ppm[0]; + RC.throttle = buf_ppm[1]; + RC.pitch = buf_ppm[2]; + RC.yaw = buf_ppm[3]; + RC.sw = buf_ppm[4]; + break; + + case 0: + RC.roll = buf_ppm[1]; + RC.throttle = buf_ppm[2]; + RC.pitch = buf_ppm[3]; + RC.yaw = buf_ppm[4]; + RC.sw = buf_ppm[5]; + break; + + default: break; + } + return; +} + +void getrc(){ + RC.rolls = (RC.roll - 1518) * 0.73; + RC.throttles = (RC.throttle - 1105) * 0.3; + RC.pitchs = (RC.pitch - 1514) * 0.73; + RC.yaws = (RC.yaw - 1518) * 0.73; + RC.sws = RC.sw - 1518; + return; +} + + +short rc_roll(){ + return (short)(RC.roll);// 1518); +} + +short rc_throttle(){ + return (short)(RC.throttle);// - 1105); +} + +short rc_pitch(){ + return (short)(RC.pitch);// - 1514); +} + +short rc_yaw(){ + return (short)(RC.yaw);// - 1518); +} + +short rc_sw(){ + return (short)(RC.sw);// - 1518); +} + + +void PPM_rise() { + tick.stop(); // Stop timer + buf_ppm[i]=tick.read_us(); // Read timer to buffer[i] i=turns + tick.reset(); // Reset timer + tick.start(); // Start timer + i++; // increment i. + if(buf_ppm[i]>5000) idx = i; + if(i==6) i = 0; + get_PPM(); + return; +} + + +/* +Channel 1 = Roll +Channel 2 = Throttle +Channel 3 = Pitch +Channel 4 = Yaw +Channel 5 = Switch +*/ \ No newline at end of file
diff -r 000000000000 -r d463d5c04541 setup.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/setup.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,20 @@ +Serial pc(USBTX, USBRX); +Serial ahrs(p28, p27); +Serial radio(p13, p14); +I2C i2c(p9, p10); //sda, scl +Ticker SendData; +Ticker Proccess; +DigitalOut myled(LED1); +AnalogIn alti(p20); +AnalogIn bat(p19); +AnalogIn IRl(p18); +AnalogIn IRb(p17); +AnalogIn IRr(p16); +AnalogIn IRf(p15); + +void setup(){ + pc.baud(115200); + radio.baud(115200); + ahrs.baud(115200); +} + \ No newline at end of file
diff -r 000000000000 -r d463d5c04541 struct.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/struct.h Tue Aug 16 05:32:33 2011 +0000 @@ -0,0 +1,97 @@ + +typedef struct _fcc_data { + + unsigned short US; //2 + unsigned short IR1; //2 + unsigned short IR2; //2 + unsigned short IR3; //2 + unsigned short IR4; //2 + unsigned short bat; //2 + char state_flag; //1 + float roll, pitch, yaw; // 4, 4, 4 + float g_roll, g_pitch, g_yaw; //4, 4, 4 + float acc_x, acc_y, acc_z; //4, 4 ,4 + +}FCC_DATA; +FCC_DATA fcc; + +typedef struct _imu{ + char *roll, *pitch, *yaw, *g_roll, *g_pitch, *g_yaw, *acc_x, *acc_y, *acc_z; +}data_IMU; +data_IMU data; + +typedef struct _ahrs { + + short packet; //2 + unsigned char data_info; //1 + float g_roll, g_pitch, g_yaw; //4, 4, 4 + float acc_x, acc_y, acc_z; //4, 4 ,4 + float roll, pitch, yaw; // 4, 4, 4 + +}AHRS_DATA; +AHRS_DATA IMU; + + +typedef struct _gps { + float lat, lon, alti, dist, bear; +}GPS_DATA; +GPS_DATA gps; + +typedef struct _rc { + short roll, throttle, pitch, yaw, sw; + short rolls, throttles, pitchs, yaws, sws; +} RC_DATA; +RC_DATA RC; + +typedef struct _last{ + float roll, pitch, yaw; // 4, 4, 4 + float g_roll, g_pitch, g_yaw; //4, 4, 4 + float acc_x, acc_y, acc_z; //4, 4 ,4 +} LAST_IMU; +LAST_IMU last; + +typedef struct _gainRoll{ + float p, i, d, r; +}GAIN_ROLL; +GAIN_ROLL gainRoll; + +typedef struct _gainPitch{ + float p, i, d, r; +}GAIN_PITCH; +GAIN_PITCH gainPitch; + +typedef struct _gainYaw{ + float p, i, d, r; +}GAIN_YAW; +GAIN_YAW gainYaw; + +typedef struct _gainAlti{ + float p, i, d, r; +}GAIN_ALTI; +GAIN_ALTI gainAlti; + + +//======== PID Struct =====/ +typedef struct GAIN_GET +{ + char Index, flight_mode, sensor_select; //3 + float p_gain, i_gain, d_gain, goal; //4, 4, 4, 4 + char dumm; //1 -dummy +}PID_GAIN; +PID_GAIN PID; + + +//======== F_MODE Struct =====/ +typedef struct F_MODE_GET +{ + unsigned short mode; + ///char lock, mode, mission; //1, 1, 1 + // unsigned short heading_ref; //2 + // unsigned short alti_ref; //2 + /// unsigned short y_trim; //2 + /// unsigned short r_trim; //2 + /// unsigned short p_trim; //2 + //char dumm; //1 -dummy +}F_MODE_DATA; +F_MODE_DATA FMD; +