From dc7a81fe88f3246e6f98f2332ddc23edae3c55e1 Mon Sep 17 00:00:00 2001
From: alanbjohnston <alan.b.johnston@gmail.com>
Date: Sun, 13 Dec 2020 16:09:36 -0500
Subject: [PATCH] added EEPROM storage of x, y, and z gyro offsets

---
 arduino/Payload_BME280_MPU6050_Pro_Micro.ino | 50 ++++++++++++++++++++
 1 file changed, 50 insertions(+)

diff --git a/arduino/Payload_BME280_MPU6050_Pro_Micro.ino b/arduino/Payload_BME280_MPU6050_Pro_Micro.ino
index 5544aa72..ac7b71f0 100644
--- a/arduino/Payload_BME280_MPU6050_Pro_Micro.ino
+++ b/arduino/Payload_BME280_MPU6050_Pro_Micro.ino
@@ -2,6 +2,7 @@
 #include <Adafruit_Sensor.h>
 #include <Adafruit_BME280.h>
 #include <MPU6050_tockn.h>
+#include <EEPROM.h>
 
 #define SEALEVELPRESSURE_HPA (1013.25)
 
@@ -19,6 +20,8 @@ int blueLED = 8;
 int Sensor1 = 0;
 int Sensor2 = 0;
 float Sensor3 = 0;
+void eeprom_word_write(int addr, int val);
+int eeprom_word_read(int addr);
 
 void setup() {
 
@@ -42,8 +45,41 @@ void setup() {
   }
   
   mpu6050.begin();
+
+  if (eeprom_word_read(0) == 0xA07)
+  {
+
+    Serial.println("Reading gyro offsets from EEPROM\n");
+    
+    float xOffset = ((float)eeprom_word_read(1))/100.0;
+    float yOffset = ((float)eeprom_word_read(2))/100.0;
+    float zOffset = ((float)eeprom_word_read(3))/100.0;
+
+    Serial.println(xOffset, DEC);
+    Serial.println(yOffset, DEC);
+    Serial.println(zOffset, DEC);   
+    
+    mpu6050.setGyroOffsets(xOffset, yOffset, zOffset);   
+  }
+  else
+  {
+
+  Serial.println("Calculating gyro offsets and storing in EEPROM\n");
+  
   mpu6050.calcGyroOffsets(true);
 
+  eeprom_word_write(0, 0xA07);
+  eeprom_word_write(1, (int)(mpu6050.getGyroXoffset() * 100.0) + 0.5);
+  eeprom_word_write(2, (int)(mpu6050.getGyroYoffset() * 100.0) + 0.5);
+  eeprom_word_write(3, (int)(mpu6050.getGyroZoffset() * 100.0) + 0.5);
+
+  Serial.println(eeprom_word_read(0), HEX);
+  Serial.println(((float)eeprom_word_read(1))/100.0, DEC);
+  Serial.println(((float)eeprom_word_read(2))/100.0, DEC);
+  Serial.println(((float)eeprom_word_read(3))/100.0, DEC);
+    
+  }
+      
   pinMode(greenLED, OUTPUT);
   pinMode(blueLED, OUTPUT);
 
@@ -204,3 +240,17 @@ void loop() {
   
   delay(100);
 }
+
+void eeprom_word_write(int addr, int val)
+{
+  
+  EEPROM.write(addr * 2, lowByte(val));
+  EEPROM.write(addr * 2 + 1, highByte(val));
+
+}
+
+int eeprom_word_read(int addr)
+{
+  return((EEPROM.read(addr * 2 + 1) << 8) | EEPROM.read(addr * 2));
+}
+