辣鸡数据害弱debug一天

RT
cfrpg · Jul 8, 2018 · 6154f3a · 6154f3a
1 parent d9e3229
commit 6154f3a
Show file tree

Hide file tree

Showing 13 changed files with 254 additions and 18 deletions.
diff --git a/SimTests/ControlSim.slx b/SimTests/ControlSim.slx
diff --git a/SimTests/ControlTest.slx b/SimTests/ControlTest.slx
diff --git a/SimTests/GetPID.asv b/SimTests/GetPID.asv
@@ -0,0 +1,24 @@
+PID.P_de_theta=10;
+PID.I_de_theta=0;
+PID.D_de_theta=3;
+PID.K_theta=0.4657; %DC gain
+
+PID.P_h_de=0.1;
+PID.I_h_de=0;
+PID.D_h_de=0;
+
+PID.P_da_phi=0.4;
+PID.I_da_phi=0;
+PID.D_da_phi=0.1;
+
+PID.P_chi_phi=4;
+PID.I_chi_phi=0;
+PID.D_chi_phi=0;
+
+PID.P_dr_v=0.3;
+PID.I_dr_v=0;
+PID.D_dr_v=0.02;
+
+PID.P_dt_u=0.01;
+PID.I_dt_u=0.002;
+PID.D_dt_u=0.003;
diff --git a/SimTests/GetPID.m b/SimTests/GetPID.m
@@ -1,11 +1,11 @@
 PID.P_de_theta=1.5;
 PID.I_de_theta=0;
 PID.D_de_theta=0.3;
-PID.K_theta=0.4657; %DC gain
+PID.K_theta=0.6; %DC gain
 
-PID.P_h_de=0.1;
+PID.P_h_de=0.3;
 PID.I_h_de=0;
-PID.D_h_de=0.02;
+PID.D_h_de=0.1;
 
 PID.P_da_phi=0.4;
 PID.I_da_phi=0;
@@ -15,10 +15,10 @@
 PID.I_chi_phi=0;
 PID.D_chi_phi=0;
 
-PID.P_dr_v=0;
+PID.P_dr_v=0.3;
 PID.I_dr_v=0;
-PID.D_dr_v=0;
+PID.D_dr_v=0.02;
 
-PID.P_dt_u=0.1;
-PID.I_dt_u=0.02;
-PID.D_dt_u=0.03;
+PID.P_dt_u=0.01;
+PID.I_dt_u=0.002;
+PID.D_dt_u=0.003;
diff --git a/SimTests/GetPosition.asv b/SimTests/GetPosition.asv
@@ -0,0 +1,33 @@
+function out=GetPosition(x,P)
+    phi=x(1);
+    theta=x(2);
+    psi=x(3);
+    Du=x(4);
+    v=x(5);
+    w=x(6);
+
+    u=Du+P.u0;
+    Va=norm([u,v,w]);
+    roll=[
+        1 0 0;
+        0 cos(phi) sin(phi);
+        0 -sin(phi) cos(phi)];
+    pitch=[
+        cos(theta), 0, -sin(theta);
+        0, 1, 0;
+        sin(theta), 0, cos(theta)];
+    yaw=[
+        cos(psi), sin(psi), 0;
+        -sin(psi), cos(psi), 0;
+        0, 0, 1];
+    R=[cos(theta)*cos(psi),sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi),sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);
+        cos(theta)*sin(psi),sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi),cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
+        -sin(theta),sin(phi)*cos(theta),cos()
+    R=roll*pitch*yaw;
+    res=R'*[u;v;w];
+    pndot = res(1);    
+    pedot = res(2); 
+    pddot = res(3);
+    chi=atan2(pedot,pndot);
+    out=[pndot;pedot;pddot;Va;chi;Va*sin(theta)];    
+end
diff --git a/SimTests/GetPosition.m b/SimTests/GetPosition.m
@@ -0,0 +1,33 @@
+function out=GetPosition(x,P)
+    phi=x(1);
+    theta=x(2);
+    psi=x(3);
+    Du=x(4);
+    v=x(5);
+    w=x(6);
+
+    u=Du+P.u0;
+    Va=norm([u,v,w]);
+    roll=[
+        1 0 0;
+        0 cos(phi) sin(phi);
+        0 -sin(phi) cos(phi)];
+    pitch=[
+        cos(theta), 0, -sin(theta);
+        0, 1, 0;
+        sin(theta), 0, cos(theta)];
+    yaw=[
+        cos(psi), sin(psi), 0;
+        -sin(psi), cos(psi), 0;
+        0, 0, 1];
+    R=[cos(theta)*cos(psi),sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi),sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);
+        cos(theta)*sin(psi),sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi),cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
+        -sin(theta),sin(phi)*cos(theta),cos(phi)*cos(theta)];
+    %R=roll*pitch*yaw;
+    res=R*[u;v;w];
+    pndot = res(1);    
+    pedot = res(2); 
+    pddot = res(3);
+    chi=atan2(pedot,pndot);
+    out=[pndot;pedot;pddot;Va;chi;Va*sin(theta)];    
+end
diff --git a/SimTests/GetState.m b/SimTests/GetState.m
@@ -1,5 +1,5 @@
 function out = GetState(x,P)
