diff --git a/sw/airborne/peripherals/adxl345.extra_i2c.h b/sw/airborne/peripherals/adxl345.extra_i2c.h
index 65985aaac0..aa9a4a9506 100644
--- a/sw/airborne/peripherals/adxl345.extra_i2c.h
+++ b/sw/airborne/peripherals/adxl345.extra_i2c.h
@@ -67,6 +67,8 @@
 #define ADXL345_I2C_DEVICE i2c1
 #endif
 
+// Config done flag
+extern bool_t adxl345_initialized;
 // Data ready flag
 extern volatile bool_t adxl345_data_available;
 // Data vector
@@ -78,9 +80,16 @@ extern struct i2c_transaction adxl345_trans;
 
 // Functions
 extern void adxl345_init(void);
-extern void adxl345_periodic(void);
+extern void adxl345_configure(void);
+extern void adxl345_read(void);
 extern void adxl345_event(void);
 
+// Macro for using ADXL345 in periodic function
+#define Adxl345Periodic() {                 \
+  if (adxl345_initialized) adxl345_read();  \
+  else adxl345_configure();                 \
+}
+
 #define AccelEvent(_handler) {     \
     adxl345_event();              \
     if (adxl345_data_available) { \
diff --git a/sw/airborne/peripherals/adxl345.i2c.c b/sw/airborne/peripherals/adxl345.i2c.c
index 378aa72655..85d968ea9a 100644
--- a/sw/airborne/peripherals/adxl345.i2c.c
+++ b/sw/airborne/peripherals/adxl345.i2c.c
@@ -62,21 +62,25 @@ static void adxl345_send_config(void)
       adxl345_i2c_trans.buf[0] = ADXL345_REG_BW_RATE;
       adxl345_i2c_trans.buf[1] = ADXL345_BW_RATE;
       I2CTransmit(ADXL345_I2C_DEVICE, adxl345_i2c_trans, ADXL345_I2C_ADDR, 2);
+      adxl345_init_status++;
       break;
     case ADXL_CONF_POWER:
       adxl345_i2c_trans.buf[0] = ADXL345_REG_POWER_CTL;
       adxl345_i2c_trans.buf[1] = ADXL345_POWER_CTL;
       I2CTransmit(ADXL345_I2C_DEVICE, adxl345_i2c_trans, ADXL345_I2C_ADDR, 2);
+      adxl345_init_status++;
       break;
     case ADXL_CONF_INT:
       adxl345_i2c_trans.buf[0] = ADXL345_REG_INT_ENABLE;
       adxl345_i2c_trans.buf[1] = ADXL345_INT_ENABLE;
       I2CTransmit(ADXL345_I2C_DEVICE, adxl345_i2c_trans, ADXL345_I2C_ADDR, 2);
+      adxl345_init_status++;
       break;
     case ADXL_CONF_FORMAT:
       adxl345_i2c_trans.buf[0] = ADXL345_REG_DATA_FORMAT;
       adxl345_i2c_trans.buf[1] = ADXL345_DATA_FORMAT;
       I2CTransmit(ADXL345_I2C_DEVICE, adxl345_i2c_trans, ADXL345_I2C_ADDR, 2);
+      adxl345_init_status++;
       break;
     case ADXL_CONF_DONE:
       adxl345_initialized = TRUE;
@@ -87,24 +91,23 @@ static void adxl345_send_config(void)
   }
 }
 
