@michaelzenI do apologize for not posting the code.Ā I was tinkering with it so much I forgot.
Great to have another on the case.Ā Together hopefully we can solve it.Ā
I have not tinkered with the sketch for some time since we were stalled at a solution to the data freeze in the I2C bus
Here is the complete sketch
//
// ESP32-GPS-Accel_to_SD
//
// TONY
// MPU6050 Init, Calibrate, Read Data
// Display Max-G X,Y,Z on OLED
// Display GPS, Vmax, Hmax, Sats on OLED
// Write GPS and Accel to SD
// DOES NOT USE INTERRUPT
//
#define BUFFER_LENGTH 32
#include "heltec.h"
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#include <TinyGPS++.h>
#include <mySD.h>
const int cs_sd=26;
int LoopCounter = 0;
int16_t rawTemp;
float tempC;
float tempF;
//GPS RX Pin-17 GREEN WIRE
//GPS TX Pin-23 YELLOW WIRE
TinyGPSPlus gps;
int maxspeed = 0, speed1 = 0;
int maxhigh = 0, high1 = 0;
int maxsatelite = 0, satelite1 = 0;
SSD1306AsciiWire oled;
MPU6050 mpu;
bool dmpReady = false; // set true if DMP init was successful
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
float MaxGx = 0, MaxGy = 0, MaxGz = 0;
// orientation motion variables
Quaternion q; // [w, x, y, z] quaternion container
//VectorInt16 aa; // [x, y, z] accel sensor measurements
//VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
//VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
//VectorFloat gravity; // [x, y, z] gravity vector
//float euler[3]; // [psi, theta, phi] Euler angle container
//float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// ================================================================
// === SETUP ===
// ================================================================
void setup() {
Heltec.begin(true /*DisplayEnable Enable*/, false /*LoRa Enable*/, true /*Serial Enable*/);
oled.begin(&Adafruit128x64, 0x3C, 16);
oled.setFont(System5x7);
oled.clear();
oled.setCursor(25,3);
oled.println("GPS and G-force");
oled.setCursor(30,4);
oled.println("Logger to SD");
delay(2000);
oled.clear();
Wire.begin(); //pin 21 and 22 for ESP32
Wire.setClock(400000); // 400kHz I2C clock
//gives error Wire.setWireTimeout(3000, true); //timeout value in uSec
// GPS RX=17, GPS TX=23
Serial1.begin(9600, SERIAL_8N1, 23, 17);
if (!SD.begin(26, 14, 13, 27))
{
oled.clear();
oled.print(" NO SD Card");
delay(2000);
oled.clear();
}
else
{oled.print(" SD Card OK");
delay(2000);
}
oled.clear();
//SD for GPS Data
File data = SD.open("GPS-data.txt",FILE_WRITE); //Open the file "GPS-data.txt"
data.println("");
data.println("Start Recording"); // Write to file
data.close();
//SD for Accel Data
File data2 = SD.open("ACC-data.txt",FILE_WRITE); //Open the file "ACC-data.txt"
data2.println("");
data2.println("Start Recording"); // Write to file
data2.close();
// initialize serial monitor
Serial.begin(115200);
// initialize I2C devices
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify I2C device connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
oled.setCursor(0,0);
oled.println("Calibrate Acc/Gyro");
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
// returns 0 if MPU6050 working
if (devStatus == 0)
{
// Generate offsets and calibrate MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
oled.print("Accel/Gyro Ready");
delay(1000);
oled.clear();
mpu.setDMPEnabled(true);
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready!"));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
// get FIFO count
fifoCount = mpu.getFIFOCount();
}
else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
//*CODE FOR GPS*
satelite1 = (abs(gps.satellites.value()));
oled.setCursor(0,0);
oled.print("Vmax ");
oled.print("Hmax ");
oled.print("SAT ");
speed1 = (gps.speed.mph());
if ( speed1 > maxspeed)
{
maxspeed = speed1;
}
oled.setCursor(0 , 1);
oled.clearToEOL();
oled.setCursor(10 , 1);
oled.print(maxspeed);
high1 = (gps.altitude.feet());
if ( high1 > maxhigh)
{
maxhigh = high1;
}
oled.setCursor(60 , 1);
oled.print(maxhigh);
oled.setCursor(100 , 1);
oled.print(satelite1);
oled.println(" ");
oled.println(" ");
//Display Lat & Long on OLED
oled.setCursor(0 , 2);
oled.print("LAT ");
oled.println(gps.location.lat(),6);
oled.print("LNG ");
oled.println(gps.location.lng(),6);
//-4 is offset for eastern time zone
String Temps=String(gps.time.hour()-4)+(":")+(gps.time.minute())+(":")+(gps.time.second());
String Date=String(gps.date.month())+("/")+(gps.date.day())+("/")+(gps.date.year());
// Writing GPS data to text file
//if (satelite1 > 1) {
File data=SD.open("GPS-data.txt",FILE_WRITE);
data.println(Date + "," + Temps + "," + String(gps.location.lat(), 6) + "," + String(gps.location.lng(), 6) + "," + String(gps.altitude.feet(),0) + "," + String(gps.speed.mph(),0) + "," + String(satelite1));
data.close();
//}
//Get temperature from MPU6050
rawTemp = mpu.getTemperature();
tempC=(rawTemp/340)+36.53;
tempF=(tempC * 9/5) + 32;
// Write ACCEL G-Force data to text file
//if (satelite1 > 1) {
File data2=SD.open("ACC-data.txt",FILE_WRITE); //Temps is string hour, minute, second
data2.println(Date + "," + Temps + "," + String(q.x, 2) + "," + String(q.y, 2) + "," + String(q.z,2) + "," + String(tempF,1));
data2.close();
//}
//ORIG Code DelayGPS(500);
//Delay of 900 gives 1 second logging interval
DelayGPS(500);
//END GPS CODE
//*BEGIN MPU6050 Code*
//mpu.resetFIFO();
//fifoCount = packetSize;
if(GetCurrentFIFOPacket(fifoBuffer, packetSize) )
{
// gets the latest packet if available and runs your code else this will skip your code because there is
// nothing new to process.
}
// display quaternion values x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
//USE THIS FOR LIVE ACCEL DATA DISPLAY
Serial.print("G-Force\t");
//Serial.print(q.w);
//Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
//calculate Max Accel X,Y,Z
if (abs(q.x) > abs(MaxGx))
{
MaxGx = q.x;
}
else
{
MaxGx = MaxGx;
}
if (abs(q.y) > abs(MaxGy))
{
MaxGy = q.y;
}
else
{
MaxGy = MaxGy;
}
if (abs(q.z) > abs(MaxGz))
{
MaxGz = q.z;
}
else
{
MaxGz = MaxGz;
}
//Display LIVE ACCEL G data on OLED
//oled.setCursor(0,5);
//oled.clearToEOL();
//oled.println("ACCEL X Y Z");
//oled.setCursor(0,6);
//oled.clearToEOL();
//oled.setCursor(20,6);
//oled.print(q.x,2);
//oled.setCursor(60,6);
//oled.print(q.y,2);
//oled.setCursor(95,6);
//oled.println(q.z,2);
//DISPLAY MAX G on OLED
oled.setCursor(0,5);
oled.println("MaxG X Y Z");
oled.setCursor(0,6);
oled.clearToEOL();
oled.setCursor(20,6);
oled.print(MaxGx);
oled.setCursor(60,6);
oled.print(MaxGy);
oled.setCursor(95,6);
oled.println(MaxGz);
oled.setCursor(0,7);
oled.println ("TEMP = " + String(tempF,1) + " F");
//Added for stable MPU data
//REMOVED 9/19 mpu.resetFIFO();
}
//Delay function but continue to receive date from GPS
static void DelayGPS(unsigned long ms)
{
unsigned long start = millis();
do
{
while (Serial1.available())
gps.encode(Serial1.read());
//REMOVED 9/19 mpu.resetFIFO();
} while (millis() - start < ms);
}
//BEGIN FUNCTION GetCurrentFIFOPacket
// Get the latest byte from FIFO buffer no matter how much time has passed.
// === GetCurrentFIFOPacket ===
// ================================================================
// Returns 1) when nothing special was done
// 2) when recovering from overflow
// 0) when no valid data is available
// ======================
//
int8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof
LoopCounter++;
int16_t fifoC;
// This section of code is for when we allowed more than 1 packet to be acquired
uint32_t BreakTimer = micros();
Serial.println("Function Loops=" + String(LoopCounter));
do {
if ((fifoC = mpu.getFIFOCount()) > length) {
if (fifoC > 200) { // if you waited to get the FIFO buffer to > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to reset the buffer and wait for the next to arrive
mpu.resetFIFO(); // Fixes any overflow corruption
fifoC = 0;
while (!(fifoC = mpu.getFIFOCount()) && ((micros() - BreakTimer) <= (11000))); // Get Next New Packet
} else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
uint8_t Trash[BUFFER_LENGTH];
while ((fifoC = mpu.getFIFOCount()) > length) { // Test each time just in case the MPU is writing to the FIFO Buffer
fifoC = fifoC - length; // Save the last packet
uint16_t RemoveBytes;
while (fifoC) { // fifo count will reach zero so this is safe
RemoveBytes = min((int)fifoC, BUFFER_LENGTH); // Buffer Length is different than the packet length this will efficiently clear the buffer
mpu.getFIFOBytes(Trash, (uint8_t)RemoveBytes);
fifoC -= RemoveBytes;
}
}
}
}
if (!fifoC) return 0; // Called too early no data or we timed out after FIFO Reset
// We have 1 packet
if ((micros() - BreakTimer) > (11000)) return 0;
} while (fifoC != length);
//ORIG LINE mpu.getFIFOBytes(data, length); //Get 1 packet
mpu.getFIFOBytes(fifoBuffer, packetSize);
return 1;
}