From d2cfeb716c135323dd1a1632bd56f554083167b7 Mon Sep 17 00:00:00 2001
From: Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
Date: Mon, 27 Feb 2017 18:03:19 -0800
Subject: [PATCH] [iris] minor config changes

---
 conf/airframes/AGGIEAIR/aggieair_conf.xml     |   2 +-
 .../airframes/AGGIEAIR/aggieair_iris_indi.xml | 147 ++++++++----------
 2 files changed, 65 insertions(+), 84 deletions(-)

diff --git a/conf/airframes/AGGIEAIR/aggieair_conf.xml b/conf/airframes/AGGIEAIR/aggieair_conf.xml
index f1ebd20113..f1b8423087 100644
--- a/conf/airframes/AGGIEAIR/aggieair_conf.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_conf.xml
@@ -40,7 +40,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_iris.xml"
    flight_plan="flight_plans/AGGIEAIR/rotorcraft_opticlow_test.xml"
    settings="settings/rotorcraft_basic.xml [settings/test_actuators_pwm.xml]"
-   settings_modules="modules/air_data.xml modules/lidar_sf11.xml modules/lidar_lite.xml modules/px4flow_i2c.xml modules/ins_extended.xml modules/ahrs_float_mlkf.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml"
+   settings_modules="modules/air_data.xml modules/lidar_sf11.xml modules/lidar_lite.xml modules/px4flow_i2c.xml modules/ins_extended.xml modules/ahrs_float_mlkf.xml modules/stabilization_float_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml"
    gui_color="#337e387bffff"
   />
   <aircraft
diff --git a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
index ab64fd87fc..af5f0e5eb9 100644
--- a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
@@ -13,6 +13,8 @@
   <firmware name="rotorcraft">
 
     <target name="ap" board="px4fmu_2.4" />
+      <configure name="PERIODIC_FREQUENCY" value="200"/>
+      <configure name="TELEMETRY_FREQUENCY" value="60"/>
 
     <!-- amount of time it take for the bat to check -->
     <!-- to avoid bat low spike detection when strong pullup withch draws short sudden power-->
@@ -26,7 +28,8 @@
 
     <module name="imu" type="px4fmu_v2.4"/>
 
-    <module name="stabilization" type="indi_simple"/>
+    <!--module name="stabilization" type="indi_simple"/-->
+    <module name="stabilization" type="float_euler"/>
 
     <!--module name="guidance" type="indi">
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
@@ -41,21 +44,20 @@
     </module-->
 
     <module name="ahrs" type="float_mlkf">
-      <!--configure name="SECONDARY_AHRS" value="float_mlkf"/-->
+      <configure name="SECONDARY_AHRS" value="float_mlkf"/>
       <define name="AHRS_MLKF_IMU_ID" value="IMU_PX4_ID"/>
       <!-- use onboard mag -->
-      <!--define name="AHRS_MLKF_MAG_ID" value="MAG_HMC58XX_SENDER_ID" /-->
+      <define name="AHRS_MLKF_MAG_ID" value="MAG_HMC58XX_SENDER_ID" />
+      <configure name="USE_MAGNETOMETER" value="TRUE"/>
     </module>
 
 
-<!--
+
     <module name="ahrs" type="vectornav">
       <configure name="VN_PORT" value="UART3"/>
       <configure name="VN_BAUD" value="B921600"/>
-      <configure name="TERTIARY_AHRS" value="vectornav"/>
+      <!--configure name="SECONDARY_AHRS" value="vectornav"/-->
     </module>
--->
-
 
     <!--module name="ins" type="float_invariant">
       <define name="INS_FINV_IMU_ID" value="IMU_PX4_ID" />
@@ -80,7 +82,7 @@
     </module>
 
     <module name="actuators" type="pwm">
-      <define name="SERVO_HZ" value="400" />
+      <define name="SERVO_HZ" value="200" />
     </module>
 
     <define name="MEDIAN_DATASIZE" value="7"/>
@@ -96,13 +98,14 @@
 
   <firmware name="rotorcraft">
     <target name="fbw" board="px4io_2.4" />
+    <configure name="PERIODIC_FREQUENCY" value="200"/>
     <module name="motor_mixing" />
     <module name="radio_control" type="ppm">
       <define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="1" />
       <define name="RADIO_KILL_SWITCH" value="RADIO_KILL" />
     </module>
     <module name="actuators" type="pwm">
