Hi all,
I'm trying to measure pitch in dynamic conditions with an accelerometer (MMA7260q) and a GPS (ls20031). In static conditions (while standing still) the pitch angle sensed by the accelerometer (after some filtering) is quite good and stable. But in dynamic conditions, the accelerometer is not only influenced by the gravity acceleration but also by the movement acceleration and so the measured angle is junk. So I thought I would measure the movement acceleration by a GPS and subtract it from the accelerometer reading. But this don't improve the angle.. I also have a gyroscope (IDG1215) that I can use..
Here is my code:
#include <TinyGPS.h>
// Accelerometer and gyroscope data
float xkal, ykal, zkal, xvolt, yvolt, zvolt, gyrovolt, ygyro;
float angleacc, anglegyro, prevAngle;
const float accsens = 0.8; //166.67 count/g
const float accoff = 1.65;//343.75; // count
const float gyrooff = 1.35;
const float gyrosens = 0.015;
const float radToDegree = 180/3.14159265;
const int calcount = 100;
float first_time, time;
float angle;
float calx, caly, calz, calgyro;
// Kalman filter data
const float Q = 1e-3;
const float R = 0.1;
float kal[3] = {337.59,337.59,501.27};
float P[3] = {10,10,10};
float kalmin[3];
float Pmin[3];
float K[3];
// GPS data
TinyGPS gps;
float gpsdump(TinyGPS &gps);
bool feedgps();
float prevVel = 0;
float accGPS;
float dt, start1;
// Averaging data
int teller = 0;
float gyrocount;
float accxcount, accycount, acczcount;
void setup()
{
Serial.begin(57600);
Serial.println("Calibrating...");
calibrate();
Serial.println("[OK]");
}
void loop()
{
boolean newdata = false;
unsigned long start = millis();
while(millis() - start < 100)
{
if (feedgps()){
newdata = true;
}
accxcount = accxcount + (analogRead(0) + calx);
accycount = accycount + (analogRead(1) + caly);
acczcount = acczcount + (analogRead(2) + calz);
//gyrocount = gyrocount + (analogRead(3) + calgyro);
teller++;
}
if(newdata){
accGPS = (float)gpsdump(gps);
getAccAngle(accGPS, gyrocount/teller, accxcount/teller, accycount/teller, acczcount/teller, 1);
}
gyrocount = accxcount = accycount = acczcount = teller = 0;
}
float getAccAngle(float acc2, float gyroavg, float accxavg, float accyavg, float acczavg, int pr){
// Time update
kalmin[0] = kal[0];
kalmin[1] = kal[1];
kalmin[2] = kal[2];
Pmin[0] = P[0] + Q;
Pmin[1] = P[1] + Q;
Pmin[2] = P[2] + Q;
// Measurement update
K[0] = Pmin[0]/(Pmin[0]+R);
K[1] = Pmin[1]/(Pmin[1]+R);
K[2] = Pmin[2]/(Pmin[2]+R);
float xmeas = accxavg - acc2;
float ymeas = accyavg;
float zmeas = acczavg;
kal[0] = kalmin[0] + K[0]*(xmeas-kalmin[0]) ; // Should be 337.59 flat
//Serial.println(kal[0]);
kal[1] = kalmin[1] + K[1]*(ymeas-kalmin[1]) ; // Should be 337.59 flat
kal[2] = kalmin[2] + K[2]*(zmeas-kalmin[2]) ; // Should be 501.27 flat
P[0] = (1-K[0])*Pmin[0];
P[1] = (1-K[1])*Pmin[1];
P[2] = (1-K[2])*Pmin[2];
xvolt = kal[0]*5/1023; // Should be 1.65V flat
yvolt = kal[1]*5/1023; // Should be 1.65V flat
zvolt = kal[2]*5/1023; // Should be 2.45V fla
xkal = (xvolt-accoff)/accsens;
//Serial.println(xkal);
ykal = (yvolt-accoff)/accsens;
zkal = (zvolt-accoff)/accsens;
angleacc = atan2(xkal,sqrt(pow(ykal,2) + pow(zkal,2)))*radToDegree;
// gyrovolt = gyroavg*5/1023;
// ygyro = -((gyrovolt-gyrooff)/gyrosens);
// if (ygyro > -1.2 && ygyro < 1.2){
// ygyro = 0;
// }
// time = (millis()-first_time)/1000;
// first_time = millis();
//
// anglegyro = anglegyro + ygyro*time;
// angle = 0.98*(angle + ygyro*time) + 0.02*angleacc;
// Printing the values
Serial.println(angleacc);
}
void calibrate(){
for (int i = 0; i < calcount; i++){
calx = calx + analogRead(0);
caly = caly + analogRead(1);
calz = calz + analogRead(2);
calgyro = calgyro + analogRead(3);
delay(50);
}
calx = 337.59 - calx/calcount;
caly = 337.59 - caly/calcount;
calz = 501.27 - calz/calcount;
calgyro = 276.21 - calgyro/calcount;
}
float gpsdump(TinyGPS &gps)
{
float flat, flon;
unsigned long fix_age;
gps.f_get_position(&flat, &flon, &fix_age);
float falt = gps.f_altitude();
float speedms = gps.f_speed_mps();
//Serial.print("Latitude: "); Serial.println(flat,7);
//Serial.print("Longitude: "); Serial.println(flon,7);
//Serial.print("Altitude: "); Serial.println(falt, 5);
//Serial.print("Speed m/s: "); Serial.println(speedms, 5);
dt = (float)(millis()-start1)/1000;
start1 = (float)(millis());
float temp = (float)(speedms-prevVel);
prevVel = speedms;
//Serial.print("Acceleration: ");Serial.println(temp/dt,5);
return (float)(temp/dt);
}
bool feedgps()
{
while (Serial.available())
{
if (gps.encode(Serial.read()))
return true;
}
return false;
}
Some help?