[paparazzi-commits] [5228] example NLI controller

[antoine drouin]

Revision: 5228
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5228
Author:   poine
Date:     2010-08-05 13:55:45 +0000 (Thu, 05 Aug 2010)
Log Message:
-----------
example NLI controller

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/beth/overo_controller.c
    paparazzi3/trunk/sw/airborne/beth/overo_controller.h

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-05 
13:14:39 UTC (rev 5227)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-05 
13:55:45 UTC (rev 5228)
@@ -1,6 +1,7 @@
 #include "overo_controller.h"
 
 #include "overo_estimator.h"
+#include "std.h"
 
 struct OveroController controller;
 
@@ -9,13 +10,38 @@
   controller.kd = 0.01;
 
   controller.tilt_sp = 0.;
+  
+  controller.omega_ref = RadOfDeg(200);
+  controller.xi_ref = 1.;
 
+  controller.tilt_ref = estimator.tilt;
+  controller.tilt_dot_ref = estimator.tilt_dot;
+  controller.tilt_ddot_ref = 0.;
+
+  controller.one_over_J = 1.;
+
+  controller.omega_cl = RadOfDeg(500);
+  controller.xi_cl = 1.;
+
+  controller.cmd_ff = 0.;
+  controller.cmd_fb = 0.;
   controller.cmd = 0.;
 }
 
 
 
 void control_run(void) {
+
+  /*
+   *  propagate reference
+   */
+  const float dt_ctl = 1./512.;
+  controller.tilt_ref = controller.tilt_ref + controller.tilt_dot_ref * dt_ctl;
+  controller.tilt_dot_ref = controller.tilt_dot_ref + controller.tilt_ddot_ref 
* dt_ctl;
+  controller.tilt_ddot_ref = 
-2*controller.omega_ref*controller.xi_ref*controller.tilt_dot_ref 
+    - 2*controller.omega_ref*controller.omega_ref*(controller.tilt_ref - 
controller.tilt_sp); 
+
+#if 0
   static int foo=0;
 
   float track_err = estimator.tilt - controller.tilt_sp;
@@ -25,5 +51,16 @@
   controller.cmd = pcmd + dcmd;
   //if (!(foo%100)) printf("%f %f\n",pcmd,dcmd);
   foo++;
+#else
+
+  const float err_tilt = estimator.tilt - controller.tilt_ref;
+  const float err_tilt_dot = estimator.tilt_dot - controller.tilt_dot_ref;
+  controller.cmd_ff = controller.one_over_J*controller.tilt_ddot_ref;
+  controller.cmd_fb = controller.one_over_J*( 
+                                            
-2*controller.xi_cl*controller.omega_cl*err_tilt_dot
+                                            
-controller.omega_cl*controller.omega_cl*err_tilt);
+  controller.cmd = controller.cmd_ff + controller.cmd_fb; 
+  
+#endif
 }
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-05 
13:14:39 UTC (rev 5227)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-05 
13:55:45 UTC (rev 5228)
@@ -6,7 +6,23 @@
   float kd;
   
   float tilt_sp;
+  
+  /* modele de reference */
+  float tilt_ref;
+  float tilt_dot_ref;
+  float tilt_ddot_ref;
+  float omega_ref;
+  float xi_ref;
 
+  /* invert control law parameter */
+  float one_over_J;
+
+  /* closed loop parameters */
+  float omega_cl;
+  float xi_cl;
+
+  float cmd_ff;
+  float cmd_fb;
   float cmd;
 };