diff --git a/lib/CMT2300a/cmt2300wrapper.cpp b/lib/CMT2300a/cmt2300wrapper.cpp
index 539630e..6bfe7b9 100644
--- a/lib/CMT2300a/cmt2300wrapper.cpp
+++ b/lib/CMT2300a/cmt2300wrapper.cpp
@@ -25,6 +25,41 @@ bool CMT2300A::isChipConnected()
     return CMT2300A_IsExist();
 }
 
+bool CMT2300A::write(const uint8_t* buf, uint8_t len)
+{
+    CMT2300A_GoStby();
+    CMT2300A_ClearInterruptFlags();
+
+    /* Must clear FIFO after enable SPI to read or write the FIFO */
+    CMT2300A_EnableWriteFifo();
+    CMT2300A_ClearTxFifo();
+
+    CMT2300A_WriteReg(CMT2300A_CUS_PKT15, len); // set Tx length
+    /* The length need be smaller than 32 */
+    CMT2300A_WriteFifo(buf, len);
+
+    if (!(CMT2300A_ReadReg(CMT2300A_CUS_FIFO_FLAG) & CMT2300A_MASK_TX_FIFO_NMTY_FLG)) {
+        return false;
+    }
+
+    if (!CMT2300A_GoTx()) {
+        return false;
+    }
+
+    uint32_t timer = millis();
+
+    while (!(CMT2300A_MASK_TX_DONE_FLG & CMT2300A_ReadReg(CMT2300A_CUS_INT_CLR1))) {
+        if (millis() - timer > 95) {
+            return false;
+        }
+    }
+
+    CMT2300A_ClearInterruptFlags();
+    CMT2300A_GoSleep();
+
+    return true;
+}
+
 bool CMT2300A::setPALevel(int8_t level)
 {
     uint16_t Tx_dBm_word;
diff --git a/lib/CMT2300a/cmt2300wrapper.h b/lib/CMT2300a/cmt2300wrapper.h
index ac1c9d8..977f226 100644
--- a/lib/CMT2300a/cmt2300wrapper.h
+++ b/lib/CMT2300a/cmt2300wrapper.h
@@ -19,6 +19,8 @@ public:
      */
     bool isChipConnected();
 
+    bool write(const uint8_t* buf, uint8_t len);
+
     bool setPALevel(int8_t level);
 
 private:
diff --git a/lib/Hoymiles/src/HoymilesRadio_CMT.cpp b/lib/Hoymiles/src/HoymilesRadio_CMT.cpp
index d920fd0..8cf565d 100644
--- a/lib/Hoymiles/src/HoymilesRadio_CMT.cpp
+++ b/lib/Hoymiles/src/HoymilesRadio_CMT.cpp
@@ -485,11 +485,15 @@ void HoymilesRadio_CMT::sendEsbPacket(CommandAbstract* cmd)
         cmd->getCommandName().c_str(), cmtChToFreq(cmtCurrentCh).c_str());
     cmd->dumpDataPayload(Hoymiles.getMessageOutput());
 
+    // Still here for to handle CMD56 correctly (inverter serial etc.)
     memcpy(cmtTxBuffer, cmd->getDataPayload(), cmd->getDataSize());
-    cmtTxLength = cmd->getDataSize();
-    _txTimeout.set(100);
 
-    cmtNextState = CMT_STATE_TX_START;
+    if (_radio->write(cmd->getDataPayload(), cmd->getDataSize())) {
+        _packetSent = false; // still bad hack, to be removed
+        cmtNextState = CMT_STATE_RX_START;
+    } else {
+        Hoymiles.getMessageOutput()->println("TX SPI Timeout");
+    }
 
     _busyFlag = true;
     _rxTimeout.set(cmd->getTimeout());