[paparazzi-commits] [5270] improved constants for better response

[Paul Cox]

Revision: 5270
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5270
Author:   paulcox
Date:     2010-08-06 18:01:48 +0000 (Fri, 06 Aug 2010)
Log Message:
-----------
improved constants for better response

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

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-06 17:52:25 UTC 
(rev 5269)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-06 18:01:48 UTC 
(rev 5270)
@@ -122,7 +122,7 @@
   
estimator_run(msg_in.bench_sensor.z,msg_in.bench_sensor.y,msg_in.bench_sensor.x);
  
 
-  controller.elevation_sp = ((int32_t)(foo/512.)%2) ? RadOfDeg(-15) : 
RadOfDeg(0);
+  controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : 
RadOfDeg(10);
 
   control_run();
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-06 
17:52:25 UTC (rev 5269)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-06 
18:01:48 UTC (rev 5270)
@@ -14,7 +14,7 @@
   controller.elevation_sp = 0.;
 
   controller.omega_tilt_ref = RadOfDeg(200);
-  controller.omega_elevation_ref = RadOfDeg(50);
+  controller.omega_elevation_ref = RadOfDeg(120);
   controller.xi_ref = 1.;
 
   controller.tilt_ref = estimator.tilt;
@@ -25,9 +25,10 @@
   controller.elevation_dot_ref = estimator.elevation_dot;
   controller.elevation_ddot_ref = 0.;
 
-  controller.one_over_J = 1.;
+  controller.one_over_J = 2.;
+  controller.mass = 5.;
 
-  controller.omega_cl = RadOfDeg(500);
+  controller.omega_cl = RadOfDeg(600);
   controller.xi_cl = 1.;
 
   controller.cmd_pitch_ff = 0.;
@@ -47,7 +48,7 @@
   /*
    *  propagate reference
    */
-  const float dt_ctl = 1./500.;
+  const float dt_ctl = 1./512.;
   const float thrust_constant = 40.;
   
   controller.tilt_ref = controller.tilt_ref + controller.tilt_dot_ref * dt_ctl;
@@ -72,17 +73,18 @@
   controller.cmd_pitch_fb = 
controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_tilt_dot) +
                        
controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_tilt);
 
-  controller.cmd_thrust_ff = 
controller.one_over_J*controller.elevation_ddot_ref;
-  controller.cmd_thrust_fb = 
-controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_elevation_dot)
 -
-                       
controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_elevation);
+  controller.cmd_thrust_ff = controller.mass*controller.elevation_ddot_ref;
+  controller.cmd_thrust_fb = 
-controller.mass*(2*controller.xi_cl*controller.omega_cl*err_elevation_dot) -
+                       
controller.mass*(controller.omega_cl*controller.omega_cl*err_elevation);
 
   controller.cmd_pitch =  controller.cmd_pitch_ff + controller.cmd_pitch_fb; 
   controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb 
+ thrust_constant;
+
   if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
 
   if (!(foo%100)) {
     //printf("pitch : ff:%f fb:%f (%f)\n",controller.cmd_pitch_ff, 
controller.cmd_pitch_fb,estimator.tilt_dot);
-    printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, 
controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot);
+    //printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, 
controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot);
     //printf("%f %f 
%f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref);
   }
   foo++; 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-06 
17:52:25 UTC (rev 5269)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-06 
18:01:48 UTC (rev 5270)
@@ -23,6 +23,7 @@
 
   /* invert control law parameter */
   float one_over_J;
+  float mass;
 
   /* closed loop parameters */
   float omega_cl;