Код показывает одинаковые углы для всех трех осей гироскопа MPU

#include<avr/io.h>
float x, y, z, baf = 0, j, i = 0, k, angle, a, b, c, d;
unsigned long current_time = 0;
unsigned long previous_time = 0;
unsigned long time_interval = 0;
float gyro_offset_x = 0.0;
float gyro_offset_y = 0.0;
float gyro_offset_z = 0.0;


void i2cinit(void)
{
  TWSR = 0x00;
  TWBR = 0x14;
  TWCR = (1 << TWEN);
}


void i2cstart(void)
{
  TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
  while (!(TWCR & (1 << TWINT)));
}

void i2cwrite( unsigned char data)
{
  TWDR = data;
  TWCR = (1 << TWINT) | (1 << TWEN);
  while (!(TWCR & (1 << TWINT)));
}

unsigned int i2c_receive(unsigned char isLast) {
  if (isLast == 0)
  {
    TWCR |= (1 << TWINT) | (1 << TWEA) | (1 << TWEN);
  }
  else
  {
    TWCR = (1 << TWINT) | (1 << TWEN);
  }
  while (!(TWCR & (1 << TWINT)));
  return TWDR;
}

void i2cstop(void)
{
  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);

}

void mpu6050_init(void) {
  i2cstart();
  i2cwrite(0xD0);
  i2cwrite(0x6B); // регистр PWR_MGMT_1
  i2cwrite(0);
  i2cstop();
}
void mpu6050f()
{
  i2cstart();
  i2cwrite(0xD0);
  i2cwrite(0x1B);
  i2cwrite(0x00);//здесь изменено
  i2cstop();
}
float mpu6050rbyte(int gyro) {
  i2cstart();
  i2cwrite(0xD0);
  i2cwrite(gyro);
  i2cstart();
  i2cwrite(0xD0 | 0x01);
  float i = i2c_receive(1);
  i2cstop();
  return i;
}
float mpu6050_whole_data(int a, int b) {
  int msb = mpu6050rbyte(a);
  int lsb = mpu6050rbyte(b);
  float x_gyro = (msb << 8) | lsb;

  return x_gyro;
}

void calibrateGyroOffset() {
  const int numSamples = 100;
  for (k = 0; k < numSamples; k++) {
    gyro_offset_x += mpu6050_whole_data(0x43, 0x44);
    gyro_offset_y += mpu6050_whole_data(0x45, 0x46);
    gyro_offset_z += mpu6050_whole_data(0x47, 0x48);
  }

  gyro_offset_x /= numSamples;
  gyro_offset_y /= numSamples;
  gyro_offset_z /= numSamples;
}

float mpu6050angle(float woho, float gyro) {
  float angular_velocity = (woho - gyro) / 131.0;
  previous_time = current_time;
  current_time = millis();
  time_interval = (current_time - previous_time);
  float filter = (0.98 * angular_velocity) + (0.04 * angular_velocity);
  float angle = baf + (filter * time_interval) / 1000.0;
  baf = angle;
  return angle;
}

void setup()
{
  Serial.begin(9600);
  i2cinit();
  mpu6050_init();
  mpu6050f();
  calibrateGyroOffset();
}
void loop()
{
  x = mpu6050_whole_data(0x43, 0x44);
  y = mpu6050_whole_data(0x45, 0x46);
  z = mpu6050_whole_data(0x47, 0x48);
  // a = mpu6050angle(y, gyro_offset_msb);
  // b = mpu6050angle(z, gyro_offset_lsb);
  j = mpu6050angle(x, gyro_offset_x);
  c = mpu6050angle(y, gyro_offset_y);
  d = mpu6050angle(z, gyro_offset_z);
  // mpu6050r(0x43, &x, &y, &z);
  // mpu6050_init();
  // Serial.print("Смещение x");
  // Serial.print(gyro_offset_x) ;
  // Serial.print(" ");
  // Serial.print("Смещение по Y");
  // Serial.print(y) ;
  // Serial.print(" ");
  // // Serial.print("Смещение z");
  // Serial.print(z) ;
  // Serial.print(" ");
  Serial.print("Angle x: ");
  Serial.print(j);
  Serial.print("      ");
  Serial.print("Angle y: ");
  Serial.print(c);
  Serial.print("      ");
  Serial.print("Angle z: ");
  Serial.println(d);
}