[paparazzi-commits] [5458] added feeback control of azimuth.

[Paul Cox]

Revision: 5458
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5458
Author:   paulcox
Date:     2010-08-18 14:36:47 +0000 (Wed, 18 Aug 2010)
Log Message:
-----------
added feeback control of azimuth. A few other minor fixups.

Modified Paths:
--------------
    paparazzi3/trunk/conf/settings/settings_beth.xml
    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_estimator.h

Modified: paparazzi3/trunk/conf/settings/settings_beth.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-18 13:19:11 UTC 
(rev 5457)
+++ paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-18 14:36:47 UTC 
(rev 5458)
@@ -6,12 +6,18 @@
 
       <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.tilt_sp" min="-15" step="0.5" max="15" 
module="beth/overo_controller"  shortname="tilt_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>
 
-      <dl_setting var="estimator.lp_coeff" min="0.01" step="0.01" max="1" 
module="beth/overo_estimator"  shortname="elev_lp_coeff">
+      <dl_setting var="controller.azim_gain" min="0.5" step=".05" max="5" 
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">
+      </dl_setting>
+
+      <dl_setting var="estimator.tilt_lp_coeff" min="0.01" step="0.01" max="1" 
module="beth/overo_estimator"  shortname="tilt_lp_coeff">
+      </dl_setting>
+
       <dl_setting var="controller.armed" min="0" step="1" max="2" shortname 
="mode"/>
 
     </dl_settings>

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-18 13:19:11 UTC 
(rev 5457)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-18 14:36:47 UTC 
(rev 5458)
@@ -126,7 +126,8 @@
                        
&msg_in.payload.msg_up.can_errs,&msg_in.payload.msg_up.spi_errs,
                        
&msg_in.payload.msg_up.thrust_out,&msg_in.payload.msg_up.pitch_out);});
  
-  
estimator_run(msg_in.payload.msg_up.bench_sensor.z,msg_in.payload.msg_up.bench_sensor.y,msg_in.payload.msg_up.bench_sensor.x);
+  
estimator_run(msg_in.payload.msg_up.bench_sensor.z,msg_in.payload.msg_up.bench_sensor.y,
+               msg_in.payload.msg_up.bench_sensor.x);
  
   if ( msg_in.payload.msg_up.cnt == 0) printf("STM indicates overo link is 
lost! %d %d\n",
                        
msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs);

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-18 
13:19:11 UTC (rev 5457)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-18 
14:36:47 UTC (rev 5458)
@@ -26,8 +26,13 @@
   controller.elevation_dot_ref = estimator.elevation_dot;
   controller.elevation_ddot_ref = 0.;
 
+  controller.azimuth_ref = estimator.azimuth;
+  controller.azimuth_dot_ref = 0.;
+  controller.azimuth_ddot_ref = 0.;
+
   controller.one_over_J = 2.;
   controller.mass = 5.;
+  controller.azim_gain = 1.;
 
   controller.omega_cl = RadOfDeg(600);
   controller.xi_cl = 1.;
@@ -66,21 +71,35 @@
 
   static int foo=0;
 
+  /*
+   *  calculate errors
+   */
+
   const float err_tilt = estimator.tilt - controller.tilt_ref;
   const float err_tilt_dot = estimator.tilt_dot - controller.tilt_dot_ref;
 
   const float err_elevation = estimator.elevation - controller.elevation_ref;
   const float err_elevation_dot = estimator.elevation_dot - 
controller.elevation_dot_ref;
 
-  controller.cmd_pitch_ff = controller.one_over_J*controller.tilt_ddot_ref;
-  controller.cmd_pitch_fb = 
controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_tilt_dot) +
-                       
controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_tilt);
+  const float err_azimuth = estimator.azimuth - controller.azimuth_ref;
+  const float err_azimuth_dot = estimator.azimuth_dot - 
controller.azimuth_dot_ref;
 
-  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);
+  /*
+   *  Compute feedforward and feedback commands
+   */
 
-  controller.cmd_pitch =  controller.cmd_pitch_ff + controller.cmd_pitch_fb; 
+  controller.cmd_pitch_ff = controller.one_over_J * controller.tilt_ddot_ref;
+  controller.cmd_pitch_fb = controller.one_over_J * (2 * controller.xi_cl * 
controller.omega_cl * err_tilt_dot) +
+                       controller.one_over_J * (controller.omega_cl * 
controller.omega_cl * err_tilt);
+
+  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);
+
+  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_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 
13:19:11 UTC (rev 5457)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-18 
14:36:47 UTC (rev 5458)
@@ -17,6 +17,10 @@
   float elevation_dot_ref;
   float elevation_ddot_ref;
 
