L3G4200D 陀螺儀積分問題

ga742112

各位前輩好  
小弟因剛接觸四軸飛行器有些問題想請教,目前所使用的感測器為adxl345 l3g4200d

目前已經可以從陀螺儀得到角速度 , 想請教的是接下來該怎麼去積分才能得我要的角度值呢??


因剛接觸,所以程式碼沒有什麼概念 ,望前輩多指教!!下面是我目前的程式碼





#include <Wire.h>
#include <L3G.h>

L3G gyro;

void setup() {
  Serial.begin(9600);
  Wire.begin();

  if (!gyro.init())
  {
    Serial.println("Failed to autodetect gyro type!");
    while (1);
  }

  gyro.enableDefault();
}

void loop() {
  gyro.read();


  float dpsgyrox;
  float dpsgyroy;
  float dpsgyroz;

dpsgyrox  = ( gyro.g.x * 8.75 ) / 1000;
dpsgyroy  = ( gyro.g.y * 8.75 ) / 1000;
dpsgyroz  = ( gyro.g.z * 8.75 ) / 1000;



  Serial.print("G ");
  Serial.print("X: ");
  Serial.print(dpsgyrox);
  Serial.print(" Y: ");
  Serial.print(dpsgyroy);
  Serial.print(" Z: ");
  Serial.println(dpsgyroz);

  delay(100);
}

ga742112

回復 2# 超新手


#include <Wire.h>
#include <L3G.h>


unsigned long nowtime, lasttime, dt;
L3G gyro;


void setup() {
  Serial.begin(9600);
  Wire.begin();


  if (!gyro.init())
  {
    Serial.println("Failed to autodetect gyro type!");
    while (1);
  }
  gyro.enableDefault();
  }


void loop() {


  nowtime=millis();
  dt = nowtime - lasttime;
  lasttime = nowtime;


  gyro.read();
  float dpsgyrox;
  float dpsgyroy;
  float dpsgyroz;
  float anglex;
  float angley;
  float anglez;


dpsgyrox  = ( gyro.g.x * 8.75 ) / 1000;
dpsgyroy  = ( gyro.g.y * 8.75 ) / 1000;
dpsgyroz  = ( gyro.g.z * 8.75 ) / 1000;


anglex = anglex + dpsgyrox * dt / 1000;
angley = angley + dpsgyroy * dt / 1000;
anglez = anglez + dpsgyroz * dt / 1000;



  Serial.print("G ");
  Serial.print("X: ");
  Serial.print(anglex);
  Serial.print(" Y: ");
  Serial.print(angley);
  Serial.print(" Z: ");
  Serial.println(anglez);


  delay(100);
}


加入積分後  出來的值似乎還是角速度   請問這是程式上哪邊出錯了呢??

ga742112

回復 4# 超新手

   關於lasttime 這部分是哪邊沒處理好呢??

ga742112

因為出來的值 從0開始會慢慢遞增...... 請問程式上出了什麼問題嗎?

ga742112

我lasttime初始值應該要怎麼設定??
出來的角速度值才能換成角度呢??

ga742112

回復 9# 超新手


可以出來是角度了
但照您的方法後,靜止時一樣會慢慢遞增

#include <Wire.h>
#include <L3G.h>

float anglex;
float angley;
float anglez;
float dpsgyrox;
float dpsgyroy;
float dpsgyroz;
unsigned long nowtime, lasttime, dt;

L3G gyro;

void setup() {
  Serial.begin(9600);
  Wire.begin();

  if (!gyro.init())
  {
    Serial.println("Failed to autodetect gyro type!");
    while (1);
  }
  gyro.enableDefault();
  lasttime = millis();
  }

void loop() {

  nowtime=millis();
  dt = nowtime - lasttime;
  lasttime = nowtime;

  gyro.read();

dpsgyrox  = ((( (gyro.g.x) * 8.75 ) / 1000) );
dpsgyroy  = ((( (gyro.g.y) * 8.75 ) / 1000) );
dpsgyroz  = ((( (gyro.g.z) * 8.75 ) / 1000) );


anglex = anglex + dpsgyrox * dt / 1000;
angley = angley + dpsgyroy * dt / 1000;
anglez = anglez + dpsgyroz * dt / 1000;

  Serial.print("G ");
  Serial.print("X: ");
  Serial.print(anglex);
  Serial.print(" Y: ");
  Serial.print(angley);
  Serial.print(" Z: ");
  Serial.println(anglez);

}