Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen
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); }