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

updated the code and corrected some mistakes

This commit is contained in:
Jake Goodwin 2023-09-06 22:56:24 -07:00
parent 8161e5c857
commit 530ea98b28
1 changed files with 73 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
src/gy521_driver/gy521_driver.c
View File

@ -5,6 +5,13 @@
*/
#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
@ -30,11 +37,11 @@ extern void _test_free(void* const ptr, const char* file, const int line);
#define XG_ST (1<<7)
#define YG_ST (1<<6)
#define ZG_ST (1<<5)
#define FS_SEL_MSK 0b00011000
#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 (3<<3)
#define FS_SEL_2000 (1<<4)|(1<<3)
@ -146,6 +153,12 @@ struct gy521_module{
accel_values_struct accel;
};
struct ft_vals{
float x;
float y;
float z;
};
/*
* ############################
* Helper/Private Functions
@ -166,7 +179,8 @@ uint8_t read_register(gy521_module *m, enum gy521_map reg)
void write_register(gy521_module *m, enum gy521_map reg, uint8_t data)
{
gy521_twi_tx(m->slave_address, (uint8_t *) &reg, data);
uint8_t in_data = reg;
gy521_twi_tx(m->slave_address, &in_data, 2);
}
@ -216,8 +230,10 @@ 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);
}
@ -227,8 +243,10 @@ 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);
}
@ -258,7 +276,7 @@ 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;
uint8_t reg = XG_ST + YG_ST + ZG_ST + FS_SEL_250;
write_register(m, gyro_config, reg);
}
@ -267,45 +285,56 @@ 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)
{
/*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;
}
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);
@ -318,22 +347,28 @@ self_test_results gy521_self_test(struct gy521_module* m)
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 = (STR.x - ft_gx) / ft_gx;
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 = (STR.y - ft_gy) / ft_gy;
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 = (STR.z - ft_gz) / ft_gz;
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;
}