diff --git a/src/shared/drivers/i2c/I2CDriver-f7.cpp b/src/shared/drivers/i2c/I2CDriver-f7.cpp
index 829194724292381a4910866efa30b9fba8e384e5..27f9c32a2430baee304607ec268943f08f74f8e8 100644
--- a/src/shared/drivers/i2c/I2CDriver-f7.cpp
+++ b/src/shared/drivers/i2c/I2CDriver-f7.cpp
@@ -39,10 +39,55 @@ static const int MAX_N_POLLING =
 static const int N_SCL_BITBANG =
     16;  ///< Number of clocks created for slave locked bus recovery
 static const uint8_t I2C_PIN_ALTERNATE_FUNCTION =
-    4;             ///< Alternate Function number of the I2C peripheral pins
-static uint8_t f;  ///< APB peripheral clock frequency
+    4;  ///< Alternate Function number of the I2C peripheral pins
+
+static uint8_t PRESC_STD;  ///< PRESC for STANDARD speed
+static uint8_t SCLH_STD;   ///< SCLH for STANDARD speed
+static uint8_t SCLL_STD;   ///< SCLL for STANDARD speed
+
+static uint8_t PRESC_F;  ///< PRESC for FAST speed
+static uint8_t SCLH_F;   ///< SCLH for FAST speed
+static uint8_t SCLL_F;   ///< SCLL for FAST speed
+
+static uint8_t PRESC_FP;  ///< PRESC for FAST PLUS speed
+static uint8_t SCLH_FP;   ///< SCLH for FAST PLUS speed
+static uint8_t SCLL_FP;   ///< SCLL for FAST PLUS speed
 }  // namespace I2CConsts
 
