char Accelerometer_init() {
int16_t c = readRegister(WHO_AM_I_ADDRESS); // Read WHO_AM_I register
if (c == WHO_AM_I_VALUE) //c != ERROR ||
{
DBPRINT("Accelerometer is online...\n");
}
else
{
DBPRINT("Accelerometer: Failed to connect (0x%X).\n",(int16_t)c);
return FAILURE;
}
setStandbyMode(); // Must be in standby to change registers
// Set up the full scale range to 2, 4, or 8g.
char fsr = GSCALE;
if(fsr > 8) fsr = 8; // Limit G-Scale
fsr >>= 2; // 00 = 2G, 01 = 4A, 10 = 8G (pg. 20)
writeRegister(XYZ_DATA_CFG_ADDRESS, fsr);
setActiveMode();
Timer_new(TIMER_ACCELEROMETER, UPDATE_DELAY);
#ifdef USE_ACCUMULATOR
resetAccumulator();
#endif
haveReading = FALSE;
return SUCCESS;
}
/*
* Class: edu_wpi_first_wpilibj_hal_AnalogJNI
* Method: resetAccumulator
* Signature: (Ljava/nio/ByteBuffer;Ljava/nio/IntBuffer;)V
*/
JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_resetAccumulator
(JNIEnv * env, jclass, jobject id, jobject status)
{
void ** javaId = (void**)env->GetDirectBufferAddress(id);
ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << *javaId;
jint * statusPtr = (jint*)env->GetDirectBufferAddress(status);
resetAccumulator(*javaId, statusPtr);
ANALOGJNI_LOG(logDEBUG) << "Status = " << *statusPtr;
}
/**
* Function: updateReadings
* @return None
* @remark Records the current G-values from the sensor. Accumulation (low-pass)
* filtering will occur if USE_ACCUMULATOR is defined.
* @author David Goodman
* @date 2013.01.23 */
static void updateReadings() {
uint8_t rawData[6]; // x/y/z accel register data stored here
readRegisters(OUT_X_MSB_ADDRESS, 6, rawData); // Read the six raw data registers into data array
int i;
// Loop to calculate 12-bit ADC and g value for each axis
for(i = 0; i < 3 ; i++)
{
int16_t gCountI = (rawData[i*2] << 8) | rawData[(i*2)+1]; //Combine the two 8 bit registers into one 12-bit number
gCountI >>= 4; //The registers are left align, here we right align the 12-bit integer
// If the number is negative, we have to make it so manually (no 12-bit data type)
if (rawData[i*2] > 0x7F)
{
gCountI = ~gCountI + 1;
gCountI *= -1; // Transform into negative 2's complement #
}
//Record this gCount into the struct or short ints
switch (i) {
#ifndef USE_ACCUMULATOR
case 0:
gCount.x = gCountI;
break;
case 1:
gCount.y = gCountI;
break;
case 2:
gCount.z = gCountI;
break;
#else
case 0:
gAccumulator.x += gCountI;
break;
case 1:
gAccumulator.y += gCountI;
break;
case 2:
gAccumulator.z += gCountI;
break;
#endif
}
}
haveReading = TRUE;
// Update gCounts if accumulating
#ifdef USE_ACCUMULATOR
accumulatorIndex++;
if (accumulatorIndex >= ACCUMULATOR_LENGTH) {
gCount.x = (uint16_t)(gAccumulator.x >> ACCUMULATOR_SHIFT);
gCount.y = (uint16_t)(gAccumulator.y >> ACCUMULATOR_SHIFT);
gCount.z = (uint16_t)(gAccumulator.z >> ACCUMULATOR_SHIFT);
resetAccumulator();
}
/**
* Resets the accumulator to the initial value.
*/
void AnalogInput::ResetAccumulator() {
if (StatusIsFatal()) return;
int32_t status = 0;
resetAccumulator(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
if (!StatusIsFatal()) {
// Wait until the next sample, so the next call to GetAccumulator*()
// won't have old values.
const float sampleTime = 1.0f / GetSampleRate();
const float overSamples = 1 << GetOversampleBits();
const float averageSamples = 1 << GetAverageBits();
Wait(sampleTime * overSamples * averageSamples);
}
}
/**
* Initialize the accumulator.
*/
void initAccumulator(void* analog_port_pointer, int32_t *status) {
setAccumulatorCenter(analog_port_pointer, 0, status);
resetAccumulator(analog_port_pointer, status);
}
