249 lines
6.4 KiB
C
249 lines
6.4 KiB
C
/*
|
|
* Author: Jake Goodwin
|
|
* Date: 2023
|
|
* Description: The implimentation file for the gy521 module, assumes TWI/I2c
|
|
*/
|
|
#include "gy521_driver.h"
|
|
|
|
/*
|
|
* ############################
|
|
* MACROS
|
|
* ############################
|
|
*/
|
|
|
|
/*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__)
|
|
|
|
/*Mock assertions: redefines assert() calls*/
|
|
extern void mock_assert(const int result, const char* const expression,
|
|
const char * const file, const int line);
|
|
#undef assert
|
|
#define assert(expression) \
|
|
mock_assert((int)(expression), #expression, __FILE__, __LINE__);
|
|
|
|
#endif // UNIT_TESTING
|
|
|
|
|
|
/*
|
|
* ############################
|
|
* 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);
|
|
|
|
/*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 = ( ((float)STR.x) - factory_trim.x) / factory_trim.x;
|
|
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;
|
|
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;
|
|
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;
|
|
}
|
|
|