syouichi imamori
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 2:59ac9df97701
- Parent:
- 0:cca1c4e84da4
- Child:
- 3:27407c4984cf
--- a/main.cpp Tue Jul 16 06:37:28 2013 +0000
+++ b/main.cpp Fri Nov 15 20:53:36 2013 +0000
@@ -1,74 +1,139 @@
#include "mbed.h"
+#include "math.h"
#include "I2cPeripherals.h"
-//#include "I2cLCD.h"
#include "InterruptIn.h"
-//#include "ITG3200.h"
-//#include "L3GD20.h"
#include "config.h"
#include "PulseWidthCounter.h"
#include "string"
#include "SerialLcd.h"
#include "IAP.h"
-//LPC1768 Flash Memory read/write
-#define MEM_SIZE 256
-#define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768
-IAP iap;
+#include "PID.h"
+#include "SoftPWM.h"
+
+//Serial pc(USBTX, USBRX);
+
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Convert from degrees to radians.
+#define toRadians(x) (x * 0.01745329252)
+//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
+#define GYROSCOPE_GAIN (1 / 14.375)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+#define ACCELEROMETER_GAIN (0.004 * g0)
+//Updating filter at 40Hz.
+#define FILTER_RATE 0.05
+//At rest the gyroscope is centred around 0 and goes between about
+//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
+//5/15 = 0.3 degrees/sec.
-#ifdef LPCXpresso
-DigitalOut led1(P0_22);
-#define led2 led1
-#else
+#ifdef TARGET_LPC1114 // LPC1114
+// PulseOut pwm[4] = { dp1,dp2,dp18,dp24 );
+ #define LED1 dp28
+ #define p5 dp4
+ #define p6 dp9
+ #define p7 dp10
+ #define p8 dp11
+ #define p9 dp25
+ #define p10 dp26
+ #define p13 dp16
+ #define p21 dp1
+ #define p22 dp2
+ #define p23 dp18
+ #define p24 dp24
+ #define p27 dp27
+ #define p28 dp5
+#else //LPC1768
+ #ifdef LPCXpresso
+ #define LED1 P0_22
+ #endif
+#endif
+DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
DigitalOut led1(LED1);
DigitalOut led2(LED2);
+InterruptIn ch1(p5);
+PulseWidthCounter ch[5] = { p6,p7,p8,p9,p10 };
+#if defined(SOFT_PWM) || defined(TARGET_LPC1114)
+SoftPWM pwm[4] = { p21,p22,p23,p24 };
+#else
+PwmOut pwm[4] = { p21,p22,p23,p24 };
#endif
-InterruptIn ch1(p5);
-PulseWidthCounter ch[5] = { PulseWidthCounter(p6,POSITIVE),
- PulseWidthCounter(p7,POSITIVE),
- PulseWidthCounter(p8,POSITIVE),
- PulseWidthCounter(p9,POSITIVE),
- PulseWidthCounter(p10,POSITIVE)
- };
-PwmOut pwm[4] = { p21,p22,p23,p24 };
+SoftPWM buzz(p26);
Timer CurTime;
-Timer ElapTime;
+//Timer ElapTime;
+Timer CycleTime;
+Timer FlyghtTime;
+//Ticker tick;
+//Ticker tick100ms;
+//Ticker mixTime;
I2cPeripherals i2c(p28,p27); //sda scl
-#ifdef SERIAL_LCD
SerialLcd lcd(p13);
-#endif
config conf;
-int StartTime;
-int Channel = 0;
-int CH[5];
-int M[6];
-int Gyro[3];
-int Accel[3];
-int Gyro_Ref[3];
-int Accel_Ref[3];
-int Stick[5];
+PID pid[3];
+#ifdef TARGET_LPC1114
+//LPC1114 Flash Memory read/write
+ #define MEM_SIZE 256
+ #define TARGET_SECTOR 7 // use sector 29 as target sector if it is on LPC1768
+#else
+//LPC1768 Flash Memory read/write
+ #define MEM_SIZE 256
+ #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768
+#endif
+#ifndef LocalFileOut
+IAP iap;
+#endif
+float TotalTime = 0;;
+int channel = 0;
+//int intrupt_cnt = 0;
+//int intrupt_cnt2 = 0;
+volatile int CH[5];
+volatile int M[6];
+volatile float Gyro[3];
+volatile float Accel[3];
+volatile float Angle[3];
+volatile float Gyro_Ref[3];
+volatile float Gyro_Save[3];
+volatile int Stick[5];
+volatile int Stick_Save[3];
+//int Stick_Max[3];
float Press;
char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel
+//volatile bool tick_flag;
+//volatile bool buzz_flag;
+float interval;
+int pid_reg[3];
+int loop_cnt;
void initialize();
void FlashLED(int );
void SetUp();
-#ifdef SERIAL_LCD
-void SetUpPrompt(config&,SerialLcd&);
-#else
void SetUpPrompt(config&,I2cPeripherals&);
-#endif
void PWM_Out(bool);
void Get_Stick_Pos();
void CalibrateGyros(void);
void CalibrateAccel(void);
void Get_Gyro();
-void Get_Accel();
+bool Get_Accel();
+void Get_Angle(float);
void ReadConfig();
void WriteConfig();
+void Interrupt_Check(bool&,int &,DigitalOut &);
void ESC_SetUp(void);
-
+void Flight_SetUp();
+//void IMUfilter_Reset(void);
+void LCD_printf(char *);
+void LCD_cls();
+void LCD_locate(int,int);
+/*
+void tick_interrupt()
+{
+ tick_flag = true;
+}
+*/
void PulseCheck()
{
- Channel++;
+ channel++;
}
void PulseAnalysis() //Interrupt Pin5
@@ -77,10 +142,10 @@
int PulseWidth = CurTime.read_us();
CurTime.reset();
CurTime.start();
- if ( PulseWidth > 3000 ) Channel = 0; //reset pulse count
+ if ( PulseWidth > 3000 ) channel = 0; //reset pulse count
else {
if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max ) {
- switch( Channel ) {
+ switch( channel ) {
case IR_THR:
THR = PulseWidth;
break;
@@ -99,49 +164,51 @@
}
}
}
- Channel++;
+ channel++;
}
int main()
{
- int i,j=0;
+ int i=0;
+ wait(0.5);
initialize();
- wait(0.5);
+
Get_Stick_Pos();
- while ( Stick[COL] > 30 || conf.StartMode == 'C' ) //Shrottol Low
+ while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' ) //Shrottol Low
{
- if ( Stick[COL] > 350 || conf.StartMode == 'C' ) // Shrottle High
+ if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High
+ ESC_SetUp();
+ if ( Stick[COL] > 890 || conf.StartMode == 'C' ) // Shrottle High
{
-#ifdef SERIAL_LCD
- SetUpPrompt(conf,lcd);
-#else
+ loop_cnt = 0;
SetUpPrompt(conf,i2c);
-#endif
- for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
break;
}
FlashLED(3);
- wait(1);
+ wait(0.3);
Get_Stick_Pos();
}
- led2 = 1;
- ElapTime.start();
-
+ Flight_SetUp();
while (1)
{
- if ( Stick[COL] < 30 )
+ if ( Stick[COL] < Thro_Zero )
{
i = 0;
- ElapTime.stop();
- while ( Stick[YAW] < -Stick_Limit && Stick[COL] < 30 )
+ while ( Stick[YAW] < -Stick_Limit && Stick[COL] < Thro_Zero ) //Rudder Left
{
if ( i > 100 ) //wait 2 sec
{
+ FlyghtTime.stop();
+ if ( Stick[PIT] < -Stick_Limit ) { //Elevetor Down
+ loop_cnt = 0;
+ FlashLED(5);
+ for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
+ i2c.start(conf.LCD_Contrast);
+ SetUpPrompt(conf,i2c);
+ }
CalibrateGyros();
- CalibrateAccel();
- FlashLED(6);
- ElapTime.start();
+ Flight_SetUp();
break;
}
wait(0.01); // wait 10 msec
@@ -149,12 +216,6 @@
i++;
}
}
- j++;
- if (j>100) { j=0; led2 = !led2;}
- ElapTime.stop();
- wait(float(conf.PWM_Interval-ElapTime.read_us()-2)/1000000);
- ElapTime.reset();
- ElapTime.start();
PWM_Out(true);
}
@@ -162,38 +223,33 @@
void initialize()
{
-#ifndef SERIAL_LCD
- i2c.start(ST7032_ADDR,conf.LCD_Contrast);
-#endif
+ buzz.period_us(500);
ReadConfig(); //config.inf file read
-// CurTime.start();
- Channel = 0;
+
+ i2c.start(conf.LCD_Contrast);
+ channel = 0;
ch1.rise(&PulseCheck); //input pulse count
- wait(0.1);
- if ( Channel > 30 ) {
+ wait(0.2);
+ if ( channel > 50 ) {
ch1.rise(&PulseAnalysis);
InPulseMode = 'S';
}
else InPulseMode = 'P';
- if ( conf.Gyro_Type == _ITG3200 )
- i2c.start(ITG3200_ADDR0);
- else
- i2c.start(L3GD20_ADDR1);
- CalibrateGyros();
- i2c.start(LDXL345_ADDR0);
- CalibrateGyros();
- CalibrateAccel();
- i2c.start(LPS331AP_ADDR1);
+ led1 = 0;
+ CycleTime.start();
for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
+ FlashLED(3);
}
void FlashLED(int cnt)
{
for ( int i = 0 ; i < cnt ; i++ ) {
led1 = !led1;
- wait(0.05);
+ buzz = 0.5f;
+ wait(0.1);
led1 = !led1;
- wait(0.05);
+ buzz = 0.0f;
+ wait(0.1);
}
}
@@ -227,7 +283,8 @@
config *conf_ptr;
if ( sizeof(config) > 255 ) {
- i2c.printf("config size over");
+ LCD_printf("config size over");
+ wait(3);
return;
}
rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
@@ -237,14 +294,10 @@
conf_ptr = (config*)sector_start_adress[TARGET_SECTOR];
if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
-// i2c.printf("config read OK");
-// wait(1);
return;
}
}
WriteConfig();
-// i2c.printf("config write OK");
-// wait(1);
#endif
}
@@ -260,7 +313,8 @@
char *send;
int i;
if ( sizeof(config) > 255 ) {
- printf("config size over");
+ LCD_printf("config size over");
+ wait(3);
return;
}
send = (char*)&conf;
@@ -268,8 +322,10 @@
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");
#endif
}
@@ -278,6 +334,10 @@
if ( InPulseMode == 'P' ) {
for (int i=0;i<5;i++) CH[i] = ch[i].count;
}
+// Stick_Save[ROL] = Stick[ROL];
+// Stick_Save[PIT] = Stick[PIT];
+// Stick_Save[YAW] = Stick[YAW];
+
Stick[ROL] = AIL - conf.Stick_Ref[ROL];
Stick[PIT] = ELE - conf.Stick_Ref[PIT];
Stick[YAW] = RUD - conf.Stick_Ref[YAW];
@@ -287,23 +347,61 @@
void Get_Gyro()
{
- int x,y,z;
- if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
- else i2c.angular(&y,&x,&z);
- Gyro[ROL] = ( x - Gyro_Ref[0] ) / 5;
- Gyro[PIT] = ( y - Gyro_Ref[1] ) / 5;
- Gyro[YAW] = ( z - Gyro_Ref[2] ) / 5;
+ float x,y,z;
+ bool err;
+ Gyro_Save[ROL] = Gyro[ROL];
+ Gyro_Save[PIT] = Gyro[PIT];
+ Gyro_Save[YAW] = Gyro[YAW];
+
+ if ( conf.Gyro_Dir[3] ==1 ) err=i2c.angular(&x,&y,&z);
+ else err=i2c.angular(&y,&x,&z);
+ if ( err == false ) return;
+ Gyro[ROL] = x - Gyro_Ref[0] ;
+ Gyro[PIT] = y - Gyro_Ref[1] ;
+ Gyro[YAW] = z - Gyro_Ref[2] ;
+
+ 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];
}
-void Get_Accel()
+bool Get_Accel()
+{
+ float x,y,z;
+ bool err;
+ 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;
+ return true;
+}
+
+void Get_Angle(float interval)
{
- int x,y,z;
- if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
- else i2c.Acceleration(&y,&x,&z);
- Accel[ROL] = ( x - Accel_Ref[0] );
- Accel[PIT] = ( y - Accel_Ref[1] );
- Accel[YAW] = ( z - Accel_Ref[2] );
+ Get_Gyro();
+ Get_Accel();
+ float x,y,z;
+
+ 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 ) {
+ 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 {
+ Angle[ROL] += x * interval;
+ Angle[PIT] += y * interval;
+ }
+ Angle[YAW] += z * interval;
}
+
void Get_Pressure()
{
Press = i2c.pressure();
@@ -311,36 +409,41 @@
void CalibrateGyros(void)
{
- int i,j,x,y,z;
- int k[3]={0,0,0};
+ int i;
+ float x,y,z;
+ float k[3]={0,0,0};
wait(1);
+ Angle[0]=Angle[1]=Angle[2]=0;
for(i=0; i<16; i++) {
- if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
- else i2c.angular(&y,&x,&z);
+ if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
+ else i2c.angular(&y,&x,&z);
k[0] += x;
k[1] += y;
k[2] += z;
- wait(0.005);
+ wait(0.01);
}
- for( j=0; j<3; j++ ) Gyro_Ref[j] = k[j]/16;
- FlashLED(3);
+ for( i=0; i<3; i++ ) Gyro_Ref[i] = k[i]/16;
+// FlashLED(3);
}
void CalibrateAccel(void)
{
- int i,j,x,y,z;
- int k[3]={0,0,0};
- wait(1);
+ int i;
+ float x,y,z;
+ float k[3]={0,0,0};
+ conf.Accel_Ref[0]=conf.Accel_Ref[1]=conf.Accel_Ref[2]=0;
for(i=0; i<16; i++) {
- if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
- else i2c.Acceleration(&y,&x,&z);
+ if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
+ else i2c.Acceleration(&y,&x,&z);
k[0] += x;
k[1] += y;
k[2] += z;
- wait(0.005);
+ wait(0.01);
}
- for( j=0; j<3; j++ ) Accel_Ref[j] = k[j]/16;
- FlashLED(3);
+ conf.Accel_Ref[0] = k[0]/16;
+ conf.Accel_Ref[1] = k[1]/16;
+ conf.Accel_Ref[2] = k[2]/16-9.8;
+// FlashLED(3);
}
void PWM_Out(bool mode)
@@ -349,20 +452,50 @@
int i;
float gain;
-// wait(0.002);
Get_Stick_Pos();
- Get_Gyro();
-// Get_Accel();
-
M1 = M2 = M3 = M4 = Stick[COL];
- for ( i=0;i<3;i++ )
- {
+ interval = CycleTime.read();
+ CycleTime.reset();
+ if ( interval > 0.1 ) return;
+ TotalTime += interval;
+ if ( TotalTime > 0.5 ) {
+ led1 = !led1;
+ if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5f;
+ else buzz=0.0;
+ TotalTime = 0;
+ }
+
+ Get_Angle(interval);
+
+ 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];
- reg[i] = ( Stick[i] * conf.Stick_Mix[i] ) + ( Gyro[i] * gain );
-// if ( Stick[GAIN] < 0 )
-// reg[i] += Accel[i] * conf.Accel_Gain[i];
+ if ( Stick[GAIN] > 0
+// || i == YAW
+// || (fabsf)(Angle[i]) > conf.Control_Exchange_Angle
+ ) {
+ reg[i] = Stick[i] * conf.Stick_Mix[i];
+ reg[i] += Gyro[i] * gain * GYRO_ADJUST;
+ }
+ else {
+ if ( (i == YAW) || (i2c.GetAddr(ACCEL_ADDR)==0) ) {
+ if ( abs(Stick_Save[i]) <= abs(Stick[i]>>2) ) {
+ Stick_Save[i] = Stick[i]>>2;
+ }
+ else {
+// pc.printf("PID RESET.%3d",i);wait(1);
+ 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);
+ }
+ pid_reg[i] = reg[i];
}
//Calculate Roll Pulse Width
M1 += reg[ROL];
@@ -384,25 +517,69 @@
for ( i=0;i<4;i++ )
{
+// 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 (Stick[COL] < 20 ) M1=M2=M3=M4=0;
- if ( mode )
- for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+M[i]);
-
+
+ if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
+
+ if ( mode ) {
+ for ( i=0;i<4;i++ ) {
+ while ( !pwmpin[i] );
+ if ( conf.PWM_Mode == 1 )
+ pwm[i].pulsewidth_us(conf.ESC_Low+M[i]);
+ else pwm[i].pulsewidth_us(M[i]);
+ }
+ }
+
}
void ESC_SetUp(void) {
while(1) {
Get_Stick_Pos();
- for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(Stick[COL]);
- wait(0.01);
+ for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+Stick[COL]);
+ wait(0.015);
}
-};
+}
+
+void Flight_SetUp(void)
+{
+ int i;
+ 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);
+ Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
+ loop_cnt = 0;
+ FlyghtTime.start();
+ CycleTime.start();
+ FlashLED(5);
+}
+
+void LCD_locate(int clm,int row)
+{
+ lcd.locate(clm,row);
+ i2c.locate(clm,row);
+}
+
+void LCD_cls()
+{
+ lcd.cls();
+ i2c.cls();
+}
+
+void LCD_printf(char* str)
+{
+ lcd.printf(str);
+ i2c.printf(str);
+}
+;
+
+