-void adxl345_periodic(void)
+// Configure
+void adxl345_configure(void)
 {
-  if (!adxl345_initialized) {
-    // Configure
+  if (adxl345_init_status == ADXL_CONF_UNINIT) {
+    adxl345_init_status++;
     if (adxl345_i2c_trans.status == I2CTransSuccess || adxl345_i2c_trans.status == I2CTransDone) {
-      adxl345_init_status++;
       adxl345_send_config();
     }
-    if (adxl345_i2c_trans.status == I2CTransFailed) {
-      adxl345_send_config(); // Retry config
-    }
   }
-  else {
-    // Normal reading
-    if (adxl345_i2c_trans.status == I2CTransDone){
-      adxl345_i2c_trans.buf[0] = ADXL345_REG_DATA_X0;
-      I2CTransceive(ADXL345_I2C_DEVICE, adxl345_i2c_trans, ADXL345_I2C_ADDR, 1, 6);
-    }
+}
+
+// Normal reading
+void adxl345_read(void)
+{
+  if (adxl345_initialized && adxl345_i2c_trans.status == I2CTransDone) {
+    adxl345_i2c_trans.buf[0] = ADXL345_REG_DATA_X0;
+    I2CTransceive(ADXL345_I2C_DEVICE, adxl345_i2c_trans, ADXL345_I2C_ADDR, 1, 6);
   }
 }
 
@@ -125,6 +128,17 @@ void adxl345_event(void)
       adxl345_i2c_trans.status = I2CTransDone;
     }
   }
+  else if (!adxl345_initialized && adxl345_init_status != ADXL_CONF_UNINIT) { // Configuring
+    if (adxl345_i2c_trans.status == I2CTransSuccess || adxl345_i2c_trans.status == I2CTransDone) {
+      adxl345_i2c_trans.status = I2CTransDone;
+      adxl345_send_config();
+    }
+    if (adxl345_i2c_trans.status == I2CTransFailed) {
+      adxl345_init_status--;
+      adxl345_i2c_trans.status = I2CTransDone;
+      adxl345_send_config(); // Retry config (TODO max retry)
+    }
+  }
 }
 
 
diff --git a/sw/airborne/peripherals/hmc58xx.c b/sw/airborne/peripherals/hmc58xx.c
index ee2d50022f..5235bc191a 100644
--- a/sw/airborne/peripherals/hmc58xx.c
+++ b/sw/airborne/peripherals/hmc58xx.c
@@ -61,16 +61,19 @@ static void hmc58xx_send_config(void)
       hmc58xx_i2c_trans.buf[0] = HMC58XX_REG_CFGA;
       hmc58xx_i2c_trans.buf[1] = HMC58XX_CRA;
       I2CTransmit(HMC58XX_I2C_DEVICE, hmc58xx_i2c_trans, HMC58XX_ADDR, 2);
+      hmc58xx_init_status++;
       break;
     case HMC_CONF_CRB:
       hmc58xx_i2c_trans.buf[0] = HMC58XX_REG_CFGB;
       hmc58xx_i2c_trans.buf[1] = HMC58XX_CRB;
       I2CTransmit(HMC58XX_I2C_DEVICE, hmc58xx_i2c_trans, HMC58XX_ADDR, 2);
+      hmc58xx_init_status++;
       break;
     case HMC_CONF_MODE:
       hmc58xx_i2c_trans.buf[0] = HMC58XX_REG_MODE;
       hmc58xx_i2c_trans.buf[1] = HMC58XX_MD;
       I2CTransmit(HMC58XX_I2C_DEVICE, hmc58xx_i2c_trans, HMC58XX_ADDR, 2);
+      hmc58xx_init_status++;
       break;
     case HMC_CONF_DONE:
       hmc58xx_initialized = TRUE;
@@ -81,24 +84,23 @@ static void hmc58xx_send_config(void)
   }
 }
 
-void hmc58xx_periodic(void)
+// Configure
+void hmc58xx_configure(void)
 {
-  if (!hmc58xx_initialized) {
-    // Configure
+  if (hmc58xx_init_status == HMC_CONF_UNINIT) {
+    hmc58xx_init_status++;
     if (hmc58xx_i2c_trans.status == I2CTransSuccess || hmc58xx_i2c_trans.status == I2CTransDone) {
-      hmc58xx_init_status++;
       hmc58xx_send_config();
     }
-    if (hmc58xx_i2c_trans.status == I2CTransFailed) {
-      hmc58xx_send_config(); // Retry config
-    }
   }
-  else {
-    // Normal reading
-    if (hmc58xx_i2c_trans.status == I2CTransDone){
-      hmc58xx_i2c_trans.buf[0] = HMC58XX_REG_DATXM;
-      I2CTransceive(HMC58XX_I2C_DEVICE, hmc58xx_i2c_trans, HMC58XX_ADDR, 1, 6);
-    }
+}
+
+// Normal reading
+void hmc58xx_read(void)
+{
+  if (hmc58xx_initialized && hmc58xx_i2c_trans.status == I2CTransDone){
+    hmc58xx_i2c_trans.buf[0] = HMC58XX_REG_DATXM;
+    I2CTransceive(HMC58XX_I2C_DEVICE, hmc58xx_i2c_trans, HMC58XX_ADDR, 1, 6);
   }
 }
 
@@ -118,5 +120,16 @@ void hmc58xx_event(void)
       hmc58xx_i2c_trans.status = I2CTransDone;
     }
   }
