syouichi imamori
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 3:27407c4984cf
- Parent:
- 2:59ac9df97701
- Child:
- 4:4060309b9cc0
--- a/main.cpp Fri Nov 15 20:53:36 2013 +0000
+++ b/main.cpp Thu Feb 13 16:07:07 2014 +0000
@@ -35,13 +35,14 @@
#define p6 dp9
#define p7 dp10
#define p8 dp11
- #define p9 dp25
+ #define p9 dp13
#define p10 dp26
#define p13 dp16
#define p21 dp1
#define p22 dp2
#define p23 dp18
#define p24 dp24
+ #define p26 dp25
#define p27 dp27
#define p28 dp5
#else //LPC1768
@@ -87,10 +88,11 @@
int channel = 0;
//int intrupt_cnt = 0;
//int intrupt_cnt2 = 0;
-volatile int CH[5];
+int volatile CH[5];
volatile int M[6];
volatile float Gyro[3];
volatile float Accel[3];
+volatile float Accel_Save[3];
volatile float Angle[3];
volatile float Gyro_Ref[3];
volatile float Gyro_Save[3];
@@ -102,6 +104,7 @@
//volatile bool tick_flag;
//volatile bool buzz_flag;
float interval;
+float pid_interval;
int pid_reg[3];
int loop_cnt;
@@ -113,8 +116,8 @@
void Get_Stick_Pos();
void CalibrateGyros(void);
void CalibrateAccel(void);
-void Get_Gyro();
-bool Get_Accel();
+void Get_Gyro(float);
+bool Get_Accel(float);
void Get_Angle(float);
void ReadConfig();
void WriteConfig();
@@ -171,7 +174,7 @@
{
int i=0;
- wait(0.5);
+ wait(1.5);
initialize();
Get_Stick_Pos();
@@ -223,7 +226,7 @@
void initialize()
{
- buzz.period_us(500);
+ buzz.period_us(400);
ReadConfig(); //config.inf file read
i2c.start(conf.LCD_Contrast);
@@ -312,20 +315,25 @@
char mem[MEM_SIZE];
char *send;
int i;
+//pc.printf("start\n\r");
if ( sizeof(config) > 255 ) {
LCD_printf("config size over");
wait(3);
return;
}
+//pc.printf("iap start\n\r");
send = (char*)&conf;
for ( i=0;i<sizeof(config);i++ ) mem[i] = send[i];
for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00;
- iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
- iap.erase( TARGET_SECTOR, TARGET_SECTOR );
-//pc.printf("erase\n");
- iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
- iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
-//pc.printf("write\n");
+//pc.printf("prepare start\n\r");
+ int rc = iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+//pc.printf("prepare1(%5d)\n\r",rc);
+ rc = iap.erase( TARGET_SECTOR, TARGET_SECTOR );
+//pc.printf("erase(%5d)\n\r",rc);
+ rc = iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+//pc.printf("prepare2(%5d)\n\r",rc);
+ rc = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
+//pc.printf("write(%5d)\n\r",rc);
#endif
}
@@ -345,7 +353,7 @@
Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4;
}
-void Get_Gyro()
+void Get_Gyro(float interval)
{
float x,y,z;
bool err;
@@ -359,43 +367,63 @@
Gyro[ROL] = x - Gyro_Ref[0] ;
Gyro[PIT] = y - Gyro_Ref[1] ;
Gyro[YAW] = z - Gyro_Ref[2] ;
-
+/*
+ x -= Gyro_Ref[0];
+ y -= Gyro_Ref[1];
+ z -= Gyro_Ref[2];
+ float i = 3.14 * 2 * 10 * interval; LPF 10Hz
+ Gyro[ROL] += -Gyro[ROL] * i + x * i;
+ Gyro[PIT] += -Gyro[PIT] * i + y * i;
+ Gyro[YAW] += -Gyro[YAW] * i + z * i;
+*/
if ( fabs(Gyro[ROL]) > 300.0 ) Gyro[ROL] = Gyro_Save[ROL];
if ( fabs(Gyro[PIT]) > 300.0 ) Gyro[PIT] = Gyro_Save[PIT];
if ( fabs(Gyro[YAW]) > 300.0 ) Gyro[YAW] = Gyro_Save[YAW];
}
-bool Get_Accel()
+bool Get_Accel(float interval)
{
float x,y,z;
- bool err;
+ bool err;
+ Accel_Save[ROL] = Accel[ROL];
+ Accel_Save[PIT] = Accel[PIT];
+ Accel_Save[YAW] = Accel[YAW];
if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z);
else err=i2c.Acceleration(&y,&x,&z);
if ( err == false ) return false;
- float wx = x - conf.Accel_Ref[0];
- float wy = y - conf.Accel_Ref[1];
- float wz = z - conf.Accel_Ref[2];
- Accel[ROL] = atan(wx/sqrt( pow(wy,2)+pow(wz,2)))*180/3.14;
- Accel[PIT] = atan(wy/sqrt( pow(wx,2)+pow(wz,2)))*180/3.14;
- Accel[YAW] = atan(sqrt( pow(wx,2)+pow(wy,2))/wz)*180/3.14;
+// Accel[ROL] = x - conf.Accel_Ref[0];
+// Accel[PIT] = y - conf.Accel_Ref[1];
+// Accel[YAW] = z - conf.Accel_Ref[2];
+
+ x -= conf.Accel_Ref[0];
+ y -= conf.Accel_Ref[1];
+ z -= conf.Accel_Ref[2];
+// float i = 3.14 * 2 * 10 * interval; //LPF 10Hz
+// Accel[ROL] += -Accel[ROL] * i + x * i;
+// Accel[PIT] += -Accel[PIT] * i + y * i;
+// Accel[YAW] += -Accel[YAW] * i + z * i;
+
+ Accel[ROL] = atan(x/sqrt( pow(y,2)+pow(z,2)))*180/3.14;
+ Accel[PIT] = atan(y/sqrt( pow(x,2)+pow(z,2)))*180/3.14;
+// Accel[YAW] = atan(sqrt( pow(x,2)+pow(y,2))/z)*180/3.14;
return true;
}
void Get_Angle(float interval)
{
- Get_Gyro();
- Get_Accel();
float x,y,z;
+ Get_Accel(interval);
+ Get_Gyro(interval);
x = ( (Gyro[ROL] + Gyro_Save[ROL]) ) * 0.5;
y = ( (Gyro[PIT] + Gyro_Save[PIT]) ) * 0.5;
z = ( (Gyro[YAW] + Gyro_Save[YAW]) ) * 0.5;
- if ( Get_Accel() == true ) {
+ if ( Get_Accel(interval) == true ) {
float i = 3.14 * 2 * conf.Cutoff_Freq * interval;
Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval;
Angle[PIT] += -Angle[PIT] * i + Accel[PIT] * i + y * interval;
}
- else {
+ else {
Angle[ROL] += x * interval;
Angle[PIT] += y * interval;
}
@@ -442,7 +470,7 @@
}
conf.Accel_Ref[0] = k[0]/16;
conf.Accel_Ref[1] = k[1]/16;
- conf.Accel_Ref[2] = k[2]/16-9.8;
+ conf.Accel_Ref[2] = k[2]/16-1;
// FlashLED(3);
}
@@ -452,11 +480,9 @@
int i;
float gain;
- Get_Stick_Pos();
- M1 = M2 = M3 = M4 = Stick[COL];
interval = CycleTime.read();
CycleTime.reset();
- if ( interval > 0.1 ) return;
+ if ( interval > 0.2 ) return;
TotalTime += interval;
if ( TotalTime > 0.5 ) {
led1 = !led1;
@@ -466,37 +492,50 @@
}
Get_Angle(interval);
-
+ pid_interval += interval;
+ if ( pid_interval < conf.PID_Interval && Stick[GAIN] < 0 ) return;
+ Get_Stick_Pos();
+ M1 = M2 = M3 = M4 = Stick[COL];
+
for ( i=0;i<3;i++ ) {
// Stick Angle Mixing
if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
else gain = ( (float)abs(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
if ( Stick[GAIN] > 0
-// || i == YAW
-// || (fabsf)(Angle[i]) > conf.Control_Exchange_Angle
+ || i == YAW
) {
reg[i] = Stick[i] * conf.Stick_Mix[i];
reg[i] += Gyro[i] * gain * GYRO_ADJUST;
}
else {
+// reg[i] = Stick[i] * conf.Stick_Mix[i];
+// reg[i] += pid[i].calc(Angle[i],0,pid_interval) * conf.Gyro_Dir[i];
+ float x = -(float)Stick[i]*45/400;
+ reg[i] = pid[i].calc(Angle[i],x*conf.Gyro_Dir[i],pid_interval) * conf.Gyro_Dir[i];
if ( (i == YAW) || (i2c.GetAddr(ACCEL_ADDR)==0) ) {
- if ( abs(Stick_Save[i]) <= abs(Stick[i]>>2) ) {
- Stick_Save[i] = Stick[i]>>2;
+ if ( ( fabsf(Angle[i]) < fabsf(x) ) && ( fabsf(Angle[i]) > fabsf(x*0.7) ) ) { Angle[i] = 0; pid[i].reset(); }
+ if ( abs(Stick_Save[i]) <= abs(Stick[i]) ) {
+ Stick_Save[i] = Stick[i];
}
else {
-// pc.printf("PID RESET.%3d",i);wait(1);
- Stick_Save[i] = 0;
- pid[i].reset();
- Angle[i] = 0;
+ if ( (abs(Stick_Save[i]) - abs(Stick[i])) > 10 ) {
+ Stick_Save[i] = 0;
+ pid[i].reset();
+ Angle[i] = 0;
+ }
}
}
- reg[i] = Stick[i] * conf.Stick_Mix[i];
- reg[i] += pid[i].calc(Angle[i],0,interval);
-// reg[i] = pid[i].calc(Angle[i],-(float)Stick[i]*60/400,interval);
+// else {
+// if ( fabsf(Accel[i]) < 0.001 ) {
+// pid[i].reset();
+// Angle[i] = 0;
+// }
+// }
}
pid_reg[i] = reg[i];
}
+ pid_interval = 0;
//Calculate Roll Pulse Width
M1 += reg[ROL];
M2 -= reg[ROL];
@@ -519,7 +558,7 @@
{
// if ( Stick[COL] > 150 && M[i] < 150 ) M[i] = 150;
if ( M[i] > Thro_Hi ) M[i] = Thro_Hi;
- if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // this is the motor idle level
+ if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // motor idle level
}
if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
@@ -549,12 +588,13 @@
for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(0);
for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
- for ( i=0;i<4;i++ ) pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]*(float)abs(Stick[GAIN])*0.02
- ,conf.PID_Limit,conf.Integral_Limit);
+ for ( i=0;i<4;i++ ) pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]
+ ,conf.PID_Limit,conf.Differential_Limit);
Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
loop_cnt = 0;
FlyghtTime.start();
CycleTime.start();
+ pid_interval = 0;
FlashLED(5);
}
@@ -583,3 +623,5 @@
+
+