Revision 0:d463d5c04541, committed 2011-08-16
- Comitter:
- agiembed
- Date:
- Tue Aug 16 05:32:33 2011 +0000
- Commit message:
Changed in this revision
--- /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());
+}
--- /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;
+}
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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;
+
+}
--- /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
--- /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
--- /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;
+
Raharja Agie