+/**
+ * @brief Helper function for calculating the timing parameters for the
+ * peripheral
+ */
+void calculateTimings(uint32_t f, uint32_t fi2c, uint8_t *presc, uint8_t *sclh,
+                      uint8_t *scll)
+{
+    // The formula for the clock is:
+    // t_SCL = [(SCLL + 1) + (SCLH + 1)] * (PRESC + 1) * t_I2CCLK + t_sync;
+
+    // calculating the "smallest" prescaler so that we can handle in a more
+    // refined way (with SCLL and SCLH) the length of high and low phases.
+    uint32_t temp_presc = f / (256 * fi2c);
+
+    // presc is 4 bit long, so avoiding overflow
+    if (temp_presc >= 16)
+    {
+        *presc = 15;
+    }
+    else if (temp_presc == 0)
+    {
+        *presc = 0;
+    }
+    else
+    {
+        *presc = temp_presc - 1;
+    }
+
+    // calculating SCLL and SCLH in order to have duty cycle of 50%. Also,
+    // correcting for the 250ns delay of the peripheral (250ns = 12 I2C clocks)
+    // distributing the correction on SCLL and SCLH
+    *sclh = *scll = (f / (fi2c * 2 * (*presc + 1)) - 1) - (7 / (*presc + 1));
+}
+
 #ifdef I2C1
 /**
  * I2C1 event interrupt
@@ -328,11 +373,20 @@ void I2CDriver::init()
     i2c->CR1 = 0;
 
     // Retrieving the frequency of the APB relative to the I2C peripheral
-    // [MHz] (I2C peripherals are always connected to APB1, Low speed bus). In
+    // [kHz] (I2C peripherals are always connected to APB1, Low speed bus). In
     // fact by default the I2C peripheral is clocked by the APB1 bus; anyway HSI
     // and SYSCLK can be chosen.
-    I2CConsts::f =
-        ClockUtils::getAPBPeripheralsClock(ClockUtils::APB::APB1) / 1000000;
+    uint32_t f =
+        ClockUtils::getAPBPeripheralsClock(ClockUtils::APB::APB1) / 1000;
+
+    calculateTimings(f, 100, &I2CConsts::PRESC_STD, &I2CConsts::SCLH_STD,
+                     &I2CConsts::SCLL_STD);
+
+    calculateTimings(f, 400, &I2CConsts::PRESC_F, &I2CConsts::SCLH_F,
+                     &I2CConsts::SCLL_F);
+
+    calculateTimings(f, 1000, &I2CConsts::PRESC_FP, &I2CConsts::SCLH_FP,
+                     &I2CConsts::SCLL_FP);
 
     // I2CCLK < (t_low - t_filters) / 4
     // I2CCLK < t_high
@@ -458,45 +512,34 @@ void I2CDriver::setupPeripheral(const I2CSlaveConfig &slaveConfig)
     i2c->CR1 &= ~I2C_CR1_PE;
 
     // setting the SCLH and SCLL bits in I2C_TIMINGR register to generate
-    // correct timings for each speed modes
+    // correct timings for each speed modes.
+    // SDADEL and SCLDEL are set to 1 just to provide a bit of margin but not
+    // too much (in order not to slow down communication)
     if (slaveConfig.speed == Speed::STANDARD)
     {
-        // PRESC = 0xb
-        // SCLL = 0x13
-        // SCLH = 0xf
-        // SDADEL = 0x2
-        // SCLDEL = 0x4
-        i2c->TIMINGR = (0xb << I2C_TIMINGR_PRESC_Pos) |   // PRESC
-                       (0x13 << I2C_TIMINGR_SCLL_Pos) |   // SCLL
-                       (0xf << I2C_TIMINGR_SCLH_Pos) |    // SCLH
-                       (0x2 << I2C_TIMINGR_SDADEL_Pos) |  // SDADEL
-                       (0x4 << I2C_TIMINGR_SCLDEL_Pos);   // SCLDEL
+        i2c->TIMINGR =
+            (I2CConsts::PRESC_STD << I2C_TIMINGR_PRESC_Pos) |  // PRESC
+            (I2CConsts::SCLL_STD << I2C_TIMINGR_SCLL_Pos) |    // SCLL
+            (I2CConsts::SCLH_STD << I2C_TIMINGR_SCLH_Pos) |    // SCLH
+            (0x1 << I2C_TIMINGR_SDADEL_Pos) |                  // SDADEL
+            (0x1 << I2C_TIMINGR_SCLDEL_Pos);                   // SCLDEL
     }
     else if (slaveConfig.speed == Speed::FAST)
     {
-        // PRESC = 0x5
-        // SCLL = 0x9
-        // SCLH = 0x3
-        // SDADEL = 0x3
-        // SCLDEL = 0x3
-        i2c->TIMINGR = (0x5 << I2C_TIMINGR_PRESC_Pos) |   // PRESC
-                       (0x9 << I2C_TIMINGR_SCLL_Pos) |    // SCLL
-                       (0x3 << I2C_TIMINGR_SCLH_Pos) |    // SCLH
-                       (0x3 << I2C_TIMINGR_SDADEL_Pos) |  // SDADEL
-                       (0x3 << I2C_TIMINGR_SCLDEL_Pos);   // SCLDEL
+        i2c->TIMINGR = (I2CConsts::PRESC_F << I2C_TIMINGR_PRESC_Pos) |  // PRESC
+                       (I2CConsts::SCLL_F << I2C_TIMINGR_SCLL_Pos) |    // SCLL
+                       (I2CConsts::SCLH_F << I2C_TIMINGR_SCLH_Pos) |    // SCLH
+                       (0x1 << I2C_TIMINGR_SDADEL_Pos) |  // SDADEL
+                       (0x1 << I2C_TIMINGR_SCLDEL_Pos);   // SCLDEL
     }
     else if (slaveConfig.speed == Speed::FAST_PLUS)
     {
-        // PRESC = 0x5
-        // SCLL = 0x3
-        // SCLH = 0x1
-        // SDADEL = 0x0
-        // SCLDEL = 0x1
-        i2c->TIMINGR = (0x5 << I2C_TIMINGR_PRESC_Pos) |   // PRESC
-                       (0x3 << I2C_TIMINGR_SCLL_Pos) |    // SCLL
-                       (0x1 << I2C_TIMINGR_SCLH_Pos) |    // SCLH
-                       (0x0 << I2C_TIMINGR_SDADEL_Pos) |  // SDADEL
-                       (0x1 << I2C_TIMINGR_SCLDEL_Pos);   // SCLDEL
+        i2c->TIMINGR =
+            (I2CConsts::PRESC_FP << I2C_TIMINGR_PRESC_Pos) |  // PRESC
+            (I2CConsts::SCLL_FP << I2C_TIMINGR_SCLL_Pos) |    // SCLL
+            (I2CConsts::SCLH_FP << I2C_TIMINGR_SCLH_Pos) |    // SCLH
+            (0x1 << I2C_TIMINGR_SDADEL_Pos) |                 // SDADEL
+            (0x1 << I2C_TIMINGR_SCLDEL_Pos);                  // SCLDEL
     }
     else
     {