Example Horizontal Outerloop Control Module (#2323)

conf/airframes/tudelft/bebop_autonomous_race_2018.xml

@@ -46,6 +46,8 @@
     <module name="logger_file">
       <define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
     </module>
+
+    <module name="ctrl_module_outerloop_demo"/>
     <module name="sonar_bebop"/>
 
     <module name="jevois_mavlink"/>

conf/modules/ctrl_module_innerloop_demo.xml

@@ -11,20 +11,20 @@
   <settings>
     <dl_settings>
       <dl_settings NAME="CtrlModDemo">
-        <dl_setting var="ctrl_module_demo_pr_ff_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="pr_ff"/>
+        <dl_setting var="ctrl_module_demo_pr_ff_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_innerloop_demo" shortname="pr_ff"/>
-        <dl_setting var="ctrl_module_demo_pr_d_gain"  min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="pr_d"/>
+        <dl_setting var="ctrl_module_demo_pr_d_gain"  min="0" step="0.01" max="1" module="ctrl/ctrl_module_innerloop_demo" shortname="pr_d"/>
-        <dl_setting var="ctrl_module_demo_y_ff_gain"  min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="y_ff"/>
+        <dl_setting var="ctrl_module_demo_y_ff_gain"  min="0" step="0.01" max="1" module="ctrl/ctrl_module_innerloop_demo" shortname="y_ff"/>
-        <dl_setting var="ctrl_module_demo_y_d_gain"   min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="y_d"/>
+        <dl_setting var="ctrl_module_demo_y_d_gain"   min="0" step="0.01" max="1" module="ctrl/ctrl_module_innerloop_demo" shortname="y_d"/>
       </dl_settings>
     </dl_settings>
   </settings>
 
   <header>
-    <file name="ctrl_module_demo.h"/>
+    <file name="ctrl_module_innerloop_demo.h"/>
   </header>
 
   <makefile>
-    <file name="ctrl_module_demo.c"/>
+    <file name="ctrl_module_innerloop_demo.c"/>
   </makefile>
 
 </module>

conf/modules/ctrl_module_outerloop_demo.xml

@@ -0,0 +1,28 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ctrl_module_outerloop_demo" dir="ctrl">
+  <doc>
+    <description>
+        Demo Control Module.
+        Only for rotorcraft firmware.
+        Simple rate controler as example on how to integrate write and call your own controller in a module.
+    </description>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings NAME="CtrlModDemo">
+        <dl_setting var="comode_time" min="0" step="0.01" max="3" module="ctrl/ctrl_module_outerloop_demo" shortname="time"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+  <header>
+    <file name="ctrl_module_outerloop_demo.h"/>
+  </header>
+
+  <makefile>
+    <file name="ctrl_module_outerloop_demo.c"/>
+  </makefile>
+
+</module>
+

sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.c

@@ -19,12 +19,12 @@
  */
 
 /**
- * @file modules/ctrl/ctrl_module_demo.h
+ * @file modules/ctrl/ctrl_module_innerloop_demo.h
  * @brief example empty controller
  *
  */
 
-#include "modules/ctrl/ctrl_module_demo.h"
+#include "modules/ctrl/ctrl_module_innerloop_demo.h"
 #include "state.h"
 #include "subsystems/radio_control.h"
 #include "firmwares/rotorcraft/stabilization.h"

sw/airborne/modules/ctrl/ctrl_module_innerloop_demo.h

@@ -19,14 +19,14 @@
  */
 
 /**
- * @file modules/ctrl/ctrl_module_demo.c
+ * @file modules/ctrl/ctrl_module_innerloop_demo.c
  * @brief example empty controller
  *
  * Implements an example simple rate controller in a module.
  */
 
-#ifndef CTRL_MODULE_DEMO_H_
+#ifndef CTRL_MODULE_INNERLOOP_DEMO_H_
-#define CTRL_MODULE_DEMO_H_
+#define CTRL_MODULE_INNERLOOP_DEMO_H_
 
 #include <std.h>
 
@@ -54,4 +54,4 @@ extern void guidance_v_module_init(void);
 extern void guidance_v_module_enter(void);
 extern void guidance_v_module_run(bool in_flight);
 
-#endif /* CTRL_MODULE_DEMO_H_ */
+#endif /* CTRL_MODULE_INNERLOOP_DEMO_H_ */

sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.c

@@ -0,0 +1,87 @@
+/*
+ * 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/ctrl_module_outerloop_demo.h
+ * @brief example empty controller
+ *
+ */
+
+#include "modules/ctrl/ctrl_module_outerloop_demo.h"
+#include "state.h"
+#include "subsystems/radio_control.h"
+#include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+
+
+// Own Variables
+
+struct ctrl_module_demo_struct {
+// RC Inputs
+  struct Int32Eulers rc_sp;
+
+// Output command
+  struct Int32Eulers cmd;
+
+} ctrl;
+
+
+// Settings
+float comode_time = 0;
+
+
+////////////////////////////////////////////////////////////////////
+// Call our controller
+// Implement own Horizontal loops
+void guidance_h_module_init(void)
+{
+}
+
+void guidance_h_module_enter(void)
+{
+  // Store current heading
+  ctrl.cmd.psi = stateGetNedToBodyEulers_i()->psi;
+
+  // Convert RC to setpoint
+  stabilization_attitude_read_rc_setpoint_eulers(&rc_sp, in_flight, false, false);
+}
+
+void guidance_h_module_read_rc(void)
+{
+  stabilization_attitude_read_rc_setpoint_eulers(&rc_sp, in_flight, false, false);
+}
+
+
+void guidance_h_module_run(bool in_flight)
+{
+  // YOUR NEW HORIZONTAL OUTERLOOP CONTROLLER GOES HERE
+  // ctrl.cmd = CallMyNewHorizontalOuterloopControl(ctrl);
+  float roll = 0.0;
+  float pitch = 0.0;
+
+  ctrl.cmd.phi = ANGLE_BFP_OF_REAL(roll);
+  ctrl.cmd.theta = ANGLE_BFP_OF_REAL(pitch);
+
+  stabilization_attitude_set_rpy_setpoint_i(&(ctrl.cmd));
+  stabilization_attitude_run(in_flight);
+
+  // Alternatively, use the indi_guidance and send AbiMsgACCEL_SP to it instead of setting pitch and roll
+}
+

sw/airborne/modules/ctrl/ctrl_module_outerloop_demo.h

@@ -0,0 +1,48 @@
+/*
+ * 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/ctrl_module_outerloop_demo.c
+ * @brief example empty controller
+ *
+ * Implements an example simple horizontal outerloop controller in a module.
+ */
+
+#ifndef CTRL_MODULE_OUTERLOOP_DEMO_H_
+#define CTRL_MODULE_OUTERLOOP_DEMO_H_
+
+#include <std.h>
+
+// Settings
+extern float comode_time;
+
+// Demo with own guidance_h
+#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
+
+// But re-using an existing altitude-hold controller
+#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER
+
+// Implement own Horizontal loops
+extern void guidance_h_module_init(void);
+extern void guidance_h_module_enter(void);
+extern void guidance_h_module_read_rc(void);
+extern void guidance_h_module_run(bool in_flight);
+
+#endif /* CTRL_MODULE_OUTERLOOP_DEMO_H_ */

sw/airborne/modules/sensors/cameras/jevois_mavlink.c

@@ -39,6 +39,7 @@
 
 #include "subsystems/imu.h"
 #include "autopilot.h"
+#include "generated/modules.h"
 
 mavlink_system_t mavlink_system;