+  else if (!hmc58xx_initialized && hmc58xx_init_status != HMC_CONF_UNINIT) { // Configuring
+    if (hmc58xx_i2c_trans.status == I2CTransSuccess || hmc58xx_i2c_trans.status == I2CTransDone) {
+      hmc58xx_i2c_trans.status = I2CTransDone;
+      hmc58xx_send_config();
+    }
+    if (hmc58xx_i2c_trans.status == I2CTransFailed) {
+      hmc58xx_init_status--;
+      hmc58xx_i2c_trans.status = I2CTransDone;
+      hmc58xx_send_config(); // Retry config (TODO max retry)
+    }
+  }
 }
 
diff --git a/sw/airborne/peripherals/hmc58xx.h b/sw/airborne/peripherals/hmc58xx.h
index 4dd01b6a83..1f4eae3e42 100644
--- a/sw/airborne/peripherals/hmc58xx.h
+++ b/sw/airborne/peripherals/hmc58xx.h
@@ -70,6 +70,8 @@
 #define HMC58XX_I2C_DEVICE i2c2
 #endif
 
+// Config done flag
+extern bool_t hmc58xx_initialized;
 // Data ready flag
 extern volatile bool_t hmc58xx_data_available;
 // Data vector
@@ -81,9 +83,16 @@ extern struct i2c_transaction hmc58xx_i2c_trans;
 
 // Functions
 extern void hmc58xx_init(void);
-extern void hmc58xx_periodic(void);
+extern void hmc58xx_configure(void);
+extern void hmc58xx_read(void);
 extern void hmc58xx_event(void);
 
+// Macro for using HMC58XX in periodic function
+#define Hmc58xxPeriodic() {                 \
+  if (hmc58xx_initialized) hmc58xx_read();  \
+  else hmc58xx_configure();                 \
+}
+
 #define MagEvent(_m_handler) {    \
     hmc58xx_event();              \
     if (hmc58xx_data_available) { \
diff --git a/sw/airborne/peripherals/itg3200.c b/sw/airborne/peripherals/itg3200.c
index 62a02211de..6c0c1ba6f6 100644
--- a/sw/airborne/peripherals/itg3200.c
+++ b/sw/airborne/peripherals/itg3200.c
@@ -62,21 +62,25 @@ static void itg3200_send_config(void)
       itg3200_i2c_trans.buf[0] = ITG3200_REG_SMPLRT_DIV;
       itg3200_i2c_trans.buf[1] = ITG3200_SMPLRT_DIV;
       I2CTransmit(ITG3200_I2C_DEVICE, itg3200_i2c_trans, ITG3200_I2C_ADDR, 2);
+      itg3200_init_status++;
       break;
     case ITG_CONF_DF:
       itg3200_i2c_trans.buf[0] = ITG3200_REG_DLPF_FS;
       itg3200_i2c_trans.buf[1] = ITG3200_DLPF_FS;
       I2CTransmit(ITG3200_I2C_DEVICE, itg3200_i2c_trans, ITG3200_I2C_ADDR, 2);
+      itg3200_init_status++;
       break;
     case ITG_CONF_INT:
       itg3200_i2c_trans.buf[0] = ITG3200_REG_INT_CFG;
       itg3200_i2c_trans.buf[1] = ITG3200_INT_CFG;
       I2CTransmit(ITG3200_I2C_DEVICE, itg3200_i2c_trans, ITG3200_I2C_ADDR, 2);
+      itg3200_init_status++;
       break;
     case ITG_CONF_PWR:
       itg3200_i2c_trans.buf[0] = ITG3200_REG_PWR_MGM;
       itg3200_i2c_trans.buf[1] = ITG3200_PWR_MGM;
       I2CTransmit(ITG3200_I2C_DEVICE, itg3200_i2c_trans, ITG3200_I2C_ADDR, 2);
+      itg3200_init_status++;
       break;
     case ITG_CONF_DONE:
       itg3200_initialized = TRUE;
@@ -87,24 +91,23 @@ static void itg3200_send_config(void)
   }
 }
 
