/* * Author: Jake Goodwin * Date: 2023 * Description: The implimentation file for the gy521 module, assumes TWI/I2c */ #include "gy521_driver.h" #include #include /*DEBUG macro*/ #define DEBUG #define debug_print(fmt, ...) \ do { if (DEBUG) fprintf(stderr, fmt, __VA_ARGS__); } while (0) /*Stuff for cmocka*/ #if UNIT_TESTING extern void* _test_malloc(const size_t size, const char* file, const int line); extern void* _test_calloc(const size_t number_of_elements, const size_t size, const char* file, const int line); extern void _test_free(void* const ptr, const char* file, const int line); #define malloc(size) _test_malloc(size, __FILE__, __LINE__) #define calloc(num, size) _test_calloc(num, size, __FILE__, __LINE__) #define free(ptr) _test_free(ptr, __FILE__, __LINE__) #endif // UNIT_TESTING #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 0x18//0b00011000 #define FS_SEL_250 0 #define FS_SEL_500 (1<<3) #define FS_SEL_1000 (1<<4) #define FS_SEL_2000 (1<<4)|(1<<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, accel_youtl, accel_zouth, accel_zoutl, temp_outh, temp_outl, gyro_xouth, gyro_xoutl, gyro_youth, 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, who_am_i, }; /* * ############################ * Structures * ############################ */ struct gy521_module{ uint8_t slave_address; gyro_values_struct gyro; accel_values_struct accel; }; struct ft_vals{ float x; float y; float z; }; /* * ############################ * Helper/Private Functions * ############################ */ uint8_t read_register(gy521_module *m, enum gy521_map reg) { /*Load the data var with athe address of the register to read*/ uint8_t data = reg; /*Indicate we want to read a value from the slave.*/ gy521_twi_rx(m->slave_address, &data, 1); /*The value should now be in the data var*/ return data; } void write_register(gy521_module *m, enum gy521_map reg, uint8_t data) { uint8_t in_data = reg; gy521_twi_tx(m->slave_address, &in_data, 2); } /* * ############################ * Function Definitions * ############################ */ gy521_module* gy521_new(void) { gy521_module *m = (gy521_module *)malloc(sizeof(gy521_module)); m->slave_address = 0x0; return m; } void gy521_free(gy521_module *m) { free(m); } _Bool gy521_init(gy521_module *m, uint8_t slave_address) { /*Check to make sure it's a valid address for this module*/ if((slave_address != TWI_GY521_ADDR1) && (slave_address != TWI_GY521_ADDR2)) { m->slave_address = 0x0; return 0; } /*Set the slave address of the module*/ m->slave_address = slave_address; /*Make sure TWI is working and read the who_am_i register*/ uint8_t data = read_register(m, who_am_i); if(data != m->slave_address) { return 0; } return 1; } void gy521_update_accel(gy521_module *m) { /*We shift the High byte over by 8 for a u16*/ m->accel.x = read_register(m, accel_xouth) <<8; m->accel.x |= read_register(m, accel_xoutl); m->accel.y = read_register(m, accel_youth) <<8; m->accel.y |= read_register(m, accel_youtl); m->accel.z = read_register(m, accel_zouth) <<8; m->accel.z |= read_register(m, accel_zoutl); } void gy521_update_gyro(gy521_module *m) { /*We shift the High byte over by 8 for a u16*/ m->gyro.x = read_register(m, gyro_xouth) <<8; m->gyro.x |= read_register(m, gyro_xoutl); m->gyro.y = read_register(m, gyro_youth) <<8; m->gyro.y |= read_register(m, gyro_youtl); m->gyro.z = read_register(m, gyro_zouth) <<8; m->gyro.z |= read_register(m, gyro_zoutl); } struct accel_values gy521_get_accel(struct gy521_module* m) { struct accel_values s; s.x = m->accel.x; s.y = m->accel.y; s.z = m->accel.z; return s; } struct gyro_values gy521_get_gyro(struct gy521_module* m) { struct gyro_values s; s.x = m->gyro.x; s.y = m->gyro.y; 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); } struct ft_vals self_test_ft_calculation(struct gy521_module* m) { /*We make an array with 0 --> x, 1--> y, 2-->z*/ float factory_trim_vals[3]; uint8_t gyro_test[3] = {0}; for(uint8_t i = 0; i < 3; i++) { /*Reads the registers for self testing the gyro*/ gyro_test[i] = read_register(m, self_test_x + i) & 0x0F; /*Skip doing calculations if the test value is zero*/ if(gyro_test[i] == 0){ factory_trim_vals[i] = 0; continue; } else if(gyro_test[i] == 1) { factory_trim_vals[i] = 1; } else { factory_trim_vals[i] = 1.046; for(uint8_t j = 0; j < (gyro_test[i] - 1); j++){ factory_trim_vals[i] *= 1.046; } } factory_trim_vals[i] *= (25 * 131); } struct ft_vals ft; ft.x = factory_trim_vals[0]; ft.y = factory_trim_vals[1]; ft.z = factory_trim_vals[2]; return ft; } /*Self Test register functions*/ self_test_results gy521_self_test(struct gy521_module* m) { /*0. Enable self test*/ enable_self_test_gyro(m); /*1. read the reg FT values*/ /*2. calulate the FT using the formula from the datasheet*/ struct ft_vals factory_trim = self_test_ft_calculation(m); printf("ft_vals: %.6f, %.6f, %.6f\n", factory_trim.x, factory_trim.y, factory_trim.z); /*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*/ printf("gy.x: %d, gy.y: %d, gy.z: %d\n", m->gyro.x, m->gyro.y, m->gyro.z); STR.x -= m->gyro.x; STR.y -= m->gyro.y; STR.z -= m->gyro.z; printf("str.x: %d, str.y: %d, str.z: %d\n", STR.x, STR.y, STR.z); /*7. Now we calculate the change from the FT of the STR*/ float testing_value = ( ((float)STR.x) - factory_trim.x) / factory_trim.x; printf("valuex: %.6f\n", testing_value); if(testing_value > GYRO_MAX_ST || testing_value < GYRO_MIN_ST) { return gyro_failed; } testing_value = ( ((float)STR.y) - factory_trim.y) / factory_trim.y; printf("valuey: %.6f\n", testing_value); if(testing_value > GYRO_MAX_ST || testing_value < GYRO_MIN_ST) { return gyro_failed; } testing_value = ( ((float)STR.z) - factory_trim.z) / factory_trim.z; printf("valuez: %.6f\n", testing_value); 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; }