From 53990d1d3c9f0ae696851e90d8fa81193d5eb8d6 Mon Sep 17 00:00:00 2001 From: jake Date: Wed, 6 Sep 2023 00:00:05 -0700 Subject: [PATCH] updated register map and created new functions for preforming the mpu6050 selftest on the gyroscope --- src/gy521_driver/gy521_driver.c | 174 ++++++++++++++++++++++++++++++++ src/gy521_driver/gy521_driver.h | 10 ++ 2 files changed, 184 insertions(+) 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 */