[paparazzi-commits] [5803] Adding twisting control of elevation.

[Paul Cox]

Revision: 5803
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5803
Author:   paulcox
Date:     2010-09-05 18:01:44 +0000 (Sun, 05 Sep 2010)
Log Message:
-----------
Adding twisting control of elevation. Works pretty well and seems to be a nice 
way to estimate the PID thrust constant as the average thrust value in twisting 
corresponds to the thrust required to keep beth at given elevation. Interesting 
resonance of the carbon tube and counterweight. Maybe changing sample/control 
period could eliminate/augment this.

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

Modified: paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c  2010-09-03 
20:14:12 UTC (rev 5802)
+++ paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c  2010-09-05 
18:01:44 UTC (rev 5803)
@@ -33,7 +33,8 @@
   controller.elevation_sp = RadOfDeg(10);
   controller.azimuth_sp = 0.;
 
-  controller.omega_tilt_ref = RadOfDeg(600);
+ // controller.omega_tilt_ref = RadOfDeg(600);
+  controller.omega_tilt_ref = RadOfDeg(1200);
   controller.omega_elevation_ref = RadOfDeg(120);
   controller.omega_azimuth_ref = RadOfDeg(60);
   controller.xi_ref = 1.;
@@ -92,17 +93,20 @@
 
 
 void control_run(void) {
-
+  static int foo=0;
   /*
    *  propagate reference
    */
   const float dt_ctl = 1./512.;
   const float thrust_constant = 40.;
+
+  //make setpoint a cosine ; for testing  
+  float new_sp = controller.tilt_sp * cos(6.28/10.*foo/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_tilt_ref*controller.xi_ref*controller.tilt_dot_ref 
-    - controller.omega_tilt_ref*controller.omega_tilt_ref*(controller.tilt_ref 
- controller.tilt_sp); 
+    - controller.omega_tilt_ref*controller.omega_tilt_ref*(controller.tilt_ref 
- new_sp); 
 
   controller.elevation_ref = controller.elevation_ref + 
controller.elevation_dot_ref * dt_ctl;
   controller.elevation_dot_ref = controller.elevation_dot_ref + 
controller.elevation_ddot_ref * dt_ctl;
@@ -115,8 +119,8 @@
   controller.azimuth_ddot_ref = 
-2*controller.omega_azimuth_ref*controller.xi_ref*controller.azimuth_dot_ref 
     - 
controller.omega_azimuth_ref*controller.omega_azimuth_ref*(controller.azimuth_ref
 - controller.azimuth_sp); 
 #endif
-  static int foo=0;
 
+
   /*
    *  calculate errors
    */
@@ -146,6 +150,7 @@
   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);
+                       
 #ifdef USE_AZIMUTH
   controller.cmd_azimuth_ff = controller.one_over_J * 
controller.azimuth_ddot_ref;
   controller.cmd_azimuth_fb = controller.one_over_J * (2 * controller.xi_cl * 
controller.omega_cl * err_azimuth_dot) +
@@ -158,7 +163,9 @@
   controller.tilt_sp = controller.azim_gain * (-controller.cmd_azimuth_fb );
 #endif
 
-  controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb 
+ thrust_constant;
+  controller.cmd_thrust = get_U_twt2();
+//  controller.cmd_thrust = controller.cmd_thrust_ff + 
controller.cmd_thrust_fb + thrust_constant;
+
   controller.cmd_thrust = controller.cmd_thrust*(1/cos(estimator.elevation));
 
   //if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
@@ -169,12 +176,13 @@
     //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("%f %f 
%f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref);
-    //printf("t: %f\n",controller.cmd_pitch_fb);
+    printf("t: %f\n",controller.cmd_thrust);
   }
   foo++; 
 
 }
 
+//TODO: replace both functions with one that takes parameters (or a structure 
of params)
 
 /*Fonction qui obtient la commande twisiting à appliquer chaque periode*/
 float get_U_twt()
@@ -259,3 +267,89 @@
        return Gain * controller.U_twt[1];
 
 }
