From 5fbe40f2a7dd6312ae12366b1d8baa8e89698f52 Mon Sep 17 00:00:00 2001
From: Paul Cox <pauldanielcox@gmail.com>
Date: Thu, 5 Aug 2010 15:39:21 +0000
Subject: [PATCH] fix on fb, still working of ff

---
 conf/settings/settings_beth.xml     | 2 +-
 sw/airborne/beth/overo_controller.c | 4 +---
 sw/airborne/beth/overo_gcs_com.c    | 3 ---
 3 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/conf/settings/settings_beth.xml b/conf/settings/settings_beth.xml
index 6cbbc99911..f5728413cc 100644
--- a/conf/settings/settings_beth.xml
+++ b/conf/settings/settings_beth.xml
@@ -6,7 +6,7 @@
     <dl_settings NAME="Controller">
       <dl_setting var="controller.kp" min="0" step="0.005" max="0.1" module="beth/overo_controller"/>
       <dl_setting var="controller.kd" min="0" step="0.002" max="0.02" module="beth/overo_controller"/>
-      <dl_setting var="controller.tilt_sp" min="-400" step="4" max="400" module="beth/overo_controller"/>
+      <dl_setting var="controller.tilt_sp" min="-0.5" step="0.05" max="0.5" module="beth/overo_controller"/>
     </dl_settings>
 
     </dl_settings>
diff --git a/sw/airborne/beth/overo_controller.c b/sw/airborne/beth/overo_controller.c
index ab299dd5c6..4b37287651 100644
--- a/sw/airborne/beth/overo_controller.c
+++ b/sw/airborne/beth/overo_controller.c
@@ -27,8 +27,6 @@ void control_init(void) {
   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
 }
 
 
@@ -63,7 +61,7 @@ void control_run(void) {
   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; 
+  controller.cmd = controller.cmd_ff + controller.cmd_fb1+ controller.cmd_fb2; 
   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);
diff --git a/sw/airborne/beth/overo_gcs_com.c b/sw/airborne/beth/overo_gcs_com.c
index e2c6f872f0..245cd53b47 100644
--- a/sw/airborne/beth/overo_gcs_com.c
+++ b/sw/airborne/beth/overo_gcs_com.c
@@ -8,8 +8,6 @@
 #include "udp_transport2.h"
 #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
@@ -74,7 +72,6 @@ static void dl_handle_msg(struct DownlinkTransport *tp) {
       float var = DL_SETTING_value(gcs_com.my_dl_buffer);
       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;