syouichi imamori
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 6:a50e6d3924f1
- Parent:
- 5:7b02775787a9
- Child:
- 7:16bf0085d914
--- a/main.cpp Fri Oct 17 03:56:56 2014 +0000
+++ b/main.cpp Tue Feb 24 09:28:29 2015 +0000
@@ -3,13 +3,14 @@
------------+-----------+-----------+-----------+-----------+---------------------
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 | -
+ Quad-3D |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC | - | -
+ Delta |FL Tro ESC |L Alv Ser |R Alv Ser |Rud Ser | - | -
+ Delta-TW |FL Tro ESC |L Alv Ser |R Alv 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
+ F:Front, B:Back, L:Left, R:Right
+ Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Alv:Alevon
ESC:for ESC, Ser:for Servo, VP:Variable Pitch
- F:Front, B:Back, L:Left, R:Right
*/
#include "mbed.h"
@@ -33,7 +34,7 @@
#if defined(TARGET_LPC1768)
DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
// #ifdef LPCXpresso
-// #define LED1 P0_22
+ #define LED1 P0_22
// #endif
DigitalOut led1(LED1);
// DigitalOut led2(LED2);
@@ -101,6 +102,7 @@
Timer FlyghtTime;
config conf;
//PID pid[4];
+Limiter throLimit = 100;
Limiter gyroLimit[3] = {300,300,300};
Limiter accLimit[3] = {0.5,0.5,0.5};
Limiter pwmLimit[4] = {50,50,50,50};
@@ -119,12 +121,20 @@
volatile float Gyro_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
+int PWM_Init[6][6] = { 1080,1080,1080,1080,1080,1080, //Quad_X
+ 1500,1500,1500,1500,1080,1080, //Quad_VP
+ 1500,1500,1500,1500,1080,1080, //Quad_3D
1080,1500,1500,1500,1500,1080, //Delta
1080,1500,1500,1500,1080,1500, //Delta_TW
1080,1500,1500,1500,1500,1080 //AirPlane
};
+char Servo_idx[6][6] = { 0,0,0,0,0,0, //Quad_X
+ 1,1,1,1,0,0, //Quad_VP
+ 0,0,0,0,0,0, //Quad_3D
+ 0,1,1,1,1,1, //Delta
+ 0,1,1,1,0,1, //Delta_TW
+ 0,1,1,1,1,1 //AirPlane
+ };
//int Stick_Max[3];
float Press;
float Base_Press;
@@ -137,8 +147,8 @@
int loop_cnt;
float target_height;
float cuurent_height;
-float base_throttol;
-int throttol;
+float base_Throttl;
+int Throttl;
bool hov_control;
float Rdata;
@@ -162,7 +172,6 @@
void LCD_printf(char *);
void LCD_cls();
void LCD_locate(int,int);
-void Servo_Reverse(int,int);
#ifdef SOFTPWM
void Tpwm_interrupt()
{
@@ -197,7 +206,8 @@
initialize();
Get_Stick_Pos();
- while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' ) //Shrottol Low
+ while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C'
+ || ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) ) //Shrottol Low
{
// if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High
// ESC_SetUp();
@@ -207,8 +217,8 @@
SetUpPrompt(conf,i2c);
break;
}
- FlashLED(3);
- wait(0.3);
+ FlashLED(2);
+ wait(0.5);
Get_Stick_Pos();
}
Flight_SetUp();
@@ -263,6 +273,8 @@
else InPulseMode = 'P';
led1 = 0;
CycleTime.start();
+ throLimit.differential(conf.Thro_Limit_Val);
+ throLimit.rate(conf.Thro_Limit_Rate);
#ifdef SOFTPWM
Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval);
#endif
@@ -439,13 +451,9 @@
if ( err == false ) return false;
//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];
-
- Accel[ROL] = accLimit[0].calc(x);
- Accel[PIT] = accLimit[1].calc(y);
- Accel[YAW] = accLimit[2].calc(z);
+ x = accLimit[0].calc(x) - conf.Accel_Ref[0];
+ y = accLimit[1].calc(y) - conf.Accel_Ref[1];
+ z = accLimit[2].calc(z) - conf.Accel_Ref[2];
//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;
@@ -456,7 +464,7 @@
void Get_Angle(float interval)
{
float x,y,z;
- Get_Accel(interval);
+// Get_Accel(interval);
Get_Gyro(interval);
x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f;
@@ -531,7 +539,7 @@
interval = CycleTime.read();
CycleTime.reset();
- if ( interval > 0.2f ) return;
+ if ( interval > 0.005f && mode ) return;
TotalTime += interval;
if ( TotalTime > 0.5f ) {
led1 = !led1;
@@ -548,22 +556,27 @@
Get_Stick_Pos();
switch ( conf.Model_Type ) {
case Quad_X:
- M1 = M2 = M3 = M4 = Stick[COL];
+ M1 = M2 = M3 = M4 = THR + conf.Throttl_Trim;
break;
- case Quad_H:
- M1 = M2 = M3 = M4 = Stick[AUX2];
- M5 = Stick[COL];
+ case Quad_VP:
+ M1 = M2 = M3 = M4 = Stick[AUX2] + conf.Stick_Ref[COL];
+ M5 = THR + conf.Throttl_Trim;
+ break;
+ case Quad_3D:
+ i = (int)throLimit.calc((float)THR);
+ if ( abs(i-conf.Reverse_Point) > 100 ) i = THR;
+ M1 = M2 = M3 = M4 = i + conf.Throttl_Trim;
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];
+ M1 = THR + conf.Throttl_Trim;
+ M2 = conf.Stick_Ref[ROL];
+ M3 = conf.Stick_Ref[PIT];
+ M4 = conf.Stick_Ref[YAW];
if ( conf.Model_Type == AirPlane )
- M5 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL];
- else M5 = Stick[COL];
+ M5 = conf.Stick_Ref[ROL];
+ else M5 = M1;
break;
}
@@ -571,11 +584,12 @@
// 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;
+ else gain = ( (float)fabsf(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
+ if ( Stick[i] > 0 ) gain -= gain * ( fabsf(Stick[i]) / 400 ) * conf.Active_Gyro_Gain;
if ( Stick[GAIN] > 0
|| i == YAW
|| conf.Model_Type > 0
+ || i2c.GetAddr(ACCEL_ADDR) == 0
) {
reg[i] = Stick[i] * conf.Stick_Mix[i];
reg[i] += Gyro[i] * gain * GYRO_ADJUST;
@@ -589,9 +603,14 @@
}
pid_interval = 0;
+// int Reverse = 1;
switch ( conf.Model_Type ) {
- case Quad_X:
- case Quad_H:
+ case Quad_3D:
+// if ( THR < conf.Reverse_Point ) {
+// Reverse = -1;
+// }
+ case Quad_X:
+ case Quad_VP:
//Calculate Roll Pulse Width
M1 += reg[ROL];
M2 -= reg[ROL];
@@ -610,8 +629,8 @@
M3 -= reg[YAW];
M4 += reg[YAW];
break;
- case Delta:
- case Delta_TW:
+ case Delta:
+ case Delta_TW:
//Calculate Roll Pulse Width
M2 += reg[ROL];
M3 -= reg[ROL];
@@ -626,58 +645,52 @@
M5 -= reg[YAW];
}
break;
- case AirPlane:
- //Calculate Roll Pulse Width
+ case AirPlane:
+ //Calculate Roll Pulse Width
M2 -= reg[ROL];
M5 -= reg[ROL];
- //Calculate Pitch Pulse Width
+ //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++ )
+ int h = conf.Model_Type;
+ for ( int i=0; i<6; i++ ) {
+ if ( M[i] > Pulse_Max ) M[i] = Pulse_Max;
+ if ( M[i] < Pulse_Min ) M[i] = Pulse_Min;
+ if ( Servo_idx[h][i] == 1 )
{
- if ( M[i] > Thro_Hi ) M[i] = Thro_Hi;
- if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // motor idle level
+ M[i] = conf.Stick_Ref[i] + ( ( M[i] - conf.Stick_Ref[i] ) * conf.Servo_Dir[i] );
+ }
+ else {
+ if ( h == Quad_3D ) {
+ if ( Stick[GAIN] > 0 ) {
+ if ( THR < conf.Reverse_Point )
+ M[i] = conf.Reverse_Point;
+ }
+ else {
+ if ( abs(THR - conf.Reverse_Point ) < 10 )
+ M[i] = conf.Reverse_Point;
+ }
+ }
+ else {
+ // if ( Stick[COL] < Thro_Zero ) M[i] = conf.Stick_Ref[COL];
+ }
}
}
-
- 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 ) {
+ h = conf.Stick_Ref[THR];
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]);
+ pwm[i].pulsewidth_us(M[i]);
+ else pwm[i].pulsewidth_us(M[i]-conf.Stick_Ref[COL]);
}
}
}
-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) {
@@ -701,6 +714,8 @@
pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]);
}
hov_control = false;
+ throLimit.differential(conf.Thro_Limit_Val);
+ throLimit.rate(conf.Thro_Limit_Rate);
Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
loop_cnt = 0;
FlyghtTime.start();