Главная » Форумы » Программирование

Трагическая нестабильность.

Нет ответов
Вс, 23/11/2014 - 22:40
Шестерин
Offline
Зарегистрирован: 22.06.2014

Доброго времени суток.
Работаю с вот таким девайсом:https://arduino.ru/forum/apparatnye-voprosy/giroskop-gy-521-na-osnove-mpu-6050
и ардуино мега. Эти два девайса и еще куча всякой мишуры соединены вместе.
На этом контроллере, с этим устройством пробовал стандартные примеры для библиотек к нему же- все работает.
Суть проблемы:
При включении в основную дофига сложную и пока корявую программу практически неизмененного кода(того самого, что в самостоятельном скетче работает стабильно), программа выполняется настабильно и рано или поздно(неизвестно, когда и, по моему, это самое "когда" зависит от производительности, хотя черт его знает) зависает.
Вот пример скетча, который сам по себе работает:

#include <Wire.h> 
 #include "Kalman.h" 
 
 
 Kalman kalmanX; 
 Kalman kalmanY; 
 
 uint8_t IMUAddress = 0x68; 
 
 /* IMU Data */ 
 int16_t accX; 
 int16_t accY; 
 int16_t accZ; 
 int16_t tempRaw; 
 int16_t gyroX; 
 int16_t gyroY; 
 int16_t gyroZ; 
 
 double accXangle; // Angle calculate using the accelerometer 
 double accYangle; 
 double temp; 
 double gyroXangle = 180; // Angle calculate using the gyro 
 double gyroYangle = 180; 
 double compAngleX = 180; // Calculate the angle using a Kalman filter 
 double compAngleY = 180; 
 double kalAngleX; // Calculate the angle using a Kalman filter 
 double kalAngleY; 
 
 uint32_t timer; 
int
countup;
 void setup() {   
 
   Serial3.begin(115200); 
   Wire.begin(); 
   Serial3.begin(9600); 
             
 
   i2cWrite(0x6B,0x00); // Disable sleep mode         
   kalmanX.setAngle(180); // Set starting angle 
   kalmanY.setAngle(180); 
   timer = micros(); 
   countup = 0;
 } 
 
 void loop() { 
   if(countup == 100)
   {
   /* Update all the values */ 
   uint8_t* data = i2cRead(0x3B,14);   
   accX = ((data[0] << 8) | data[1]); 
   accY = ((data[2] << 8) | data[3]); 
   accZ = ((data[4] << 8) | data[5]);   
   tempRaw = ((data[6] << 8) | data[7]);   
   gyroX = ((data[8] << 8) | data[9]); 
   gyroY = ((data[10] << 8) | data[11]); 
   gyroZ = ((data[12] << 8) | data[13]); 
 
   /* Calculate the angls based on the different sensors and algorithm */ 
   accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; 
   accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;     
 
   double gyroXrate = (double)gyroX/131.0; 
   double gyroYrate = -((double)gyroY/131.0); 
   gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate 
   gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000); 
 
   kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter 
   kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000); 
   timer = micros(); 
 
 
   Serial3.print ("X="); 
   Serial3.print (kalAngleX,0); 
   Serial3.print (" Y="); 
   Serial3.print (kalAngleY,0); 
   countup = 0;
 }
   delay (1); 
countup = countup + 1;
 
   // The accelerometer's maximum samples rate is 1kHz 
 } 
 void i2cWrite(uint8_t registerAddress, uint8_t data){ 
   Wire.beginTransmission(IMUAddress); 
   Wire.write(registerAddress); 
   Wire.write(data); 
   Wire.endTransmission(); // Send stop 
 } 
 uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) { 
   uint8_t data[nbytes];   
   Wire.beginTransmission(IMUAddress); 
   Wire.write(registerAddress); 
   Wire.endTransmission(false); // Don't release the bus 
   Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading 
   for(uint8_t i = 0; i < nbytes; i++) 
     data [i]= Wire.read(); 
   return data; 
 }

А, вот практически то же самое, как часть основной программы:

#include <Servo.h>
#include <Wire.h>
#include <NewPing.h>
#include "Kalman.h"
Kalman kalmanX; 
Kalman kalmanY; 
 
