[ctrl_module] example

conf/modules/ctrl_module_demo.xml

@@ -5,13 +5,16 @@
     <description>
         Demo Control Module
 
-        Attitude controller with own code
+        Rate-controller as module sample
     </description>
   </doc>
   <settings>
     <dl_settings>
     <dl_settings NAME="CtrlModDemo">
-        <dl_setting var="ctrl_module_demo_gain" min="0" step="1" max="1" module="ctrl/ctrl_module_demo" shortname="gain"/>
+      <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_d_gain"  min="0" step="0.01" max="1" module="ctrl/ctrl_module_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_d_gain"   min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="y_d"/>
     </dl_settings>    </dl_settings>
   </settings>
 
@@ -19,8 +22,7 @@
     <file name="ctrl_module_demo.h"/>
   </header>
 
-
+  <makefile>
-  <makefile target="ap">
     <file name="ctrl_module_demo.c"/>
   </makefile>
 

sw/airborne/modules/ctrl/ctrl_module_demo.c

@@ -26,16 +26,80 @@
  */
 
 #include "modules/ctrl/ctrl_module_demo.h"
+#include "state.h"
+#include "subsystems/radio_control.h"
+#include "firmwares/rotorcraft/stabilization.h"
 
+struct ctrl_module_demo_struct {
+  int rc_x;
+  int rc_y;
+  int rc_z;
+  int rc_t;
 
-void my_ctrl_init(void)
+} ctrl_module_demo;
+
+float ctrl_module_demo_pr_ff_gain = 0.2f;  // Pitch/Roll
+float ctrl_module_demo_pr_d_gain = 0.1f;
+float ctrl_module_demo_y_ff_gain = 0.4f;   // Yaw
+float ctrl_module_demo_y_d_gain = 0.05f;
+
+void ctrl_module_init(void);
+void ctrl_module_run(bool_t in_flight);
+
+void ctrl_module_init(void)
 {
-
+  ctrl_module_demo.rc_x = 0;
+  ctrl_module_demo.rc_y = 0;
+  ctrl_module_demo.rc_z = 0;
+  ctrl_module_demo.rc_t = 0;
 }
-void my_ctrl_run(void)
-{
 
+// Old-fashened rate control without reference model nor attitude
+void ctrl_module_run(bool_t in_flight)
+{
+  if (!in_flight) {
+    // Reset integrators
+    stabilization_cmd[COMMAND_ROLL] = 0;
+    stabilization_cmd[COMMAND_PITCH] = 0;
+    stabilization_cmd[COMMAND_YAW] = 0;
+    stabilization_cmd[COMMAND_THRUST] = 0;
+  } else {
+    stabilization_cmd[COMMAND_ROLL]   = ctrl_module_demo.rc_x * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->p *
+                                        ctrl_module_demo_pr_d_gain;
+    stabilization_cmd[COMMAND_PITCH]  = ctrl_module_demo.rc_y * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->q *
+                                        ctrl_module_demo_pr_d_gain;
+    stabilization_cmd[COMMAND_YAW]    = ctrl_module_demo.rc_z * ctrl_module_demo_y_ff_gain - stateGetBodyRates_i()->r *
+                                        ctrl_module_demo_y_d_gain;
+    stabilization_cmd[COMMAND_THRUST] = ctrl_module_demo.rc_t;
+  }
 }
 
 
 
+////////////////////////////////////////////////////////////////////
+// Call our controller
+// Implement own Horizontal loops
+void guidance_h_module_enter(void)
+{
+  ctrl_module_init();
+}
+void guidance_h_module_read_rc(void)
+{
+  // -MAX_PPRZ to MAX_PPRZ
+  ctrl_module_demo.rc_t = radio_control.values[RADIO_THROTTLE];
+  ctrl_module_demo.rc_x = radio_control.values[RADIO_ROLL];
+  ctrl_module_demo.rc_y = radio_control.values[RADIO_PITCH];
+  ctrl_module_demo.rc_z = radio_control.values[RADIO_YAW];
+}
+
+void guidance_h_module_run(bool_t in_flight)
+{
+  // Call full inner-/outerloop / horizontal-/vertical controller:
+  ctrl_module_run(in_flight);
+}
+
+// Implement own Horizontal loops
+inline void guidance_v_module_enter(void) {}
+inline void guidance_v_module_run(UNUSED bool_t in_flight) {}
+
+

sw/airborne/modules/ctrl/ctrl_module_demo.h

@@ -30,30 +30,27 @@
 
 #include <std.h>
 
-// Demo Controller Module
-extern void my_ctrl_init(void);
-extern void my_ctrl_run(void);
 // Settings
-extern int ctrl_module_demo_gain;
+extern float ctrl_module_demo_pr_ff_gain;  // Pitch/Roll
+extern float ctrl_module_demo_pr_d_gain;
+extern float ctrl_module_demo_y_ff_gain;   // Yaw
+extern float ctrl_module_demo_y_d_gain;
 
 
-
+// Demo with own guidance_h
-
-
-// Vertical loop re-uses Alt-hold
-#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
-
-// Horizontal mode is a specific controller
 #define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
 
-// Implement own Horizontal loops
+// and own guidance_v
-inline void guidance_h_module_enter(void) { my_ctrl_init();}
+#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
-inline void guidance_h_module_read_rc(void) {}
-inline void guidance_h_module_run(bool_t in_flight) {my_ctrl_run();}
 
 // Implement own Horizontal loops
-inline void guidance_v_module_enter(void){}
+extern void guidance_h_module_enter(void);
-inline void guidance_v_module_run(bool_t in_flight){}
+extern void guidance_h_module_read_rc(void);
+extern void guidance_h_module_run(bool_t in_flight);
+
+// Implement own Horizontal loops
+extern void guidance_v_module_enter(void);
+extern void guidance_v_module_run(bool_t in_flight);
 
 
 #endif /* HOVER_STABILIZATION_H_ */