KroozSD imu driver improvements

Fixed magnetometer axes.
Accel median filter disabled by default.

closes #501

conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml

@@ -28,10 +28,11 @@
     <define name="USE_ATTITUDE_REF" value="0"/>
   </firmware>
 
-  <modules main_freq="512">
-    <!--<load name="gps_ubx_ucenter.xml"/>
-    <load name="geo_mag.xml"/>
-    <load name="sys_mon.xml"/>-->
+	<modules main_freq="512">
+		<!--<load name="gps_ubx_ucenter.xml"/>
+    <load name="sys_mon.xml"/>
+		<load name="geo_mag.xml"/>
+    <load name="osd_max7456.xml"/>-->
   </modules>
 
   <commands>
@@ -80,11 +81,6 @@
     <set servo="FL"   value="motor_mixing.commands[SERVO_FL]"/>
   </command_laws>
 
-  <section name="VERSION" prefix="VERSION_">
-    <define name="MAJOR" value="1"/>
-    <define name="MINOR" value="2"/>
-  </section>
-
   <section name="IMU" prefix="IMU_">
 
     <!-- replace this with your own calibration -->
@@ -178,16 +174,16 @@
     <define name="REF_MAX_RDOT" value="RadOfDeg(180.)"/>
 
     <!-- feedback -->
-    <define name="PHI_PGAIN"  value="1000"/>
-    <define name="PHI_DGAIN"  value="350"/>
-    <define name="PHI_IGAIN"  value="250"/>
+    <define name="PHI_PGAIN"  value="850"/>
+    <define name="PHI_DGAIN"  value="400"/>
+    <define name="PHI_IGAIN"  value="100"/>
 
-    <define name="THETA_PGAIN"  value="1000"/>
-    <define name="THETA_DGAIN"  value="350"/>
-    <define name="THETA_IGAIN"  value="250"/>
+    <define name="THETA_PGAIN"  value="850"/>
+    <define name="THETA_DGAIN"  value="400"/>
+    <define name="THETA_IGAIN"  value="100"/>
 
     <define name="PSI_PGAIN"  value="4000"/>
-    <define name="PSI_DGAIN"  value="1000"/>
+    <define name="PSI_DGAIN"  value="1500"/>
     <define name="PSI_IGAIN"  value="300"/>
 
     <!-- feedforward -->
@@ -218,7 +214,7 @@
   <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
     <define name="USE_REF" value="1"/>
     <define name="USE_SPEED_REF" value="1"/> <!-- using RC to control horizontal setpoint -->
-    <define name="MAX_BANK" value="20" unit="deg"/>
+    <define name="MAX_BANK" value="27" unit="deg"/>
     <define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 -->
     <define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s -->
     <define name="RC_SPEED_DEAD_BAND" value="200000"/>

conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml

@@ -32,8 +32,9 @@
 	<modules main_freq="512">
 		<!--<load name="gps_ubx_ucenter.xml"/>
     <load name="sys_mon.xml"/>
-    <load name="geo_mag.xml"/>-->
-    </modules>
+		<load name="geo_mag.xml"/>
+    <load name="osd_max7456.xml"/>-->
+  </modules>
 
   <commands>
     <axis name="ROLL"   failsafe_value="0"/>
@@ -83,11 +84,6 @@
     <set servo="FL"   value="motor_mixing.commands[SERVO_FL]"/>
   </command_laws>
 
-	<section name="VERSION" prefix="VERSION_">
-    <define name="MAJOR" value="1"/>
-    <define name="MINOR" value="2"/>
-  </section>
-
   <section name="IMU" prefix="IMU_">
 
     <!-- replace this with your own calibration -->
@@ -181,16 +177,16 @@
     <define name="REF_MAX_RDOT" value="RadOfDeg(180.)"/>
 
     <!-- feedback -->
-    <define name="PHI_PGAIN"  value="1000"/>
-    <define name="PHI_DGAIN"  value="350"/>
-    <define name="PHI_IGAIN"  value="250"/>
+    <define name="PHI_PGAIN"  value="850"/>
+    <define name="PHI_DGAIN"  value="400"/>
+    <define name="PHI_IGAIN"  value="100"/>
 
