From dd7c68a0169fdb0cc945314b54710effe1d269c0 Mon Sep 17 00:00:00 2001
From: "Larry (TBS)" <larryho5@gmail.com>
Date: Thu, 23 Jun 2016 12:48:40 +0800
Subject: [PATCH] - Fix the BST error problem

---
 src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c | 15 +++++++++++++++
 src/main/target/COLIBRI_RACE/i2c_bst.c           |  5 ++++-
 2 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c
index 8ab8f6cf51..b9007dd84e 100644
--- a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c
+++ b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c
@@ -71,6 +71,10 @@ bool receiverAddress = false;
 uint8_t readData[DATA_BUFFER_SIZE] = {0};
 uint8_t bufferPointer = 0;
 
+bool cleanflight_data_ready = false;
+uint8_t interruptCounter = 0;
+#define DEALY_SENDING_BYTE	40
+
 void bstProcessInCommand(void);
 void I2C_EV_IRQHandler()
 {
@@ -102,6 +106,17 @@ void I2C_EV_IRQHandler()
 	} else if(I2C_GetITStatus(BSTx, I2C_IT_TXIS)) {
 		if(receiverAddress) {
 			if(currentWriteBufferPointer > 0) {
+				if(!cleanflight_data_ready) {
+					I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS);
+					return;
+				}
+				if(interruptCounter < DEALY_SENDING_BYTE) {
+					interruptCounter++;
+					I2C_ClearITPendingBit(BSTx, I2C_IT_TXIS);
+					return;
+				} else {
+					interruptCounter = 0;
+				}
 				if(writeData[0] == currentWriteBufferPointer) {
 					receiverAddress = false;
 					crc8Cal(0);
diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c
index ec7122ddea..94a29e2d4e 100644
--- a/src/main/target/COLIBRI_RACE/i2c_bst.c
+++ b/src/main/target/COLIBRI_RACE/i2c_bst.c
@@ -960,7 +960,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
 #ifdef USE_FLASHFS
 	    case BST_DATAFLASH_READ:
 			{
-				uint32_t readAddress = read32();
+				uint32_t readAddress = bstRead32();
 
 				bstWriteDataflashReadReply(readAddress, 128);
 			}
@@ -1458,6 +1458,7 @@ static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/)
 #define BST_RESET_TIME			1.2*1000*1000 //micro-seconds
 uint32_t resetBstTimer = 0;
 bool needResetCheck = true;
+extern bool cleanflight_data_ready;
 
 void bstProcessInCommand(void)
 {
@@ -1466,6 +1467,7 @@ void bstProcessInCommand(void)
 		if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) {
 			uint8_t i;
 			writeBufferPointer = 1;
+			cleanflight_data_ready = false;
 			for(i = 0; i < DATA_BUFFER_SIZE; i++) {
 				writeData[i] = 0;
 			}
@@ -1487,6 +1489,7 @@ void bstProcessInCommand(void)
 					// we do not know how to handle the (valid) message, indicate error BST
 					break;
 			}
+			cleanflight_data_ready = true;
 		}
 	} else if(bstCurrentAddress() == 0x00) {
 		if(bstReadCRC() == CRC8 && bstRead8()==BST_GENERAL_HEARTBEAT) {