Я программирую высотомер с помощью Arduino, данные гироскопа такие же, как я могу это исправить?

Question

У меня возникают некоторые проблемы с точностью изменения высоты и выясняется, как отключить пирозаряд, когда ракета начинает падать. Также у меня есть проблема с данными гироскопа, он дает мне те же точные данные для осей X и Y. Мне также нужно, чтобы кто-то просмотрел мой код в целом Части моего кода основаны на коде, который я нашел в интернете.

Этот код предназначен для альтиметра для моей ракеты средней школы, которая поднимется на 56323 метра в воздух и заняла урок год, чтобы закончить, и поэтому я решаю, использовать ли запрограммированный для меня arduino. набор пирозарядов или использовать stratologger и просто использовать Arduino в качестве регистратора данных.

Я использую arduino geniuno uno, mpu-6000 (гироскоп и акселерометр) и ms5611 (барометр)

// Импорт библиотеки #include

//The variables for the gyroscope
float elapsedTime, time, timePrev; // these are time 
int gyro_error =0;// used to calculate error
float Gyro_rawX, Gyro_rawY, Gyro_rawZ; //the raw angle data
float angleX, angleY;
float Gyro_raw_errorX, Gyro_raw_errorY;
float Total_angle_x, Total_angle_y;


void setup(){

  //starting up the I2C device
  Wire.begin();

  Wire.beginTransmission(0x69);//This starts the transmission to the I2C device, while most things online will tell you that it's 0x68 it's not
  Wire.write(0x6B);
  Wire.write(0x00);//reset
  Wire.endTransmission(true);//end Transmission

  //Gyroscope configuration
  Wire.beginTransmission(0x69);
  Wire.write(0x1B);
  Wire.write(0x10);//for the gyroscope use 0x10 for the accelerometer use 0x18
  Wire.endTransmission(true);

  Serial.begin(9600);//for this we are using a 9600 baud rate
  time = millis();

//This gets the data for the gyroscope
if(gyro_error==0)
  {
    for(int i=0; i<200; i++)
    {
      Wire.beginTransmission(0x69);            //begin, Send the slave adress (in this case 68) 
      Wire.write(0x43);                        //First adress of the Gyro data
      Wire.endTransmission(false);
      Wire.requestFrom(0x69,4,true);           //We ask for just 4 registers 

      Gyro_rawX=Wire.read()<<8|Wire.read();     //Once again we shif and sum
      Gyro_rawY=Wire.read()<<8|Wire.read();

      /*---X---*/
      Gyro_raw_errorX = Gyro_raw_errorX + (Gyro_rawX/32.8); 
      /*---Y---*/
      Gyro_raw_errorY = Gyro_raw_errorY + (Gyro_rawY/32.8);

 if(i==199)
      {
        Gyro_raw_errorX = Gyro_raw_errorX/200;
        Gyro_raw_errorY = Gyro_raw_errorY/200;
        gyro_error=1;
      }
    }
  }


  //beeper just to know if it's working
  pinMode(LED, OUTPUT);
  digitalWrite(LED, HIGH);
  delay(400);
  digitalWrite(LED, LOW);
  delay(400);

  }


  void loop(){
    timePrev =time;
    time= millis();
    elapsedTime = (time - timePrev)/1000;

   //Gyroscope
   Wire.beginTransmission(0x69);
   Wire.write(0x43);
   Wire.endTransmission(false);
   Wire.requestFrom(0x69,6,true);

    Gyro_rawX=Wire.read()<<8|Wire.read();     
    Gyro_rawY=Wire.read()<<8|Wire.read();

    Gyro_rawX = (Gyro_rawX/32.8) - Gyro_raw_errorX; 

    Gyro_rawY = (Gyro_rawY/32.8) - Gyro_raw_errorY;

    angleX = Gyro_rawX * elapsedTime;

    angleY = Gyro_rawX * elapsedTime; 

    Total_angle_x = 0.98 *(Total_angle_x + angleX) + 0.02*Accel_angleX;

    Total_angle_y = 0.98 *(Total_angle_y + angleY) + 0.02*Accel_angleY;


    //This prints out rotation
    Serial.print("X rotation");//in the x axis
    Serial.println(Total_angle_x,4);
    Serial.print("Y rotation");//in the y axis
    Serial.println(Total_angle_y,4);


      }

Данные, которые выводятся на последовательный монитор для оси x и y, точно такие же, как x = 1.0302, y = 1.0302. Другая проблема заключается в том, что если я изменю угол и положу его обратно, он не вернется к нулю.