syouichi imamori
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 4:4060309b9cc0
- Parent:
- 3:27407c4984cf
- Child:
- 5:7b02775787a9
--- a/main.cpp Thu Feb 13 16:07:07 2014 +0000
+++ b/main.cpp Tue Oct 14 08:15:03 2014 +0000
@@ -1,3 +1,17 @@
+/* PWM Output
+ type | M1 | M2 | M3 | M4 | M5 | M6
+ ------------+-----------+-----------+-----------+-----------+---------------------
+ Quad-X |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC | - | -
+ Quad-H |FL VP Ser |FR VP Ser |BL VP Ser |BR VP Ser |Thr ESC | -
+ Delta |FL Tro ESC |L Ale Ser |R Ale Ser |Rud Ser | - | -
+ Delta-TW |FL Tro ESC |L Ale Ser |R Ale Ser |Rud Ser |FR Thr ESC | -
+ Airplane |Thr ESC |Ail Ser |Ele Ser |Rud Ser |Ail Ser | -
+
+ Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Ale:Alevon
+ ESC:for ESC, Ser:for Servo, VP:Variable Pitch
+ F:Front, B:Back, L:Light, R:Right
+*/
+
#include "mbed.h"
#include "math.h"
#include "I2cPeripherals.h"
@@ -6,107 +20,127 @@
#include "PulseWidthCounter.h"
#include "string"
#include "SerialLcd.h"
-#include "IAP.h"
-#include "PID.h"
+//#include "PID.h"
#include "SoftPWM.h"
+#include "PulseOut.h"
+#include "Limiter.h"
+#ifndef TARGET_NUCLEO_F401RE
+ #include "IAP.h"
+#endif
//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.
+#if defined(TARGET_LPC1768)
+ DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
+// #ifdef LPCXpresso
+// #define LED1 P0_22
+// #endif
+ DigitalOut led1(LED1);
+// DigitalOut led2(LED2);
+ InterruptIn ch1(p5);
+ PulseWidthCounter ch[6] = { p6,p7,p8,p9,p10,p11 };
+ PwmOut pwm[6] = { p21,p22,p23,p24,p25,p26 };
+// SoftPWM pwm[6] = { p21,p22,p23,p24,p25,p26 };
+ SoftPWM buzz(p20);
+ I2cPeripherals i2c(p28,p27); //sda scl
+ SerialLcd lcd(p13,p14);
+ #define MEM_SIZE 256
+ #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768
+ IAP iap;
+#elif defined(TARGET_NUCLEO_F401RE)
+ DigitalOut led1(LED1);
+ InterruptIn ch1(PC_2);
+// PulseWidthCounter ch[6] = { PA_0,PA_1,PA_4,PB_0,PC_1,PC_0 };
+ PulseWidthCounter ch[6] = { A0,A1,A2,A3,A4,A5 };
+ PwmOut pwm[6] = { D8,D9,D10,D11,D12,D14 };
+// PwmOut pwm[6] = { D2,D3,D4,D5,D6,D7 };
+// SoftPWM pwm[6] = { PB_3,PB_4,PB_10,PC_6,PB_6,PA_7 };
+ SoftPWM buzz(PB_13);
+// I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
+ I2cPeripherals i2c(D5,D7); //sda scl
+ SerialLcd lcd(PA_11,PA_12);
+ #define MEM_SIZE 256
+ #define EXTERNAL_EEPROM //24AAXX/24LCXX/24FCXX EEPROM
+#elif defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
+ DigitalInOut pwmpin[] = { P0_14,P0_2,P0_23,P0_17 };
+ DigitalOut led1(P0_21);
+// DigitalOut led2(P0_21);
+ InterruptIn ch1(P0_9);
+ PulseWidthCounter ch[5] = { P0_8,P0_10,P0_7,P0_22,P1_15 };
+// SoftPWM pwm[4] = { P0_14,P0_2,P0_23,P0_17 };
+ PulseOut pwm[6] = { P0_14,P0_2,P0_23,P0_17,p0_20,p015 };
+ Ticker Tpwm;
+ #define SOFTPWM
+ SoftPWM buzz(P1_19);
+ I2cPeripherals i2c(P0_5,P0_4); //sda scl
+ SerialLcd lcd(P0_19,P0_18);
+ #define MEM_SIZE 256
+ #define TARGET_EEPROM_ADDRESS 64
+// #define EXTERNAL_EEPROM
+ #define INTERNAL_EEPROM
+ IAP iap;
+#elif defined(TARGET_LPC1114) // LPC1114
+ DigitalInOut pwmpin[] = { dp1,dp2,dp18,dp24 };
+ DigitalOut led1(dp28);
+ InterruptIn ch1(dp4);
+ PulseWidthCounter ch[5] = { dp9,dp10,dp11,dp13,dp26 };
+// SoftPWM pwm[4] = { dp1,dp2,dp18,dp24 };
+ PulseOut pwm[6] = { dp1,dp2,dp18,dp24,dp17,dp6 };
+ Ticker Tpwm;
+ #define SOFTPWM
+ SoftPWM buzz(dp25);
+ I2cPeripherals i2c(dp5,dp27); //sda scl
+ SerialLcd lcd(dp16,dp15);
+ #define MEM_SIZE 256
+ #define EXTERNAL_EEPROM
+#endif
-#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 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
- #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
-SoftPWM buzz(p26);
Timer CurTime;
//Timer ElapTime;
Timer CycleTime;
Timer FlyghtTime;
-//Ticker tick;
-//Ticker tick100ms;
-//Ticker mixTime;
-I2cPeripherals i2c(p28,p27); //sda scl
-SerialLcd lcd(p13);
config conf;
-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
+//PID pid[4];
+Limiter gyroLimit[3] = {300,300,300};
+Limiter accLimit[3] = {0.5,0.5,0.5};
+Limiter pwmLimit[4] = {50,50,50,50};
+//PID height;
float TotalTime = 0;;
int channel = 0;
-//int intrupt_cnt = 0;
-//int intrupt_cnt2 = 0;
-int volatile CH[5];
+int Signal[9] = { _THR,_AIL,_ELE,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4 };
+volatile int CH[9];
volatile int M[6];
volatile float Gyro[3];
-volatile float Accel[3];
-volatile float Accel_Save[3];
+volatile float Accel[3]= {0,0,0};
+volatile float Accel_Angle[3];
+volatile float Accel_Save[3]= {0,0,0};
volatile float Angle[3];
volatile float Gyro_Ref[3];
volatile float Gyro_Save[3];
-volatile int Stick[5];
-volatile int Stick_Save[3];
+volatile int Stick[6];
+volatile int Stick_Save[6];
+int PWM_Init[5][6] = { 1080,1080,1080,1080,1080,1080, //Quad_X
+ 1500,1500,1500,1500,1080,1080, //Quad_H
+ 1080,1500,1500,1500,1500,1080, //Delta
+ 1080,1500,1500,1500,1080,1500, //Delta_TW
+ 1080,1500,1500,1500,1500,1080 //AirPlane
+ };
//int Stick_Max[3];
float Press;
+float Base_Press;
char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel
//volatile bool tick_flag;
//volatile bool buzz_flag;
-float interval;
+volatile float interval;
float pid_interval;
-int pid_reg[3];
+//int pid_reg[4];
int loop_cnt;
+float target_height;
+float cuurent_height;
+float base_throttol;
+int throttol;
+bool hov_control;
+float Rdata;
void initialize();
void FlashLED(int );
@@ -128,18 +162,19 @@
void LCD_printf(char *);
void LCD_cls();
void LCD_locate(int,int);
-/*
-void tick_interrupt()
+void Servo_Reverse(int,int);
+#ifdef SOFTPWM
+void Tpwm_interrupt()
{
- tick_flag = true;
+ for ( int i=0; i<4; i++ ) pwm[i].start();
}
-*/
-void PulseCheck()
+#endif
+void PulseCheck() //cppm信号のチェック
{
channel++;
}
-void PulseAnalysis() //Interrupt Pin5
+void PulseAnalysis() //cppm信号の解析
{
CurTime.stop();
int PulseWidth = CurTime.read_us();
@@ -147,25 +182,9 @@
CurTime.start();
if ( PulseWidth > 3000 ) channel = 0; //reset pulse count
else {
- if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max ) {
- switch( channel ) {
- case IR_THR:
- THR = PulseWidth;
- break;
- case IR_AIL:
- AIL = PulseWidth;
- break;
- case IR_ELE:
- ELE = PulseWidth;
- break;
- case IR_RUD:
- RUD = PulseWidth;
- break;
- case IR_AUX:
- AUX = PulseWidth;
- break;
- }
- }
+ if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max && channel < 10 ) {
+ CH[Signal[channel-1]] = PulseWidth;
+ }
}
channel++;
}
@@ -180,8 +199,8 @@
Get_Stick_Pos();
while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' ) //Shrottol Low
{
- if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High
- ESC_SetUp();
+// if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High
+// ESC_SetUp();
if ( Stick[COL] > 890 || conf.StartMode == 'C' ) // Shrottle High
{
loop_cnt = 0;
@@ -206,7 +225,10 @@
if ( Stick[PIT] < -Stick_Limit ) { //Elevetor Down
loop_cnt = 0;
FlashLED(5);
- for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
+
+ for ( int x=0; x<6; x++ ) {
+ pwm[x].pulsewidth_us(PWM_Init[conf.Model_Type][x]);
+ }
i2c.start(conf.LCD_Contrast);
SetUpPrompt(conf,i2c);
}
@@ -227,9 +249,10 @@
void initialize()
{
buzz.period_us(400);
+ i2c.start(conf.LCD_Contrast);
+ for ( int i=0;i<6;i++ ) pwm[i].pulsewidth_us(0);
ReadConfig(); //config.inf file read
- i2c.start(conf.LCD_Contrast);
channel = 0;
ch1.rise(&PulseCheck); //input pulse count
wait(0.2);
@@ -240,7 +263,10 @@
else InPulseMode = 'P';
led1 = 0;
CycleTime.start();
- for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
+#ifdef SOFTPWM
+ Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval);
+#endif
+ Base_Press = (float)i2c.pressure() / 4096;
FlashLED(3);
}
@@ -282,15 +308,40 @@
#else
char *send;
char *recv;
- int i,rc;
+ int i;
config *conf_ptr;
- if ( sizeof(config) > 255 ) {
+ if ( sizeof(config) > MEM_SIZE ) {
LCD_printf("config size over");
wait(3);
return;
}
- rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
+//#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
+#if defined(INTERNAL_EEPROM) || defined(EXTERNAL_EEPROM)
+ char buf[MEM_SIZE];
+ #if defined(INTERNAL_EEPROM)
+ iap.read_eeprom( (char*)TARGET_EEPROM_ADDRESS, buf, MEM_SIZE );
+ #else
+ //External Flash Memory Wreite
+ short pos = 0;
+ if ( i2c.read_EEPROM(pos,buf,MEM_SIZE) != 0 ) {
+ while(1) {
+ FlashLED(3);
+ wait(0.5);
+return;
+ }
+ }
+ #endif
+ send = buf;
+ recv = (char*)&conf;
+ conf_ptr = (config*)buf;
+ if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
+ for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
+ return;
+ }
+
+#else
+ int rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
if ( rc == SECTOR_NOT_BLANK ) {
send = sector_start_adress[TARGET_SECTOR];
recv = (char*)&conf;
@@ -300,6 +351,7 @@
return;
}
}
+#endif
WriteConfig();
#endif
}
@@ -315,25 +367,27 @@
char mem[MEM_SIZE];
char *send;
int i;
-//pc.printf("start\n\r");
- if ( sizeof(config) > 255 ) {
+ if ( sizeof(config) > MEM_SIZE ) {
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;
-//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);
+//#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
+#if defined(INTERNAL_EEPROM)
+ iap.write_eeprom( mem, (char*)TARGET_EEPROM_ADDRESS, MEM_SIZE );
+#elif defined(EXTERNAL_EEPROM)
+//External Flash Memory Wreite
+ short pos = 0;
+ i2c.write_EEPROM( pos,mem,MEM_SIZE) ;
+#else
+ iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+ iap.erase( TARGET_SECTOR, TARGET_SECTOR );
+ iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+ iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
+#endif
#endif
}
@@ -345,12 +399,14 @@
// Stick_Save[ROL] = Stick[ROL];
// Stick_Save[PIT] = Stick[PIT];
// Stick_Save[YAW] = Stick[YAW];
+// Stick_Save[COL] = Stick[COL];
Stick[ROL] = AIL - conf.Stick_Ref[ROL];
Stick[PIT] = ELE - conf.Stick_Ref[PIT];
Stick[YAW] = RUD - conf.Stick_Ref[YAW];
Stick[COL] = THR - conf.Stick_Ref[COL];
Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4;
+ Stick[AUX2] = AX2 - conf.Stick_Ref[COL];
}
void Get_Gyro(float interval)
@@ -364,49 +420,37 @@
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] ;
-/*
- 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];
+ Gyro[ROL] = gyroLimit[0].calc(x) - Gyro_Ref[0] ;
+ Gyro[PIT] = gyroLimit[1].calc(y) - Gyro_Ref[1] ;
+ Gyro[YAW] = gyroLimit[2].calc(z) - Gyro_Ref[2] ;
+//pc.printf("%6.1f,%6.1f\r\n",x,Gyro[ROL]);
+
}
bool Get_Accel(float interval)
{
float x,y,z;
bool err;
- Accel_Save[ROL] = Accel[ROL];
- Accel_Save[PIT] = Accel[PIT];
- Accel_Save[YAW] = Accel[YAW];
+// Accel_Save[ROL] = Accel_Angle[ROL];
+// Accel_Save[PIT] = Accel_Angle[PIT];
+// Accel_Save[YAW] = Accel_Angle[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;
-// Accel[ROL] = x - conf.Accel_Ref[0];
-// Accel[PIT] = y - conf.Accel_Ref[1];
-// Accel[YAW] = z - conf.Accel_Ref[2];
+//pc.printf("%6.4f,%6.4f,%6.4f\r\n",x,y,z);
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;
+ Accel[ROL] = accLimit[0].calc(x);
+ Accel[PIT] = accLimit[1].calc(y);
+ Accel[YAW] = accLimit[2].calc(z);
+//pc.printf("%6.4f,%6.4f,%6.4f\r\n",Accel[ROL],Accel[PIT],Accel[YAW]);
+ Accel[ROL] = atan(x/sqrtf( powf(y,2)+powf(z,2)))*180/3.14f;
+ Accel[PIT] = atan(y/sqrtf( powf(x,2)+powf(z,2)))*180/3.14f;
+ Accel[YAW] = atan(sqrtf( powf(x,2)+powf(y,2))/z)*180/3.14f;
+ return true;
}
void Get_Angle(float interval)
@@ -415,9 +459,9 @@
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;
+ x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f;
+ y = ( Gyro[PIT] + Gyro_Save[PIT] ) * 0.5f;
+ z = ( Gyro[YAW] + Gyro_Save[YAW] ) * 0.5f;
if ( Get_Accel(interval) == true ) {
float i = 3.14 * 2 * conf.Cutoff_Freq * interval;
Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval;
@@ -428,12 +472,15 @@
Angle[PIT] += y * interval;
}
Angle[YAW] += z * interval;
+//pc.printf("%6.1f,%6.1f,%6.3f\r\n",Angle[ROL],Gyro[ROL],Accel[ROL]);
}
void Get_Pressure()
{
- Press = i2c.pressure();
-}
+ float P = (float)i2c.pressure()/4096;
+// Press = 153.8 * ( T + 273.2 ) * ( 1.0 - ( P / Base_Press ) ^ 0.1902f );
+ Press = 8.43f * ( P - Base_Press );
+}
void CalibrateGyros(void)
{
@@ -471,109 +518,167 @@
conf.Accel_Ref[0] = k[0]/16;
conf.Accel_Ref[1] = k[1]/16;
conf.Accel_Ref[2] = k[2]/16-1;
+// conf.Accel_Ref[2] = k[2]/16;
// FlashLED(3);
}
void PWM_Out(bool mode)
{
- int reg[3];
+ int reg[3];
int i;
float gain;
+// float cur_height;
interval = CycleTime.read();
CycleTime.reset();
- if ( interval > 0.2 ) return;
+ if ( interval > 0.2f ) return;
TotalTime += interval;
- if ( TotalTime > 0.5 ) {
+ if ( TotalTime > 0.5f ) {
led1 = !led1;
- if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5f;
+ if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5;
else buzz=0.0;
TotalTime = 0;
}
Get_Angle(interval);
pid_interval += interval;
- if ( pid_interval < conf.PID_Interval && Stick[GAIN] < 0 ) return;
+ if ( (pid_interval < (float)conf.PWM_Interval/1000000) && (Stick[GAIN] < 0) ) return;
+ pid_interval = 0;
+
Get_Stick_Pos();
- M1 = M2 = M3 = M4 = Stick[COL];
-
+ switch ( conf.Model_Type ) {
+ case Quad_X:
+ M1 = M2 = M3 = M4 = Stick[COL];
+ break;
+ case Quad_H:
+ M1 = M2 = M3 = M4 = Stick[AUX2];
+ M5 = Stick[COL];
+ break;
+ case Delta:
+ case Delta_TW:
+ case AirPlane:
+ M1 = Stick[COL];
+ M2 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL];
+ M3 = conf.Stick_Ref[PIT] - conf.Stick_Ref[COL];
+ M4 = conf.Stick_Ref[YAW] - conf.Stick_Ref[COL];
+ if ( conf.Model_Type == AirPlane )
+ M5 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL];
+ else M5 = Stick[COL];
+ break;
+ }
+
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 ( fabsf(Stick[i]) > 0 ) gain -= gain * ( fabsf(Stick[i]) / 500 ) * conf.Active_Jyro_Gain;
if ( Stick[GAIN] > 0
- || i == YAW
+ || i == YAW
+ || conf.Model_Type > 0
) {
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 ( ( 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 {
- if ( (abs(Stick_Save[i]) - abs(Stick[i])) > 10 ) {
- Stick_Save[i] = 0;
- pid[i].reset();
- Angle[i] = 0;
- }
- }
- }
-// else {
-// if ( fabsf(Accel[i]) < 0.001 ) {
-// pid[i].reset();
-// Angle[i] = 0;
-// }
-// }
+ reg[i] = ( Angle[i]*conf.Gyro_Dir[i]*400/50 + (float)Stick[i] ) * conf.Stick_Mix[i];
+ reg[i] += Gyro[i] * gain * GYRO_ADJUST;
}
- pid_reg[i] = reg[i];
+
+// pid_reg[i] = reg[i];
}
pid_interval = 0;
- //Calculate Roll Pulse Width
- M1 += reg[ROL];
- M2 -= reg[ROL];
- M3 -= reg[ROL];
- M4 += reg[ROL];
+
+ switch ( conf.Model_Type ) {
+ case Quad_X:
+ case Quad_H:
+ //Calculate Roll Pulse Width
+ M1 += reg[ROL];
+ M2 -= reg[ROL];
+ M3 -= reg[ROL];
+ M4 += reg[ROL];
- //Calculate Pitch Pulse Width
- M1 += reg[PIT];
- M2 += reg[PIT];
- M3 -= reg[PIT];
- M4 -= reg[PIT];
+ //Calculate Pitch Pulse Width
+ M1 += reg[PIT];
+ M2 += reg[PIT];
+ M3 -= reg[PIT];
+ M4 -= reg[PIT];
- //Calculate Yaw Pulse Width
- M1 -= reg[YAW];
- M2 += reg[YAW];
- M3 -= reg[YAW];
- M4 += reg[YAW];
-
- 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; // motor idle level
+ //Calculate Yaw Pulse Width
+ M1 -= reg[YAW];
+ M2 += reg[YAW];
+ M3 -= reg[YAW];
+ M4 += reg[YAW];
+ break;
+ case Delta:
+ case Delta_TW:
+ //Calculate Roll Pulse Width
+ M2 += reg[ROL];
+ M3 -= reg[ROL];
+ //Calculate Pitch Pulse Width
+ M2 += reg[PIT];
+ M3 += reg[PIT];
+ //Calculate Yaw Pulse Width
+ M4 -= reg[YAW];
+ //Calculate Yaw Pulse Width
+ if ( conf.Model_Type == Delta_TW ) {
+ M1 += reg[YAW];
+ M5 -= reg[YAW];
+ }
+ break;
+ case AirPlane:
+ //Calculate Roll Pulse Width
+ M2 -= reg[ROL];
+ M5 -= reg[ROL];
+ //Calculate Pitch Pulse Width
+ M3 += reg[PIT];
+ //Calculate Yaw Pulse Width
+ M4 -= reg[YAW];
+ break;
+ }
+ if ( conf.Model_Type != AirPlane ) {
+ for ( i=0;i<4;i++ )
+ {
+ if ( M[i] > Thro_Hi ) M[i] = Thro_Hi;
+ if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // motor idle level
+ }
}
- if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
-
+ switch ( conf.Model_Type ) {
+ case Quad_X:
+ if ( Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
+ break;
+ case Quad_H:
+ if ( Stick[COL] < Thro_Zero ) M5=0;
+ Servo_Reverse(1,4);
+ break;
+ case Delta:
+ case Delta_TW:
+ if ( Stick[COL] < Thro_Zero ) M1=M5=0;
+ Servo_Reverse(2,4);
+ break;
+ case AirPlane:
+ Servo_Reverse(2,5);
+ break;
+ }
if ( mode ) {
- for ( i=0;i<4;i++ ) {
- while ( !pwmpin[i] );
- if ( conf.PWM_Mode == 1 )
+ for ( i=0;i<6;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]);
+ else pwm[i].pulsewidth_us(M[i]);
}
}
}
+void Servo_Reverse(int start,int end) {
+ for ( int i=start-1; i<end; i++ ) {
+ if ( conf.Servo_Dir[i] == -1 )
+ M[i] = 1500 + ( ( conf.Stick_Ref[COL] + M[i] - 1500 ) * conf.Servo_Dir[i] ) - conf.Stick_Ref[COL];
+ }
+}
+
void ESC_SetUp(void) {
while(1) {
Get_Stick_Pos();
@@ -585,16 +690,23 @@
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]
- ,conf.PID_Limit,conf.Differential_Limit);
+#ifdef SOFTPWM
+ Tpwm.detach();
+ Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval);
+#else
+ for ( i=0;i<6;i++ ) pwm[i].pulsewidth_us(0);
+ for ( i=0;i<6;i++ ) pwm[i].period_us(conf.PWM_Interval);
+#endif
+ for ( i=0; i<6; i++ ) {
+ pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]);
+ }
+ hov_control = false;
Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
loop_cnt = 0;
FlyghtTime.start();
CycleTime.start();
pid_interval = 0;
+ Stick_Save[COL] = Stick[COL];
FlashLED(5);
}
@@ -625,3 +737,9 @@
+
+
+
+
+
+