diff --git a/tests/gy521_driver/test_gy521_driver.c b/tests/gy521_driver/test_gy521_driver.c
index 6860f22..4a42f6c 100644
--- a/tests/gy521_driver/test_gy521_driver.c
+++ b/tests/gy521_driver/test_gy521_driver.c
@@ -19,10 +19,19 @@ typedef struct reg_addr{
 reg_addr reg_addr_arr[REG_SIZE] = {{0x0, 0x0, 0x0}};
 uint8_t idx = 0;
 
+void mock_twi_tx(uint8_t slave_addr, uint8_t *data, uint8_t size)
+{
+    check_expected(slave_addr);
+    check_expected_ptr(data);
+    check_expected(size);
+
+}
+
+
+
 /* Fake Object for TWI_TX*/
 void fake_twi_tx(uint8_t slave_addr, uint8_t *data, uint8_t size)
 {
-
     /*read the device address.*/
     reg_addr_arr[idx].dev_addr = slave_addr;
 
@@ -47,6 +56,7 @@ void fake_twi_tx(uint8_t slave_addr, uint8_t *data, uint8_t size)
 /* Fake Object for TWI_RX*/
 void fake_twi_rx(uint8_t slave_addr, uint8_t *data, uint8_t size)
 {
+
     /*Read the device address*/
     reg_addr_arr[idx].dev_addr = slave_addr;
 
@@ -76,6 +86,7 @@ void (*gy521_twi_rx)(uint8_t, uint8_t*, uint8_t) = &fake_twi_rx;
  */ 
 
 
+
 void print_reg_arr(void)
 {
     printf("FAKE REGISTERS::\n");
@@ -99,6 +110,22 @@ void clear_twi(void)
 }
 
 
+static int setup_gy521_instance(void **state)
+{
+    clear_twi();
+    *state = gy521_new();
+    assert_non_null(*state);
+    return 0;
+}
+
+static int teardown_gy521_instance(void **state)
+{
+    gy521_free(*state);
+    clear_twi();
+    return 0;
+}
+
+
 /*
  * ############################
  * TESTS
@@ -216,7 +243,7 @@ static void test_gy521_update_gyro(void **sate)
 }
 
 
-static void test_gy521_self_test(void **sate)
+static void test_gy521_self_test_gyro(void **sate)
 {
     /*Create instance of struct.*/  
     gy521_module *m  = gy521_new();
@@ -264,13 +291,80 @@ static void test_gy521_self_test(void **sate)
 }
 
 
+static void test_gy521_self_test_accel(void **sate)
+{
+    /*Create instance of struct.*/  
+    gy521_module *m  = gy521_new();
+    reg_addr_arr[0].addr = TWI_GY521_ADDR1;
+    gy521_init(m, TWI_GY521_ADDR1);
+    
+    /*Zero the global index for the twi*/
+    clear_twi(); 
+    
+    /*Setup the full-scale range to +-250dps*/
+    
+    /*Load up the gyro regs with passing test values*/
+    /*Load up the selftest regs with passing test values.*/
+    /*Load up the gyro regs for disabled ST reads*/
+    for(uint8_t i = 1; i < 17; i++) {   /*(6regs * 2) + 3ST regs = 15*/
+        reg_addr_arr[i].value = 2; 
+    }
+
+    self_test_results result = gy521_self_test(m); 
+    assert_false(result);   /*The value should be zero, aka zero faults*/
+
+    /*Check to make sure it can fail.*/
+    clear_twi();
+    for(uint8_t i = 1; i < 5; i++){
+        reg_addr_arr[i].value = 2;
+    }
+    for(int i = 4; i < 9; i++){
+        reg_addr_arr[i].value = 255;
+    }
+    for(int i = 9; i < 17; i++){
+        reg_addr_arr[i].value = 2;
+    }
+        
+    result = gy521_self_test(m);
+
+
+    /*Print out the register values*/
+    print_reg_arr();
+
+    assert_true(result); 
+
+    /*Free the struct*/
+    gy521_free(m);
+}
+
+
+/*Mock Object/function for TWI RX*/
+
+
+static void test_gy521_testing(void **sate)
+{
+    assert_false(1);
+}
+
 int main(void)
 {
+    int result = 0;
+
+    /*For testing TWI/I2C related stuff*/
+    const struct CMUnitTest twi_group[] = {
+        cmocka_unit_test_setup_teardown(test_gy521_testing, setup_gy521_instance, teardown_gy521_instance),
+        //cmocka_unit_test(test_gy521_testing),
+    }; 
     const struct CMUnitTest tests[] = {
         cmocka_unit_test(test_gy521_init),
         cmocka_unit_test(test_gy521_update_accel),
         cmocka_unit_test(test_gy521_update_gyro),
-        cmocka_unit_test(test_gy521_self_test),
-    }; 
-    return cmocka_run_group_tests(tests, NULL, NULL);
+        cmocka_unit_test(test_gy521_self_test_gyro),
+        cmocka_unit_test(test_gy521_self_test_accel),
+    };
+    //result = cmocka_run_group_tests(twi_group, setup_gy521_instance, teardown_gy521_instance); 
+    result = cmocka_run_group_tests(twi_group, NULL, NULL);
+    result = cmocka_run_group_tests(tests, NULL, NULL);
+
+    return result;
 }