-    <define name="THETA_PGAIN"  value="1000"/>
-    <define name="THETA_DGAIN"  value="350"/>
-    <define name="THETA_IGAIN"  value="250"/>
+    <define name="THETA_PGAIN"  value="850"/>
+    <define name="THETA_DGAIN"  value="400"/>
+    <define name="THETA_IGAIN"  value="100"/>
 
     <define name="PSI_PGAIN"  value="4000"/>
-    <define name="PSI_DGAIN"  value="1000"/>
+    <define name="PSI_DGAIN"  value="1500"/>
     <define name="PSI_IGAIN"  value="300"/>
 
     <!-- feedforward -->
@@ -221,7 +217,7 @@
   <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
 		<define name="USE_REF" value="1"/>
     <define name="USE_SPEED_REF" value="1"/> <!-- using RC to control horizontal setpoint -->
-    <define name="MAX_BANK" value="20" unit="deg"/>
+    <define name="MAX_BANK" value="27" unit="deg"/>
     <define name="REF_MAX_ACCEL" value="1.2"/> <!-- max reference horizontal acceleration in m/s2 -->
     <define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s -->
 		<define name="RC_SPEED_DEAD_BAND" value="200000"/>

conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml

@@ -32,6 +32,7 @@
 		<!--<load name="gps_ubx_ucenter.xml"/>
     <load name="sys_mon.xml"/>
 		<load name="geo_mag.xml"/>-->
+    <load name="osd_max7456.xml"/>
   </modules>
 
   <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
@@ -61,11 +62,6 @@
     <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
   </command_laws>
 
-	<section name="VERSION" prefix="VERSION_">
-    <define name="MAJOR" value="1"/>
-    <define name="MINOR" value="2"/>
-  </section>
-
   <section name="MIXING" prefix="MOTOR_MIXING_">
     <define name="TRIM_ROLL" value="0"/>
     <define name="TRIM_PITCH" value="0"/>
@@ -95,12 +91,12 @@
     <define name="ACCEL_Y_SENS" value="0.6131" integer="16"/>
     <define name="ACCEL_Z_SENS" value="0.6131" integer="16"/>
 
-    <define name="MAG_X_NEUTRAL" value="-282"/>
-    <define name="MAG_Y_NEUTRAL" value="-78"/>
-    <define name="MAG_Z_NEUTRAL" value="109"/>
-    <define name="MAG_X_SENS" value="3.33430456557" integer="16"/>
-    <define name="MAG_Y_SENS" value="3.53180739126" integer="16"/>
-    <define name="MAG_Z_SENS" value="3.68297143457" integer="16"/>
+    <define name="MAG_X_NEUTRAL" value="74"/>
+    <define name="MAG_Y_NEUTRAL" value="-118"/>
+    <define name="MAG_Z_NEUTRAL" value="104"/>
+    <define name="MAG_X_SENS" value="3.44419577366" integer="16"/>
+    <define name="MAG_Y_SENS" value="3.40350478221" integer="16"/>
+    <define name="MAG_Z_SENS" value="3.77559859867" integer="16"/>
 
     <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
     <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
@@ -167,20 +163,20 @@
 
     <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.)"/>
+    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(180.)"/>
 
     <!-- feedback -->
-    <define name="PHI_PGAIN"  value="1000"/>
-    <define name="PHI_DGAIN"  value="250"/>
-    <define name="PHI_IGAIN"  value="250"/>
+    <define name="PHI_PGAIN"  value="850"/>
+    <define name="PHI_DGAIN"  value="400"/>
+    <define name="PHI_IGAIN"  value="100"/>
 
-    <define name="THETA_PGAIN"  value="1000"/>
-    <define name="THETA_DGAIN"  value="250"/>
-    <define name="THETA_IGAIN"  value="250"/>
+    <define name="THETA_PGAIN"  value="850"/>
+    <define name="THETA_DGAIN"  value="400"/>
+    <define name="THETA_IGAIN"  value="100"/>
 
     <define name="PSI_PGAIN"  value="4000"/>
