diff --git a/workspace/TS100/Core/BSP/Pine64/IRQ.cpp b/workspace/TS100/Core/BSP/Pine64/IRQ.cpp
index 60ef5f8b..1049fc18 100644
--- a/workspace/TS100/Core/BSP/Pine64/IRQ.cpp
+++ b/workspace/TS100/Core/BSP/Pine64/IRQ.cpp
@@ -107,13 +107,17 @@ void I2C0_EV_IRQHandler(void) {
 			}
 			break;
 		case I2C_TRANSMIT_DATA:
-			if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) {
-				/* the master sends a data byte */
-				i2c_data_transmit(I2C0, *i2c_write++);
-				i2c_nbytes--;
-				if (RESET == i2c_nbytes) {
-					i2c_write_process = I2C_STOP;
+			if (i2c_nbytes) {
+				if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) {
+					/* the master sends a data byte */
+					i2c_data_transmit(I2C0, *i2c_write++);
+					i2c_nbytes--;
+					if (0 == i2c_nbytes) {
+						i2c_write_process = I2C_STOP;
+					}
 				}
+			} else {
+				i2c_write_process = I2C_STOP;
 			}
 			break;
 		case I2C_STOP:
@@ -180,25 +184,27 @@ void I2C0_EV_IRQHandler(void) {
 			break;
 		case I2C_TRANSMIT_DATA:
 			if (i2c_nbytes > 0) {
-				/* read a byte from the EEPROM */
-				if (i2c_nbytes == 2) {
-					/* wait until BTC bit is set */
-					i2c_ackpos_config(I2C0, I2C_ACKPOS_CURRENT);
-					/* clear the ACKEN before the ADDSEND is cleared */
-					i2c_ack_config(I2C0, I2C_ACK_DISABLE);
-				}
-				*i2c_read = i2c_data_receive(I2C0);
-				i2c_read++;
-				i2c_nbytes--;
-				if (i2c_nbytes == 0) {
-					/* the master sends a stop condition to I2C bus */
-					i2c_stop_on_bus(I2C0);
-					/* disable the I2C0 interrupt */
-					i2c_interrupt_disable(I2C0, I2C_INT_ERR);
-					i2c_interrupt_disable(I2C0, I2C_INT_BUF);
-					i2c_interrupt_disable(I2C0, I2C_INT_EV);
-					i2c_process_flag = RESET;
-					i2c_read_process = I2C_DONE;
+				if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_RBNE)) {
+					/* read a byte from the EEPROM */
+					if (i2c_nbytes == 2) {
+						/* wait until BTC bit is set */
+						i2c_ackpos_config(I2C0, I2C_ACKPOS_CURRENT);
+						/* clear the ACKEN before the ADDSEND is cleared */
+						i2c_ack_config(I2C0, I2C_ACK_DISABLE);
+					}
+					*i2c_read = i2c_data_receive(I2C0);
+					i2c_read++;
+					i2c_nbytes--;
+					if (i2c_nbytes == 0) {
+						/* the master sends a stop condition to I2C bus */
+						i2c_stop_on_bus(I2C0);
+						/* disable the I2C0 interrupt */
+						i2c_interrupt_disable(I2C0, I2C_INT_ERR);
+						i2c_interrupt_disable(I2C0, I2C_INT_BUF);
+						i2c_interrupt_disable(I2C0, I2C_INT_EV);
+						i2c_process_flag = RESET;
+						i2c_read_process = I2C_DONE;
+					}
 				}
 			} else {
 				i2c_read_process = I2C_STOP;
diff --git a/workspace/TS100/Core/Drivers/BMA223.cpp b/workspace/TS100/Core/Drivers/BMA223.cpp
index 8abe543f..939b3d31 100644
--- a/workspace/TS100/Core/Drivers/BMA223.cpp
+++ b/workspace/TS100/Core/Drivers/BMA223.cpp
@@ -6,9 +6,7 @@
  */
 
 #include <BMA223.hpp>
-#include "BMA223_defines.h"
 #include <array>
-#define BMA223_ADDRESS 0x18<<1
 
 bool BMA223::detect() {
 	return FRToSI2C::probe(BMA223_ADDRESS);
@@ -22,7 +20,8 @@ static const FRToSI2C::I2C_REG i2c_registers[] = { //
 				{ BMA223_ACCD_HBW, 0b00000000, 0 },    //filtered data out
 				{ BMA223_INT_OUT_CTRL, 0b00001010, 0 },    //interrupt active low and OD to get it hi-z
 				{ BMA223_INT_RST_LATCH, 0b10000000, 0 },    //interrupt active low and OD to get it hi-z
-//				{ BMA223_OFC_CTRL, 0b00000111, 0 },    //High pass en
+				{ BMA223_INT_EN_0, 0b01000000, 0 },    //Enable orientation
+				{ BMA223_INT_A, 0b00100111, 0 },    //Setup orientation detection
 
 		//
 		};
diff --git a/workspace/TS100/Core/Drivers/BMA223.hpp b/workspace/TS100/Core/Drivers/BMA223.hpp
index 06af2436..85fa8576 100644
--- a/workspace/TS100/Core/Drivers/BMA223.hpp
+++ b/workspace/TS100/Core/Drivers/BMA223.hpp
@@ -8,9 +8,8 @@
 #ifndef CORE_DRIVERS_BMA223_HPP_
 #define CORE_DRIVERS_BMA223_HPP_
 #include "I2C_Wrapper.hpp"
-#include "LIS2DH12_defines.hpp"
 #include "BSP.h"
-
+#include "BMA223_defines.h"
 
 class BMA223 {
 public:
@@ -18,21 +17,19 @@ public:
 	static bool initalize();
 	//1 = rh, 2,=lh, 8=flat
 	static Orientation getOrientation() {
-#ifdef ACCEL_ORI_FLIP
-		uint8_t val = (FRToSI2C::I2C_RegisterRead(LIS2DH_I2C_ADDRESS,
-		LIS_INT2_SRC) >> 2);
-		if (val == 8)
-			val = 3;
-		else if (val == 1)
-			val = 1;
-		else if (val == 2)
-			val = 0;
-		else
-			val = 3;
-		return static_cast<Orientation>(val);
-#else
-		return static_cast<Orientation>((FRToSI2C::I2C_RegisterRead(LIS2DH_I2C_ADDRESS,LIS_INT2_SRC) >> 2) - 1);
-#endif
+		uint8_t val = FRToSI2C::I2C_RegisterRead(BMA223_ADDRESS,
+		BMA223_INT_STATUS_3);
+		val >>= 4; //we dont need high values
+		val &= 0b11;
+		if(val &0b10){
+			return ORIENTATION_FLAT;
+		}else{
+			return static_cast<Orientation>(!val);
+		}
+		//0 = rhs
+		//1 =lhs
+		//2 & 3 == ignore
+
 	}
 	static void getAxisReadings(int16_t& x, int16_t& y, int16_t& z);
 
diff --git a/workspace/TS100/Core/Drivers/BMA223_defines.h b/workspace/TS100/Core/Drivers/BMA223_defines.h
index 12481300..c7322a56 100644
--- a/workspace/TS100/Core/Drivers/BMA223_defines.h
+++ b/workspace/TS100/Core/Drivers/BMA223_defines.h
@@ -8,6 +8,7 @@
 #ifndef CORE_DRIVERS_BMA223_DEFINES_H_
 #define CORE_DRIVERS_BMA223_DEFINES_H_
 
+#define BMA223_ADDRESS 0x18<<1
 #define BMA223_BGW_CHIPID 0x00
 #define BMA223_ACCD_X_LSB 0x02
 #define BMA223_ACCD_X_MSB 0x03