jakegoodwin
/
gy-521
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

updated register map and created new functions for preforming the mpu6050 selftest on the gyroscope

This commit is contained in:
Jake Goodwin 2023-09-06 00:00:05 -07:00
parent 89291840dd
commit 53990d1d3c
2 changed files with 184 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

174
src/gy521_driver/gy521_driver.c
View File

@ -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;
}

10
src/gy521_driver/gy521_driver.h
View File

@ -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 */