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

[virtuald]

Thats a really neat idea.. try inheriting from SensorBase, that might do it (thats what Gyro inherits from).

Also.. #pragma once is MSVC only... it won't work here. Use the standard #ifndef .. #define .. #endif clauses.

[virtuald]

Oh, I see. Thats not it at all. If you look at AnalogChannel.cpp, there appear to be two accumulator channels:

const UINT32 AnalogChannel::kAccumulatorChannels[] = {1, 2};

So if you're not using one of those, then it won't work.

[wt200999]

I am looking into the same thing, post any progress if you could.

Take note of the following as well:

In Gyro.cpp

	if (!m_analog->IsAccumulatorChannel())
	{
		wpi_fatal(GyroNotAccumulatorChannel);

In WPIStatus.h

S(GyroNotAccumulatorChannel, -6, "Gyro can only be used with Analog Channel 1 on either module");

I guess I will have to use that 2nd breakout afterall...

[virtuald]

Odd, I wonder why the code seems to indicate that channel 2 can be used as well. I haven't messed with it yet.

[wt200999]

Yeah I was just wondering the same thing. Hopefully 1 and 2 work, then I can take out the 2nd analog thing and save some weight.

I remember getting that one fatal error, I had my gyro in slot 3, I guess one quick way to test it is to see if it gives that error in slot 2.

[Joe Ross]

During the beta period, the FPGA had 1 accumulator on channel 1 of module 1 and 1 accumulator on channel 1 of module 2. It was decided that it made more sense to have them both on the same module, to lessen the chance that people would have to use module 2.

The const in AnalogChannel.cpp is correct, the text of the assertion is not. As noted in the documentation, bugs should be on the forums at forums.usfirst.org. Patches are also welcome there.

[ellisk]

Thanks for the help! When I put it on Channel 1, it (sort of) works. It works in the sense that it now gets values, but they are all bad (they're always increasing, and at a VERY high rate). Does anyone know what might be wrong?

Thanks.

[wt200999]

I kind of have the same issue. It spits out numbers, it doesn't really go up at a really high rate, but the data is still completly unuseable as it is right now... How can I increase the useablility of this sensor...