I was wondering if anyone can help me. I am using an accelerometer, the ADXL345 to measure displacement using double integration. I took a lot of information for reading the accelerometer data from a very useful tutorial page at http://www.den-uijl.nl/gyro.html so thanks to him. This is my first post but reading this forum is very interesting and helpful. I should also mention that this is an algorithm I'm writing for heave compensation.
As I am integrating , the results will obviously drift. To counteract this, I have set up a regression line fit to zero the results about the time axis. I found a relatively simple calculation for it from here; http://www.revisesmart.co.uk/maths/statistics-2/the-least-squares-regression-line.html. Before, I was using time averaging but this was not satisfactory as it was not responsive enough. After the first integration to get velocity and plotting the new offset velocity (offsetVelZ), the plot looked good. The problem is when I integrate again this value always goes to zero. I am baffled and equally frustrated!! I can't understand why it isn't summing up the values, I have checked and there are individual values there but whenever I ask it to sum them up it is always zero.
Also, this code is working fine when I put through the unfiltered velocity data VelZ_2 into it.
The gyro data is not used in this part of the code so you can ignore that.
Thanks to anyone who can help. Any questions, let me know.
Dan.
#include <Wire.h>
int gyroResult[3], accResult[3];
float biasGyroX, biasGyroY, biasGyroZ, biasAccX, biasAccY, biasAccZ;
float timeStep = 0.02;
float t = 0;
float VelZ_2 = 0;
float e = 0;
int S2 = 100; //Number of samples taken for the time averaged velocity to find zero mean
float arrayVelZ[100]; //Time averaging array for the y input
float T = 100;
float arrayTime[100];
int S1 = 20; //Number of samples taken for the time averaged acceleration
float arrayAccZ[20];
unsigned long timer;
float sum1 = 0;
float AccZ_smooth = 0; // averaged accelaration of acc in heave direction
float offsetVelZ = 0;
float sumS2Time;
float sumS2Time_squared;
float S2Time_ave = 0;
float sumArrayVelZ = 0;
float sumXY = 0;
float VelZ_ave = 0;
float distZ;
float slopeVelZ = 0;
float slopeVelZ2 = 0;
float offsetVelZ_regression = 0;
int c, d, f, j;
int m = 1, n = 1, p = 1;
float SampleSize = 0;
float accZ = 0;
float y;
void writeTo(byte device, byte toAddress, byte val) {
Wire.beginTransmission(device);
Wire.write(toAddress);
Wire.write(val);
Wire.endTransmission();
}
void readFrom(byte device, byte fromAddress, int num, byte result[])
{
Wire.beginTransmission(device);
Wire.write(fromAddress);
Wire.endTransmission();
Wire.requestFrom((int)device, num);
int i = 0;
while(Wire.available()) {
result[i] = Wire.read();
i++;
}
}
void getGyroscopeReadings(int gyroResult[]) {
byte buffer[6];
readFrom(0x68,0x1D,6,buffer);
gyroResult[0] = (((int)buffer[0]) << 8 ) | buffer[1]; //Combine two bytes into one int
gyroResult[1] = (((int)buffer[2]) << 8 ) | buffer[3];
gyroResult[2] = (((int)buffer[4]) << 8 ) | buffer[5];
}
void getAccelerometerReadings(int accResult[]) {
byte buffer[6];
readFrom(0x53,0x32,6,buffer);
accResult[0] = (((int)buffer[1]) << 8 ) | buffer[0]; //Note, byte order different from gyros'
accResult[1] = (((int)buffer[3]) << 8 ) | buffer[2];
accResult[2] = (((int)buffer[5]) << 8 ) | buffer[4];
}
void setup() {
int totalGyroXValues = 0;
int totalGyroYValues = 0;
int totalGyroZValues = 0;
int totalAccXValues = 0;
int totalAccYValues = 0;
int totalAccZValues = 0;
int i;
Wire.begin(); //Open I2C communications as master
Serial.begin(115200); //Open serial communications to the PC to see what's happening
writeTo(0x53,0x31,0x01); //Set accelerometer to 10bit, +/-2g writeTo(0x53,0x31,0x09)Set accelerometer to 11bit, +/-4g
writeTo(0x53,0x2D,0x08); //Set accelerometer to measure mode
writeTo(0x68,0x16,0x1A); //Set gyro to +/-2000deg/sec and 98Hz low pass filter should it be writeTo(0x68,0x16,0x20)?
writeTo(0x68,0x15,0x09); //Set gyro to 100Hz sample rate
delay(100); //wait for gyro to "spin" up
for (i = 0; i < 50; i += 1) {
getGyroscopeReadings(gyroResult);
getAccelerometerReadings(accResult);
totalGyroXValues += gyroResult[0];
totalGyroYValues += gyroResult[1];
totalGyroZValues += gyroResult[2];
totalAccXValues += accResult[0];
totalAccYValues += accResult[1];
totalAccZValues += accResult[2];
delay(50);
}
biasGyroX = totalGyroXValues / 50;
biasGyroY = totalGyroYValues / 50;
biasGyroZ = totalGyroZValues / 50;
biasAccX = totalAccXValues / 50;
biasAccY = totalAccYValues / 50;
biasAccZ = (totalAccZValues / 50) - 256; //To include gravity in after zeroing
}
void loop() {
timer = millis();
getGyroscopeReadings(gyroResult);
getAccelerometerReadings(accResult);
accZ = (accResult[2] - biasAccZ - 256) / 256 * 9.81; // Accelration in m/s^2
SampleSize += 1;
arrayAccZ[m - 1] = accZ; // Creates an array of the Z axis acceleration for running average smoothing
m = m + 1;
if (m == (S1+1)){ // Puts the value of n back to zero when the matrix size + 1 is reached so that the array is continuously filled with new data
m = 1;
}
for (j = 0; j < S1; ++ j){ //Sums up all the values of the array
sum1 += arrayAccZ[j];
}
AccZ_smooth = sum1 / SampleSize;
VelZ_2 = VelZ_2 + AccZ_smooth * timeStep; //Integrate the accelerometer averaged results to give velocity
if (SampleSize == (S2 + 1)) {
SampleSize = S2;
}
if (SampleSize < S2) {
arrayVelZ[n - 1] = VelZ_2; // Creates an array of the y values for the zero mean
arrayTime[n - 1] = t; // Create a time array 1,2,3,..
n = n + 1;
}
else {
arrayVelZ[S2 - 1] = VelZ_2; // Creates an array of the y values for the zero mean
arrayTime[S2 - 1] = t;
}
for (d = 0; d < SampleSize; ++ d){ //Sums up all the values of S2
sumS2Time += arrayTime[d];
sumS2Time_squared += pow(arrayTime[d],2);
}
S2Time_ave = sumS2Time / SampleSize;
for (c = 0; c < SampleSize; ++ c){ //Sums up all the values of the array for the least squares regression line
sumArrayVelZ += arrayVelZ[c];
sumXY += arrayVelZ[c] * arrayTime[c];
}
VelZ_ave = sumArrayVelZ / SampleSize;
slopeVelZ = ((sumXY / SampleSize) - S2Time_ave * VelZ_ave) / ((sumS2Time_squared / SampleSize) - pow(S2Time_ave, 2));
offsetVelZ_regression = slopeVelZ * t - slopeVelZ * S2Time_ave + VelZ_ave;
offsetVelZ = VelZ_2 - offsetVelZ_regression;
distZ += offsetVelZ * timeStep;
Serial.print(accZ);
Serial.print("\t");
Serial.print(AccZ_smooth);
Serial.print("\t");
Serial.print(VelZ_2);
Serial.print("\t");
Serial.print(offsetVelZ);
Serial.print("\t");
Serial.print(distZ,8);
Serial.print("\n");
sum1 = 0;
sumArrayVelZ = 0;
sumXY = 0;
sumS2Time = 0;
sumS2Time_squared = 0;
t += 1;
if (SampleSize == S2) {
for (f = 0; f < S2; f++) {
arrayVelZ[f] = arrayVelZ[f + 1];
arrayTime[f] = arrayTime[f + 1];
}
}
timer = millis() - timer;
timer = (timeStep * 1000) - timer;
delay(timer);
}
