[nav] takeoff detection module

set the launch variable to true based on pitch angle

conf/modules/takeoff_detect.xml

@@ -0,0 +1,30 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="takeoff_detect" dir="nav">
+  <doc>
+    <description>
+      Fixed-wing automatic launch detection
+
+      When loaded, the pitch angle of plane is monitored, and will trigger the launch variable
+      if the angle goes over a specified value (nose up) for 2 seconds.
+      The launch variable can be reset to FALSE by pointing the nose to the ground.
+      The module disables itself within a few seconds (4s)  after detecting the launch.
+      It can be re-actiaved by hand from the GCS.
+    </description>
+    <section name="TO_DETECT">
+      <define name="TAKEOFF_DETECT_LAUNCH_PITCH" value="30." unit="deg" description="Picth angle for takeoff detection (set 'launch' to TRUE)"/>
+      <define name="TAKEOFF_DETECT_ABORT_PITCH" value="-20." unit="deg" description="Pitch angle to abort takeoff (set 'launch' to FALSE)"/>
+      <define name="TAKEOFF_DETECT_TIMER" value="2." description="Timer for takeoff detection in seconds (default 2s above pitch angle threshold)"/>
+      <define name="TAKEOFF_DETECT_DISABLE_TIMER" value="4." description="Timer for module de-activation (default 4s after the launch detection)"/>
+    </section>
+  </doc>
+  <header>
+    <file name="takeoff_detect.h"/>
+  </header>
+  <init fun="takeoff_detect_init()"/>
+  <periodic fun="takeoff_detect_periodic()" freq="20" autorun="TRUE" start="takeoff_detect_start()"/>
+  <makefile>
+    <file name="takeoff_detect.c"/>
+  </makefile>
+</module>
+

sw/airborne/modules/nav/takeoff_detect.c

@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * This file is part of paparazzi
+
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * @file modules/nav/takeoff_detect.c
+ *
+ * Automatic takeoff assistance for fixed-wing.
+ * The planes's launching can be triggered/aborted
+ * by pointing the nose up or down for a given time.
+ */
+
+#include "std.h"
+#include "modules/nav/takeoff_detect.h"
+#include "firmwares/fixedwing/autopilot.h"
+#include "state.h"
+#include "generated/modules.h" // for status and function's period
+
+/** Default pitch angle to trigger launch */
+#ifndef TAKEOFF_DETECT_LAUNCH_PITCH
+#define TAKEOFF_DETECT_LAUNCH_PITCH RadOfDeg(30.)
+#endif
+
+/** Default pitch angle to cancel launch */
+#ifndef TAKEOFF_DETECT_ABORT_PITCH
+#define TAKEOFF_DETECT_ABORT_PITCH RadOfDeg(-20.)
+#endif
+
+/** Detection timer in seconds */
+#ifndef TAKEOFF_DETECT_TIMER
+#define TAKEOFF_DETECT_TIMER 2.
+#endif
+
+/** Disable timer in seconds */
+#ifndef TAKEOFF_DETECT_DISABLE_TIMER
+#define TAKEOFF_DETECT_DISABLE_TIMER 4.
+#endif
+
+/** Takeoff detection states */
+enum takeoff_detect_state {
+  TO_DETECT_DISABLED,
+  TO_DETECT_ARMED,
+  TO_DETECT_LAUNCHING
+};
+
+/** Takeoff detection structure */
+struct takeoff_detect_struct {
+  enum takeoff_detect_state state;
+  uint32_t timer;
+};
+
+static struct takeoff_detect_struct takeoff_detect;
+
+// Init
+void takeoff_detect_init(void)
+{
+  // variable init is done in start function
+}
+
+// Start
+void takeoff_detect_start(void)
+{
+  takeoff_detect.state = TO_DETECT_ARMED; // always start periodic with ARMED state
+  takeoff_detect.timer = 0; // and reset timer
+}
+
+// Periodic
+void takeoff_detect_periodic(void)
+{
+  // Run detection state machine here
+  switch (takeoff_detect.state) {
+    case TO_DETECT_ARMED:
+      // test for "nose up" + AP in AUTO2 (+ GPS OK ? FIXME)
+      if (stateGetNedToBodyEulers_f()->theta > TAKEOFF_DETECT_LAUNCH_PITCH
+          && pprz_mode == PPRZ_MODE_AUTO2) {
+        takeoff_detect.timer++;
+      }
+      else {
+        // else reset timer
+        takeoff_detect.timer = 0;
+      }
+      // if timer is finished, start launching
+      if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_TIMER)) {
+        launch = TRUE;
+        takeoff_detect.state = TO_DETECT_LAUNCHING;
+        takeoff_detect.timer = 0;
+      }
+      break;
+    case TO_DETECT_LAUNCHING:
+      // abort if pitch goes below threshold while launching
+      if (stateGetNedToBodyEulers_f()->theta < TAKEOFF_DETECT_ABORT_PITCH
+          || pprz_mode != PPRZ_MODE_AUTO2) {
+        // back to ARMED state
+        launch = FALSE;
+        takeoff_detect.state = TO_DETECT_ARMED;
+      }
+      // increment timer and disable detection after some time
+      takeoff_detect.timer++;
+      if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_DISABLE_TIMER)) {
+        takeoff_detect.state = TO_DETECT_DISABLED;
+      }
+      break;
+    case TO_DETECT_DISABLED:
+      // stop periodic call
+      takeoff_detect_takeoff_detect_periodic_status = MODULES_STOP;
+      break;
+    default:
+      // No kidding ?!
+      takeoff_detect.state = TO_DETECT_DISABLED;
+      break;
+  }
+}
+
+

sw/airborne/modules/nav/takeoff_detect.h

@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * This file is part of paparazzi
+
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * @file modules/nav/takeoff_detect.h
+ *
+ * Automatic takeoff assistance for fixed-wing.
+ * The planes's launching can be triggered/aborted
+ * by pointing the nose up or down for a given time.
+ */
+
+#ifndef TAKEOFF_DETECT_H
+#define TAKEOFF_DETECT_H
+
+/** Init function */
+extern void takeoff_detect_init(void);
+
+/** Start function called once before periodic
+ */
+extern void takeoff_detect_start(void);
+
+/** Periodic call
+ *
+ * - can be enabled or disabled from settings
+ * - enabled by default, disable himself after launch
+ */
+extern void takeoff_detect_periodic(void);
+
+#endif
+