First release.

README.md

@@ -3,6 +3,17 @@
 1. 输入ABCD
 2. 运行param，看一眼飞行品质
 3. 视情况调整一下Stab里的代码，运行Stab配平(能用根轨迹最好先用根轨迹）
-4. 运行GetTFModel算出不知道对不对的传递函数
-5. 打开TuneSim，调PID参数，调不了自行降阶
-6. 调参失败，弃疗跑路
+4. 运行ControlTest，尝试调参，分别试一下增稳前后的A阵
+5. 调参失败，修改题目
+6. 重复2-5步，直到输出看起来像正经飞机
+7. 修改Autopilot脚本
+8. 运行AutopilotSim，自行添加示波器截图
+9. 打开绘图软件，把曲线上的坑都抹平
+10. 编造报告
+
+如有问题，可以通过以下方式解决：
+
+1. ~~发issue婊作者~~
+2. ~~通过微信QQ婊作者~~
+3. ~~到教研室打作者一顿~~
+4. 请作者吃饭

SimTests/Autopilot.m

@@ -0,0 +1,49 @@
+function res=Autopilot(x,P)
+    alt=x(1);
+    dist=x(4);
+    spd=x(2);
+    hdg=x(3);
+
+    persistent state;
+    persistent lastDist;
+
+    if dist<1
+        state=1;
+        lastDist=dist;
+    end
+    res=[5000;0;0.6*P.a;state];
+    switch state
+        case 1
+            res=[5000;0;0.6*P.a;state];
+            if dist-lastDist>10000
+                state=state+1;
+                lastDist=dist;
+            end
+        case 2
+            res=[8000;0;0.6*P.a;state];
+            if alt>8000
+                state=state+1;
+                lastDist=dist;
+            end
+        case 3
+            res=[8000;0;0.8*P.a;state];
+            if dist-lastDist>15000
+                state=state+1;
+                lastDist=dist;
+            end
+        case 4
+            res=[8000;180*pi/180;0.8*P.a;state];
+            if abs(hdg-res(2))<0.5*pi/180
+                state=state+1;
+                lastDist=dist;
+            end
+        case 5
+            res=[8000;180*pi/180;0.8*P.a;state];
+            if dist-lastDist>15000
+                state=state+1;
+                lastDist=dist;
+            end
+        case 6
+            res=[8000;180*pi/180;0.8*P.a;state];
+    end
+end

SimTests/GetPosition.m

@@ -29,5 +29,9 @@
     pedot = res(2); 
     pddot = res(3);
     chi=atan2(pedot,pndot);
-    out=[pndot;pedot;pddot;Va;chi;Va*sin(theta)];    
+    if chi<-0.1*pi
+        chi=chi+2*pi;
+    end
+    Vg=norm([pndot;pedot]);
+    out=[pndot;pedot;pddot;Va;chi;Vg];    
 end

SimTests/deltaChi.m

@@ -0,0 +1,21 @@
+function res=deltaChi(x)
+    chi_c=x(1);
+    chi=x(2);
+    if chi_c<-0.1*pi
+        chi_c=chi_c+2*pi;
+    end
+    if chi<-0.1*pi
+        chi=chi+2*pi;
+    end
+%      if chi_c>=chi
+%         chi_c2=chi_c-2*pi;
+%     else
+%         chi_c2=chi_c+2*pi;
+%     end
+%     if(abs(chi_c-chi)<abs(chi_c2-chi))
+%         res = chi_c - chi;
+%     else
+%         res = chi_c2 - chi;
+%     end
+    res = chi_c - chi;
+end