[paparazzi-commits] [5356] Adding tilt and elevation angle compensation in estimator and controller

[Paul Cox]

Revision: 5356
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5356
Author:   paulcox
Date:     2010-08-13 12:16:28 +0000 (Fri, 13 Aug 2010)
Log Message:
-----------
Adding tilt and elevation angle compensation in estimator and controller
Also added first order low pass on elevation_dot
--Tested on bench, appears robust.

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

Modified: paparazzi3/trunk/conf/airframes/Poine/beth.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-13 07:26:32 UTC 
(rev 5355)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-13 12:16:28 UTC 
(rev 5356)
@@ -225,7 +225,7 @@
 main_overo.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
 main_overo.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
 main_overo.srcs    += $(SRC_FMS)/fms_network.c
-main_overo.LDFLAGS += -levent
+main_overo.LDFLAGS += -levent -lm
 
 main_overo.srcs    += $(SRC_BETH)/overo_gcs_com.c
 main_overo.srcs    += $(SRC_BETH)/overo_file_logger.c

Modified: paparazzi3/trunk/conf/settings/settings_beth.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-13 07:26:32 UTC 
(rev 5355)
+++ paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-13 12:16:28 UTC 
(rev 5356)
@@ -9,6 +9,9 @@
       <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>
 
+      <dl_setting var="estimator.lp_coeff" min="0.01" step="0.01" max="1" 
module="beth/overo_estimator"  shortname="elev_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/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-13 
07:26:32 UTC (rev 5355)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-13 
12:16:28 UTC (rev 5356)
@@ -82,6 +82,7 @@
 
   controller.cmd_pitch =  controller.cmd_pitch_ff + controller.cmd_pitch_fb; 
   controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb 
+ thrust_constant;
+  controller.cmd_thrust = controller.cmd_thrust*(1/cos(estimator.elevation));
 
   //if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
   Bound(controller.cmd_thrust,0,100);

Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-13 07:26:32 UTC 
(rev 5355)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-13 12:16:28 UTC 
(rev 5356)
@@ -1,11 +1,12 @@
 #include "overo_estimator.h"
 
 #include "booz/booz_imu.h"
+#include <math.h>
 
 struct OveroEstimator estimator;
 
 void estimator_init(void) {
-
+  estimator.lp_coeff = 0.9;
 }
 
 //zyx
@@ -20,12 +21,18 @@
 
 
   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);
 
-  //TODO: Add rotation compensation to remove tilt effect on elev/azi
   estimator.elevation = (elevation_neutral - (int32_t)elevation_measure ) * 
elevation_scale;
-  estimator.elevation_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.p);
+  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);
+  //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.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-13 07:26:32 UTC 
(rev 5355)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-13 12:16:28 UTC 
(rev 5356)
@@ -12,6 +12,8 @@
   float azimuth_dot;
   float elevation_dot;
   float tilt_dot;
+
+  float lp_coeff;
 };