-    <define name="PSI_DGAIN"  value="1000"/>
+    <define name="PSI_DGAIN"  value="1500"/>
     <define name="PSI_IGAIN"  value="300"/>
 
     <!-- feedforward -->
@@ -210,7 +206,7 @@
   <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
 		<define name="USE_REF" value="1"/>
     <define name="USE_SPEED_REF" value="1"/> <!-- using RC to control horizontal setpoint -->
-    <define name="MAX_BANK" value="20" unit="deg"/>
+    <define name="MAX_BANK" value="27" unit="deg"/>
     <define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 -->
     <define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s -->
 		<define name="RC_SPEED_DEAD_BAND" value="200000"/>

conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml

@@ -29,9 +29,11 @@
     <define name="USE_ATTITUDE_REF" value="0"/>
   </firmware>
 
-  <modules main_freq="512">
-  <!--<load name="gps_ubx_ucenter.xml"/>
-  <load name="geo_mag.xml"/>-->
+	<modules main_freq="512">
+		<!--<load name="gps_ubx_ucenter.xml"/>
+    <load name="sys_mon.xml"/>
+		<load name="geo_mag.xml"/>
+    <load name="osd_max7456.xml"/>-->
   </modules>
 
   <servos driver="Pwm">
@@ -56,11 +58,6 @@
     <set servo="LEFT"   value="motor_mixing.commands[SERVO_LEFT]"/>
   </command_laws>
 
-  <section name="VERSION" prefix="VERSION_">
-    <define name="MAJOR" value="1"/>
-    <define name="MINOR" value="2"/>
-  </section>
-
   <section name="MIXING" prefix="MOTOR_MIXING_">
     <define name="TRIM_ROLL" value="0"/>
     <define name="TRIM_PITCH" value="0"/>
@@ -162,21 +159,21 @@
 
     <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.)"/>
+    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(180.)"/>
 
     <!-- feedback -->
-    <define name="PHI_PGAIN"  value="600"/>
-    <define name="PHI_DGAIN"  value="350"/>
-    <define name="PHI_IGAIN"  value="150"/>
+    <define name="PHI_PGAIN"  value="850"/>
+    <define name="PHI_DGAIN"  value="400"/>
+    <define name="PHI_IGAIN"  value="100"/>
 
-    <define name="THETA_PGAIN"  value="600"/>
-    <define name="THETA_DGAIN"  value="350"/>
-    <define name="THETA_IGAIN"  value="150"/>
+    <define name="THETA_PGAIN"  value="850"/>
+    <define name="THETA_DGAIN"  value="400"/>
+    <define name="THETA_IGAIN"  value="100"/>
 
-    <define name="PSI_PGAIN"  value="3500"/>
-    <define name="PSI_DGAIN"  value="1000"/>
-    <define name="PSI_IGAIN"  value="100"/>
+    <define name="PSI_PGAIN"  value="4000"/>
+    <define name="PSI_DGAIN"  value="1500"/>
+    <define name="PSI_IGAIN"  value="300"/>
 
     <!-- feedforward -->
     <define name="PHI_DDGAIN"   value="300"/>
@@ -205,7 +202,7 @@
   <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
     <define name="USE_REF" value="1"/>
     <define name="USE_SPEED_REF" value="1"/> <!-- using RC to control horizontal setpoint -->
-    <define name="MAX_BANK" value="20" unit="deg"/>
+    <define name="MAX_BANK" value="27" unit="deg"/>
     <define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 -->
     <define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s -->
 		<define name="RC_SPEED_DEAD_BAND" value="200000"/>

sw/airborne/boards/krooz/imu_krooz.c

@@ -33,6 +33,7 @@
 #include "subsystems/imu/imu_krooz_sd_arch.h"
 #include "mcu_periph/i2c.h"
 #include "led.h"
+#include "filters/median_filter.h"
 
 // Downlink
 #include "mcu_periph/uart.h"
@@ -63,10 +64,13 @@ PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
 struct ImuKrooz imu_krooz;
 
 