-    nx      = x(1)
+    nx      = x(1);
     nz      = x(2);
     ny      = x(3);
     alpha   = x(4);
@@ -18,8 +18,26 @@
     pn      = x(15);
     pe      = x(16);
     pd      = x(17);
+    u=P.u0+du;
+    Va=norm([u,v,w]);
+     roll=[
+        1 0 0;
+        0 cos(phi) sin(phi);
+        0 -sin(phi) cos(phi)];
+    pitch=[
+        cos(theta), 0, -sin(theta);
+        0, 1, 0;
+        sin(theta), 0, cos(theta)];
+    yaw=[
+        cos(psi), sin(psi), 0;
+        -sin(psi), cos(psi), 0;
+        0, 0, 1];
 
-    Va=norm([u+P.u0,v,w]);
+    R=roll*pitch*yaw;
+    res=R'*[u;v;w];
+    pndot = res(1);    
+    pedot = res(2); 
+    chi=atan2(pedot,pndot);
 
-    out=[nx;ny;nz;p;q;r;phi;theta;psi;du;v;w;u;Va;pn;pe;pd];
+    out=[nx;ny;nz;p;q;r;phi;theta;psi;du;v;w;u;Va;pn;pe;-pd+P.h0;chi];
 end
diff --git a/SimTests/TuneSim.slx b/SimTests/TuneSim.slx
diff --git a/SimTests/gps.asv b/SimTests/gps.asv
@@ -0,0 +1,86 @@
+function [sys,X0,str,ts,simStateCompliance] = gps(t,x,u,flag,P)
+    switch flag
+        case 0 %initialize
+            [sys,X0,str,ts,simStateCompliance]=mdlInitializeSizes(P);
+        case 1 %derivatives
+            sys=mdlDerivatives(t,x,u,P);
+        case 2 %update
+            sys=mdlUpdate(t,x,u);
+        case 3 %output
+            sys=mdlOutputs(t,x,u);
+        case 4 %get time of next var hit
+            sys=mdlGetTimeOfNextVarHit(t,x,u);
+        case 9 %terminate
+            sys=mdlTerminate(t,x,u);
+        otherwise
+            DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));     
+    end
+end
+
+function [sys,X0,str,ts,simStateCompliance]=mdlInitializeSizes(P)
+    sizes = simsizes;
+
+    sizes.NumContStates  = 3;
+    sizes.NumDiscStates  = 0;
+    sizes.NumOutputs     = 3;
+    sizes.NumInputs      = 9;
+    sizes.DirFeedthrough = 0;
+    sizes.NumSampleTimes = 1;   % at least one sample time is needed
+
+    sys = simsizes(sizes);    
+    X0  = P.x0;
+    str = [];
+
+    ts  = [0 0];
+
+    simStateCompliance = 'UnknownSimState';
+end
+
+function sys=mdlDerivatives(t,x,uu)
+    u=uu(1);
+    w=uu(2);
+    q=uu(3);
+    theta=uu(4);
+    v=uu(5);
+    p=uu(6);
+    r=uu(7);
+    phi=uu(8);
+    psi=uu(9);
+
+    roll=[
+        1 0 0;
+        0 cos(phi) sin(phi);
+        0 -sin(phi) cos(phi)];
+    pitch=[
+        cos(theta), 0, -sin(theta);
+        0, 1, 0;
+        sin(theta), 0, cos(theta)];
+    yaw=[
+        cos(psi), sin(psi), 0;
+        -sin(psi), cos(psi), 0;
+        0, 0, 1];
+
+    R=roll*pitch*yaw;
+    res=R'*[u;v;w];
+    pndot = res(1);    
+    pedot = res(2); 
+    pddot = res(3);
+    sys = [pndot; pedot; pddot];
+end
+
+function sys=mdlUpdate(t,x,u)
+    sys=[];
+end
+
+function sys=mdlOutputs(t,x,u)
+    sys=x;
+end
+
+function sys=mdlGetTimeOfNextVarHit(t,x,u)
+    sampleTime=1;
+    sys=t+sampleTime;
+end
+
+function sys=mdlTerminate(t,x,u)
+    sys=[];
+end
diff --git a/SimTests/gps.m b/SimTests/gps.m
@@ -3,7 +3,7 @@
         case 0 %initialize
             [sys,X0,str,ts,simStateCompliance]=mdlInitializeSizes(P);
         case 1 %derivatives
