diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h
index d60c67711e..693068ad3c 100644
--- a/sw/airborne/peripherals/mpu60x0.h
+++ b/sw/airborne/peripherals/mpu60x0.h
@@ -46,6 +46,11 @@
 /// Default clock: PLL with X gyro reference
 #define MPU60X0_DEFAULT_CLK_SEL 1
 
+// Default number of I2C slaves
+#ifndef MPU60X0_I2C_NB_SLAVES
+#define MPU60X0_I2C_NB_SLAVES 5
+#endif
+
 enum Mpu60x0ConfStatus {
   MPU60X0_CONF_UNINIT,
   MPU60X0_CONF_RESET,
@@ -88,7 +93,7 @@ struct Mpu60x0Config {
 
   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
+  struct Mpu60x0I2cSlave slaves[MPU60X0_I2C_NB_SLAVES];     ///< 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 658f0d6c2c..1a403896cc 100644
--- a/sw/airborne/peripherals/mpu60x0_i2c.c
+++ b/sw/airborne/peripherals/mpu60x0_i2c.c
@@ -151,7 +151,7 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu)
       break;
     case MPU60X0_I2C_CONF_SLAVES_CONFIGURE:
       /* configure each slave until all nb_slaves are done */
-      if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves) {
+      if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU60X0_I2C_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++;
diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c
index d55dd7f843..2b672606fb 100644
--- a/sw/airborne/peripherals/mpu60x0_spi.c
+++ b/sw/airborne/peripherals/mpu60x0_spi.c
@@ -176,7 +176,7 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu)
       break;
     case MPU60X0_SPI_CONF_SLAVES_CONFIGURE:
       /* configure each slave until all nb_slaves are done */
-      if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves) {
+      if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU60X0_I2C_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++;
diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h
index 214887f3a0..fd8daa8057 100644
--- a/sw/airborne/peripherals/mpu9250.h
+++ b/sw/airborne/peripherals/mpu9250.h
@@ -47,6 +47,11 @@
 /// Default clock: PLL with X gyro reference
 #define MPU9250_DEFAULT_CLK_SEL 1
 
+// Default number of I2C slaves
+#ifndef MPU9250_I2C_NB_SLAVES
+#define MPU9250_I2C_NB_SLAVES 5
+#endif
+
 enum Mpu9250ConfStatus {
   MPU9250_CONF_UNINIT,
   MPU9250_CONF_RESET,
@@ -91,7 +96,7 @@ struct Mpu9250Config {
 
   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
+  struct Mpu9250I2cSlave slaves[MPU9250_I2C_NB_SLAVES];     ///< 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 72d620c4bb..9cd52538a3 100644
--- a/sw/airborne/peripherals/mpu9250_i2c.c
+++ b/sw/airborne/peripherals/mpu9250_i2c.c
@@ -184,7 +184,7 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu)
       break;
     case MPU9250_I2C_CONF_SLAVES_CONFIGURE:
       /* configure each slave until all nb_slaves are done */
-      if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves) {
+      if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU9250_I2C_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++;
diff --git a/sw/airborne/peripherals/mpu9250_spi.c b/sw/airborne/peripherals/mpu9250_spi.c
index 5bd282717e..bf01fbadc6 100644
--- a/sw/airborne/peripherals/mpu9250_spi.c
+++ b/sw/airborne/peripherals/mpu9250_spi.c
@@ -173,7 +173,7 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu)
       break;
     case MPU9250_SPI_CONF_SLAVES_CONFIGURE:
       /* configure each slave until all nb_slaves are done */
-      if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves) {
+      if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU9250_I2C_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++;