Diff: Autopilot.cpp

Revision:

13:75636841e43b

Parent:

12:59e547742cd8

Child:

14:7b6c3f8b85fb

diff -r 59e547742cd8 -r 75636841e43b Autopilot.cpp
--- a/Autopilot.cpp	Mon Nov 29 13:12:47 2021 +0000
+++ b/Autopilot.cpp	Wed Dec 01 09:23:26 2021 +0000
@@ -78,13 +78,20 @@
     //TECS使用(https://docs.px4.io/master/en/flight_stack/controller_diagrams.html)， p制御のみ
     float v2 = vel_ned.x*vel_ned.x + vel_ned.y*vel_ned.y + vel_ned.z*vel_ned.z;
     float E = alt + v2 / (2*G);
-    float Esp = alt_obj + vel_obj*vel_obj / (2*G);
+    float alt_sp;
+    if (alt_obj - alt > 5.0f)
+        alt_sp = alt + 5.0f;
+    else if (alt_obj - alt < -5.0f)
+        alt_sp = alt - 5.0f;
+    else
+        alt_sp = alt_obj;
+    float Esp = alt_sp + vel_obj*vel_obj / (2*G);
     float dT_cruise = 0.0f;
     dT_obj = dT_cruise;
     dT_obj += p_control(Esp - E, 0.1f);
     
     float B = alt - v2 / (2*G);
-    float Bsp = alt_obj - vel_obj*vel_obj / (2*G);
+    float Bsp = alt_sp - vel_obj*vel_obj / (2*G);
     pitch_obj = p_control(Bsp - B, 0.05f);
 }