-      <define name="SERVO_HZ" value="400" />
+      <define name="SERVO_HZ" value="200" />
     </module>
     <define name="RC_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE" />
     <!-- Switch to Failsafe or to Autopilot on RC loss? -->
@@ -116,9 +119,9 @@
     </module>
   </firmware>
 
-  <modules main_freq="512">
+  <modules main_freq="PERIODIC_FREQUENCY">
     <!-- allow flashing FBW target through AP -->
-    <!--module name="px4_flash"/-->
+    <module name="px4_flash"/>
 
     <!-- CPU utilization monitor -->
     <module name="sys_mon"/>
@@ -143,7 +146,7 @@
     </module>
 
     <!-- external mag for better heading estimate -->
-    <!--module name="mag" type="hmc58xx">
+    <module name="mag" type="hmc58xx">
       <configure name="MAG_HMC58XX_I2C_DEV" value="i2c1"/>
       <define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE"/>
       <define name="HMC58XX_CHAN_X" value="1"/>
@@ -152,7 +155,7 @@
       <define name="HMC58XX_CHAN_X_SIGN" value="-"/>
       <define name="HMC58XX_CHAN_Y_SIGN" value="+"/>
       <define name="HMC58XX_CHAN_Z_SIGN" value="+"/>
-    </module-->
+    </module>
 
     <module name="air_data"/>
   </modules>
@@ -165,22 +168,22 @@
   <section name="IMU" prefix="IMU_">
 
     <!-- #3 calibraton -->
+    <!--
     <define name="ACCEL_X_NEUTRAL" value="-2"/>
     <define name="ACCEL_Y_NEUTRAL" value="23"/>
     <define name="ACCEL_Z_NEUTRAL" value="21"/>
     <define name="ACCEL_X_SENS" value="7.68290727703" integer="16"/>
     <define name="ACCEL_Y_SENS" value="7.41725548536" integer="16"/>
     <define name="ACCEL_Z_SENS" value="7.25648506436" integer="16"/>
+    -->
 
     <!--#1 calibration -->
-    <!--
     <define name="ACCEL_X_NEUTRAL" value="0"/>
     <define name="ACCEL_Y_NEUTRAL" value="12"/>
     <define name="ACCEL_Z_NEUTRAL" value="23"/>
     <define name="ACCEL_X_SENS" value="7.40346702354" integer="16"/>
     <define name="ACCEL_Y_SENS" value="7.31092326815" integer="16"/>
     <define name="ACCEL_Z_SENS" value="7.60884042632" integer="16"/>
-    -->
 
     <!-- #3 internal mag -->
     <!--
@@ -203,23 +206,23 @@
     -->
 
     <!-- #1 external mag -->
-    <!--
     <define name="MAG_X_NEUTRAL" value="90"/>
     <define name="MAG_Y_NEUTRAL" value="-12"/>
     <define name="MAG_Z_NEUTRAL" value="3"/>
     <define name="MAG_X_SENS" value="3.9581923374" integer="16"/>
     <define name="MAG_Y_SENS" value="4.06927263588" integer="16"/>
     <define name="MAG_Z_SENS" value="4.43592657252" integer="16"/>
-    -->
+
 
     <!-- #1 internal mag -->
+    <!--
     <define name="MAG_X_NEUTRAL" value="-770"/>
     <define name="MAG_Y_NEUTRAL" value="2338"/>
     <define name="MAG_Z_NEUTRAL" value="-4760"/>
     <define name="MAG_X_SENS" value="0.37262897642" integer="16"/>
     <define name="MAG_Y_SENS" value="0.367752873196" integer="16"/>
     <define name="MAG_Z_SENS" value="0.388678431908" integer="16"/>
-
+    -->
 
     <define name="BODY_TO_IMU_PHI" value="0." unit="deg" />
     <define name="BODY_TO_IMU_THETA" value="0." unit="deg" />
@@ -279,77 +282,55 @@
     <define name="H_Y" value="0.101896"/>
     <define name="H_Z" value="0.923411"/>
     <!-- trust more the baro over the gps alt -->
-    <!--
     <define name="INV_NXZ" value="0.3"/>
     <define name="INV_NH" value="2.0"/>
-    -->
   </section>
 
