[paparazzi-commits] [6272] finishing off diff.

[Paul Cox]

Revision: 6272
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6272
Author:   paulcox
Date:     2010-10-26 16:26:41 +0000 (Tue, 26 Oct 2010)
Log Message:
-----------
finishing off diff. flatness control. using sin funcs to drive lissajous 
patterns on azimuth/elevation (pretending these are x and z cartesian for now). 
Not able to send SFB DOWNLINK message so using TWIST instead as workaround. 
Next need to look at param values (for now 'au pif') and generate more 
interesting trajectories.

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

Modified: paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c    2010-10-26 
14:08:06 UTC (rev 6271)
+++ paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c    2010-10-26 
16:26:41 UTC (rev 6272)
@@ -12,15 +12,15 @@
 
 struct OveroController _CO;
 
-#define GAIN (2.)
+#define GAIN (RadOfDeg(15))
 
-static float z2=0, z3=-GAIN, z4=0;
+static float z0=0, z1=GAIN, z2=0, z3=-GAIN, z4=0;
+//static float x2=0, x3=-GAIN, x4=0;
+static float x0=GAIN, x1=0, x2=-GAIN, x3=0, x4=0;
 
 void control_send_messages(void) {
 
-  RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER_SFB(gcs_com.udp_transport,
-                       &_CO.tilt_sp,&z2,&z3,&z4);});
-
+  RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER_TWIST(gcs_com.udp_transport, 
&z0, &z1, &z2, &z3);});
 }
 
 
@@ -31,11 +31,11 @@
   _CO.azimuth_sp = 0.;
 
   _CO.tilt_ref = 0.;
-  _CO.elevation_ref = 0;
+  _CO.elevation_ref = 0.;
   _CO.azimuth_ref = 0.;
 
   _CO.tilt_dot_ref = 0.;
-  _CO.elevation_dot_ref = 0;
+  _CO.elevation_dot_ref = 0.;
   _CO.azimuth_dot_ref = 0.;
 
   _CO.cmd_sfb_pitch = 0.;
@@ -48,18 +48,20 @@
   _CO.cmd_thrust = 0.;
       
   _CO.a = 0.03;//theoretical=19.62
-  _CO.b = 0.27;//theoretical=157.21
-  _CO.u_t_ref = 40;
+  _CO.a = 0.06;
+  //_CO.b = 0.27;//theoretical=157.21
+  _CO.b = 0.86;
+  _CO.u_t_ref = 70;
   
   /*omegas - natural frequencies*/
-  _CO.o_tilt = RadOfDeg(100);
+  _CO.o_tilt = RadOfDeg(300);//was 100
   _CO.o_elev = RadOfDeg(100);
   _CO.o_azim = RadOfDeg(100);
 
   /*zetas - damping ratios*/
-  _CO.z_tilt = 1;
-  _CO.z_elev = 1;
-  _CO.z_azim = 1;  
+  _CO.z_tilt = 1.;
+  _CO.z_elev = 1.;
+  _CO.z_azim = 1.;  
 
   _CO.armed = 0;
 }
@@ -76,7 +78,7 @@
 
   calc_sfb_cmd();
   
-  _CO.cmd_pitch = _CO.cmd_sfb_pitch ;//+ _CO.cmd_df_pitch;
+  _CO.cmd_pitch = _CO.cmd_sfb_pitch + _CO.cmd_df_pitch;
   _CO.cmd_thrust = _CO.cmd_sfb_thrust + _CO.cmd_df_thrust;
 
   if (!(foo%100)) {
@@ -96,21 +98,32 @@
 
   const float dt = 1./512.;
   const float g = 9.8;
-  const float freq = 1./(2. * 3.14159);
-  float x2,x3,x4;
-  const float const1 = 9.8/20.;
-  const float const2 = 1.;
+  const float freq1 = 1./(2. * 3.14159);
+  const float freq2 = 1./(1. * 3.14159);
+  const float const1 = 9.8/75.;
+  const float const2 = 0.04;
   
   if (_CO.armed)
     time = timecnt++ * dt;
   
-  x2 = 0.;
-  x3 = 0.;
-  x4 = 0.;
+/*  x2 = x2 + x3*dt;
+  x3 = x3 + x4*dt;*/
+  //x4 = GAIN*sin (2 * 3.14159 * freq2 * time);
+  x0 = GAIN*cos (2 * 3.14159 * freq2 * time); 
+  x1 = -GAIN*sin (2 * 3.14159 * freq2 * time); 
+  x2 = -GAIN*cos (2 * 3.14159 * freq2 * time); 
+  x3 = GAIN*sin (2 * 3.14159 * freq2 * time); 
+  x4 = GAIN*cos (2 * 3.14159 * freq2 * time);  
   
+/*  z0 = z0 + z1*dt ;
+  z1 = z1 + z2*dt ;
   z2 = z2 + z3*dt ; 
-  z3 = z3 + z4*dt ;
-  z4 = GAIN*sin (2 * 3.14159 * freq * time); 
+  z3 = z3 + z4*dt ;*/
+  z0 = GAIN*sin (2 * 3.14159 * freq1 * time);
+  z1 = GAIN*cos (2 * 3.14159 * freq1 * time);
+  z2 = -GAIN*sin (2 * 3.14159 * freq1 * time);
+  z3 = -GAIN*cos (2 * 3.14159 * freq1 * time);
+  z4 = GAIN*sin (2 * 3.14159 * freq1 * time); 
 
   _CO.cmd_df_thrust = (1/const1) * sqrt( powf( x2,2) + powf( (z2 + g) ,2) ) ;
   _CO.cmd_df_pitch  = (1/const2) * 
@@ -126,12 +139,17 @@
    *  calculate errors
    */
 
-  const float err_tilt = estimator.tilt - _CO.tilt_ref;
-  const float err_tilt_dot = estimator.tilt_dot - _CO.tilt_dot_ref;
+/*  const float err_tilt = estimator.tilt - _CO.tilt_ref;
+  const float err_tilt_dot = estimator.tilt_dot - _CO.tilt_dot_ref;*/
+  const float err_tilt = estimator.tilt - x0;
+  const float err_tilt_dot = estimator.tilt_dot - x1;
 
-  const float err_elevation = estimator.elevation - _CO.elevation_ref;
-  const float err_elevation_dot = estimator.elevation_dot - 
_CO.elevation_dot_ref;
+/*  const float err_elevation = estimator.elevation - _CO.elevation_ref;
+  const float err_elevation_dot = estimator.elevation_dot - 
_CO.elevation_dot_ref;*/
 
+  const float err_elevation = estimator.elevation - z0;
+  const float err_elevation_dot = estimator.elevation_dot - z1;
+
   const float err_azimuth = estimator.azimuth - _CO.azimuth_ref;
   const float err_azimuth_dot = estimator.azimuth_dot - _CO.azimuth_dot_ref;