MPU6050 Tilt Angle Measurement (nan Error)

Hello guys I have some problem here hope you guys can help imma make it quick. Had an MPU6050 breakout board. I have been programming Pitch and Roll angle out of it for drone PID. I have been working on it for a month and sadly it doesn’t really function well.

At first, it wasn’t a problem, I actually made it. It works, but, on another board. I used NodeMcu with ESP-12E attached as a test board for my drone. I even ran the PID stabilization test using that and it went fine, all outputs are good, have been monitoring all the data from raw till the filtered data. Until I apply it to my drone which I used an atmega328p from arduino uno. It appears it outputs nan for all my programmed Pitch and Roll values. Like I have explained it went out well on my NodeMcu board somehow.

So as for troubleshooting, I tried i2c Scanner which shows that the Mpu6050 is very well detected. So I tried outputing the raw accelerometer and gyro values which works.So then I ran through all my codes from raw values to find out till where did my Pitch and Roll data starts messing up causing the nan error, then it comes till here:

Gvector = sqrt((accelx*accelx)+(accely*accely)+(accelz*accelz));

This calculation basically collects all the acceleration data of all axis from the effects of gravitational magnitude results in the vector of the gravity itself. I’m not sure if that makes sense but it’s basically calculating 3D vector of gravity using acceleration.

One thing that makes me uncertain if something went wrong with the code, then how can it work on other boards. Except the current board doesn’t have enough memory, but when I uploaded it on my board it only said that around 7% of storage and 10% of the dynamic memory is used, but I don’t really understand it. But I do found out that the NodeMcu Esp-12E that I first used where the code works has 4MB of flash memory meanwhile an atmega328p just had around 32KB I also tried the code on arduino mega with the memory of 256KB which also results in the same output. I also founded some threads about this nan error problem, some people also were talking about not having enough memory.

I only wanted to be certain here before I’m doing a major change in my project here, I really could use some help on this. Well I am not really a professional I just see problems based on other threads and what I could think of, it could be library or the code itself, that being said. Note that I removed the get_mpu_data() and setup_mpu_registers() voids to keep it short.

#include <Wire.h>
#include <math.h>

int16_t accelx, accely, accelz, temp, gyrox, gyroy, gyroz; 
float Avggyrox, Avggyroy;
float Gvector;
float thetaAcc,phiAcc,thetaaccold,phiaccold,thetaFacc, phiFacc;
float thetaGyr,phiGyr;
float theta,phi, thetaF, phiF, thetaold, phiold;
float dt;
unsigned long millisOld;
int x;
boolean startup = false;

void setup() {
  Wire.begin();
  setup_mpu6050_registers();
  Serial.begin(9600);
  calib();
  millisOld = millis();
}

void calib(){
  for(x;x <1000;x++){
    get_mpu6050_data();
    Avggyrox += gyrox;
    Avggyroy += gyroy;
    delay(2);
  }
  Avggyrox /= 1000;
  Avggyroy /= 1000;
}

void loop() {
  get_mpu6050_data();

  gyrox -= Avggyrox;
  gyroy -= Avggyroy;
  dt = (millis() - millisOld) /1000.0;
  millisOld = millis();
  thetaGyr += gyroy*dt/65.5;
  phiGyr += gyrox*dt/65.5;

  Gvector = sqrt((accelx*accelx)+(accely*accely)+(accelz*accelz));
  thetaAcc = -asin((accelx/Gvector)/(Gvector/Gvector))*(180/PI);
  phiAcc = asin((accely/Gvector)/(Gvector/Gvector))*(180/PI);
  phiFacc = 0.95*phiaccold + 0.05*phiAcc;     //accelerometer low pass filter
  thetaFacc = 0.95*thetaaccold + 0.05*thetaAcc;

  theta = 0.97*(theta + gyroy*dt/65.5) + 0.03*thetaFacc;  //complementary filter
  phi = 0.97*(phi + gyrox*dt/65.5) + 0.03*phiFacc;
  thetaF = 0.7*thetaold + 0.3*theta;   //low pas filter of complementary filter
  phiF = 0.7*phiold + 0.3*phi;  

  Serial.print(Gvector);
  
  thetaaccold = thetaFacc;
  phiaccold = phiFacc;
  thetaold = thetaF;
  phiold = phiF;
}