diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c
index 70cf0e0233..4bfb5ffdff 100644
--- a/sw/airborne/peripherals/mpu60x0.c
+++ b/sw/airborne/peripherals/mpu60x0.c
@@ -44,6 +44,7 @@ void mpu60x0_set_default_config(struct Mpu60x0Config *c)
    */
   c->nb_bytes = 15;
   c->nb_slaves = 0;
+  c->nb_slave_init = 0;
 
   c->i2c_bypass = FALSE;
 }
diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h
index 419b5b59e1..d60c67711e 100644
--- a/sw/airborne/peripherals/mpu60x0.h
+++ b/sw/airborne/peripherals/mpu60x0.h
@@ -87,6 +87,7 @@ struct Mpu60x0Config {
   bool_t i2c_bypass;
 
   uint8_t nb_slaves;                    ///< number of used I2C slaves
+  uint8_t nb_slave_init;                ///< number of already configured/initialized slaves
   struct Mpu60x0I2cSlave slaves[5];     ///< I2C slaves
   enum Mpu60x0MstClk i2c_mst_clk;       ///< MPU I2C master clock speed
   uint8_t i2c_mst_delay;                ///< MPU I2C slaves delayed sample rate
diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c
index 7157ea10e3..658f0d6c2c 100644
--- a/sw/airborne/peripherals/mpu60x0_i2c.c
+++ b/sw/airborne/peripherals/mpu60x0_i2c.c
@@ -130,7 +130,7 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
   }
 }
 
-/** @todo: only one slave so far. */
+/** configure the registered I2C slaves */
 bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu)
 {
   struct Mpu60x0_I2c *mpu_i2c = (struct Mpu60x0_I2c *)(mpu);
@@ -150,8 +150,15 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu)
       mpu_i2c->slave_init_status++;
       break;
     case MPU60X0_I2C_CONF_SLAVES_CONFIGURE:
-      /* configure each slave. TODO: currently only one */
-      if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) {
+      /* configure each slave until all nb_slaves are done */
+      if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves) {
+         // proceed to next slave if configure for current one returns true
+        if (mpu_i2c->config.slaves[mpu_i2c->config.nb_slave_init].configure(mpu_set, mpu)) {
+          mpu_i2c->config.nb_slave_init++;
+        }
+      }
+      else {
+        /* all slave devies configured, continue MPU side configuration of I2C slave stuff */
         mpu_i2c->slave_init_status++;
       }
       break;
diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c
index 5b53f42b9d..d55dd7f843 100644
--- a/sw/airborne/peripherals/mpu60x0_spi.c
+++ b/sw/airborne/peripherals/mpu60x0_spi.c
@@ -148,7 +148,7 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
   }
 }
 
-/** @todo: only one slave so far. */
+/** configure the registered I2C slaves */
 bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu)
 {
   struct Mpu60x0_Spi *mpu_spi = (struct Mpu60x0_Spi *)(mpu);
@@ -175,8 +175,15 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu)
       mpu_spi->slave_init_status++;
       break;
     case MPU60X0_SPI_CONF_SLAVES_CONFIGURE:
-      /* configure first slave, only one slave supported so far */
-      if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) {
+      /* configure each slave until all nb_slaves are done */
+      if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves) {
+         // proceed to next slave if configure for current one returns true
+        if (mpu_spi->config.slaves[mpu_spi->config.nb_slave_init].configure(mpu_set, mpu)) {
+          mpu_spi->config.nb_slave_init++;
+        }
+      }
+      else {
+        /* all slave devies configured, continue MPU side configuration of I2C slave stuff */
         mpu_spi->slave_init_status++;
       }
       break;
diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c
index a2326bbc2e..a65f65ca62 100644
--- a/sw/airborne/peripherals/mpu9250.c
+++ b/sw/airborne/peripherals/mpu9250.c
@@ -45,6 +45,7 @@ void mpu9250_set_default_config(struct Mpu9250Config *c)
    */
   c->nb_bytes = 15;
   c->nb_slaves = 0;
+  c->nb_slave_init = 0;
 
   c->i2c_bypass = FALSE;
 }
diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h
index 81ca095cd5..214887f3a0 100644
--- a/sw/airborne/peripherals/mpu9250.h
+++ b/sw/airborne/peripherals/mpu9250.h
@@ -90,6 +90,7 @@ struct Mpu9250Config {
   bool_t i2c_bypass;
 
   uint8_t nb_slaves;                    ///< number of used I2C slaves
+  uint8_t nb_slave_init;                ///< number of already configured/initialized slaves
   struct Mpu9250I2cSlave slaves[5];     ///< I2C slaves
   enum Mpu9250MstClk i2c_mst_clk;       ///< MPU I2C master clock speed
   uint8_t i2c_mst_delay;                ///< MPU I2C slaves delayed sample rate
diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c
index 158ebdc22e..72d620c4bb 100644
--- a/sw/airborne/peripherals/mpu9250_i2c.c
+++ b/sw/airborne/peripherals/mpu9250_i2c.c
@@ -163,7 +163,7 @@ bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__((u
   }
 }
 
-/** @todo: only one slave so far. */
+/** configure the registered I2C slaves */
 bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu)
 {
   struct Mpu9250_I2c *mpu_i2c = (struct Mpu9250_I2c *)(mpu);
@@ -183,8 +183,15 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu)
       mpu_i2c->slave_init_status++;
       break;
     case MPU9250_I2C_CONF_SLAVES_CONFIGURE:
-      /* configure each slave. TODO: currently only one */
-      if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) {
+      /* configure each slave until all nb_slaves are done */
+      if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves) {
+         // proceed to next slave if configure for current one returns true
+        if (mpu_i2c->config.slaves[mpu_i2c->config.nb_slave_init].configure(mpu_set, mpu)) {
+          mpu_i2c->config.nb_slave_init++;
+        }
+      }
+      else {
+        /* all slave devies configured, continue MPU side configuration of I2C slave stuff */
         mpu_i2c->slave_init_status++;
       }
       break;
diff --git a/sw/airborne/peripherals/mpu9250_spi.c b/sw/airborne/peripherals/mpu9250_spi.c
index 6af6631f6b..5bd282717e 100644
--- a/sw/airborne/peripherals/mpu9250_spi.c
+++ b/sw/airborne/peripherals/mpu9250_spi.c
@@ -145,7 +145,7 @@ void mpu9250_spi_event(struct Mpu9250_Spi *mpu)
   }
 }
 
-/** @todo: only one slave so far. */
+/** configure the registered I2C slaves */
 bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu)
 {
   struct Mpu9250_Spi *mpu_spi = (struct Mpu9250_Spi *)(mpu);
@@ -172,8 +172,15 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu)
       mpu_spi->slave_init_status++;
       break;
     case MPU9250_SPI_CONF_SLAVES_CONFIGURE:
-      /* configure first slave, only one slave supported so far */
-      if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) {
+      /* configure each slave until all nb_slaves are done */
+      if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves) {
+         // proceed to next slave if configure for current one returns true
+        if (mpu_spi->config.slaves[mpu_spi->config.nb_slave_init].configure(mpu_set, mpu)) {
+          mpu_spi->config.nb_slave_init++;
+        }
+      }
+      else {
+        /* all slave devies configured, continue MPU side configuration of I2C slave stuff */
         mpu_spi->slave_init_status++;
       }
       break;