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

---
 arduino/Payload_BME280_MPU6050_STM32.ino | 82 +++++++++++++++++++++++-
 1 file changed, 80 insertions(+), 2 deletions(-)

diff --git a/arduino/Payload_BME280_MPU6050_STM32.ino b/arduino/Payload_BME280_MPU6050_STM32.ino
index 45d9783a..80871d87 100644
--- a/arduino/Payload_BME280_MPU6050_STM32.ino
+++ b/arduino/Payload_BME280_MPU6050_STM32.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)
 
@@ -13,6 +14,11 @@ MPU6050 mpu6050(Wire);
 int counter = 0;
 long timer = 0;
 int bmePresent;
+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() {
 
@@ -35,6 +41,40 @@ 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);   
+    
+  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);
+
+  }
+    
+  }
+  else
+  {
+
+  Serial.println("Calculating gyro offsets and storing in EEPROM\n");
+  
   mpu6050.calcGyroOffsets(true);
 }
 
@@ -53,6 +93,10 @@ void loop() {
       delay(500);
       setup(); 
     }
+    
+    if (result == '?')
+    {
+    
     if (bmePresent) {
       Serial.print("OK BME280 ");
       Serial.print(bme.readTemperature());
@@ -73,10 +117,18 @@ void loop() {
     Serial.print(" ");
     Serial.print(mpu6050.getGyroY());
     Serial.print(" ");
-    Serial.println(mpu6050.getGyroZ());
+    Serial.printl(mpu6050.getGyroZ());
+    
+    Serial.print(" XS ");
+    Serial.print(Sensor1);   
+    Serial.print(" ");
+    Serial.print(Sensor2);              
+    Serial.print(" ");
+    Serial.println(Sensor3); 
 
     //  Serial1.println(counter++);
   }
+  }    
 #else
   if (Serial1.available() > 0) {
     digitalWrite(PC13, LOW);   // turn the LED on
@@ -90,6 +142,10 @@ void loop() {
       delay(500);
       setup(); 
     }  
+    
+    if (result == '?')
+    {
+    
     if (bmePresent) {  
       Serial1.print("OK BME280 ");
       Serial1.print(bme.readTemperature());
@@ -110,11 +166,33 @@ void loop() {
     Serial1.print(" ");
     Serial1.print(mpu6050.getGyroY());
     Serial1.print(" ");
-    Serial1.println(mpu6050.getGyroZ());   
+    Serial1.print(mpu6050.getGyroZ());  
+      
+    Serial1.print(" XS ");
+    Serial1.print(Sensor1);   
+    Serial1.print(" ");
+    Serial1.print(Sensor2);              
+    Serial1.print(" ");
+    Serial1.println(Sensor3);   
 
 //  Serial1.println(counter++);
   }
+  }    
 #endif
   
   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));
+}