diff --git a/conf/modules/vertical_ctrl_module_demo.xml b/conf/modules/vertical_ctrl_module_demo.xml
new file mode 100644
index 00000000000..d5bbe005094
--- /dev/null
+++ b/conf/modules/vertical_ctrl_module_demo.xml
@@ -0,0 +1,34 @@
+
+
+
+
+
+ Demo Vertical Control Module.
+ Only for rotorcraft firmware.
+ Example on how to reads the sonar and directly control the thrust.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index c6d74dae38e..aa756bccc84 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -187,6 +187,10 @@ void guidance_v_init(void)
gv_adapt_init();
+#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
+ guidance_v_module_init();
+#endif
+
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "VERT_LOOP", send_vert_loop);
register_periodic_telemetry(DefaultPeriodic, "TUNE_VERT", send_tune_vert);
diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.c b/sw/airborne/modules/ctrl/ctrl_module_demo.c
index f76145a0e36..a86bc71713d 100644
--- a/sw/airborne/modules/ctrl/ctrl_module_demo.c
+++ b/sw/airborne/modules/ctrl/ctrl_module_demo.c
@@ -102,6 +102,11 @@ void guidance_h_module_run(bool_t in_flight)
ctrl_module_run(in_flight);
}
+void guidance_v_module_init(void)
+{
+ // initialization of your custom vertical controller goes here
+}
+
// Implement own Vertical loops
void guidance_v_module_enter(void)
{
diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.h b/sw/airborne/modules/ctrl/ctrl_module_demo.h
index bea86124445..67d20c03dd4 100644
--- a/sw/airborne/modules/ctrl/ctrl_module_demo.h
+++ b/sw/airborne/modules/ctrl/ctrl_module_demo.h
@@ -50,6 +50,7 @@ extern void guidance_h_module_read_rc(void);
extern void guidance_h_module_run(bool_t in_flight);
// Implement own Vertical loops
+extern void guidance_v_module_init(void);
extern void guidance_v_module_enter(void);
extern void guidance_v_module_run(bool_t in_flight);
diff --git a/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.c b/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.c
new file mode 100644
index 00000000000..352e721cc28
--- /dev/null
+++ b/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2015
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file modules/ctrl/vertical_control_module.h
+ * @brief example vertical controller
+ *
+ */
+
+#include "modules/ctrl/vertical_ctrl_module_demo.h"
+
+#include "generated/airframe.h"
+#include "paparazzi.h"
+#include "subsystems/abi.h"
+#include "firmwares/rotorcraft/stabilization.h"
+
+/* Default sonar/agl to use */
+#ifndef VERTICAL_CTRL_MODULE_AGL_ID
+#define VERTICAL_CTRL_MODULE_AGL_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(VERTICAL_CTRL_MODULE_AGL_ID)
+
+#ifndef VERTICAL_CTRL_MODULE_PGAIN
+#define VERTICAL_CTRL_MODULE_PGAIN 1.0
+#endif
+
+#ifndef VERTICAL_CTRL_MODULE_IGAIN
+#define VERTICAL_CTRL_MODULE_IGAIN 0.01
+#endif
+
+static abi_event agl_ev; ///< The altitude ABI event
+
+/// Callback function of the ground altitude
+static void vertical_ctrl_agl_cb(uint8_t sender_id __attribute__((unused)), float distance);
+
+struct VerticalCtrlDemo v_ctrl;
+
+
+void vertical_ctrl_module_init(void);
+void vertical_ctrl_module_run(bool_t in_flight);
+
+void vertical_ctrl_module_init(void)
+{
+ v_ctrl.agl = 0.0f;
+ v_ctrl.setpoint = 1.0f;
+ v_ctrl.pgain = VERTICAL_CTRL_MODULE_PGAIN;
+ v_ctrl.igain = VERTICAL_CTRL_MODULE_IGAIN;
+ v_ctrl.sum_err = 0.0f;
+
+ // Subscribe to the altitude above ground level ABI messages
+ AbiBindMsgAGL(VERTICAL_CTRL_MODULE_AGL_ID, &agl_ev, vertical_ctrl_agl_cb);
+}
+
+
+void vertical_ctrl_module_run(bool_t in_flight)
+{
+ if (!in_flight) {
+ // Reset integrators
+ v_ctrl.sum_err = 0;
+ stabilization_cmd[COMMAND_THRUST] = 0;
+ } else {
+ int32_t nominal_throttle = 0.5 * MAX_PPRZ;
+ float err = v_ctrl.setpoint - v_ctrl.agl;
+ int32_t thrust = nominal_throttle + v_ctrl.pgain * err + v_ctrl.igain * v_ctrl.sum_err;
+ Bound(thrust, 0, MAX_PPRZ);
+ stabilization_cmd[COMMAND_THRUST] = thrust;
+ v_ctrl.sum_err += err;
+ }
+}
+
+static void vertical_ctrl_agl_cb(uint8_t sender_id, float distance)
+{
+ v_ctrl.agl = distance;
+}
+
+
+////////////////////////////////////////////////////////////////////
+// Call our controller
+void guidance_v_module_init(void)
+{
+ vertical_ctrl_module_init();
+}
+
+void guidance_v_module_enter(void)
+{
+ // reset integrator
+ v_ctrl.sum_err = 0.0f;
+}
+
+void guidance_v_module_run(bool_t in_flight)
+{
+ vertical_ctrl_module_run(in_flight);
+}
diff --git a/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.h b/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.h
new file mode 100644
index 00000000000..9e0bdb3d71f
--- /dev/null
+++ b/sw/airborne/modules/ctrl/vertical_ctrl_module_demo.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2015
+ *
+ * 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
+ * .
+ */
+
+/**
+ * @file modules/ctrl/vertical_ctrl_module_demo.c
+ * @brief example vertical
+ *
+ * Implements an example vertical controller in a module.
+ */
+
+#ifndef VERTICAL_CTRL_MODULE_DEMO_H_
+#define VERTICAL_CTRL_MODULE_DEMO_H_
+
+#include "std.h"
+
+struct VerticalCtrlDemo {
+ float agl;
+ float setpoint;
+ float pgain;
+ float igain;
+ float sum_err;
+};
+
+extern struct VerticalCtrlDemo v_ctrl;
+
+// for example use the standard horizontal (hover) mode
+#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_HOVER
+
+// and own guidance_v
+#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
+
+// Implement own Vertical loops
+extern void guidance_v_module_init(void);
+extern void guidance_v_module_enter(void);
+extern void guidance_v_module_run(bool_t in_flight);
+
+#endif /* VERTICAL_CTRL_MODULE_DEMO_H_ */