diff --git a/tests/gy521_driver/test_gy521_driver.c b/tests/gy521_driver/test_gy521_driver.c
index 2471611..c18413a 100644
--- a/tests/gy521_driver/test_gy521_driver.c
+++ b/tests/gy521_driver/test_gy521_driver.c
@@ -167,6 +167,51 @@ static void test_gy521_update_gyro(void **sate)
 }
 
 
+static void test_gy521_self_test(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*/
+    idx = 0;
+    
+    
+    /*Setup the full-scale range to +-250dps*/
+    
+    /*Load up the gyro regs with passing test values*/
+    for(uint8_t i = 0; i < 6; i++) {
+        reg_addr_arr[i].value = 2; 
+    }
+    gy521_update_gyro(m);
+
+
+
+    /*Load up the testing registers 13-15 with the FT values*/     
+    /*FT: factory trim*/
+    idx = 0;
+    for(int i = 0; i < 3; i++){
+        reg_addr_arr[i].value = 2;  /*The values all have to be 4bit*/
+    }
+
+    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.*/
+    
+    /*Load up bad values*/
+    idx = 0;
+    for(int i = 0; i < 6; i++){
+        reg_addr_arr[i].value = 16;  /*The values all have to be 4bit*/
+    }
+    
+    result = gy521_self_test(m);
+    assert_true(result);   /*The value should be greater than one.*/
+
+    /*Free the struct*/
+    gy521_free(m);
+}
 
 
 int main(void)
@@ -175,6 +220,7 @@ int main(void)
         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);
 }