uint8_t IMUAddress = 0x68; 
 
/* IMU Data */
int16_t accX; 
int16_t accY; 
int16_t accZ; 
int16_t tempRaw; 
int16_t gyroX; 
int16_t gyroY; 
int16_t gyroZ; 
 
double accXangle; // Angle calculate using the accelerometer 
double accYangle; 
double temp; 
double gyroXangle = 180; // Angle calculate using the gyro 
double gyroYangle = 180; 
double compAngleX = 180; // Calculate the angle using a Kalman filter 
double compAngleY = 180; 
double kalAngleX; // Calculate the angle using a Kalman filter 
double kalAngleY; 
////////////////////////////////
//////servo names///////////////
////////////////////////////////
Servo 
Leftlung, 
Rightlung, 
Leftknee, 
Rightknee, 
Leftfoot, 
Rightfoot, 
Back,
Backknee, 
Backfoot,  
Head, 
Flap;
////////////////////////////////
//////servo names///////////////
////////////////////////////////
boolean
sleepmode,
buttonrelease;
byte
command,
speedy;
int
LED,
RED,
Lights = 8,
sleep[253] = {
  25,10000,1,
  0,   0,  0,  -6,   6,    0,  0,  0,  0,  0,
  0,   0,  0,  -6,   6,   -3,  0,  0,  0,  0,
  0,   0,  0,  -6,   6,  -10,  0,  0,  0,  0,
  0,   0,  0,  -6,   6,  -10,  0,  0,  5,  0,
  0,   0,  0,  -6,   6,  -10,  0,  0,  5,  0,
  0,   0,  0,   0,   0,  -10,  0,  0,  5,  0,
  0,   0,  0,   6,  -6,  -10,  0,  0,  5,  0,
  7,   2, -9,   6,  -6,  -15,  0,  0,  5,  0,
  8,   3, -9,  -2,  -6,  -15,  0,  0,  5,  0,
  8,   3, -9,  -2,  -6,    0, 10,  0,  0,  0,
  7,   2, -9,  -2,  -6,    0, 10,  0,  0,  0,
  -5,  0,  0,  -12,  0,    0, 10,  0, -14, 0,
  -5, -5,  0,  -12,  0,  -14,  8,  0, -14, 0,
  -5, -5,  0,  -12,  0,  -14,  2,  0, -14, 0,
  -5, -5,  0,  -12,  8,  -14,  0,  0, -14, 0,
  0, -5,  0,   0,   0,    0,  0, -6, 15,  0,
  -2, -7,  9,   0,   0,    0, -5, -6,-15,  0,
  -3, -8,  9,   0,   10,   0, -5, -6,  0,  0,
  -3, -8,  9,   0,   12,  10, -5, -6,  0,  0,
  -2, -2,  9,   0,   12,  10,  0, -6, 14,  0,
  -5, 15,  0,   0,   12,  15, -2,  0, 15,  0,
  3, 10,  0,   6,   6,   15, -2,  5,  7,  0,
  2, 10,  0,   6,   0,    7, -1,  5,  7,  0,
  0,  0,  0,   0,   0,    7,  0,  0,  7,  0,
  0,  0,  0,   0,   0,    5,  0,  0,  5,  0
}
,
sleep1[253] = {
  25,10000,1,
  0,  0,  0,   0,   0,   -5,  0,  0, -5,  0,
  0,  0,  0,   0,   0,   -7,  0,  0, -7,  0,
  -2,-10,  0,  -6,   0,   -7, -1, -5, -7,  0,
  -3,-10,  0,  -6,  -6,  -15,  2, -5, -7,  0,
  5,-15,  0,   0,  -12, -15,  2,  0,-15,  0,
  2,  2, -9,   0,  -12, -10,  0,  6,-14,  0,
  3,  8, -9,   0,  -12, -10,  5,  6,  0,  0,
  3,  8, -9,   0,  -10,   0,  5,  6,  0,  0,
  2,  7, -9,   0,   0,    0,  5,  6, 15,  0,
  0,  5,  0,   0,   0,    0,  0,  6,-15,  0,
  5,  5,  0,   12, -8,   14,  0,  0,  14, 0,
  5,  5,  0,   12,  0,   14, -2,  0,  14, 0,
  5,  5,  0,   12,  0,   14, -8,  0,  14, 0,
  5,  0,  0,   12,  0,    0,-10,  0,  14, 0,
  -7, -2,  9,   2,   6,    0,-10,  0,  0,  0,
  -8, -3,  9,   2,   6,    0,-10,  0,  0,  0,
  -8, -3,  9,   2,   6,   15,  0,  0, -5,  0,
  -7, -2,  9,  -6,   6,   15,  0,  0, -5,  0,
  0,   0,  0,  -6,   6,   10,  0,  0, -5,  0,
  0,   0,  0,   0,   0,   10,  0,  0, -5,  0,
  0,   0,  0,   6,  -6,   10,  0,  0, -5,  0,
  0,   0,  0,   6,  -6,   10,  0,  0, -5,  0,
  0,   0,  0,   6,  -6,   10,  0,  0,  0,  0,
  0,   0,  0,   6,  -6,    3,  0,  0,  0,  0,
  0,   0,  0,   6,  -6,    0,  0,  0,  0,  0
}
,
hanging[253]= {
  25,50000,1,
 -2, -2, -5,  -5,   5,    0, -5,  5,  0,  0,
 -2, -2, -5,  -5,   5,    0, -5,  5,  0,  0,
 -1, -2, -5,  -5,   5,    0, -5,  5,  0,  0,
  0, -2, -5,  -5,   5,    0, -5,  5,  3,  3,
  0, -2,  5,   0,   0,    0,  0,  0,  3,  3,
  0,  0,  5,   0,   0,    0,  0,  0, -3, -3,
  0,  0,  5,   0,   0,    0,  0, 30, -3, -3,
  0,  0,  5,   0,   0,    0,  0,-30,  0,  0,
  0,  0,  0,   0,   0,    0,  0,  0,  0,  0,
  0,  2,  0,   5,  -5,    0,  5, 30,  0,  0,
  0,  2,  0,   5,  -5,    0,  5,-30,  0,  0,
  1,  2,  0,   5,  -5,    0,  5,  0,  0,  0,
  2,  2,  0,   5,  -5,    0,  5, -5,  0,  3,
  2,  2,  5,   0,   0,    0,  5, -5,  0,  3,
  2,  2,  5,   5,   5,    0,  5, -5,  0, -3,
  2,  2,  5,   5,   5,    0,  5, -5,  3, -3,
  2,  2,  5,   5,   5,    0,  5, -5,  3,  0,
  2,  2,  5,   5,   5,    0,-30, -5, -3,  0,
  2,  2,  5,   0,   0,    0, 30, -5, -3,  4,
  0,  0, -5,  -5,  -5,    0, -0, -5,  0,  4,
  0,  0, -5,  -5,  -5,    0,-30,  0,  0,  4,
  0,  0, -5,  -5,  -5,    0, 30,  0,  0, -4,
  0,  0,  0,   0,   0,    0,  5,  5,  0, -4,
  0,  0,  0,   0,   0,    0, 10, 10,  0, -4,
  0,  0,  0,   0,   0,    0,  5,  5,  0,  0
},
numbergyroparameter,
actualgyroparameter,
gyroparameter,
moduleswitch,
speedanalisys,
gyrocheck,
pingcheck,
gyroax,
gyroay,
gyroaz,
gyrogx,
gyrogy,
gyrogz,
LeftlungAngle, 
RightlungAngle, 
LeftkneeAngle, 
RightkneeAngle, 
LeftfootAngle, 
RightfootAngle,
BackAngle, 
BackkneeAngle, 
BackfootAngle, 
FlapAngle,
HeadAngle;
long
gyrocountup,
countup,
deviationspeed;
NewPing 
LeftEye(4, 3, 400),
RightEye(6, 5, 400);
char error[ ] = "Fatal progarmm error. Please, contact to service manager.";
uint32_t timer; 
void setup()
{
  Wire.begin();
  Serial3.begin(115200); 
  Wire.begin(); 
  Serial3.begin(9600);
  buttonrelease = true;
  sleepmode = true;
  gyrocountup = 0;
  gyrocheck = 0;
  pingcheck = 0;
  countup = -100;
   i2cWrite(0x6B,0x00); // Disable sleep mode         
   kalmanX.setAngle(180); // Set starting angle 
   kalmanY.setAngle(180); 
   timer = micros(); 
}
void loop() 
{
  ////////////////////////////////
  //////ping values calculating///
  ////////////////////////////////  
  pingcheck = pingcheck + 1;
  if(pingcheck == 100)
  {
    LED = (LeftEye.ping()/US_ROUNDTRIP_CM);
  }
  if(pingcheck == 200)
  {
    RED = (RightEye.ping()/US_ROUNDTRIP_CM);
    pingcheck = 0;
  }
  ////////////////////////////////
  //////ping values calculating///
  ////////////////////////////////
  ////////////////////////////////
  //////gyro values calculating///
  ////////////////////////////////
  if(gyrocountup == 1)
  {
    /* Update all the values */
    uint8_t* data = i2cRead(0x3B,14);   
    accX = ((data[0] << 8) | data[1]); 
    accY = ((data[2] << 8) | data[3]); 
    accZ = ((data[4] << 8) | data[5]);   
    tempRaw = ((data[6] << 8) | data[7]);   
    gyroX = ((data[8] << 8) | data[9]); 
    gyroY = ((data[10] << 8) | data[11]); 
    gyroZ = ((data[12] << 8) | data[13]); 
 
    /* Calculate the angls based on the different sensors and algorithm */
    accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; 
    accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;     
 
    double gyroXrate = (double)gyroX/131.0; 
    double gyroYrate = -((double)gyroY/131.0); 
    gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate 
    gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000); 
 
    kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter 
    kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000); 
    timer = micros(); 
 
    gyrocountup = 0;
  }
  gyrocountup = gyrocountup + 1;
  ////////////////////////////////
  //////gyro values calculating///
  ////////////////////////////////
  ////////////////////////////////
  //////tine dependent operations/
  ////////////////////////////////
 
  ///////////servo initialising///
  if(countup == - 100)
  {
    Leftfoot.attach(50);
    Rightfoot.attach(25);
    Back.attach(28);
    LeftlungAngle = 1500;
    RightlungAngle = 14800;
    BackAngle = 7500;
    LeftkneeAngle = 13200;
    RightkneeAngle = 3600;
    BackkneeAngle = 17300;
    LeftfootAngle = 6900;
    RightfootAngle = 11000;
    BackfootAngle = 4700;
    FlapAngle = 0;
    HeadAngle = 0;
  }
  if(countup == - 50)
  {
    Leftknee.attach(49);
    Rightknee.attach(26);
    Backfoot.attach(29);
  }
  if(countup == - 1)
  {
    Backknee.attach(47);
    Leftlung.attach(48);
    Rightlung.attach(27);
    Flap.attach(46);
    Head.attach(7);
    Serial3.print("Are you my Mommy?");
  }
    if((kalAngleX-52)*100 > 0 && (kalAngleX-60)*100 < 17500 && sleepmode == false)
  {
    HeadAngle = (kalAngleX-62)*100;
  }
  ///////////servo initialising///
  if((command > 97) && (command < 121))
  {
    if(command == 118 && sleepmode == true)
    {
      if(buttonrelease == true)
      {
        countup = 0;
        buttonrelease = false;
      }
      deviationspeed = sleep[1];
      gyroparameter = sleep[2];
      LeftlungAngle = LeftlungAngle + sleep[moduleswitch*10+2+gyroparameter]*10;                                                                                               
      RightlungAngle = RightlungAngle + sleep[moduleswitch*10+3+gyroparameter]*10;
      BackAngle = BackAngle + sleep[moduleswitch*10+4+gyroparameter]*10;
      LeftkneeAngle = LeftkneeAngle + sleep[moduleswitch*10+5+gyroparameter]*10;
      RightkneeAngle = RightkneeAngle + sleep[moduleswitch*10+6+gyroparameter]*10;
      BackkneeAngle = BackkneeAngle + sleep[moduleswitch*10+7+gyroparameter]*10;
      LeftfootAngle = LeftfootAngle + sleep[moduleswitch*10+8+gyroparameter]*10;
      RightfootAngle = RightfootAngle + sleep[moduleswitch*10+9+gyroparameter]*10;
      BackfootAngle = BackfootAngle + sleep[moduleswitch*10+10+gyroparameter]*10;
      FlapAngle = FlapAngle + sleep[moduleswitch*10+11+gyroparameter]*10;
      if((countup % 10) == 0 && (countup != 0))
      {
        moduleswitch = moduleswitch + 1;
      }
      if(moduleswitch == sleep[0])
      {
        countup = 0;
        buttonrelease = true;
        command = Serial3.read();
        sleepmode = false;
        moduleswitch = 0;
      }
 
    }
    if(command == 118 && sleepmode == false)
    {
      if(buttonrelease == true)
      {
        countup = 0;
        buttonrelease = false;
      }
      deviationspeed = sleep1[1];
      gyroparameter = sleep1[2];
      LeftlungAngle = LeftlungAngle + sleep1[moduleswitch*10+2+gyroparameter]*10;                                                                                               
      RightlungAngle = RightlungAngle + sleep1[moduleswitch*10+3+gyroparameter]*10;
      BackAngle = BackAngle + sleep1[moduleswitch*10+4+gyroparameter]*10;
      LeftkneeAngle = LeftkneeAngle + sleep1[moduleswitch*10+5+gyroparameter]*10;
      RightkneeAngle = RightkneeAngle + sleep1[moduleswitch*10+6+gyroparameter]*10;
      BackkneeAngle = BackkneeAngle + sleep1[moduleswitch*10+7+gyroparameter]*10;
      LeftfootAngle = LeftfootAngle + sleep1[moduleswitch*10+8+gyroparameter]*10;
      RightfootAngle = RightfootAngle + sleep1[moduleswitch*10+9+gyroparameter]*10;
      BackfootAngle = BackfootAngle + sleep1[moduleswitch*10+10+gyroparameter]*10;
      FlapAngle = FlapAngle + sleep1[moduleswitch*10+11+gyroparameter]*10;
      if((countup % 10) == 0 && (countup != 0))
      {
        moduleswitch = moduleswitch + 1;
      }
      if(moduleswitch == sleep1[0])
      {
        countup = 0;
        buttonrelease = true;
        command = Serial3.read();
        sleepmode = true;
        moduleswitch = 0;
      }
 
    }
  }
      if(command == 255 && sleepmode == false && countup > 00)
    {
      if(buttonrelease == true)
      {
        countup = 0;
        buttonrelease = false;
      }
      deviationspeed = hanging[1];
      gyroparameter = hanging[2];
      LeftlungAngle = LeftlungAngle + hanging[moduleswitch*10+2+gyroparameter]*10;                                                                                               
      RightlungAngle = RightlungAngle + hanging[moduleswitch*10+3+gyroparameter]*10;
      BackAngle = BackAngle + hanging[moduleswitch*10+4+gyroparameter]*10;
      LeftkneeAngle = LeftkneeAngle + hanging[moduleswitch*10+5+gyroparameter]*10;
      RightkneeAngle = RightkneeAngle + hanging[moduleswitch*10+6+gyroparameter]*10;
      BackkneeAngle = BackkneeAngle + hanging[moduleswitch*10+7+gyroparameter]*10;
      LeftfootAngle = LeftfootAngle + hanging[moduleswitch*10+8+gyroparameter]*10;
      RightfootAngle = RightfootAngle + hanging[moduleswitch*10+9+gyroparameter]*10;
      BackfootAngle = BackfootAngle + hanging[moduleswitch*10+10+gyroparameter]*10;
      FlapAngle = FlapAngle + hanging[moduleswitch*10+11+gyroparameter]*10;
      if((countup % 10) == 0 && (countup != 0))
      {
        moduleswitch = moduleswitch + 1;
      }
      if(moduleswitch == hanging[0])
      {
      }
 
    }
  ////////////////////////////////
  speedy = (micros()-speedanalisys)*1000;
  speedanalisys = micros();
  delayMicroseconds(speedy + deviationspeed);
  countup = countup + 1;
  if(Serial3.available())
  {
    if(buttonrelease == true)
    {
      command = Serial3.read();
    }
  }
  ////////////////////////////////
  //////tine dependent operations/
  ////////////////////////////////
  ////////////////////////////////
  //////motion command insert/////
  ////////////////////////////////
  if
    (
   LeftlungAngle >= 500 && LeftlungAngle <= 16300 && 
    RightlungAngle >= 0 && LeftlungAngle <= 16500 &&
    BackAngle >= 0 && BackAngle <= 18000 &&
    LeftkneeAngle >= 0 && LeftkneeAngle <= 14600 &&
    RightkneeAngle >= 2600 && RightkneeAngle <= 18000 &&
    BackkneeAngle >= 0 && BackkneeAngle <= 17300 &&
    LeftfootAngle >= 0 && LeftfootAngle <= 18000 &&
    RightfootAngle >= 0 && RightfootAngle <= 18000 &&
    BackfootAngle >= 0 && BackfootAngle <= 18000 &&
    FlapAngle >= 0 && FlapAngle <= 4500 
    )
  {
    Leftlung.write(LeftlungAngle / 100); 
    Rightlung.write(RightlungAngle / 100); 
    Back.write(BackAngle / 100); 
    Leftknee.write(LeftkneeAngle / 100); 
    Rightknee.write(RightkneeAngle / 100); 
    Backknee.write(BackkneeAngle / 100); 
    Leftfoot.write(LeftfootAngle / 100);
    Rightfoot.write(RightfootAngle / 100); 
    Backfoot.write(BackfootAngle / 100);
    Flap.write(FlapAngle / 100); 
    Head.write(HeadAngle / 100);
  }
  else
  {
    Serial3.print(error);
    Leftlung.detach();
    Rightlung.detach();
    Leftknee.detach();
    Rightknee.detach();
    Backknee.detach();
    Leftfoot.detach();
    Back.detach();
    Rightfoot.detach();
    Backfoot.detach();
    Flap.detach();
    Head.detach();
    }
 
  ////////////////////////////////
  //////motion command insert/////
  ////////////////////////////////  
}
////////////////////////////////
//////functions/////////////////
////////////////////////////////
 void i2cWrite(uint8_t registerAddress, uint8_t data){ 
   Wire.beginTransmission(IMUAddress); 
   Wire.write(registerAddress); 
   Wire.write(data); 
   Wire.endTransmission(); // Send stop 
 } 
 uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) { 
   uint8_t data[nbytes];   
   Wire.beginTransmission(IMUAddress); 
   Wire.write(registerAddress); 
   Wire.endTransmission(false); // Don't release the bus 
   Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading 
   for(uint8_t i = 0; i < nbytes; i++) 
     data [i]= Wire.read(); 
   return data; 
 }
////////////////////////////////
//////functions/////////////////
////////////////////////////////
Не судите строго. Искал везде, где только можно- проблема именно в куске кода, используемом для получения и обработки значений гироскопа по фильтру Калмана, то есть вот в этом куске, точнее в его наличии:
    /* Update all the values */
    uint8_t* data = i2cRead(0x3B,14);   
    accX = ((data[0] << 8) | data[1]); 
    accY = ((data[2] << 8) | data[3]); 
    accZ = ((data[4] << 8) | data[5]);   
    tempRaw = ((data[6] << 8) | data[7]);   
    gyroX = ((data[8] << 8) | data[9]); 
    gyroY = ((data[10] << 8) | data[11]); 
    gyroZ = ((data[12] << 8) | data[13]); 
 
    /* Calculate the angls based on the different sensors and algorithm */
    accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; 
    accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;     
 
    double gyroXrate = (double)gyroX/131.0; 
    double gyroYrate = -((double)gyroY/131.0); 
    gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate 
    gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000); 
 
    kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter 
    kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000); 
    timer = micros(); 
Помогите, кто чем может!

 

Верх