From 1614d35d5cd6e49cdd1a66011feb7b147a87d4b7 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Jul 2014 17:12:20 +0200 Subject: [PATCH] [imu] aspirin_2_spi: wait 1.5s before configuring mag Seems that sometimes the mag is not correctly set to continuous measurement mode if it is configured immediately. Delaying the configuration until the first single measurement was done seems to do the trick. Delay configurable via ASPIRIN_2_MAG_STARTUP_DELAY closes #779 --- sw/airborne/subsystems/imu/imu_aspirin_2_spi.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index ef708b53ee5..cdc2e2eef49 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -25,7 +25,7 @@ */ #include "subsystems/imu.h" - +#include "mcu_periph/sys_time.h" #include "mcu_periph/spi.h" #include "peripherals/hmc58xx_regs.h" @@ -91,6 +91,10 @@ PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE) #define HMC58XX_CRA ((HMC58XX_DO<<2)|(HMC58XX_MS)) #define HMC58XX_CRB (HMC58XX_GN<<5) +/** delay in seconds before starting to configure HMC58xx mag slave */ +#ifndef ASPIRIN_2_MAG_STARTUP_DELAY +#define ASPIRIN_2_MAG_STARTUP_DELAY 1.5 +#endif struct ImuAspirin2Spi imu_aspirin2; @@ -216,6 +220,11 @@ static inline void mpu_set_and_wait(Mpu60x0ConfigSet mpu_set, void* mpu, uint8_t */ bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu) { + // wait before starting the configuration of the HMC58xx mag + // doing to early may void the mode configuration + if (get_sys_time_float() < ASPIRIN_2_MAG_STARTUP_DELAY) + return FALSE; + mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA); mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA);