diff --git a/src/hardware.c b/src/hardware.c
index e433f03..5fc096d 100644
--- a/src/hardware.c
+++ b/src/hardware.c
@@ -478,10 +478,9 @@ void GetVtxParameter() {
 // last_SA_lock
 #if defined USE_SMARTAUDIO_SW || defined USE_SMARTAUDIO_HW
         last_SA_lock = I2C_Read8_Wait(10, ADDR_EEPROM, EEP_ADDR_SA_LOCK);
-        WAIT(10);
         if (last_SA_lock == 0xff) {
             last_SA_lock = 0;
-            I2C_Write8(ADDR_EEPROM, EEP_ADDR_SA_LOCK, last_SA_lock);
+            I2C_Write8_Wait(10, ADDR_EEPROM, EEP_ADDR_SA_LOCK, last_SA_lock);
         }
 #ifdef _DEBUG_MODE
         debugf("\r\nlast_SA_lock %x", (uint16_t)last_SA_lock);
@@ -1616,7 +1615,7 @@ void RF_Delay_Init() {
 
     if (SA_saved == 0) {
         if (seconds >= WAIT_SA_CONFIG) {
-            I2C_Write8(ADDR_EEPROM, EEP_ADDR_SA_LOCK, SA_lock);
+            I2C_Write8_Wait(10, ADDR_EEPROM, EEP_ADDR_SA_LOCK, SA_lock);
             SA_saved = 1;
 #ifdef _DEBUG_MODE
             debugf("\r\nSave SA_lock(%x) to EEPROM", (uint16_t)SA_lock);
diff --git a/src/i2c.c b/src/i2c.c
index fb0c477..2f165e5 100644
--- a/src/i2c.c
+++ b/src/i2c.c
@@ -121,9 +121,10 @@ uint8_t I2C_Write8(uint8_t slave_addr, uint8_t reg_addr, uint8_t val) {
 }
 
 uint8_t I2C_Write8_Wait(uint16_t ms, uint8_t slave_addr, uint8_t reg_addr, uint8_t val) {
-    WAIT(ms);
-    // debugf("\r\nEEP:0x%04x, 0x%04x", reg_addr, (uint16_t)val);
-    return I2C_Write8(slave_addr, reg_addr, val);
+    uint8_t ret;
+    ret = I2C_Write8(slave_addr, reg_addr, val);
+    WAIT(ms + 2);
+    return ret;
 }
 
 uint8_t I2C_Write16(uint8_t slave_addr, uint16_t reg_addr, uint16_t val) {