-#if KROOZ_USE_MEDIAN_FILTER
-#include "filters/median_filter.h"
-struct MedianFilter3Int median_gyro, median_accel, median_mag;
+#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
+struct MedianFilter3Int median_gyro;
 #endif
+#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
+struct MedianFilter3Int median_accel;
+#endif
+struct MedianFilter3Int median_mag;
 
 void imu_impl_init( void )
 {
@@ -82,12 +86,14 @@ void imu_impl_init( void )
 
   hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
 
-#if KROOZ_USE_MEDIAN_FILTER
   // Init median filters
+#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
   InitMedianFilterRatesInt(median_gyro);
+#endif 
+#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
   InitMedianFilterVect3Int(median_accel);
-  InitMedianFilterVect3Int(median_mag);
 #endif
+  InitMedianFilterVect3Int(median_mag);
 
   RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
   VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
@@ -110,14 +116,25 @@ void imu_periodic( void )
     hmc58xx_start_configure(&imu_krooz.hmc);
 
   if (imu_krooz.meas_nb) {
-    RATES_ASSIGN(imu.gyro_unscaled, imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
-#if KROOZ_USE_MEDIAN_FILTER
+    RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
+#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
     UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
 #endif
-    VECT3_ASSIGN(imu.accel_unscaled, imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
-#if KROOZ_USE_MEDIAN_FILTER
+    VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
+#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
     UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
 #endif
+    
+    RATES_SMUL(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, IMU_KROOZ_GYRO_AVG_FILTER);
+    RATES_ADD(imu_krooz.gyro_filtered, imu.gyro_unscaled);
+    RATES_SDIV(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, (IMU_KROOZ_GYRO_AVG_FILTER + 1));
+    RATES_COPY(imu.gyro_unscaled, imu_krooz.gyro_filtered);
+    
+    VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
+    VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
+    VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
+    VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
+    
     RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
     VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
     imu_krooz.meas_nb = 0;
@@ -150,10 +167,8 @@ void imu_krooz_event( void )
   // If the HMC5883 I2C transaction has succeeded: convert the data
   hmc58xx_event(&imu_krooz.hmc);
   if (imu_krooz.hmc.data_available) {
-    VECT3_COPY(imu.mag_unscaled, imu_krooz.hmc.data.vect);
-#if KROOZ_USE_MEDIAN_FILTER
+    VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.z, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.y);
     UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
-#endif
     imu_krooz.hmc.data_available = FALSE;
     imu_krooz.mag_valid = TRUE;
   }

sw/airborne/boards/krooz/imu_krooz.h

@@ -39,12 +39,12 @@
 
 // Default configuration
 #if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
-#define IMU_GYRO_P_SIGN  -1
+#define IMU_GYRO_P_SIGN   1
 #define IMU_GYRO_Q_SIGN   1
 #define IMU_GYRO_R_SIGN   1
 #endif
 #if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
-#define IMU_ACCEL_X_SIGN -1
+#define IMU_ACCEL_X_SIGN  1
 #define IMU_ACCEL_Y_SIGN  1
 #define IMU_ACCEL_Z_SIGN  1
 #endif
@@ -100,6 +100,13 @@
 #define IMU_ACCEL_Z_NEUTRAL 0
 #endif
 
+#ifndef IMU_KROOZ_GYRO_AVG_FILTER
+#define IMU_KROOZ_GYRO_AVG_FILTER       5
+#endif
+#ifndef IMU_KROOZ_ACCEL_AVG_FILTER
+#define IMU_KROOZ_ACCEL_AVG_FILTER      10
+#endif
+
 struct ImuKrooz {
   volatile bool_t gyr_valid;
   volatile bool_t acc_valid;
@@ -109,6 +116,8 @@ struct ImuKrooz {
   struct Int32Rates rates_sum;
   struct Int32Vect3 accel_sum;
   volatile uint8_t  meas_nb;
+  struct Int32Vect3 accel_filtered;
+  struct Int32Rates gyro_filtered;
 };
 
 extern struct ImuKrooz imu_krooz;