debugging variables and gyro-cross components

conf/airframes/TU_Delft/MicrojetCDW.xml

@@ -4,13 +4,13 @@
 
 <!-- commands section -->
   <servos>
-    <servo name="THROTTLE"      no="0" min="1110" neutral="1110" max="1900"/>
+    <servo name="THROTTLE"      no="0" min="1050" neutral="1050" max="1900"/>
-    <servo name="AILEVON_LEFT"  no="1" min="1000" neutral="1450" max="2000"/>
+    <servo name="AILEVON_RIGHT" no="1" min="1850" neutral="1480" max="1100"/> <!-- 400 - 380 --> 
-    <servo name="AILEVON_RIGHT" no="2" min="2000" neutral="1450" max="1000"/>
+    <servo name="AILEVON_LEFT"  no="2" min="1250" neutral="1580" max="1980"/> <!-- 300 - 400 -->
   </servos>
 
   <commands>
-    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="THROTTLE" failsafe_value="9600"/>
     <axis name="ROLL"     failsafe_value="0"/>
     <axis name="PITCH"    failsafe_value="0"/>
   </commands>
@@ -22,8 +22,8 @@
   </rc_commands>
 
   <section name="MIXER">
-    <define name="AILEVON_AILERON_RATE" value="0.4"/>
+    <define name="AILEVON_AILERON_RATE" value="0.75"/>
-    <define name="AILEVON_ELEVATOR_RATE" value="0.7"/>
+    <define name="AILEVON_ELEVATOR_RATE" value="0.9"/>
   </section>
 
   <command_laws>
@@ -61,6 +61,13 @@
     <define name="GYRO_Q_SENS" value="2.234" integer="16"/>
     <define name="GYRO_R_SENS" value="2.234" integer="16"/>
 
+    <define name="GYRO_P_Q" value="0." />
+    <define name="GYRO_P_R" value="(1.0f/75.0f)" />
+    <define name="GYRO_Q_P" value="0." />
+    <define name="GYRO_Q_R" value="0." />
+    <define name="GYRO_R_P" value="0." />
+    <define name="GYRO_R_Q" value="0." />
+
     <define name="GYRO_P_SIGN" value="1"/>
     <define name="GYRO_Q_SIGN" value="-1"/>
     <define name="GYRO_R_SIGN" value="-1"/>
@@ -145,18 +152,18 @@
     <define name="COURSE_PGAIN" value="-1.4"/>
 
     <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.4" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
 
-    <define name="PITCH_PGAIN" value="-10000."/>
+    <define name="PITCH_PGAIN" value="-6000."/>
-    <define name="PITCH_DGAIN" value="0."/>
+    <define name="PITCH_DGAIN" value="5."/>
 
-    <define name="ELEVATOR_OF_ROLL" value="0."/>
+    <define name="ELEVATOR_OF_ROLL" value="1200."/>
 
     <define name="ROLL_SLEW" value="0."/>
 
-    <define name="ROLL_ATTITUDE_GAIN" value="-10000."/>
+    <define name="ROLL_ATTITUDE_GAIN" value="-5000."/>
-    <define name="ROLL_RATE_GAIN" value="0."/>
+    <define name="ROLL_RATE_GAIN" value="500."/>
   </section>
 
   <section name="AGGRESSIVE" prefix="AGR_">

conf/settings/tuning_ins.xml

@@ -34,7 +34,7 @@
       <dl_settings NAME="ins">
         <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_health" shortname="health" module="subsystems/ahrs" />
         <dl_setting MAX="100" MIN="0" STEP="1" VAR="renorm_sqrt_count" shortname="err_norm" module="subsystems/ahrs" />
-        <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun_error" shortname="err_adc" module="subsystems/imu" />
+        <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun" shortname="ADC overrun" module="subsystems/imu" />
         <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
         <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
       </dl_settings>

sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c

@@ -23,7 +23,7 @@
 
 #include "subsystems/imu.h"
 
