Skip to content

Commit

Permalink
KroozSD imu driver improvements
Browse files Browse the repository at this point in the history 
Fixed magnetometer axes.
Accel median filter disabled by default.

closes paparazzi#501
  • Loading branch information
@softsr @flixr
softsr authored and flixr committed Aug 12, 2013
1 parent f4e1387 commit 47d272d
Show file tree
Hide file tree
Showing 6 changed files with 96 additions and 87 deletions.
30 changes: 13 additions & 17 deletions conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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 -->
Expand Down Expand Up @@ -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 -->
Expand Down Expand Up @@ -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"/>
Expand Down
26 changes: 11 additions & 15 deletions conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down Expand Up @@ -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 -->
Expand Down Expand Up @@ -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 -->
Expand Down Expand Up @@ -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"/>
Expand Down
38 changes: 17 additions & 21 deletions conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_">
Expand Down Expand Up @@ -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"/>
Expand Down Expand Up @@ -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"/>
Expand Down Expand Up @@ -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 -->
Expand Down Expand Up @@ -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"/>
Expand Down
37 changes: 17 additions & 20 deletions conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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">
Expand All @@ -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"/>
Expand Down Expand Up @@ -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"/>
Expand Down Expand Up @@ -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"/>
Expand Down
39 changes: 27 additions & 12 deletions sw/airborne/boards/krooz/imu_krooz.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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 )
{
Expand All @@ -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);
Expand All @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down
13 changes: 11 additions & 2 deletions sw/airborne/boards/krooz/imu_krooz.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down

0 comments on commit 47d272d

Please sign in to comment.