-            sys=mdlDerivatives(t,x,u,P.mass,P.Jx,P.Jy,P.Jz,P.Jxz);
+            sys=mdlDerivatives(t,x,u,P);
         case 2 %update
             sys=mdlUpdate(t,x,u);
         case 3 %output
@@ -36,8 +36,36 @@
     simStateCompliance = 'UnknownSimState';
 end
 
-function sys=mdlDerivatives(t,x,uu)
-
+function sys=mdlDerivatives(t,x,uu,P)
+    u=uu(1);
+    w=uu(2);
+    q=uu(3);
+    theta=uu(4);
+    v=uu(5);
+    p=uu(6);
+    r=uu(7);
+    phi=uu(8);
+    psi=uu(9);
+    u=u+P.u0;
+    roll=[
+        1 0 0;
+        0 cos(phi) sin(phi);
+        0 -sin(phi) cos(phi)];
+    pitch=[
+        cos(theta), 0, -sin(theta);
+        0, 1, 0;
+        sin(theta), 0, cos(theta)];
+    yaw=[
+        cos(psi), sin(psi), 0;
+        -sin(psi), cos(psi), 0;
+        0, 0, 1];
+
+    R=roll*pitch*yaw;
+    res=R'*[u;v;w];
+    pndot = res(1);    
+    pedot = res(2); 
+    pddot = res(3);
+
     sys = [pndot; pedot; pddot];
 end
 

diff --git a/Stab.m b/Stab.m
@@ -9,7 +9,7 @@
     R_lon(4)];
 B_stab=B_lon(:,1);
 gain=place(A_lon,B_stab,R_dsr);
-gain(1)=0;
+%gain(1)=0;
 A_lon_stab=A_lon-B_stab*gain;
 A_stab=[A_lon_stab,zeros(4,5);zeros(5,4),A_lat];
 [~,R_lon_stab]=eig(A_lon_stab);

diff --git a/drawplane.m b/drawplane.m
@@ -9,11 +9,11 @@ function drawplane(ps)
 
     persistent hplane;
     persistent hcontrol;
-    scale=2000;  %plane scale
-    field=1000;  %field scale
+    scale=3000;  %plane scale
+    field=10000;  %field scale
     [Vplane,Fplane,Cplane]=planemodel;
     Vplane=Vplane*scale;   
-    Vplane=transform(Vplane',phi,theta,psi,pn,pe,pd);
+    Vplane=transform(Vplane',phi,theta,psi,pn,pe,-pd);
 
     R=[        
         0 1 0;

diff --git a/param.m b/param.m
@@ -1,3 +1,12 @@
+% A=[-0.0153803,0.0439192,-7.83906,-9.80253,0,0,0,0,0;...
+%     -0.0438131,-0.12042,20.3051,-0.378441,0,0,0,0,0;...
+%     0.00123923,-0.0022159,-0.403074,0,0,0,0,0,0;...
+%     0,0,1.0,0,0,0,0,0,0;...
+%     0,0,0,0,-0.456379,7.83906,-203.051,9.80253,0;...
+%     0,0,0,0,-0.137907,-2.99727,2.13345,0,0;...
+%     0,0,0,0,0.0863470,0.0124572,-0.735554,0,0;...
+%     0,0,0,0,0,1,0.0386064,0,0;...
+%     0,0,0,0,0,0,1.00074,0,0];
 A=[-0.0153803,0.0439192,-7.83906,-9.80253,0,0,0,0,0;...
     -0.0438131,0.012042,203.051,-0.378441,0,0,0,0,0;...
     0.00123923,-0.0022159,-0.403074,0,0,0,0,0,0;...
@@ -38,5 +47,10 @@
 X0=[0;0;0;0;0;0;0;0;0];         %init
 %X0=[0;17.7459;0;0;0;0;0;0;0];  %alpha=5deg
 %X0=[0;0;0;0;17.7327;0;0;0;0];  %beta=5deg
+
+P.u0=180;
+P.h0=0;
+P.x0=[0,0,-P.h0];
+
 MainWork;