im using those 5 floats in my arduino program to interface my gyros with my accel.
even when i switch the input values, only two axes will work, not all 3 together. maybe there's something wrong with the algorithms in my arduino code? can u take a look cause i rly dont know :(
#define GXPinAmp 0 //4x
#define GYPinAmp 1 //4x
#define GZPinAmp 2 //4x
#define AXPin 4
#define AYPin 5
#define AZPin 3
// offsets are chip specific. int g_offxAmp = 400;
int g_offyAmp = 400;
int g_offzAmp = 375; //364
int a_offx = 631;
int a_offy = 383;
int a_offz = 764;
char str[512];
boolean firstSample = true;
float RwAcc[3]; //projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
float Gyro_ds[3]; //Gyro readings
float RwGyro[3]; //Rw obtained from last estimated value and gyro movement
float Awz[3]; //angles between projection of R on XZ/YZ plane and Z axis (deg)
float RwEst[3]; //corrected output values from gyro data and previous estimates
int lastTime = 0;
int interval = 0;
float wGyro = 10.0; // w2/w1
void getAccelerometerData(int * result)
//raw acc data - offset
{
result[0] = (analogRead(AXPin) - a_offx); //X
result[1] = (analogRead(AYPin) - a_offy); //Y
result[2] = (analogRead(AZPin) - a_offz); //Z
}
void rawAccToG(int * raw, float * RwAcc)
//convert raw readings to g (9.8 m/s^2)
// Rx = AdcRx / Sensitivity
{
RwAcc[0] = ((float) raw[0]) / 300.0; //X
RwAcc[1] = ((float) raw[1]) / 300.0; //Y
RwAcc[2] = ((float) raw[2]) / 300.0; //Z
}
void getGyroscopeData(int * result)
//raw gyro data - offset
{
result[0] = (analogRead(GXPinAmp) - g_offxAmp); //X
result[1] = (analogRead(GYPinAmp) - g_offyAmp); //Y
result[2] = (analogRead(GZPinAmp) - g_offzAmp); //Z
}
void rawGyroToDegsec(int * raw, float * gyro_ds)
// convert raw readings to degrees/sec
// RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensitivity
{
gyro_ds[0] = (((float)raw[0] * 3.3) / 1023.0) / 0.00333;
gyro_ds[1] = (((float)raw[1] * 3.3) / 1023.0) / 0.00333;
gyro_ds[2] = (((float)raw[2] * 3.3) / 1023.0) / 0.00333;
}
void normalize3DVec(float * vector)
// Racc(normalized) = [RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|]
{
float R; // R = |Racc|
R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]); //|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2)
vector[0] /= R; // RxAcc/|Racc|
vector[1] /= R; // RyAcc/|Racc|
vector[2] /= R; // RzAcc/|Racc|
}
float squared(float x) //equation everytime "squared" is used
{
return x*x; // x^2
}
void getInclination()
{
int w = 0;
float tmpf = 0.0;
int currentTime;
int signRzGyro;
currentTime = millis();
interval = currentTime - lastTime;
lastTime = currentTime;
if (firstSample)
{ // the 'NaN' check is used to wait for good data from the Arduino
for(w=0;w<=2;w++)
{
RwEst[w] = RwAcc[w]; //initialize with accelerometer readings
}
}
else
{
if(abs(RwEst[2]) < 0.1) //RwEst = Rz
{
//Rz is too small and because it is used as reference for computing Axz & Ayz, it's error fluctuations will amplify leading to bad results
//in this case skip the gyro data and use previous estimate
for(w=0;w<=2;w++)
{
RwGyro[w] = RwEst[w]; //initialize with gyro readings
}
}
else
{
//get angles between projection of R on ZX/ZY plane and Z axis, based on last RwEst
for(w=0;w<=2;w++)
{
tmpf = Gyro_ds[w]; //get current gyro rate in deg/s
tmpf *= interval / 1000.0f; //get angle change in deg
Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 / PI; //get angle and convert to degrees; atan2 returns values between [-PI,PI]
Awz[w] += tmpf; //get updated angle according to gyro movement
}
//estimate sign of RzGyro
//RzGyro is positive if Axz is in range [-90,90] => cos(Awz) >= 0
signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1; //+1 when signRzGyro<=0; -1 when signRzGyro<0
//reverse calculation of RwGyro from Awz angles
for(w=0;w<=2;w++)
{
RwGyro[0] = sin(Awz[0] * PI / 180); //RxGyro; sin(Axz(x)) in rad
RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) ); // RxGyro/[(1+cos(Axz(x))^2]*[tan(Ayz(y))^2]
RwGyro[1] = sin(Awz[1] * PI / 180); //RyGyro; sin(Ayz(y)) in rad
RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) ); //RyGyro/[(1+cos(Ayz(y))^2]*[tan(Axz(x))^2]
}
RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1])); //To find RzGyro
}
//combine Accelerometer and gyro readings
for(w=0;w<=2;w++)
{
RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro); //to update estimated values
}
normalize3DVec(RwEst); //to normalize the above vector
}
firstSample = false;
}
void setup()
{
Serial.begin(9600); //baud rate
analogReference(EXTERNAL); //AREF pin for reference voltage
pinMode (GXPinAmp, INPUT);
pinMode (GYPinAmp, INPUT);
pinMode (GZPinAmp, INPUT);
pinMode (AXPin, INPUT);
pinMode (AYPin, INPUT);
pinMode (AZPin, INPUT);
}
void loop()
{
if(!Serial.available())
{
int acc[3];
int gyro[3];
getAccelerometerData(acc);
rawAccToG(acc, RwAcc);
normalize3DVec(RwAcc);
getGyroscopeData(gyro);
rawGyroToDegsec(gyro, Gyro_ds);
getInclination();
serialPrintFloatArr(RwAcc, 3);
serialPrintFloatArr(Gyro_ds, 3);
serialPrintFloatArr(RwGyro, 3);
serialPrintFloatArr(Awz, 3);
serialPrintFloatArr(RwEst, 3);
Serial.println();
}
}
void serialPrintFloatArr(float * arr, int length)
{
for(int i=0; i<length; i++)
{
serialFloatPrint(arr[i]);
Serial.print(",");
}
}
//to display for serial monitor
void serialFloatPrint(float f) {
byte * b = (byte *) &f;
Serial.print("f:");
for(int i=0; i<4; i++) {
byte b1 = (b[i] >> 4) & 0x0f;
byte b2 = (b[i] & 0x0f);
char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
Serial.print(c1);
Serial.print(c2);
}
}
I think the best way to find a solution is to isolate the problem. You haven't tried that in either your Arduino nor your Processing code. Currently there is just way too much going on in both of them.