diff --git a/src/gy521_driver/gy521_driver.c b/src/gy521_driver/gy521_driver.c
index 6b0ab0a..5d475de 100644
--- a/src/gy521_driver/gy521_driver.c
+++ b/src/gy521_driver/gy521_driver.c
@@ -21,13 +21,64 @@ extern void _test_free(void* const ptr, const char* file, const int line);
 #define     NUM_ACCEL_REGS  6
 #define     NUM_GYRO_REGS   6
 
+/*GYRO MIN MAX PARAMETERS(percentage)*/
+#define     GYRO_MIN_ST -14
+#define     GYRO_MAX_ST 14
+
+
+/*GYRO_CONFIG*/
+#define     XG_ST   (1<<7)
+#define     YG_ST   (1<<6)
+#define     ZG_ST   (1<<5)
+#define     FS_SEL_MSK  0b00011000
+#define     FS_SEL_250  0
+#define     FS_SEL_500  (1<<3)
+#define     FS_SEL_1000 (1<<4)
+#define     FS_SEL_2000 (3<<3)
+
+
+
 /*
  * ############################
  * REGISTER MAP 
  * ############################
  */ 
 
+/*This thing is fairly huge, but it's got pretty much all the registers
+ * on it.*/
+
 enum gy521_map {
+    self_test_x = 0x0D,
+    self_test_y,
+    self_test_z,
+    self_test_a,
+    smplrt_div = 0x19,
+    config,
+    gyro_config,
+    accel_config,
+    fifo_en = 0x23,
+    i2c_mst_ctrl,
+    i2c_slv0_addr,
+    i2c_slv0_reg,
+    i2c_slv0_ctrl,
+    i2c_slv1_addr,
+    i2c_slv1_reg,
+    i2c_slv1_ctrl,
+    i2c_slv2_addr,
+    i2c_slv2_reg,
+    i2c_slv2_ctrl,
+    i2c_slv3_addr,
+    i2c_slv3_reg,
+    i2c_slv3_ctrl,
+    i2c_slv4_addr,
+    i2c_slv4_reg,
+    i2c_slv4_do,
+    i2c_slv4_ctrl,
+    i2c_slv4_di,
+    i2c_mst_status,
+    int_pin_cfg,
+    int_enable,
+    int_status,
     accel_xouth = 0x3B,
     accel_xoutl,
     accel_youth,
@@ -42,6 +93,39 @@ enum gy521_map {
     gyro_youtl,
     gyro_zouth,
     gyro_zoutl,
+	ext_sens_data_00,
+	ext_sens_data_01,
+	ext_sens_data_02,
+	ext_sens_data_03,
+	ext_sens_data_04,
+	ext_sens_data_05,
+	ext_sens_data_06,
+	ext_sens_data_07,
+	ext_sens_data_08,
+	ext_sens_data_09,
+	ext_sens_data_10,
+	ext_sens_data_11,
+	ext_sens_data_12,
+	ext_sens_data_13,
+	ext_sens_data_14,
+	ext_sens_data_15,
+	ext_sens_data_16,
+	ext_sens_data_17,
+	ext_sens_data_18,
+	ext_sens_data_19,
+	ext_sens_data_20,
+	ext_sens_data_21,
+	ext_sens_data_22,
+	ext_sens_data_23,
+	i2c_slv0_do,
+	i2c_slv1_do,
+	i2c_slv2_do,
+	i2c_slv3_do,
+    i2c_mst_delay_ctrl,
+    signal_path_reset,
+    user_ctrl,
+    pwr_mgmt_1,
+    pwr_mgmt_2,
     fifo_couth = 0x72,
     fifo_contl,
     fifo_r_w,
@@ -168,3 +252,93 @@ struct gyro_values gy521_get_gyro(struct gy521_module* m)
     s.z = m->gyro.z;
     return s;
 }
+
+
+void enable_self_test_gyro(struct gy521_module* m)
+{
+    /*Set the values in the GYRO_CONFIG register*/
+    /*According to the datasheet setting it for 250 is needed*/
+    uint8_t reg = XG_ST | YG_ST | ZG_ST | FS_SEL_250;
+    write_register(m, gyro_config, reg);
+}
+
+void disable_self_test_gyro(struct gy521_module* m)
+{
+    write_register(m, gyro_config, 0x00);
+}
+
+/*Self Test register functions*/
+self_test_results gy521_self_test(struct gy521_module* m)
+{
+    /*STR: self test response*/
+    /*STR = (gyro_output with self_test_enabled) - (gyro_output with self_test_disabled)*/
+    /*FT: Factory trim value of selftest response, availble via motion apps software.*/
+
+    /*change from factory trim of STR(%) = (STR - FT) / FT*/
+    /*The Part will have failed if it's not within the datasheets specs*/
+
+    /*0. Enable self test*/
+    enable_self_test_gyro(m); 
+
+    /*1. read the reg FT values*/ 
+    uint8_t xg_test = read_register(m, self_test_x) & 0x0F;
+    uint8_t yg_test = read_register(m, self_test_y) & 0x0F;
+    uint8_t zg_test = read_register(m, self_test_z) & 0x0F;
+   
+   
+    float ft_gx = 1.046;
+    float ft_gy = 1.046;
+    float ft_gz = 1.046;
+
+    /*2. calulate the FT using the formula from the datasheet*/
+    if(xg_test > 0) {
+        xg_test -= 1;
+        for(uint8_t i = 0; i < xg_test; i++) {ft_gx *= 1.046;}
+        ft_gx *= 25 * 131;
+    }
+    if(yg_test > 0) {
+        yg_test -= 1;
+        for(uint8_t i = 0; i < yg_test; i++) {ft_gy *= 1.046;}
+        ft_gy *= 25 * 131;
+    }
+     if(zg_test > 0) {
+        zg_test -= 1;
+        for(uint8_t i = 0; i < zg_test; i++) {ft_gz *= 1.046;}
+        ft_gz *= 25 * 131;
+    }
+   
+    /*3. Read the STR data from the gyro*/
+    gy521_update_gyro(m);
+    struct gyro_values STR = m->gyro;
+
+    /*4. Disable the self test mode.*/
+    disable_self_test_gyro(m);
+    
+    /*5. Now get the normal gyro data*/  
+    gy521_update_gyro(m);
+
+    /*6. Finish the STR calculation*/
+    STR.x -= m->gyro.x;
+    STR.y -= m->gyro.y;
+    STR.z -= m->gyro.z;
+
+    /*7. Now we calculate the change from the FT of the STR*/
+    float testing_value = (STR.x - ft_gx) / ft_gx;
+    if(testing_value > GYRO_MAX_ST || testing_value < GYRO_MIN_ST) {
+        return gyro_failed;
+    }
+
+    testing_value = (STR.y - ft_gy) / ft_gy;
+    if(testing_value > GYRO_MAX_ST || testing_value < GYRO_MIN_ST) {
+        return gyro_failed;
+    }
+
+    testing_value = (STR.z - ft_gz) / ft_gz;
+    if(testing_value > GYRO_MAX_ST || testing_value < GYRO_MIN_ST) {
+        return gyro_failed;
+    }
+
+    /*Return that we passed the self test if it's within parameters*/
+    return passed;
+}
+
diff --git a/src/gy521_driver/gy521_driver.h b/src/gy521_driver/gy521_driver.h
index 6349f47..5a8fa83 100644
--- a/src/gy521_driver/gy521_driver.h
+++ b/src/gy521_driver/gy521_driver.h
@@ -25,6 +25,7 @@
  * Types/Structures 
  * ############################
  */
+
 typedef struct gyro_values{
     uint16_t x;
     uint16_t y;
@@ -39,6 +40,12 @@ typedef struct accel_values{
 
 typedef struct gy521_module gy521_module;
 
+typedef enum{
+    passed = 0,
+    gyro_failed,
+    accel_failed,
+}self_test_results;
+
 /*
  * ############################
  * Function Prototypes
@@ -63,4 +70,7 @@ void gy521_free(struct gy521_module *m);
 struct accel_values gy521_get_accel(struct gy521_module* m);
 struct gyro_values gy521_get_gyro(struct gy521_module* m);
 
+self_test_results gy521_self_test(struct gy521_module* m);
+
+
 #endif /* GY521_DRIVER_H */