[paparazzi-commits] [5781] Dialed in twisting params and did some cleanup.

[Paul Cox]

Revision: 5781
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5781
Author:   paulcox
Date:     2010-09-02 14:18:52 +0000 (Thu, 02 Sep 2010)
Log Message:
-----------
Dialed in twisting params and did some cleanup.

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

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-09-02 13:24:27 UTC 
(rev 5780)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-09-02 14:18:52 UTC 
(rev 5781)
@@ -47,7 +47,7 @@
 
 static void main_periodic(int);
 //static void main_parse_cmd_line(int argc, char *argv[]);
-static void drive_output(uint8_t last_state);
+static void drive_output(void);
 static void main_exit(int sig);
 static void main_talk_with_stm32(void);
 
@@ -61,7 +61,8 @@
 
 static uint32_t foo = 0;
 static uint8_t spi_crc_ok = 1;
-
+static uint8_t last_state = 1;
+  
 int main(int argc, char *argv[]) {
   
   (void) signal(SIGINT, main_exit);
@@ -103,7 +104,7 @@
 #define PITCH_MAGIC_NUMBER (121)
 
 static void main_periodic(int my_sig_num) {
-  static uint8_t last_state = 1;
+
 /*  static int bar=0;
   if (!(foo%2000)) {
     if (bar) {
@@ -144,7 +145,7 @@
   }
 
 
-  drive_output(last_state);
+  drive_output();
 
   control_send_messages();
 
@@ -196,7 +197,7 @@
 }
 #endif
 
-static void drive_output(uint8_t last_state) {
+static void drive_output() {
   switch (controller.armed) {
     case 0:
       if (last_state == 2) {

Modified: paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c  2010-09-02 
13:24:27 UTC (rev 5780)
+++ paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c  2010-09-02 
14:18:52 UTC (rev 5781)
@@ -69,22 +69,24 @@
 
   controller.armed = 0;
 
-  /***** Coeficients twisting ****/
-  controller.ulim = 1.0;
-  controller.Vm = 0.1; //should this now be 1/512?
+  /***** Coef twisting ****/
+  controller.ulim = 1.0;
+  controller.Vm = 0.1; //should this now be 1/512?
   controller.VM = 300.0;
 
   controller.S[1] = 0.0;
-  controller.S[0] = 0.0;
-
+  controller.S[0] = 0.0;
+
   controller.U_twt[1] = 0.0;
   controller.U_twt[0] = 0.0;
 
-  controller.satval1 = 0.176;
-  controller.satval2 = 1;
+  controller.satval1 = 0.087;
+  controller.satval2 = 0.141;
 
-  controller.c = 0.4;
+  controller.c = 4.5;
   controller.error = 0;
+  
+  printf("Vm=%f VM=%f satval1=%f satval2=%f 
c=%f\n",controller.Vm,controller.VM,controller.satval1,controller.satval2,controller.c);
 }
 
 
@@ -107,11 +109,12 @@
   controller.elevation_ddot_ref = 
-2*controller.omega_elevation_ref*controller.xi_ref*controller.elevation_dot_ref
 
     - 
controller.omega_elevation_ref*controller.omega_elevation_ref*(controller.elevation_ref
 - controller.elevation_sp); 
 
+#ifdef USE_AZIMUTH
   controller.azimuth_ref = controller.azimuth_ref + controller.azimuth_dot_ref 
* dt_ctl;
   controller.azimuth_dot_ref = controller.azimuth_dot_ref + 
controller.azimuth_ddot_ref * dt_ctl;
   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;
 
   /*
@@ -124,8 +127,10 @@
   const float err_elevation = estimator.elevation - controller.elevation_ref;
   const float err_elevation_dot = estimator.elevation_dot - 
controller.elevation_dot_ref;
 
+#ifdef USE_AZIMUTH
   const float err_azimuth = estimator.azimuth - controller.azimuth_ref;
   const float err_azimuth_dot = estimator.azimuth_dot - 
controller.azimuth_dot_ref;
+#endif
 
   /*
    *  Compute feedforward and feedback commands
@@ -141,14 +146,17 @@
   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) +
                         controller.one_over_J * (controller.omega_cl * 
controller.omega_cl * err_azimuth);
+#endif
 
   controller.cmd_pitch =  /*controller.cmd_pitch_ff*/ + 
controller.cmd_pitch_fb;
 
-  //controller.tilt_sp = controller.azim_gain * (-controller.cmd_azimuth_fb );
+#ifdef USE_AZIMUTH
+  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 = controller.cmd_thrust*(1/cos(estimator.elevation));
@@ -161,103 +169,93 @@
     //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_pitch_fb);
   }
   foo++; 
 
 }
 
 
