[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

@@ -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);