-int imu_overrun_error = 0;
+int imu_overrun = 0;
 volatile uint8_t imu_ssp_status;
 static void SSP_ISR(void) __attribute__((naked));
 #if 0
@@ -86,7 +86,10 @@ void imu_b2_arch_init(void) {
 void imu_periodic(void) {
   // check ssp idle
   if (imu_ssp_status != IMU_SSP_STA_IDLE)
+  {
+    imu_overrun++;
     return; //, DEBUG_IMU, IMU_ERR_OVERUN);
+  }
 
   // setup 16 bits
   ImuSetSSP16bits();

sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h

@@ -45,7 +45,7 @@
 #define IMU_SSP_STA_BUSY_MAX1168   1
 #define IMU_SSP_STA_BUSY_MS2100    2
 extern volatile uint8_t imu_ssp_status;
-extern int imu_overrun_error;
+extern int imu_overrun;
 
 
 

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -129,7 +129,6 @@ void ahrs_update_fw_estimator( void )
 
   ));
 
-//  estimator_p     = 
 }
 
 
@@ -188,8 +187,20 @@ void ahrs_propagate(void)
   /* unbias rate measurement */
   RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
 
-  ahrs_float.imu_rate.p += (ahrs_float.imu_rate.r / 75);
+  /* Uncouple Motions */
+#ifdef GYRO_P_Q
+  float dp=0,dq=0,dr=0;
+  dp += ahrs_float.imu_rate.q * GYRO_P_Q;
+  dp += ahrs_float.imu_rate.r * GYRO_P_R;
+  dq += ahrs_float.imu_rate.p * GYRO_Q_P;
+  dq += ahrs_float.imu_rate.r * GYRO_Q_R;
+  dr += ahrs_float.imu_rate.p * GYRO_R_P;
+  dr += ahrs_float.imu_rate.q * GYRO_R_Q;
 
+  ahrs_float.imu_rate.p += dp;
+  ahrs_float.imu_rate.q += dq;
+  ahrs_float.imu_rate.r += dr;
+#endif
 
   Matrix_update();
   // INFO, ahrs struct only updated in ahrs_update_fw_estimator

sw/airborne/subsystems/imu/imu_analog.c

@@ -24,12 +24,14 @@
 #include "mcu_periph/uart.h"
 
 volatile bool_t analog_imu_available;
+int imu_overrun;
 
 static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC];
 
 void imu_impl_init(void) {
 
   analog_imu_available = FALSE;
+  imu_overrun = 0;
 
   adc_buf_channel(ADC_CHANNEL_GYRO_P, &analog_imu_adc_buf[0], ADC_CHANNEL_GYRO_NB_SAMPLES);
   adc_buf_channel(ADC_CHANNEL_GYRO_Q, &analog_imu_adc_buf[1], ADC_CHANNEL_GYRO_NB_SAMPLES);
@@ -41,6 +43,15 @@ void imu_impl_init(void) {
 }
 
 void imu_periodic(void) {
+  // Actual Nr of ADC measurements per channel per periodic loop
+  static int last_head = 0;
+  
+  imu_overrun = analog_imu_adc_buf[0].head - last_head;
+  if (imu_overrun < 0)
+    imu_overrun += ADC_CHANNEL_GYRO_NB_SAMPLES;
+  last_head = analog_imu_adc_buf[0].head;
+
+  // Read All Measurements
   imu.gyro_unscaled.p = analog_imu_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
   imu.gyro_unscaled.q = analog_imu_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
   imu.gyro_unscaled.r = analog_imu_adc_buf[2].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;

sw/airborne/subsystems/imu/imu_analog.h

@@ -27,6 +27,7 @@
 #define NB_ANALOG_IMU_ADC 6
 
 extern volatile bool_t analog_imu_available;
+extern int imu_overrun;
 
 #define ImuEvent(_gyro_accel_handler, _mag_handler) {   \
     if (analog_imu_available) {                         \