From 9e579861a127269a3cee32cbd1e8418a59101420 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 20 Apr 2017 17:38:32 +0200
Subject: [PATCH] add guidance loop to 'new' control

all this guidance code should really be better factorized
---
 conf/modules/guidance_full_pid_fw.xml         |  2 +
 .../fixedwing/guidance/guidance_v_n.c         | 38 +++++++++++++++++++
 2 files changed, 40 insertions(+)

diff --git a/conf/modules/guidance_full_pid_fw.xml b/conf/modules/guidance_full_pid_fw.xml
index 8e03b24235b..c19b23e5850 100644
--- a/conf/modules/guidance_full_pid_fw.xml
+++ b/conf/modules/guidance_full_pid_fw.xml
@@ -75,10 +75,12 @@
   </settings>
   <header>
     <file name="guidance_v_n.h"/>
+    <file name="guidance_h.h"/>
   </header>
   <init fun="v_ctl_init()"/>
   <makefile target="ap|sim|nps" firmware="fixedwing">
     <file name="guidance_v_n.c" dir="$(SRC_FIRMWARE)/guidance"/>
+    <file name="guidance_h.c" dir="$(SRC_FIRMWARE)/guidance"/>
     <define name="CTRL_TYPE_H" value="firmwares/fixedwing/guidance/guidance_v_n.h" type="string"/>
   </makefile>
 </module>
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
index c986eee22d2..850faf3688b 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
@@ -179,6 +179,44 @@ void v_ctl_init(void)
   v_ctl_throttle_setpoint = 0;
 }
 
+void v_ctl_guidance_loop(void)
+{
+  if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) {
+    v_ctl_altitude_loop();
+  }
+#if CTRL_VERTICAL_LANDING
+  if (v_ctl_mode == V_CTL_MODE_LANDING) {
+    v_ctl_landing_loop();
+  } else {
+#endif
+    if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
+      v_ctl_throttle_setpoint = nav_throttle_setpoint;
+      v_ctl_pitch_setpoint = nav_pitch;
+    } else {
+      if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) {
+        v_ctl_climb_loop();
+      } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
+    } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
+#if CTRL_VERTICAL_LANDING
+  } /* v_ctl_mode == V_CTL_MODE_LANDING */
+#endif
+
+#if defined V_CTL_THROTTLE_IDLE
+  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
+#endif
+
+#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
+  if (vsupply > 0.) {
+    v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
+    v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
+  }
+#endif
+
+  if (autopilot.kill_throttle || (!autopilot.flight_time && !autopilot.launch)) {
+    v_ctl_throttle_setpoint = 0;
+  }
+}
+
 /**
  * outer loop
  * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode