Hardware integration of sensors?

[ellisk]

Hi everyone,
I am trying to integrate the readings from an accelerometer to get velocity. When I tried to do this in software, it was laughingly inaccurate. So I looked at Gyro.cpp in WPILib to see how they integrate angular velocity to get heading. They use a hardware accumulator (integrator). I tried to copy their code and adapt it for use with an accelerometer (see accelerometer.cpp). I came up with this class:

Accel2Vel.h:

#pragma once

class AnalogChannel;

//This class is an acceleromiter which integrates over times to get velocity
// Uses hardware integration, just like the gyro :)
class Accel2Vel {
public:
	Accel2Vel(unsigned slot,unsigned channel);
	~Accel2Vel();
	double getVel();
	void reset();
private:
	AnalogChannel *achan; //different from 4chan
	double voltsPerG;
	double zeroGVoltage;
	double offset;
	
	void initAccel(); // sets voltsPerG and zeroGVoltage
};

Accel2Vel.cpp:

#include "Accel2Vel.h"
#include "AnalogModule.h"
#include "AnalogChannel.h"
#include "Utility.h"
#include "WPIStatus.h"
#include "Timer.h"

//stolen from gyro.cpp
static const UINT32 kOversampleBits = 7;
static const UINT32 kAverageBits = 3;
static const float kCalibrationSampleTime = 5.0;
static const float kSamplesPerSecond = 50.0;

Accel2Vel::Accel2Vel(unsigned slot,unsigned channel)
{
	achan = new AnalogChannel(slot,channel);
	initAccel();
}

void Accel2Vel::initAccel()
{
	if (!achan->IsAccumulatorChannel()) { // we need accumulator for integration
		printf ("ERROR: accel chan not accumulator\n");
		delete achan;
		achan = NULL;
		return;
	}
	
	voltsPerG = 1.0;
	zeroGVoltage = 2.5;
	
	// mostly stolen from Gyro.cpp
	achan->SetAverageBits(kAverageBits);
	achan->SetOversampleBits(kOversampleBits);
	float sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
	achan->GetModule()->SetSampleRate(sampleRate);
	Wait(1.0); // calibration
	
	achan->InitAccumulator();
	Wait(kCalibrationSampleTime);
	
	INT64 value;
	UINT32 count;
	
	achan->GetAccumulatorOutput(&value, &count);
	
	UINT32 center = (UINT32)((float)value / (float)count + .5);
	
	offset = ((float)value / (float)count) - (float)center;
	
	achan->SetAccumulatorCenter(center);
	achan->SetAccumulatorDeadband(0); ///< TODO: compute / parameterize this
	achan->ResetAccumulator();
}

void Accel2Vel::reset()
{
	achan->ResetAccumulator();
}

// get the current velocity
double Accel2Vel::getVel()
{
	if (achan == NULL)
		return 0.0;
	
	// "adapted" (stolen) from gyro.cpp
	INT64 rawValue;
	UINT32 count;
	
	achan->GetAccumulatorOutput(&rawValue, &count);
	
	INT64 value = rawValue - (INT64)((float)count * offset);
	
	double scaledValue = value * 1e-9 * (double)achan->GetLSBWeight() * (double)(1 << achan->GetAverageBits()) /
	                       (achan->GetModule()->GetSampleRate() * voltsPerG);
	
	return scaledValue;
}


Accel2Vel::~Accel2Vel()
{
	if (achan != NULL)
		delete achan;
}

However, it seems to always fail when checking to make sure that the analog channel has an accumulator ( if (!achan->IsAccumulatorChannel()) ). Does anyone know why this might be? Do I need to do something special to give an analog channel an accumulator?

Thanks,
-Kevin