-void itg3200_periodic(void)
+// Configure
+void itg3200_configure(void)
 {
-  if (!itg3200_initialized) {
-    // Configure
+  if (itg3200_init_status == ITG_CONF_UNINIT) {
+    itg3200_init_status++;
     if (itg3200_i2c_trans.status == I2CTransSuccess || itg3200_i2c_trans.status == I2CTransDone) {
-      itg3200_init_status++;
       itg3200_send_config();
     }
-    if (itg3200_i2c_trans.status == I2CTransFailed) {
-      itg3200_send_config(); // Retry config
-    }
   }
-  else {
-    // Normal reading
-    if (itg3200_i2c_trans.status == I2CTransDone) {
-      itg3200_i2c_trans.buf[0] = ITG3200_REG_INT_STATUS;
-      I2CTransceive(ITG3200_I2C_DEVICE, itg3200_i2c_trans, ITG3200_I2C_ADDR, 1, 9);
-    }
+}
+
+// Normal reading
+void itg3200_read(void)
+{
+  if (itg3200_initialized && itg3200_i2c_trans.status == I2CTransDone) {
+    itg3200_i2c_trans.buf[0] = ITG3200_REG_INT_STATUS;
+    I2CTransceive(ITG3200_I2C_DEVICE, itg3200_i2c_trans, ITG3200_I2C_ADDR, 1, 9);
   }
 }
 
@@ -128,5 +131,16 @@ void itg3200_event(void)
       itg3200_i2c_trans.status = I2CTransDone;
     }
   }
+  else if (!itg3200_initialized && itg3200_init_status != ITG_CONF_UNINIT) { // Configuring
+    if (itg3200_i2c_trans.status == I2CTransSuccess || itg3200_i2c_trans.status == I2CTransDone) {
+      itg3200_i2c_trans.status = I2CTransDone;
+      itg3200_send_config();
+    }
+    if (itg3200_i2c_trans.status == I2CTransFailed) {
+      itg3200_init_status--;
+      itg3200_i2c_trans.status = I2CTransDone;
+      itg3200_send_config(); // Retry config (TODO max retry)
+    }
+  }
 }
 
diff --git a/sw/airborne/peripherals/itg3200.extra.h b/sw/airborne/peripherals/itg3200.extra.h
index 5bfb5bb95f..ba4d761eec 100644
--- a/sw/airborne/peripherals/itg3200.extra.h
+++ b/sw/airborne/peripherals/itg3200.extra.h
@@ -64,6 +64,8 @@
 #define ITG3200_I2C_DEVICE i2c1
 #endif
 
+// Config done flag
+extern bool_t itg3200_initialized;
 // Data ready flag
 extern volatile bool_t itg3200_data_available;
 // Data vector
@@ -75,9 +77,16 @@ extern struct i2c_transaction itg3200_trans;
 
 // Functions
 extern void itg3200_init(void);
-extern void itg3200_periodic(void);
+extern void itg3200_configure(void);
+extern void itg3200_read(void);
 extern void itg3200_event(void);
 
+// Macro for using ITG3200 in periodic function
+#define Itg3200Periodic() {                 \
+  if (itg3200_initialized) itg3200_read();  \
+  else itg3200_configure();                 \
+}
+
 #define GyroEvent(_handler) {     \
     itg3200_event();              \
     if (itg3200_data_available) { \