#include <math.h>
class Gyro{
private:
float AccelMultipl=2*9.81; //
float AngleMultipl=250;
float correction[3]={0.9890, 0.9920,0.9844}; //correction factor for Acceleration
float bias[3]={-0.3713,-0.0095,0.6367}; //Offset factor für Acceleration
float Offset[3]={ 0.86433,0.99755,-0.07829}; //Offsett faktor der Drehwinkelgeber (-Mittelwert des Stillstands) //Offset for Angular Rate ~Mean if the Gyro is not Moving (minimizes drift)
public:
//Werte mit _accel wurden aus beschleunigungen berechnet (wird zur Sensorfusion genutzt)
//Raw Angular-Rates: (just scaled)
float Roll_rate=0;
float Gier_rate=0;
float Nick_rate=0;
float Gier_angle=0;
//Angles from acceleration:
float Roll_accel=0;
float Nick_accel=0;
//Angles after Sensor funsion:
float Roll_filtered=0;
float Nick_filtered=0;
float Gier_filtered=0; //"FILTERED" for a real filtering we actually need something like a compass which is not implemented!
//Acceleration (Raw and Integrated) (Werte sind bis für Sensorfusion eig Nutzlos, insbesondere die Integrierten, diese sollten (hier fehlt Gravity-Offset) den Geschwindigkeiten entsprechen.
float x_accel=0;
float Ix_accel=0;
float y_accel=0;
float Iy_accel=0;
float z_accel=0;
float Iz_accel=0;
float temperature;
// LastSampleTime is used for integrating correctly
float LastSampleTime=0;
int Adress=0x68;
void init(int Adress)
{
/*
* Init Function, sets Adress Wakes up the MPU and sets the LowPassFilter -> LowPass is set pretty High if the chip is dampened maybe set it lower
*/
this->Adress=Adress;
Wire.begin();
Wire.beginTransmission(Adress);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission();
Wire.beginTransmission(Adress);
Wire.write(0x1A); // CONFIG register
byte val=B00000110; //
Wire.write(val); // Set the LowPassFilter
Wire.endTransmission();
}
void setConfig(int mode)
{
//Set the mode / range (not yet tested!)
byte bMode;
switch (mode){
case 0:
bMode=B00000000;
this->AccelMultipl=2*9.81;
this->AngleMultipl=250;
break;
case 1:
bMode=B00001000;
this->AccelMultipl=4*9.81;
this->AngleMultipl=500;
break;
case 2:
bMode=B00011000;
this->AccelMultipl=8*9.81;
this->AngleMultipl=1000;
break;
case 3:
bMode=B00011000;
this->AccelMultipl=16*9.81;
this->AngleMultipl=2000;
break;
default:
bMode=B00000000;
this->AccelMultipl=2*9.81;
this->AngleMultipl=250;
break;
}
//Serial.println(bMode);
Wire.beginTransmission(this->Adress);
Wire.write(0x1B); // starting with register 0x1B ()
Wire.write(bMode); //1->+-500°/s
Wire.endTransmission();
Wire.write(0x1C); // starting with register 0x1C ()
Wire.write(bMode); //1->+-4g°/s
Wire.endTransmission();
}
void getConfigs() //Not Tested!
{
Wire.beginTransmission(this->Adress);
Wire.write(0x1B); // starting with register 0x1B (GYRI CIBFUG)
Wire.endTransmission();
Wire.beginTransmission(this->Adress);
Wire.requestFrom(this->Adress,2); // request a total of 2 registers
short conf=Wire.read()<<8|Wire.read();
// Serial.println(conf,BIN);
}
void grabData()//grabs Data from Gyro and processes it (filteres it)
{
// First Get Times for the integrating stuff that happenes later
float T_now=micros()*0.000001; //Jetzt Zeit in s
float dT=T_now-(this->LastSampleTime); //Time that has run by (integration span)
this->LastSampleTime=T_now;
//Reading & Saving Raw-Values:
Wire.beginTransmission(this->Adress);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission();
Wire.requestFrom(this->Adress,14); // request a total of 14 registers
this->x_accel=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
this->y_accel=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
this->z_accel=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
this->temperature=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
this->Roll_rate=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
this->Nick_rate=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
this->Gier_rate=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)}
//Preprocessing:
//1. Scaling works by an affin function x_scaled=a * x_raw +b
this->x_accel=(float)this->x_accel*this->AccelMultipl/32767*this->correction[0] + this->bias[0];
this->y_accel=(float)this->y_accel*this->AccelMultipl/32767*this->correction[1] + this->bias[1];
this->z_accel=(float)this->z_accel*this->AccelMultipl/32767*this->correction[2] + this->bias[2];
//2. Integrate acceleration (useless so far) //maybe used later by integrating it (twice) to determine position (should be pretty uncertain)
this->Ix_accel+= this->x_accel*dT;
this->Iy_accel+= this->y_accel*dT;
this->Iz_accel+= this->z_accel*dT;
//3. Temperature
this->temperature=(float) this->temperature/340.00+36.53;
//4.Scale Angular Rates: (Angular Rates are just adjusted by an Offset, which is determined by the mean and minimizes drift)
this->Roll_rate=(float)this->Roll_rate*this->AngleMultipl/32767+Offset[0];
this->Nick_rate=(float)this->Nick_rate*this->AngleMultipl/32767+Offset[1];
this->Gier_rate=(float)this->Gier_rate*this->AngleMultipl/32767+Offset[2];
//Roll und Nicktwerte aus Beschleunigungen:
this->Roll_accel=RAD_TO_DEG*(atan2(-this->y_accel,-this->z_accel)); //NICK
this->Nick_accel=RAD_TO_DEG*(atan2(this->x_accel,sqrt(this->z_accel*this->z_accel+this->y_accel*this->y_accel)));//ROLL
//Sensor fusion is here
//Sensor fusion uses just a complementary filter, however works pretty solid (maybe use a kalman filter later)
this->Roll_filtered=0.994 *(this->Roll_filtered+this->Roll_rate*dT) + 0.006*this->Roll_accel ;
this->Nick_filtered=0.994 *(this->Nick_filtered+this->Nick_rate*dT) + 0.006*this->Nick_accel ;
//this->Roll_filtered -=Nick_filtered*sin(this->Gier_rate);
//this->Nick_filtered +=Roll_filtered*sin(this->Gier_rate);
this->Gier_angle+=Gier_rate*dT;
//Gier_filtered is not filtered since for that a compass would be needed!
this->Gier_filtered=0.98*this->Gier_angle;
}
void dispData(){
//Winkelbeschleunigungen:
//Serial.print(" aX: "); Serial.print(this->x_accel); Serial.print(" aY: "); Serial.print(this->y_accel); Serial.print(" aZ: "); Serial.print(this->z_accel);
//Nick& Roll aus Acceleration:
//Serial.print(" Nick_acc; ");Serial.print(this->Nick_accel);
//Serial.print(" Roll_acc: ");Serial.print(this->Roll_accel);
//NICK&ROLL-ANGLE-Rates
// Serial.print(" Roll_rate: ");Serial.print(this->Roll_rate);
//Serial.print(" Gier_filtered: ");Serial.print(this->Gier_rate);
//NICK&ROLL ANGLES
Serial.print(" Roll_filtered: ");Serial.print(this->Roll_filtered);
Serial.print(" Nick_filtered: ");Serial.print(this->Nick_filtered);
Serial.print(" GierAngle: "); Serial.print(this->Gier_filtered);
//TEMPERATUR:
// Serial.print(" Tmp: "); Serial.print(this->temperature); //
//NULL-Linie zur Orientierung:
//Serial.print(" Null: "); Serial.print(0);
//Serial.print(" GasMax: "); Serial.print(60);
//Serial.print(" -GasMax: "); Serial.print(-60);
}
};
that is why I am asking..