-/*Fonction qui obtient la commande twisiting à appliquer chaque periode*/
-float get_U_twt()
+/*Fonction qui obtient la commande twisiting à appliquer chaque periode*/
+float get_U_twt()
 { 
 
-       /**Definition des constantes du modèle**/
-       const float Res = 0.4 ;
-       const double Kphi = 0.0129;
-       const double alpha = 3.2248e-7 ;
-       const float cte = 60.0  ;
+       /**Definition des constantes du modèle**/
+       const float Gain = -45;
        const float Te = 1/512.;
-
-       /**Variables utilisés par la loi de commande**/
-       static volatile float yd[2] = {0.0,0.0};
-       static volatile float y[2] = {0.,0.}; 
+
+       /**Variables utilisés par la loi de commande**/
+       static volatile float yd[2] = {0.0,0.0};
+       static volatile float y[2] = {0.,0.}; 
        //static float emax = 0.035;            // en rad, initialement 2
 
-       //Variables auxiliaires utilisés
-       static volatile int aux_y = 0;
-
-       /**Variables pour l'algorithme**/
-       float udot;
-       float sens;
-       
-        /**Acquisiton des donnes**/
-       //Acquisition consigne
-       yd[1] = controller.tilt_ref;
-       //Acquisition mesure
-       y[1] = estimator.tilt;
-
-       //On initialise au début angle courant=angle anterieur
-       if (aux_y == 0){        
-               y[0] = y[1];
-               aux_y = 1;
-       } 
-
-       /***************************/
-
-       /**Calcul Surface et derive Surface**/
-       // S[1],y[1],yd[1] new value
-       // S[0],y[0],yd[0] last value
-
-       //gain K=Te
+       /**Variables pour l'algorithme**/
+       float udot;
+       float sens;
+       
+       //Acquisition consigne
+       yd[1] = controller.tilt_ref;
+       //Acquisition mesure
+       y[1] = estimator.tilt;
+
+       /***************************/
+
+       /**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 ) ;
-       //controller.S[1] = (float)( ( controller.c * (y[1]-yd[1]) ) + 
(estimator.tilt_dot - controller.tilt_dot_ref)  );
-       controller.S_dot = (controller.S[1] - controller.S[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 ) {  
+       //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - 
estimator.tilt_dot ) * 0.8 ) ;
+       controller.S[1] = (float)( controller.c * (y[1]-yd[1]) + 
estimator.tilt_dot - controller.tilt_dot_ref );
+       controller.S_dot = (controller.S[1] - controller.S[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 ( controller.S[1] < 0.0 ) sens = -1.0;
-               else if ( controller.S[1] > 0.0 ) sens = 1.0;
-               if ( abs(controller.U_twt[1]) < controller.ulim ) {
-                       if ( (controller.S[1] * controller.S_dot) > 0) {
-                               udot = -controller.VM * sens;
-                       }
-                       else {
-                               udot = -controller.Vm * sens;
-                       }
-               }
-               else {
-                       udot = -controller.U_twt[1];
-               }
-       
-               // Integration de u, qu'avec 2 valeurs, penser à faire plus
-               // u[1] new , u[0] old
-               controller.U_twt[1] = controller.U_twt[0] + (Te * udot);
-       //}
-       /**********************/ 
-
+       } else {*/
+               /**Algorithme twisting**/
+               if ( controller.S[1] < 0.0 ) 
+                       sens = -1.0;
+               else 
+                       sens = 1.0;
+                       
+               if ( abs(controller.U_twt[1]) < controller.ulim ) {
+                       if ( (controller.S[1] * controller.S_dot) > 0) {
+                               udot = -controller.VM * sens;
+                       }
+                       else {
+                               udot = -controller.Vm * sens;
+                       }
+               }
+               else {
+                       udot = -controller.U_twt[1];
+               }
+       
+               // Integration de u, qu'avec 2 valeurs, penser à faire plus
+               // u[1] new , u[0] old
+               controller.U_twt[1] = controller.U_twt[0] + (Te * udot);
+       //}
+       /**********************/ 
+
        /**Saturation de l'integrateur**/
-
+
        if ( (controller.S[1] > -controller.satval1) && (controller.S[1] < 
controller.satval1) ){
-               
Bound(controller.U_twt[1],-controller.satval1,controller.satval1);
-       }
+               
Bound(controller.U_twt[1],-controller.satval1,controller.satval1);
+       }
        else {
-               
Bound(controller.U_twt[1],-controller.satval2,controller.satval2);
-       }        
-       /********************************/
-         
-       /**Mises à jour**/
-       controller.U_twt[0] = controller.U_twt[1];
-       yd[0] = yd[1];
-       y[0] = y[1];
-        
+               
Bound(controller.U_twt[1],-controller.satval2,controller.satval2);
+       }        
+       /********************************/
+         
+       /**Mises à jour**/
+       controller.U_twt[0] = controller.U_twt[1];
+       yd[0] = yd[1];
+       y[0] = y[1];
+        
        controller.S[0] = controller.S[1];
 
-       return -80000. * (cte * Res * alpha/(2 * Kphi) ) * controller.U_twt[1];
-
+       return Gain * controller.U_twt[1];
+
 }