[paparazzi-commits] [5230] feedback portion working, still debugging ff..

[Paul Cox]

Revision: 5230
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5230
Author:   paulcox
Date:     2010-08-05 15:25:28 +0000 (Thu, 05 Aug 2010)
Log Message:
-----------
feedback portion working, still debugging ff..

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
    paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
    paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-05 15:17:03 UTC 
(rev 5229)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-05 15:25:28 UTC 
(rev 5230)
@@ -75,6 +75,8 @@
   
   /* Initalize the event library */
   event_init();
+
+  control_init();
   
   if (fms_periodic_init(main_periodic)) {
     TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
@@ -97,20 +99,13 @@
 
 
 static void main_periodic(int my_sig_num) {
-#if 0
-DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);
-main_talk_with_stm32();
-estimator_run(msg_in.bench_sensor.z);
-control_run();
-BoozImuScaleGyro();
-gcs_com_periodic();
-#endif
 
-#if 01 
   RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);});
  
   main_talk_with_stm32();
 
+  BoozImuScaleGyro();
+
   RunOnceEvery(50, 
{DOWNLINK_SEND_BETH(gcs_com.udp_transport,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
                                       &msg_in.bench_sensor.z,&foo);});
   
@@ -130,7 +125,7 @@
                             //&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z
                                
&booz_imu.accel_unscaled.x,&booz_imu.accel_unscaled.y,&booz_imu.accel_unscaled.z);});
 
-  BoozImuScaleGyro();
+
   RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport,
                             //&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
                                
&booz_imu.gyro.p,&booz_imu.gyro.q,&booz_imu.gyro.r);});
@@ -140,9 +135,7 @@
                             //&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z
                                
&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});*/
 
-  //RunOnceEvery(33, {UdpTransportPeriodic();});
   RunOnceEvery(33, gcs_com_periodic());
-#endif
 
 }
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-05 
15:17:03 UTC (rev 5229)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-05 
15:25:28 UTC (rev 5230)
@@ -24,8 +24,11 @@
   controller.xi_cl = 1.;
 
   controller.cmd_ff = 0.;
-  controller.cmd_fb = 0.;
+  controller.cmd_fb1 = 0.;
+  controller.cmd_fb2 = 0.;
   controller.cmd = 0.;
+  //printf("omega_ref: %f omega_cl: 
%f",controller.omega_ref,controller.omega_cl);
+  //omega_ref: 3.490659 omega_cl: 8.726646
 }
 
 
@@ -35,32 +38,36 @@
   /*
    *  propagate reference
    */
-  const float dt_ctl = 1./512.;
+  const float dt_ctl = 1./500.;
   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); 
+    - controller.omega_ref*controller.omega_ref*(controller.tilt_ref - 
controller.tilt_sp); 
 
+
+  static int foo=0;
 #if 0
-  static int foo=0;
-
   float track_err = estimator.tilt - controller.tilt_sp;
   float pcmd = controller.kp*track_err;
   float dcmd = controller.kd*estimator.tilt_dot;
   //controller.cmd = controller.kp*track_err + 
controller.kd*estimator.tilt_dot;
   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; 
-  
+  controller.cmd_fb1 = 
controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_tilt_dot);
+  controller.cmd_fb2 = 
controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_tilt);
+
+  controller.cmd = controller.cmd_ff + controller.cmd_fb1+ controller.cmd_fb1; 
+  if (!(foo%100)) 
+  //printf("ff:%f fb:%f %f (%f)\n",controller.cmd_ff, controller.cmd_fb1, 
controller.cmd_fb2,estimator.tilt_dot);
+  printf("%f %f 
%f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref);
+  foo++; 
 #endif
 }
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-05 
15:17:03 UTC (rev 5229)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-05 
15:25:28 UTC (rev 5230)
@@ -22,7 +22,8 @@
   float xi_cl;
 
   float cmd_ff;
-  float cmd_fb;
+  float cmd_fb1;
+  float cmd_fb2;
   float cmd;
 };
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-05 15:17:03 UTC 
(rev 5229)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-05 15:25:28 UTC 
(rev 5230)
@@ -12,11 +12,12 @@
 void estimator_run(uint16_t tilt_measure) {
   
   const int32_t tilt_neutral = 2815;
-  //const float   tilt_scale = 1./580.;
-  const float   tilt_scale = 1.;
+  const float   tilt_scale = 1./580.;
+  //const float   tilt_scale = 1.;
 
   estimator.tilt = (tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
-  estimator.tilt_dot = booz_imu.gyro.q;
+  //estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
+  estimator.tilt_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.q);
 
 
 }

Modified: paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c   2010-08-05 15:17:03 UTC 
(rev 5229)
+++ paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c   2010-08-05 15:25:28 UTC 
(rev 5230)
@@ -9,6 +9,7 @@
 #include "dl_protocol.h"
 #include "settings.h"
 //#include "downlink.h"
+#include "overo_controller.h"
 
 #define GCS_HOST "10.31.4.104"
 #define GCS_PORT 4242
@@ -71,8 +72,9 @@
     {
       uint8_t i = DL_SETTING_index(gcs_com.my_dl_buffer);
       float var = DL_SETTING_value(gcs_com.my_dl_buffer);
-      //DlSetting(i, var);
+      DlSetting(i, var);
       printf("datalink : %d %f\n",i,var);
+      printf("controller.tilt_sp : %f \n",controller.tilt_sp);
       DOWNLINK_SEND_DL_VALUE(tp, &i, &var);
     }
     break;