[modules] example vertical_ctrl_module_demo

conf/modules/vertical_ctrl_module_demo.xml

@@ -0,0 +1,34 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="vertical_ctrl_module_demo" dir="ctrl">
+  <doc>
+    <description>
+        Demo Vertical Control Module.
+        Only for rotorcraft firmware.
+        Example on how to reads the sonar and directly control the thrust.
+    </description>
+    <define name="VERTICAL_CTRL_MODULE_AGL_ID" value="ABI_BROADCAST" description="Sender id of the AGL (sonar) ABI message"/>
+    <section name="VERTICAL_CTRL_MODULE" prefix="VERTICAL_CTRL_MODULE_">
+      <define name="PGAIN" value="1.0" description="P gain on height error"/>
+      <define name="IGAIN" value="0.01" description="I gain on summed height error"/>
+    </section>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings NAME="VertCtrlModDemo">
+        <dl_setting var="v_ctrl.setpoint" min="0" step="0.01" max="10" module="ctrl/vertical_ctrl_module_demo" shortname="height sp"/>
+        <dl_setting var="v_ctrl.pgain" min="0" step="0.01" max="1" module="ctrl/vertical_ctrl_module_demo" shortname="pgain" param="VERTICAL_CTRL_MODULE_PGAIN"/>
+        <dl_setting var="v_ctrl.igain" min="0" step="0.01" max="1" module="ctrl/vertical_ctrl_module_demo" shortname="igain" param="VERTICAL_CTRL_MODULE_IGAIN"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+  <header>
+    <file name="vertical_ctrl_module_demo.h"/>
+  </header>
+
+  <makefile>
+    <file name="vertical_ctrl_module_demo.c"/>
+  </makefile>
+
+</module>

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);

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)
 {

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);
 

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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @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);
+}

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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @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_ */