+  float azimuth_ref;
+  float azimuth_dot_ref;
+  float azimuth_ddot_ref;
+
   float omega_tilt_ref;
   float omega_elevation_ref;
   float xi_ref;
@@ -28,6 +32,7 @@
   /* closed loop parameters */
   float omega_cl;
   float xi_cl;
+  float azim_gain;
 
   float cmd_pitch_ff;
   float cmd_pitch_fb;
@@ -35,6 +40,8 @@
   float cmd_thrust_ff;
   float cmd_thrust_fb;
 
+  float cmd_azimuth_fb;
+
   float cmd_pitch;
   float cmd_thrust;
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-18 13:19:11 UTC 
(rev 5457)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-18 14:36:47 UTC 
(rev 5458)
@@ -6,15 +6,16 @@
 struct OveroEstimator estimator;
 
 void estimator_init(void) {
-  estimator.lp_coeff = 0.9;
+  estimator.tilt_lp_coeff = 0.9;
+  estimator.elevation_lp_coeff = 0.9;
 }
 
-//zyx
+//bench sensors z,y,x values passed in
 void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t 
azimuth_measure) {
   
   const int32_t tilt_neutral = 1970;
   const float   tilt_scale = 1./580.;
-  const int32_t azimuth_neutral = 1500;
+  const int32_t azimuth_neutral = 2875;
   const float   azimuth_scale = 1./580.;
   const int32_t elevation_neutral = 670;
   const float   elevation_scale = 1./580.;
@@ -22,16 +23,25 @@
 
   estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
   Bound(estimator.tilt,-89,89);
-  //TODO: lp filter tilt_dot
-  estimator.tilt_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.q);
+  //low pass filter gyro values
+  estimator.tilt_dot = estimator.tilt_dot + 
+                         estimator.tilt_lp_coeff * 
(RATE_FLOAT_OF_BFP(booz_imu.gyro.q) - estimator.tilt_dot);
+  /* Second order filter to be tested
+  estimator.tilt_dot = estimator.tilt_dot * (2 - estimator.tilt_lp_coeff1 - 
estimator.tilt_lp_coeff2) +
+                         estimator.tilt_lp_coeff1 * estimator.tilt_lp_coeff2 * 
RATE_FLOAT_OF_BFP(booz_imu.gyro.q) -
+                         estimator.tilt_dot_old * (1 - 
estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2 +
+                         estimator.tilt_lp_coeff1*estimator.tilt_lp_coeff2);
+  */
 
   estimator.elevation = (elevation_neutral - (int32_t)elevation_measure ) * 
elevation_scale;
   Bound(estimator.elevation,-45,45);
 
   //rotation compensation (mixing of two gyro values to generate a reading 
that reflects rate along beth axes
-  float rotated_elev_dot = 
RATE_FLOAT_OF_BFP(booz_imu.gyro.p)*cos(estimator.tilt)+RATE_FLOAT_OF_BFP(booz_imu.gyro.r)*sin(estimator.tilt);
+  float rotated_elev_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.p) * 
cos(estimator.tilt) +
+                             RATE_FLOAT_OF_BFP(booz_imu.gyro.r) * 
sin(estimator.tilt);
   //low pass filter -- should probably increase order and maybe move filtering 
to measured values.
-  estimator.elevation_dot = estimator.elevation_dot 
+estimator.lp_coeff*(rotated_elev_dot-estimator.elevation_dot);
+  estimator.elevation_dot = estimator.elevation_dot + 
+                              estimator.elevation_lp_coeff * (rotated_elev_dot 
- estimator.elevation_dot);
 
   estimator.azimuth = (azimuth_neutral - (int32_t)azimuth_measure ) * 
azimuth_scale;
   estimator.azimuth_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.r);

Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-18 13:19:11 UTC 
(rev 5457)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-18 14:36:47 UTC 
(rev 5458)
@@ -13,7 +13,8 @@
   float elevation_dot;
   float tilt_dot;
 
-  float lp_coeff;
+  float tilt_lp_coeff;
+  float elevation_lp_coeff;
 };