Adding tilt and elevation angle compensation in estimator and controller

Also added first order low pass on elevation_dot
--Tested on bench, appears robust.

conf/airframes/Poine/beth.xml

@@ -225,7 +225,7 @@ main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG
 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

conf/settings/settings_beth.xml

@@ -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>

sw/airborne/beth/overo_controller.c

@@ -82,6 +82,7 @@ void control_run(void) {
 
   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);

sw/airborne/beth/overo_estimator.c

@@ -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,11 +21,17 @@ void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t a
 
 
   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);

sw/airborne/beth/overo_estimator.h

@@ -12,6 +12,8 @@ struct OveroEstimator {
   float azimuth_dot;
   float elevation_dot;
   float tilt_dot;
+
+  float lp_coeff;
 };