diff --git a/fw/Makefile b/fw/Makefile
index 5aa6221..1ed1a84 100644
--- a/fw/Makefile
+++ b/fw/Makefile
@@ -12,7 +12,7 @@ U8G2_SRC:=$(U8G2_DIR)/u8x8_d_$(DISPLAY).c $(filter-out $(U8G2_DIR)/u8x8_d_%.c, \
 	$(filter-out $(U8G2_DIR)/mui%.c, $(wildcard $(U8G2_DIR)/*.c)))
 
 EXTRA_CFLAGS += -I$(U8G2_DIR)
-ADDITIONAL_C_FILES += lib_i2c.c display.c
+ADDITIONAL_C_FILES += lib_i2c.c display.c sc7a20.c
 
 include ch32fun/ch32fun/ch32fun.mk
 
diff --git a/fw/filter.h b/fw/filter.h
index b1aaf6e..6d4deb9 100644
--- a/fw/filter.h
+++ b/fw/filter.h
@@ -11,5 +11,10 @@
 #define U16_FP_EMA_K8(x, s)  (u16)((((u32)(x)<<8) - (x) + (s)) >> 8)
 #define U16_FP_EMA_K16(x, s) (u16)((((u32)(x)<<16) - (x) + (s)) >> 16)
 
+#define I16_FP_EMA_K2(x, s)  (s16)((((s32)(x)<<2) - (x) + (s)) >> 2)
+#define I16_FP_EMA_K4(x, s)  (s16)((((s32)(x)<<4) - (x) + (s)) >> 4)
+#define I16_FP_EMA_K8(x, s)  (s16)((((s32)(x)<<8) - (x) + (s)) >> 8)
+#define I16_FP_EMA_K16(x, s) (s16)((((s32)(x)<<16) - (x) + (s)) >> 16)
+
 
 #endif // _FILTER_H
diff --git a/fw/main.c b/fw/main.c
index f9368b7..3434ac0 100644
--- a/fw/main.c
+++ b/fw/main.c
@@ -9,6 +9,7 @@
 #include "lib_i2c.h"
 #include "display.h"
 #include "filter.h"
+#include "sc7a20.h"
 
 
 // Pin definitions
@@ -324,6 +325,7 @@ __attribute__((noreturn)) int main(void)
 	Delay_Ms(500);
 
 	u8g2 = display_init();
+	sc7a20_init();
 
 	// Init USBPD
 	USBPD_VCC_e vcc = eUSBPD_VCC_3V3;
@@ -396,13 +398,13 @@ __attribute__((noreturn)) int main(void)
 
 		poll_input(); // usb
 
-		vbus_mv = U16_FP_EMA_K2(vbus_mv, ((u32)adc_buffer[0]*VCC_MV*11)/4096);
-		current_ma = U16_FP_EMA_K2(current_ma, get_current_ma(adc_buffer[1]));
-		temp_k = U16_FP_EMA_K2(temp_k, get_temp_k(adc_buffer[2]));
+		vbus_mv = U16_FP_EMA_K4(vbus_mv, ((u32)adc_buffer[0]*VCC_MV*11)/4096);
+		current_ma = U16_FP_EMA_K4(current_ma, get_current_ma(adc_buffer[1]));
+		temp_k = I16_FP_EMA_K4(temp_k, get_temp_k(adc_buffer[2]));
 		if (!adc_injection_conversion()) {
 		    printf("injection conversion failed");
 		} else {
-		    tip_mv = U16_FP_EMA_K2(tip_mv, (u32)(injection_results[0]*VCC_MV)/4096);
+		    tip_mv = U16_FP_EMA_K4(tip_mv, (u32)(injection_results[0]*VCC_MV)/4096);
 		}
 
 		u8g2_ClearBuffer(u8g2);
@@ -411,14 +413,26 @@ __attribute__((noreturn)) int main(void)
 		u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
 #define x_off 0
 #define y_off 8
-		u8g2_DrawStr(u8g2, x_off+0, y_off+7, "TIP:");
-		u8g2_DrawStr(u8g2, x_off+20, y_off+7, u8x8_u16toa(tip_mv, 4));
-		u8g2_DrawStr(u8g2, x_off+0, y_off+15, "VBUS:");
-		u8g2_DrawStr(u8g2, x_off+25, y_off+15, u8x8_u16toa(vbus_mv, 4));
-		u8g2_DrawStr(u8g2, x_off+51, y_off+7, "TEMP:");
-		u8g2_DrawStr(u8g2, x_off+75, y_off+7, u8x8_u16toa(temp_k, 2));
-		u8g2_DrawStr(u8g2, x_off+51, y_off+15, "V:");
-		u8g2_DrawStr(u8g2, x_off+60, y_off+15, u8x8_u16toa(max_v, 2));
+		static bool mode = true;
+		if (mode) {
+			u8g2_DrawStr(u8g2, x_off+0, y_off+7, "TIP:");
+			u8g2_DrawStr(u8g2, x_off+20, y_off+7, u8x8_u16toa(tip_mv, 4));
+			u8g2_DrawStr(u8g2, x_off+0, y_off+15, "VBUS:");
+			u8g2_DrawStr(u8g2, x_off+25, y_off+15, u8x8_u16toa(vbus_mv, 4));
+			u8g2_DrawStr(u8g2, x_off+51, y_off+7, "TEMP:");
+			u8g2_DrawStr(u8g2, x_off+75, y_off+7, u8x8_u16toa(temp_k, 2));
+			u8g2_DrawStr(u8g2, x_off+51, y_off+15, "V:");
+			u8g2_DrawStr(u8g2, x_off+60, y_off+15, u8x8_u16toa(max_v, 2));
+		} else {
+			static int16_t ax, ay, az;
+			sc7a20_get_readings(&ax, &ay, &az);
+			u8g2_DrawStr(u8g2, x_off+0, y_off+7, ax > 0 ? "AX:+" : "AX:-");
+			u8g2_DrawStr(u8g2, x_off+20, y_off+7, u8x8_u16toa(ax > 0 ? ax : -ax, 5));
+			u8g2_DrawStr(u8g2, x_off+0, y_off+15, ay > 0 ? "AY:+" : "AY:-");
+			u8g2_DrawStr(u8g2, x_off+20, y_off+15, u8x8_u16toa(ay > 0 ? ay : -ay, 5));
+			u8g2_DrawStr(u8g2, x_off+50, y_off+7, az > 0 ? "AZ:+" : "AZ:-");
+			u8g2_DrawStr(u8g2, x_off+70, y_off+7, u8x8_u16toa(az > 0 ? az : -az, 5));
+		}
 
 		u8g2_SendBuffer(u8g2);
 
@@ -426,6 +440,10 @@ __attribute__((noreturn)) int main(void)
 			USBPD_SelectPDO(idx_9v, 0);
 			Delay_Ms(200);
 		}
+		if (encoder != 0) {
+			mode = !mode;
+			encoder = 0;
+		}
 
 //		printf("VBUS=%d, CURRENT=%d, TEMP=%d, TIP=%d, COUNTER=%d\n", vbus_mv, current_ma, temp_k, tip_mv, encoder);
 
diff --git a/fw/sc7a20.c b/fw/sc7a20.c
new file mode 100644
index 0000000..e2239cd
--- /dev/null
+++ b/fw/sc7a20.c
@@ -0,0 +1,54 @@
+#include <ch32fun.h>
+#include "lib_i2c.h"
+
+#include "sc7a20.h"
+
+
+// Initialization values for control registers, in reg-value pairs
+// Basically setup the unit to run, and enable 4D orientation detection
+// TODO: Make better comments and define values
+static const uint8_t i2c_registers[][2] = {
+    {    SC7A20_CTRL_REG1, 0b01100111}, // 200Hz, XYZ enabled
+    {    SC7A20_CTRL_REG2, 0b00000000}, // Setup filter to 0x00 ??
+    {    SC7A20_CTRL_REG3, 0b00000000}, // INT1 off
+    {    SC7A20_CTRL_REG4, 0b00001000}, // Block mode off,little-endian,2G,High-pres,self test off
+    {    SC7A20_CTRL_REG5, 0b00000100}, // fifo off, D4D on INT1
+    {    SC7A20_CTRL_REG6,       0x00}, // INT2 off
+    {     SC7A20_INT2_CFG, 0b01111110}, // setup for movement detection
+    {     SC7A20_INT2_THS,       0x28}, //
+    {SC7A20_INT2_DURATION,         64}, //
+    {     SC7A20_INT1_CFG, 0b01111110}, //
+    {     SC7A20_INT1_THS,       0x28}, //
+    {SC7A20_INT1_DURATION,         64}
+};
+
+
+void sc7a20_init()
+{
+	// Setup acceleration readings
+	// 2G range
+	// bandwidth = 250Hz
+	// High pass filter on (Slow compensation)
+	// Turn off IRQ output pins
+	// Orientation recognition in symmetrical mode
+	// Hysteresis is set to ~ 16 counts
+	// Theta blocking is set to 0b10
+	for (uint8_t i = 0; i < sizeof(i2c_registers) / sizeof(i2c_registers[0]); i++) {
+		uint8_t bytes[2] = {i2c_registers[i][0], i2c_registers[i][1]};
+		i2c_sendBytes(I2C_TARGET, SC7A20_ADDRESS, bytes, 2);
+	}
+}
+
+void sc7a20_get_readings(int16_t *x, int16_t *y, int16_t *z)
+{
+	// We can tell the accelerometer to output in LE mode which makes this simple
+	int16_t sensorData[3] = {0};
+
+	i2c_sendByte(I2C_TARGET, SC7A20_ADDRESS, SC7A20_OUT_X_L | 0x80);
+	i2c_readBytes(I2C_TARGET, SC7A20_ADDRESS, (u8*)sensorData, 6);
+
+	// Lower byte is sent first
+	*x = (int16_t)sensorData[0];
+	*y = (int16_t)sensorData[1];
+	*z = (int16_t)sensorData[2];
+}
diff --git a/fw/sc7a20.h b/fw/sc7a20.h
new file mode 100644
index 0000000..75b9f88
--- /dev/null
+++ b/fw/sc7a20.h
@@ -0,0 +1,56 @@
+/*
+ * SC7A20_defines.h
+ *
+ *  Created on: 18 Sep. 2020
+ *      Author: Ralim
+ */
+
+#ifndef _SC7A20_H_
+#define _SC7A20_H_
+
+#include <ch32fun.h>
+
+#define SC7A20_ADDRESS 0x18
+
+#define SC7A20_WHO_AM_I_VALUE (0b00010001)
+#define SC7A20_WHO_AMI_I      0x0F
+#define SC7A20_CTRL_REG1      0x20
+#define SC7A20_CTRL_REG2      0x21
+#define SC7A20_CTRL_REG3      0x22
+#define SC7A20_CTRL_REG4      0x23
+#define SC7A20_CTRL_REG5      0x24
+#define SC7A20_CTRL_REG6      0x25
+#define SC7A20_REFERENCE      0x26
+#define SC7A20_STATUS_REG     0x27
+#define SC7A20_OUT_X_L        0x28
+#define SC7A20_OUT_X_L_ALT    0xA8
+#define SC7A20_OUT_X_H        0x29
+#define SC7A20_OUT_Y_L        0x2A
+#define SC7A20_OUT_Y_H        0x2B
+#define SC7A20_OUT_Z_L        0x2C
+#define SC7A20_OUT_Z_H        0x2D
+#define SC7A20_FIFO_CTRL      0x2E
+#define SC7A20_FIFO_SRC       0x2F
+#define SC7A20_INT1_CFG       0x30
+#define SC7A20_INT1_SOURCE    0x31
+#define SC7A20_INT1_THS       0x32
+#define SC7A20_INT1_DURATION  0x33
+#define SC7A20_INT2_CFG       0x34
+#define SC7A20_INT2_SOURCE    0x35
+#define SC7A20_INT2_THS       0x36
+#define SC7A20_INT2_DURATION  0x37
+#define SC7A20_CLICK_CFG      0x38
+#define SC7A20_CLICK_SRC      0x39
+#define SC7A20_CLICK_THS      0x3A
+#define SC7A20_TIME_LIMIT     0x3B
+#define SC7A20_TIME_LATENCY   0x3C
+#define SC7A20_TIME_WINDOW    0x3D
+#define SC7A20_ACT_THS        0x3E
+#define SC7A20_ACT_DURATION   0x3F
+
+
+void sc7a20_init(void);
+void sc7a20_get_readings(int16_t *x, int16_t *y, int16_t *z);
+
+
+#endif /* CORE_DRIVERS_BMA223_DEFINES_H_ */