Gyro math filters

[ekapalka]

I've been working on a couple of different filters for our gyro (without the robot, so I can't verify anything I'm writing), and I've run into a problem. I'm trying to incorporate the gyro and the accelerometer in a complimentary filter, but it just occurred to me that I'm measuring the wrong axis with the accelerometer. I'm measuring pitch/roll when I need to be measuring yaw. All of the filters I've been working on rely on the following flawed code.

#include <WPILib.h>
class DefaultRobot : public SimpleRobot {
	ADXL345_I2C *accl; 
	Gyro *gyro;
	
	public:

	DefaultRobot(void){
		accl = new ADXL345_I2C(1, ADXL345_I2C::kRange_2G);
		gyro = new Gyro(1);
	}
	
	void Autonomous(void){
		while(IsAutonomous()){
		}				
	}
	
	void OperatorControl(void){ 
		DriverStationLCD *driverStationLCD = DriverStationLCD::GetInstance();
		gyro->SetSensitivity(0.007); //default sensitivity.  You can adjust to account for drift (to a degree)
		gyro->Reset(); 
		
		float tau      = 0.075;
		float a        = 0.000;
		float x_angleC = 0;
		float angle;
		
		float y, z, accAngle, gyrRate;
		while (IsOperatorControl()){
			y = acc.GetAcceleration(ADXL345_I2C::kAxis_Y);
			z = acc.GetAcceleration(ADXL345_I2C::kAxis_Z);		
			accAngle = atan2(accY,accZ)/**180/PI*/; //last part to convert from radians to degrees if not working properly
			gyrRate = gyro->GetRate(); 
			
			angle = Complimentary(accAngle, gyrRate, 20);
			
			driverStationLCD->PrintfLine((DriverStationLCD::Line) 0, "Angle: %f degrees", angle);
			driverStationLCD->UpdateLCD();
		}
	}
	
	float Complementary(float newAngle, float newRate,int looptime) {
		float dtC = float(looptime)/1000.0;                                    
		a=tau/(tau+dtC) ;
		x_angleC = a * (x_angleC + newRate * dtC) + (1-a) * (newAngle);
		return x_angleC;
	}
};

START_ROBOT_CLASS(DefaultRobot);

In order to determine any angle from the accelerometer, I need to use the Z axis (for measuring acceleration due to gravity, and thus finding the angle), but I don't think there's any way to use the Z axis when determining yaw because the accelerometer will always be parallel to the ground, and so the accelerometer's Z axis should always stay the same. Since I realize I've been doing this all wrong, is it even possible to utilize the accelerometer when determining the yaw rotation?