-  <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
-    <!-- setpoint limits for attitude stabilization rc flight -->
-    <define name="SP_MAX_PHI" value="45" unit="deg" />
-    <define name="SP_MAX_THETA" value="45" unit="deg" />
-    <define name="SP_MAX_R" value="300" unit="deg/s" />
-    <define name="DEADBAND_A" value="0" />
-    <define name="DEADBAND_E" value="0" />
-    <define name="DEADBAND_R" value="50" />
-  </section>
+  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_PHI"     value="35." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="35." unit="deg"/>
+    <define name="SP_MAX_R"       value="40." unit="deg/s"/>
+    <define name="DEADBAND_A"     value="5"/>
+    <define name="DEADBAND_E"     value="5"/>
+    <define name="DEADBAND_R"     value="10"/>
 
-  <section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
-    <!-- attitude reference generation model -->
-    <define name="REF_OMEGA_P" value="450" unit="deg/s" />
-    <define name="REF_ZETA_P" value="0.9" />
-    <define name="REF_MAX_P" value="600." unit="deg/s" />
-    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)" />
-    <define name="REF_OMEGA_Q" value="450" unit="deg/s" />
-    <define name="REF_ZETA_Q" value="0.9" />
-    <define name="REF_MAX_Q" value="600." unit="deg/s" />
-    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)" />
-    <define name="REF_OMEGA_R" value="450" unit="deg/s" />
-    <define name="REF_ZETA_R" value="0.9" />
-    <define name="REF_MAX_R" value="600." unit="deg/s" />
-    <define name="REF_MAX_RDOT" value="RadOfDeg(8000.)" />
-  </section>
+    <!-- inetgrator limits -->
+    <define name="MAX_SUM_ERR"   value="50"/>
 
-  <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
-    <!-- control effectiveness -->
-<!-- Original values
-    <define name="G1_P" value="0.017" />
-    <define name="G1_Q" value="0.019" />
-    <define name="G1_R" value="0.0011" />
-    <define name="G2_R" value="0.089" />
--->
-<!-- adapted values -->
-    <define name="G1_P" value="0.0111687" />
-    <define name="G1_Q" value="0.013964" />
-    <define name="G1_R" value="0.000955" />
-    <define name="G2_R" value="0.019337" />
-    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="FALSE" />
-    <define name="FILTER_PITCH_RATE" value="FALSE" />
-    <define name="FILTER_YAW_RATE" value="FALSE" />
-    <!-- reference acceleration for attitude control -->
-    <define name="REF_ERR_P" value="70.0" />
-    <define name="REF_ERR_Q" value="70.0" />
-    <define name="REF_ERR_R" value="70.0" />
-    <define name="REF_RATE_P" value="10.0" />
-    <define name="REF_RATE_Q" value="10.0" />
-    <define name="REF_RATE_R" value="10.0" />
-    <!-- second order filter parameters -->
-    <define name="FILT_OMEGA" value="20.0" />
-    <define name="FILT_ZETA" value="0.55" />
-    <define name="FILT_OMEGA_R" value="20.0" />
-    <define name="FILT_ZETA_R" value="0.55" />
-    <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.04" />
-    <define name="ACT_DYN_Q" value="0.04" />
-    <define name="ACT_DYN_R" value="0.04" />
-    <!-- Adaptive Learning Rate -->
-    <define name="USE_ADAPTIVE" value="FALSE" />
-    <define name="ADAPTIVE_MU" value="0.0001" />
-    <!-- max rates (conservative) -->
-    <define name="MAX_RATE" value="90" unit="rad/s"/>
-    <define name="MAX_R" value="90" unit="rad/s"/>
+    <!-- reference -->
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
+    <define name="REF_MAX_P"    value="400." unit="deg/s"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
+    <define name="REF_ZETA_Q"   value="0.85"/>
+    <define name="REF_MAX_Q"    value="400." unit="deg/s"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
+    <define name="REF_ZETA_R"   value="0.85"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
+
+    <!-- Big fat ARK Gains -->
+    <define name="PHI_PGAIN"  value="2700"/>
+    <define name="PHI_DGAIN"  value="700"/>
+    <define name="PHI_IGAIN"  value="5.0"/>
+
+    <define name="THETA_PGAIN"  value="2700"/>
+    <define name="THETA_DGAIN"  value="700"/>
+    <define name="THETA_IGAIN"  value="5.0"/>
+
+    <define name="PSI_PGAIN" value="4000"/>
+    <define name="PSI_DGAIN" value="3500"/>
+    <define name="PSI_IGAIN" value="1.5"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value="5"/>
+    <define name="THETA_DDGAIN" value="5"/>
+    <define name="PSI_DDGAIN"   value="10"/>
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">