From 56e568643b801d27da5b25423364775ad2eb2690 Mon Sep 17 00:00:00 2001
From: jake <jakegoodwin@tutanota.com>
Date: Thu, 31 Aug 2023 17:07:23 -0700
Subject: [PATCH] updated code to use read_register functions

---
 src/gy521_driver/gy521_driver.c | 53 +++++++++++++++++++++++----------
 1 file changed, 37 insertions(+), 16 deletions(-)

diff --git a/src/gy521_driver/gy521_driver.c b/src/gy521_driver/gy521_driver.c
index 8018cf2..429ebc1 100644
--- a/src/gy521_driver/gy521_driver.c
+++ b/src/gy521_driver/gy521_driver.c
@@ -19,6 +19,7 @@ extern void _test_free(void* const ptr, const char* file, const int line);
 #endif // UNIT_TESTING
 
 #define     NUM_ACCEL_REGS  6
+#define     NUM_GYRO_REGS   6
 
 /*
  * ############################
@@ -77,14 +78,21 @@ struct gy521_module{
  * ############################
  */ 
     
-uint8_t read_register(enum gy521_map reg)
+uint8_t read_register(gy521_module *m, enum gy521_map reg)
 {
-    return 0;
+    /*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;
 }
 
-uint8_t write_register(enum gy521_map reg)
+void write_register(gy521_module *m, enum gy521_map reg, uint8_t data)
 {
-    return 0;
+    gy521_twi_tx(m->slave_address, (uint8_t *) &reg, data);
 }
 
 
@@ -96,7 +104,6 @@ uint8_t write_register(enum gy521_map reg)
 
 gy521_module* gy521_new(void)
 {
-    //gy521_module *m = malloc(sizeof(gy521_module));
     gy521_module *m = (gy521_module *)malloc(sizeof(gy521_module));
     m->slave_address = 0x0;
     return m;
@@ -110,6 +117,7 @@ void gy521_free(gy521_module *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;
@@ -118,10 +126,8 @@ _Bool gy521_init(gy521_module *m, uint8_t slave_address) {
     /*Set the slave address of the module*/
     m->slave_address = slave_address;
 
-    /*Try  to talk with i2c module.*/
-    uint8_t data = who_am_i;
-    gy521_twi_tx(m->slave_address, &data, 1);
-    gy521_twi_rx(m->slave_address, &data, 1);
+    /*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;
@@ -131,11 +137,6 @@ _Bool gy521_init(gy521_module *m, uint8_t slave_address) {
 }
 
 
-void gy521_update_gyro(gy521_module *m)
-{
- 
-}
-
 void gy521_update_accel(gy521_module *m)
 {
     uint8_t read_regs[NUM_ACCEL_REGS] = {accel_xouth, accel_xoutl,
@@ -143,8 +144,10 @@ void gy521_update_accel(gy521_module *m)
             accel_zouth, accel_zoutl};
 
     /*update individually the structure by communicating with the device.*/
-    gy521_twi_rx(m->slave_address, read_regs, NUM_ACCEL_REGS);
-   
+     for(uint8_t i = 0; i < NUM_ACCEL_REGS; i++) {
+        read_regs[i] = read_register(m, read_regs[i]);
+    }
+
     /*bitshift to reassembly the high/low regs into a single 16bit*/
     m->accel.x = (read_regs[0] << 8) | read_regs[1];
     m->accel.y = (read_regs[2] << 8) | read_regs[3];
@@ -152,4 +155,22 @@ void gy521_update_accel(gy521_module *m)
 
 }
 
+void gy521_update_gyro(gy521_module *m)
+{
+    uint8_t read_regs[NUM_GYRO_REGS] = {gyro_xouth, gyro_xoutl,
+            gyro_youth, gyro_youth,
+            gyro_zouth, gyro_zoutl};
+
+    /*update individually the structure by communicating with the device.*/
+    for(uint8_t i = 0; i < NUM_GYRO_REGS; i++) {
+        read_regs[i] = read_register(m, read_regs[i]);
+    }
+
+    /*bitshift to reassembly the high/low regs into a single 16bit*/
+    m->gyro.x = (read_regs[0] << 8) | read_regs[1];
+    m->gyro.y = (read_regs[2] << 8) | read_regs[3];
+    m->gyro.z = (read_regs[4] << 8) | read_regs[5];
+
+}
+