[paparazzi-commits] [5459] adding azimuth ff control.

[Paul Cox]

Revision: 5459
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5459
Author:   paulcox
Date:     2010-08-18 15:43:32 +0000 (Wed, 18 Aug 2010)
Log Message:
-----------
adding azimuth ff control. works but *very* slow.

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/Poine/beth.xml
    paparazzi3/trunk/conf/settings/settings_beth.xml
    paparazzi3/trunk/sw/airborne/beth/overo_controller.c
    paparazzi3/trunk/sw/airborne/beth/overo_controller.h

Modified: paparazzi3/trunk/conf/airframes/Poine/beth.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-18 14:36:47 UTC 
(rev 5458)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-18 15:43:32 UTC 
(rev 5459)
@@ -148,7 +148,7 @@
 main_stm32.srcs += $(SRC_BOOZ)/actuators/booz_actuators_asctec.c 
 #\
 #                             
$(SRC_BOOZ_ARCH)/actuators/booz_actuators_asctec_arch.c
-main_stm32.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1 
+main_stm32.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
 # -DBOOZ_START_DELAY=3  -DUSE_TIM2_IRQ
 main_stm32.CFLAGS += -DUSE_I2C1
 main_stm32.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
@@ -208,7 +208,8 @@
 #HOST = beth
 #HOST = overo
 #HOST = auto7
-HOST= regis
+#HOST= regis
+HOST = auto3
 TARGET_DIR = ~
 SRC_FMS=fms
 

Modified: paparazzi3/trunk/conf/settings/settings_beth.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-18 14:36:47 UTC 
(rev 5458)
+++ paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-18 15:43:32 UTC 
(rev 5459)
@@ -6,10 +6,10 @@
 
       <dl_setting var="controller.elevation_sp" min="-25" step="1" max="20" 
module="beth/overo_controller" shortname="elev_sp" unit="rad" alt_unit="deg" 
alt_unit_coef="57.29578"/>
 
-      <dl_setting var="controller.azimuth_ref" min="-15" step="0.5" max="15" 
module="beth/overo_controller"  shortname="azim_sp" unit="rad" alt_unit="deg" 
alt_unit_coef="57.29578">
+      <dl_setting var="controller.azimuth_sp" min="-15" step="0.5" max="15" 
module="beth/overo_controller"  shortname="azim_sp" unit="rad" alt_unit="deg" 
alt_unit_coef="57.29578">
       </dl_setting>
 
-      <dl_setting var="controller.azim_gain" min="0.5" step=".05" max="5" 
module="beth/overo_estimator"  shortname="azim_gain">
+      <dl_setting var="controller.azim_gain" min="0.1" step=".05" max="2" 
module="beth/overo_estimator"  shortname="azim_gain">
       </dl_setting>
 
       <dl_setting var="estimator.elevation_lp_coeff" min="0.01" step="0.01" 
max="1" module="beth/overo_estimator"  shortname="elev_lp_coeff">

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-18 
14:36:47 UTC (rev 5458)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-18 
15:43:32 UTC (rev 5459)
@@ -12,9 +12,11 @@
 
   controller.tilt_sp = 0.;
   controller.elevation_sp = RadOfDeg(10);
+  controller.azimuth_sp = 0.;
 
   controller.omega_tilt_ref = RadOfDeg(200);
   controller.omega_elevation_ref = RadOfDeg(120);
+  controller.omega_azimuth_ref = RadOfDeg(60);
   controller.xi_ref = 1.;
 
   controller.tilt_ref = estimator.tilt;
@@ -69,6 +71,11 @@
   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); 
 
+  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); 
+
   static int foo=0;
 
   /*
@@ -96,10 +103,12 @@
   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);
 
+  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);
 
-  controller.cmd_pitch =  controller.cmd_pitch_ff + controller.cmd_pitch_fb + 
controller.azim_gain * controller.cmd_azimuth_fb; 
+  controller.cmd_pitch =  controller.cmd_pitch_ff + controller.cmd_pitch_fb + 
+                            controller.azim_gain * (controller.cmd_azimuth_fb 
+ controller.cmd_azimuth_ff); 
   controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb 
+ thrust_constant;
   controller.cmd_thrust = controller.cmd_thrust*(1/cos(estimator.elevation));
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-18 
14:36:47 UTC (rev 5458)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-18 
15:43:32 UTC (rev 5459)
@@ -7,6 +7,7 @@
   
   float tilt_sp;
   float elevation_sp;
+  float azimuth_sp;
   
   /* modele de reference */
   float tilt_ref;
@@ -23,6 +24,7 @@
 
   float omega_tilt_ref;
   float omega_elevation_ref;
+  float omega_azimuth_ref;
   float xi_ref;
 
   /* invert control law parameter */
@@ -40,6 +42,7 @@
   float cmd_thrust_ff;
   float cmd_thrust_fb;
 
+  float cmd_azimuth_ff;
   float cmd_azimuth_fb;
 
   float cmd_pitch;