379 lines
8.7 KiB
C
379 lines
8.7 KiB
C
/*
|
|
* Author: Jake Goodwin
|
|
* Date: 2023
|
|
* Description: The implimentation file for the gy521 module, assumes TWI/I2c
|
|
*/
|
|
#include "gy521_driver.h"
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
|
|
/*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;
|
|
}
|
|
|