+
+float get_U_twt2()
+{ 
+
+       /**Definition des constantes du modèle**/
+       const float Gain = 600.;
+       const float Te = 1/512.;
+
+       /**Variables utilisés par la loi de commande**/
+       static volatile float yd2[2] = {0.0,0.0};
+       static volatile float y2[2] = {0.,0.}; 
+       //static float emax = 0.035;            // en rad, initialement 2
+
+       /**Variables pour l'algorithme**/
+       float udot2;
+       float sens2;
+       static float S2[2]= {0.0,0.0};
+       static float S2_dot=0;
+       static float U2_twt[2]= {0.0,0.0};
+               
+       //Acquisition consigne
+       yd2[1] = controller.elevation_ref;
+       //Acquisition mesure
+       y2[1] = estimator.elevation;
+
+       /***************************/
+
+       /**Calcul Surface et derive Surface**/
+       // S[1],y[1],yd[1] new value
+       // S[0],y[0],yd[0] last value
+
+       //gain K=Te
+       //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - 
(y[0]-yd[0]) )  ) ;
+       //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - 
estimator.tilt_dot ) * 0.8 ) ;
+       S2[1] = (float)( controller.c * (y2[1]-yd2[1]) + 
estimator.elevation_dot - controller.elevation_dot_ref );
+       S2_dot = (S2[1] - S2[0]);
+       /*************************************/
+       
+       //On va dire que si l'erreur est d'un valeur inferieur a emax, on 
applique la commande anterieure
+/*     if ( abs(y[1] - yd[1]) < emax ) {  
+               U_twt[1] = U_twt[0];    
+       } else {*/
+               /**Algorithme twisting**/
+               if ( S2[1] < 0.0 ) 
+                       sens2 = -1.0;
+               else 
+                       sens2 = 1.0;
+                       
+               if ( abs(U2_twt[1]) < controller.ulim ) {
+                       if ( (S2[1] * S2_dot) > 0) {
+                               udot2 = -controller.VM * sens2;
+                       }
+                       else {
+                               udot2 = -controller.Vm * sens2;
+                       }
+               }
+               else {
+                       udot2 = -U2_twt[1];
+               }
+       
+               // Integration de u, qu'avec 2 valeurs, penser à faire plus
+               // u[1] new , u[0] old
+               U2_twt[1] = U2_twt[0] + (Te * udot2);
+       //}
+       /**********************/ 
+
+       /**Saturation de l'integrateur**/
+
+       if ( (S2[1] > -controller.satval1) && (S2[1] < controller.satval1) ){
+               Bound(U2_twt[1],-controller.satval1,controller.satval1);
+       }
+       else {
+               Bound(U2_twt[1],-controller.satval2,controller.satval2);
+       }        
+       /********************************/
+         
+       /**Mises à jour**/
+       U2_twt[0] = U2_twt[1];
+       yd2[0] = yd2[1];
+       y2[0] = y2[1];
+        
+       S2[0] = S2[1];
+
+       return Gain * U2_twt[1];
+
+}

Modified: paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.h  2010-09-03 
20:14:12 UTC (rev 5802)
+++ paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.h  2010-09-05 
18:01:44 UTC (rev 5803)
@@ -52,14 +52,14 @@
 
 /***Twisting stuff***/
 
-  float  S[2];
-  float S_dot;
+  float  S[2];
+  float S_dot;
   float U_twt[2];
 
-  /***** Coeficients twisting ****/
-  float ulim;
-  float Vm;
-  float VM;
+  /***** Coeficients twisting ****/
+  float ulim;
+  float Vm;
+  float VM;
 
   float satval1;
   float satval2;
@@ -67,7 +67,7 @@
   float c;
 
   float error;
-
+
 };
 
 
@@ -77,5 +77,6 @@
 extern void control_send_messages(void);
 extern void control_run(void);
 float get_U_twt(void);
+float get_U_twt2(void);
 
 #endif /* OVERO_TWIST_CONTROLLER_H */