/**
* Start the device through address with offset's informed
* @param address
* @param xGyroOffset
* @param yGyroOffset
* @param zGyroOffset
* @return boolean
*/
bool MPU6050::dmpStartDevice(uint8_t address, int xGyroOffset, int yGyroOffset, int zGyroOffset) {
setAddress(address);
initialize();
setXGyroOffset(xGyroOffset);
setYGyroOffset(yGyroOffset);
setZGyroOffset(zGyroOffset);
if (testConnection())
{
// load and configure the DMP
if (dmpInitialize() == 0)
{
// turn on the DMP, now that it's ready
setDMPEnabled(true);
dmpStatus = getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;
// get expected DMP packet size for later comparison
dmpPacketSize = dmpGetFIFOPacketSize();
timeReset();
resetFIFO();
return true;
}
}
return false;
}
int auto_init()
{
// Map the registers
InitMapping();
// MZHappy
Pulser(1,0.5,1e9,MappedHappyBaseAddress);
// Reset the FIFO
resetFIFO();
//Put caen in attenuating mode
CAENWords(255, 255);
//setup DGT and LO* delay lengths
GTDelays(153, 153);
//Set MTCA MIMIC DAC value
DACThresholds(4095);
//Set Control Reg Value
ControlReg(58);
// Speaker mask to GT
speakerMask(0x1000000);
// Counter mask to GT
counterMask(0x1000000);
// Trigger mask to 0
triggerMask(0,0);
// Set Speaker pre-scale to 1
//speakerScale(1);
return 0;
}
void drvSIS3820::resetScaler()
{
/* Reset scaler */
setAcquireMode(ACQUIRE_MODE_SCALER);
registers_->key_op_disable_reg = 1;
resetFIFO();
registers_->key_counter_clear = 1;
}
void drvSIS3801::resetScaler()
{
//static const char *functionName="resetScaler";
setAcquireMode(ACQUIRE_MODE_SCALER);
resetFIFO();
nextChan_ = 0;
nextSignal_ = 0;
}
void Accelerometer::bypassDrift()
{
sleep(20);
resetFIFO();
while(getFIFOCount() < 42);
float calibratedValues[3];
getYawPitchRoll(calibratedValues);
zeroValues[0] = calibratedValues[0];
zeroValues[1] = calibratedValues[1];
zeroValues[2] = calibratedValues[2];
}
uint16_t Accelerometer::getFIFOCount()
{
i2cReadBytes(MPU6050_RA_FIFO_COUNTH, 2, buffer);
uint16_t ret = (((uint16_t)buffer[0]) << 8) | buffer[1];
if(ret == 1024)
{
resetFIFO();
return 0;
}
else
return ret;
}
bool MPU9250Device::checkFIFO() {
uint8_t mpuIntStatus = getMPUStatus();
uint16_t fifoCount = getFIFOCount();
if ((mpuIntStatus & MPU_OVERFLOW_VALUE) || fifoCount >= MAX_FIFO_COUNT) {
resetFIFO();
return false;
}
if(fifoCount == 0) {
return false;
}
return true;
}
void drvSIS3801::erase()
{
//static const char *functionName="erase";
if (!exists_) return;
// Call the base class method
drvSIS38XX::erase();
/* Erase FIFO and counters on board */
resetFIFO();
}
/** @brief Set the Configuration of the Transmitter for RFM01.
Not all parameters are configurable. The wakeup timer, low battery and the
clock out are disabled. The transmitter chain is powered off.
*/
void receiverConfigure()
{
_delay_ms(150);
writeCMD(CONFIGURATION, 16); /* Configure command */
/* Set the low battery threshold and the microprocessor clock output */
writeCMD(BATTERY, 16);
writeCMD(FREQUENCY, 16); /* Set carrier frequency */
/* Receiver Setting Command, disable the receiver chain */
writeCMD(RECEIVER, 16);
/* Receiver Setting Command, enable the entire receiver chain */
writeCMD(RECEIVER | _BV(0), 16);
/* Set the FIFO options */
resetFIFO();
/* Set the Baseband Data Filter and Clock Recovery options */
writeCMD(BASEFILTER, 16);
/* Set the expected data bitrate */
writeCMD(DATARATE, 16);
/* Set the AFC enable pattern, range limits and measurement mode */
writeCMD(AFC, 16);
}
Accelerometer::Accelerometer()
{
m_i2cfd = open("/dev/i2c-1", O_RDWR);
if(!m_i2cfd < 0)
abort();
if(ioctl(m_i2cfd, I2C_SLAVE, 0x68) < 0)
abort();
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); // Set Clock source
i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250); // Full scale gyro range
i2cWriteBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2); // Full scale accel range
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1, true); // Reset
usleep(30000);
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode
setMemoryBank(0x10, true, true); // Get MPU hardware revision
uint8_t value = 0x06; // Alright, this is dirty but a pointer to this value will be sent to every i2cWriteBytes...
uint16_t value16; // Yeah... I know, I won't go to heaven but that's ok
i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &value); // Set memory start address
i2cReadBytes(MPU6050_RA_MEM_R_W, 1, buffer); // Get hardware revision
uint8_t hwRevision __attribute__((__unused__)) = buffer[0];
setMemoryBank(0, false, false);
i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 1, buffer);
uint8_t otpValid __attribute__((__unused__)) = buffer[0]; // Check OTP bank validity
i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
int8_t xgOffset = buffer[0];
i2cReadBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
int8_t ygOffset = buffer[0];
i2cReadBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
int8_t zgOffset = buffer[0];
value = 0x7F; // Setup weird slave stuff
i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value);
i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 1, false); // Disable I2C Master Mode
value = 0x68;
i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value);
i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1, true); // Reset I2C Master
usleep(20000);
if (writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, true, true)) { // Load DMP code into memory banks
if (writeDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE, true)) { // Let's write the DMP configuration
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); // Set clock source
value = 0x12;
i2cWriteBytes(MPU6050_RA_INT_ENABLE, 1, &value); // Enable 0x12 interuption
value = 4; // 1khz / (1 + 4) = 200 Hz
i2cWriteBytes(MPU6050_RA_SMPLRT_DIV, 1, &value);
i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); // External Frame Sync
i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); // DLPF Mode
i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); // Full scale gyro range
value = 0x03;
i2cWriteBytes(MPU6050_RA_DMP_CFG_1, 1, &value);
value = 0x00;
i2cWriteBytes(MPU6050_RA_DMP_CFG_2, 1, &value);
// Setting X Y and Z gyro offsets to previous values
i2cWriteBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, xgOffset);
i2cWriteBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, ygOffset);
i2cWriteBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, zgOffset);
value16 = 0; // And gyro offsets users to 0
i2cWriteWords(MPU6050_RA_XG_OFFS_USRH, 1, &value16);
i2cWriteWords(MPU6050_RA_YG_OFFS_USRH, 1, &value16);
i2cWriteWords(MPU6050_RA_ZG_OFFS_USRH, 1, &value16);
// Memory update 1/7
uint8_t dmpUpdate[16], j;
uint16_t pos = 0;
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//Memory update 2/7
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
resetFIFO();
uint8_t fifoCount = getFIFOCount();
uint8_t fifoBuffer[128];
value = 2;
i2cWriteBytes(MPU6050_RA_MOT_THR, 1, &value); // Motion threshold
value = 156;
i2cWriteBytes(MPU6050_RA_ZRMOT_THR, 1, &value); // Zero Motion threshold
value = 80;
i2cWriteBytes(MPU6050_RA_MOT_DUR, 1, &value); // Motion duration
value = 0;
i2cWriteBytes(MPU6050_RA_ZRMOT_DUR, 1, &value); // Zero Motion duration
resetFIFO();
setFIFOEnabled(true);
setDMPEnabled(true);
i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1, true); // Reset DMP
// Memory update 3/7
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
// Memory update 4/7
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
// Memory update 5/7
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
while ((fifoCount = getFIFOCount()) < 3);
getFIFOBytes(fifoBuffer, fifoCount);
uint8_t mpuIntStatus __attribute__((__unused__)) = getIntStatus();
// Memory update 6/7
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
while ((fifoCount = getFIFOCount()) < 3);
getFIFOBytes(fifoBuffer, fifoCount);
mpuIntStatus = getIntStatus();
// Memory update 7/7
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
setDMPEnabled(false);
dmpPacketSize = 42;
resetFIFO();
getIntStatus();
} else
abort();
} else
abort();
i2cReadBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
if(buffer[0] != 0x34)
abort();
setDMPEnabled(true);
zeroValues[0] = 0;
zeroValues[1] = 0;
zeroValues[2] = 0;
}
unsigned char dmpInitialize()
{
// reset device
mpu_reset();
DELAY_US(30*1000);
// disable sleep mode
setSleepEnabled(false);
// get MPU hardware revision
setMemoryBank(0x10, true, true);
setMemoryStartAddress(0x06);
unsigned char hwRevision = readMemoryByte();
setMemoryBank(0, false, false);
// check OTP bank valid
unsigned char otpValid = getOTPBankValid();
// get X/Y/Z gyro offsets
char xgOffsetTC = getXGyroOffset();
char ygOffsetTC = getYGyroOffset();
char zgOffsetTC = getZGyroOffset();
// setup weird slave stuff (?)
// setSlaveAddress(0, 0x7F);
// setI2CMasterModeEnabled(false);
// setSlaveAddress(0, 0x68);
// resetI2CMaster();
DELAY_US(20*1000);
// load DMP code into memory banks
if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE,0,0,false))
{
if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
setIntEnabled(0x12);
setRate(4); // 1khz / (1 + 4) = 200 Hz
setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
setDLPFMode(MPU6050_DLPF_BW_42);
// setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
// setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
setDMPConfig1(0x03);
setDMPConfig2(0x00);
setOTPBankValid(false);
// setXGyroOffset(xgOffsetTC);
// setYGyroOffset(ygOffsetTC);
// setZGyroOffset(zgOffsetTC);
unsigned char dmpUpdate[16], j;
unsigned int pos = 0;
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false);
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false);
resetFIFO();
unsigned int fifoCount = getFIFOCount();
if (fifoCount > 128)
{
fifoCount = 128;
}
getFIFOBytes(fifoBuffer, fifoCount);
setMotionDetectionThreshold(2);
setZeroMotionDetectionThreshold(156);
setMotionDetectionDuration(80);
setZeroMotionDetectionDuration(0);
resetFIFO();
setFIFOEnabled(true);
setDMPEnabled(true);
resetDMP();
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false);
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false);
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false);
// while ((getFIFOCount()) < 3) {DELAY_US(500);}
//
// fifoCount = getFIFOCount();
//
// getFIFOBytes(fifoBuffer, fifoCount);
unsigned char mpuIntStatus = getIntStatus();
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
// while ((getFIFOCount()) < 3) {DELAY_US(500);}
//
// fifoCount = getFIFOCount();
// getFIFOBytes(fifoBuffer, fifoCount);
mpuIntStatus = getIntStatus();
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false);
setDMPEnabled(true);
resetFIFO();
getIntStatus();
} else {
return 2; // configuration block loading failed
}
} else {
return 1; // main binary block loading failed
}
return 0; // success
}
void ResetFIFO(client *c, int argc, sds *argv)
{
resetFIFO();
addReplyStatus(c, "+OK");
}
uint8_t MPU6050DMP::dmpInitialize() {
// reset device
DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
reset();
delay(30); // wait after reset
// enable sleep mode and wake cycle
/*Serial.println(F("Enabling sleep mode..."));
setSleepEnabled(true);
Serial.println(F("Enabling wake cycle..."));
setWakeCycleEnabled(true);*/
// disable sleep mode
DEBUG_PRINTLN(F("Disabling sleep mode..."));
setSleepEnabled(false);
// get MPU hardware revision
DEBUG_PRINTLN(F("Selecting user bank 16..."));
setMemoryBank(0x10, true, true);
DEBUG_PRINTLN(F("Selecting memory byte 6..."));
setMemoryStartAddress(0x06);
DEBUG_PRINTLN(F("Checking hardware revision..."));
uint8_t hwRevision = readMemoryByte();
DEBUG_PRINT(F("Revision @ user[16][6] = "));
DEBUG_PRINTLNF(hwRevision, HEX);
DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
setMemoryBank(0, false, false);
// check OTP bank valid
DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
uint8_t otpValid = getOTPBankValid();
DEBUG_PRINT(F("OTP bank is "));
DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!"));
// get X/Y/Z gyro offsets
DEBUG_PRINTLN(F("Reading gyro offset TC values..."));
int8_t xgOffsetTC = getXGyroOffsetTC();
int8_t ygOffsetTC = getYGyroOffsetTC();
int8_t zgOffsetTC = getZGyroOffsetTC();
DEBUG_PRINT(F("X gyro offset = "));
DEBUG_PRINTLN(xgOffset);
DEBUG_PRINT(F("Y gyro offset = "));
DEBUG_PRINTLN(ygOffset);
DEBUG_PRINT(F("Z gyro offset = "));
DEBUG_PRINTLN(zgOffset);
// setup weird slave stuff (?)
DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F..."));
setSlaveAddress(0, 0x7F);
DEBUG_PRINTLN(F("Disabling I2C Master mode..."));
setI2CMasterModeEnabled(false);
DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)..."));
setSlaveAddress(0, 0x68);
DEBUG_PRINTLN(F("Resetting I2C Master control..."));
resetI2CMaster();
delay(20);
// load DMP code into memory banks
DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
DEBUG_PRINT(MPU6050_DMP_CODE_SIZE);
DEBUG_PRINTLN(F(" bytes)"));
if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
DEBUG_PRINTLN(F("Success! DMP code written and verified."));
// write DMP configuration
DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks ("));
DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE);
DEBUG_PRINTLN(F(" bytes in config def)"));
if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
setIntEnabled(0x12);
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
setRate(4); // 1khz / (1 + 4) = 200 Hz
DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
setDLPFMode(MPU6050_DLPF_BW_42);
DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)..."));
setDMPConfig1(0x03);
setDMPConfig2(0x00);
DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
setOTPBankValid(false);
DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values..."));
setXGyroOffsetTC(xgOffsetTC);
setYGyroOffsetTC(ygOffsetTC);
setZGyroOffsetTC(zgOffsetTC);
//DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
//setXGyroOffset(0);
//setYGyroOffset(0);
//setZGyroOffset(0);
DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)..."));
uint8_t dmpUpdate[16], j;
uint16_t pos = 0;
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)..."));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
DEBUG_PRINTLN(F("Resetting FIFO..."));
resetFIFO();
DEBUG_PRINTLN(F("Reading FIFO count..."));
uint16_t fifoCount = getFIFOCount();
uint8_t fifoBuffer[128];
DEBUG_PRINT(F("Current FIFO count="));
DEBUG_PRINTLN(fifoCount);
getFIFOBytes(fifoBuffer, fifoCount);
DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
setMotionDetectionThreshold(2);
DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
setZeroMotionDetectionThreshold(156);
DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
setMotionDetectionDuration(80);
DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
setZeroMotionDetectionDuration(0);
DEBUG_PRINTLN(F("Resetting FIFO..."));
resetFIFO();
DEBUG_PRINTLN(F("Enabling FIFO..."));
setFIFOEnabled(true);
DEBUG_PRINTLN(F("Enabling DMP..."));
setDMPEnabled(true);
DEBUG_PRINTLN(F("Resetting DMP..."));
resetDMP();
DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)..."));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)..."));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)..."));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
while ((fifoCount = getFIFOCount()) < 3);
DEBUG_PRINT(F("Current FIFO count="));
DEBUG_PRINTLN(fifoCount);
DEBUG_PRINTLN(F("Reading FIFO data..."));
getFIFOBytes(fifoBuffer, fifoCount);
DEBUG_PRINTLN(F("Reading interrupt status..."));
uint8_t mpuIntStatus = getIntStatus();
DEBUG_PRINT(F("Current interrupt status="));
DEBUG_PRINTLNF(mpuIntStatus, HEX);
DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)..."));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
while ((fifoCount = getFIFOCount()) < 3);
DEBUG_PRINT(F("Current FIFO count="));
DEBUG_PRINTLN(fifoCount);
DEBUG_PRINTLN(F("Reading FIFO data..."));
getFIFOBytes(fifoBuffer, fifoCount);
DEBUG_PRINTLN(F("Reading interrupt status..."));
mpuIntStatus = getIntStatus();
DEBUG_PRINT(F("Current interrupt status="));
DEBUG_PRINTLNF(mpuIntStatus, HEX);
DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)..."));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
DEBUG_PRINTLN(F("DMP is good to go! Finally."));
DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
setDMPEnabled(false);
DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer..."));
dmpPacketSize = 42;
/*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
return 3; // TODO: proper error code for no memory
}*/
DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
resetFIFO();
getIntStatus();
} else {
DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
return 2; // configuration block loading failed
}
} else {
DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
return 1; // main binary block loading failed
}
return 0; // success
}
/*Constructor */
drvSIS3820::drvSIS3820(const char *portName, int baseAddress, int interruptVector, int interruptLevel,
int maxChans, int maxSignals, bool useDma, int fifoBufferWords)
: drvSIS38XX(portName, maxChans, maxSignals),
useDma_(useDma)
{
int status;
epicsUInt32 controlStatusReg;
epicsUInt32 moduleID;
static const char* functionName="SIS3820";
setIntegerParam(SIS38XXModel_, MODEL_SIS3820);
/* Call devLib to get the system address that corresponds to the VME
* base address of the board.
*/
status = devRegisterAddress("drvSIS3820",
SIS3820_ADDRESS_TYPE,
(size_t)baseAddress,
SIS3820_BOARD_SIZE,
(volatile void **)®isters_);
if (status) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: %s, Can't register VME address 0x%x\n",
driverName, functionName, portName, baseAddress);
return;
}
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: Registered VME address: 0x%x to local address: %p size: 0x%X\n",
driverName, functionName, baseAddress, registers_, SIS3820_BOARD_SIZE);
/* Call devLib to get the system address that corresponds to the VME
* FIFO address of the board.
*/
fifoBaseVME_ = (epicsUInt32 *)(baseAddress + SIS3820_FIFO_BASE);
status = devRegisterAddress("drvSIS3820",
SIS3820_ADDRESS_TYPE,
(size_t)fifoBaseVME_,
SIS3820_FIFO_BYTE_SIZE,
(volatile void **)&fifoBaseCPU_);
if (status) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: %s, Can't register FIFO address 0x%x\n",
driverName, functionName, portName, baseAddress + SIS3820_FIFO_BASE);
return;
}
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: Registered VME FIFO address: %p to local address: %p size: 0x%X\n",
driverName, functionName, fifoBaseVME_,
fifoBaseCPU_, SIS3820_FIFO_BYTE_SIZE);
/* Probe VME bus to see if card is there */
status = devReadProbe(4, (char *) &(registers_->control_status_reg),
(char *) &controlStatusReg);
if (status) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: devReadProbe failure for address %p = %d\n",
driverName, functionName, ®isters_->control_status_reg, status);
return;
}
/* Get the module info from the card */
moduleID = (registers_->moduleID_reg & 0xFFFF0000) >> 16;
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: module ID=%x\n",
driverName, functionName, moduleID);
firmwareVersion_ = registers_->moduleID_reg & 0x0000FFFF;
setIntegerParam(SIS38XXFirmware_, firmwareVersion_);
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: firmware=%d\n",
driverName, functionName, firmwareVersion_);
// Allocate FIFO readout buffer
// fifoBufferWords input argument is in words, must be less than SIS3820_FIFO_WORD_SIZE
if (fifoBufferWords == 0) fifoBufferWords = SIS3820_FIFO_WORD_SIZE;
if (fifoBufferWords > SIS3820_FIFO_WORD_SIZE) fifoBufferWords = SIS3820_FIFO_WORD_SIZE;
fifoBufferWords_ = fifoBufferWords;
#ifdef vxWorks
fifoBuffer_ = (epicsUInt32*) memalign(8, fifoBufferWords_*sizeof(epicsUInt32));
#else
void *ptr;
status = posix_memalign(&ptr, 8, fifoBufferWords_*sizeof(epicsUInt32));
if(status != 0) {
printf("Error: posix_memalign status = %d\n", status);
}
fifoBuffer_ = (epicsUInt32*)ptr;
#endif
if (fifoBuffer_ == NULL) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: posix_memalign failure for fifoBuffer_ = %d\n",
driverName, functionName, status);
return;
}
dmaDoneEventId_ = epicsEventCreate(epicsEventEmpty);
// Create the DMA ID
if (useDma_) {
dmaId_ = sysDmaCreate(dmaCallbackC, (void*)this);
if (dmaId_ == 0 || (int)dmaId_ == -1) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: sysDmaCreate failed, errno=%d. Disabling use of DMA.\n",
driverName, functionName, errno);
useDma_ = false;
}
}
/* Reset card */
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: resetting port %s\n",
driverName, functionName, portName);
registers_->key_reset_reg = 1;
/* Clear FIFO */
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: clearing FIFO\n",
driverName, functionName);
resetFIFO();
// Disable 25MHz test pulses and test mode
registers_->control_status_reg = CTRL_COUNTER_TEST_25MHZ_DISABLE;
registers_->control_status_reg = CTRL_COUNTER_TEST_MODE_DISABLE;
/* Set up the interrupt service routine */
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: interruptServiceRoutine pointer %p\n",
driverName, functionName, intFuncC);
status = devConnectInterruptVME(interruptVector, intFuncC, this);
if (status) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: Can't connect to vector % d\n",
driverName, functionName, interruptVector);
return;
}
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: Connected interrupt vector: %d\n\n",
driverName, functionName, interruptVector);
/* Write interrupt level to hardware */
registers_->irq_config_reg &= ~SIS3820_IRQ_LEVEL_MASK;
registers_->irq_config_reg |= (interruptLevel << 8);
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: irq after setting IntLevel= 0x%x\n",
driverName, functionName, registers_->irq_config_reg);
/* Write interrupt vector to hardware */
registers_->irq_config_reg &= ~SIS3820_IRQ_VECTOR_MASK;
registers_->irq_config_reg |= interruptVector;
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: irq = 0x%08x\n",
driverName, functionName, registers_->irq_config_reg);
/* Initialize board in MCS mode. This will also set the initial value of the operation mode register. */
setAcquireMode(ACQUIRE_MODE_MCS);
/* Create the thread that reads the FIFO */
if (epicsThreadCreate("SIS3820FIFOThread",
epicsThreadPriorityLow,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC)readFIFOThreadC,
this) == NULL) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: epicsThreadCreate failure\n",
driverName, functionName);
return;
}
else
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: epicsThreadCreate success\n",
driverName, functionName);
erase();
/* Enable interrupts in hardware */
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: irq before enabling interrupts= 0x%08x\n",
driverName, functionName, registers_->irq_config_reg);
registers_->irq_config_reg |= SIS3820_IRQ_ENABLE;
/* Enable interrupt level in EPICS */
status = devEnableInterruptLevel(intVME, interruptLevel);
if (status) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: Can't enable enterrupt level %d\n",
driverName, functionName, interruptLevel);
return;
}
exists_ = true;
return;
}
float MPU6050::dmpGetFirstYPRData()
{
long long time = timeCheck();
if(dmpReady && time > 0)
{
// get current FIFO count
dmpFifoCount = getFIFOCount();
if (dmpFifoCount == 1024) {
// reset so we can continue cleanly
resetFIFO();
timeReset();
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (dmpFifoCount >= 42) {
timeCount(time);
// read a packet from FIFO
getFIFOBytes(dmpFifoBuffer, dmpPacketSize);
// display quaternion values in easy matrix form: w x y z
dmpGetQuaternion(&dmpQuat, dmpFifoBuffer);
// if(dmpDebug) printf("quat %7.2f %7.2f %7.2f %7.2f ", dmpQuat.w,dmpQuat.x,dmpQuat.y,dmpQuat.z);
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
dmpGetEuler(dmpEuler, &dmpQuat);
dmpEuler[0] = dmpEuler[0] * 180/M_PI;
dmpEuler[1] = dmpEuler[1] * 180/M_PI;
dmpEuler[2] = dmpEuler[2] * 180/M_PI;
// if(dmpDebug) printf("euler %7.2f %7.2f %7.2f ", dmpEuler[0], dmpEuler[1], dmpEuler[2]);
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
dmpGetGravity(&dmpGravity, &dmpQuat);
dmpGetYawPitchRoll(dmpYawPitchRoll, &dmpQuat, &dmpGravity);
dmpYawPitchRoll[0] = dmpYawPitchRoll[0] * 180/M_PI;
dmpYawPitchRoll[1] = dmpYawPitchRoll[1] * 180/M_PI;
dmpYawPitchRoll[2] = dmpYawPitchRoll[2] * 180/M_PI;
// if(dmpDebug) printf("ypr %7.2f %7.2f %7.2f ", dmpYawPitchRoll[0], dmpYawPitchRoll[1], dmpYawPitchRoll[2]);
if(dmpDebug)
return dmpYawPitchRoll[0];
#endif
#ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity
dmpGetAccel(&dmpAccel, dmpFifoBuffer);
dmpGetGravity(&dmpGravity, &dmpQuat);
dmpGetLinearAccel(&dmpAccelReal, &dmpAccel, &dmpGravity);
// if(dmpDebug) printf("areal %6d %6d %6d ", dmpAccelReal.x, dmpAccelReal.y, dmpAccelReal.z);
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
dmpGetAccel(&dmpAccel, dmpFifoBuffer);
dmpGetGravity(&dmpGravity, &dmpQuat);
dmpGetLinearAccelInWorld(&dmpAccelWorld, &dmpAccelReal, &dmpQuat);
// if(dmpDebug) printf("aworld %6d %6d %6d ", dmpAccelWorld.x, dmpAccelWorld.y, dmpAccelWorld.z);
#endif
#ifdef OUTPUT_TEAPOT
// display quaternion values in InvenSense Teapot demo format:
dmpTeapotPacket[2] = dmpFifoBuffer[0];
dmpTeapotPacket[3] = dmpFifoBuffer[1];
dmpTeapotPacket[4] = dmpFifoBuffer[4];
dmpTeapotPacket[5] = dmpFifoBuffer[5];
dmpTeapotPacket[6] = dmpFifoBuffer[8];
dmpTeapotPacket[7] = dmpFifoBuffer[9];
dmpTeapotPacket[8] = dmpFifoBuffer[12];
dmpTeapotPacket[9] = dmpFifoBuffer[13];
//Serial.write(dmpTeapotPacket, 14);
dmpTeapotPacket[11]++; // packetCount, loops at 0xFF on purpose
#endif
return -255;
}
}
return -255;
}
void drvSIS3801::startMCSAcquire()
{
int nChans;
int prescale;
int channelAdvanceSource;
int countOnStart;
static const char *functionName="startMCSAcquire";
getIntegerParam(mcaNumChannels_, &nChans);
getIntegerParam(mcaChannelAdvanceSource_, &channelAdvanceSource);
getIntegerParam(SIS38XXCountOnStart_, &countOnStart);
getIntegerParam(mcaPrescale_, &prescale);
setAcquireMode(ACQUIRE_MODE_MCS);
// Reset the FIFO and nextSignal_, but not nextChan_ because we could be resuming acquisition
resetFIFO();
nextSignal_ = 0;
if (firmwareVersion_ >= 5) {
if (channelAdvanceSource == mcaChannelAdvance_Internal) {
/* The SIS3801 requires the value in the LNE prescale register to be one
* less than the actual number of incoming signals. We do this adjustment
* here, so the user sees the actual number at the record level.
*/
double dwellTime;
getDoubleParam(mcaDwellTime_, &dwellTime);
registers_->csr_reg = CONTROL_M_ENABLE_10MHZ_LNE_PRESCALER;
prescale = (epicsUInt32) (SIS3801_10MHZ_CLOCK * dwellTime) - 1;
}
else if (channelAdvanceSource == mcaChannelAdvance_External) {
getIntegerParam(mcaPrescale_, &prescale);
/* The SIS3801 requires the value in the LNE prescale register to be one
* less than the actual number of incoming signals. We do this adjustment
* here, so the user sees the actual number at the record level.
*/
prescale--;
registers_->csr_reg = CONTROL_M_DISABLE_10MHZ_LNE_PRESCALER;
}
else {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: unsupported channel advance source %d\n",
driverName, functionName, channelAdvanceSource);
}
/* Set the prescale factor. Note, it is necessary to do the following in
* for the prescale counter to be properly cleared:
* - Disable prescaler
* - Write prescale factor
* - Enable prescaler
*/
registers_->csr_reg = CONTROL_M_DISABLE_LNE_PRESCALER;
registers_->prescale_factor_reg = prescale;
registers_->csr_reg = CONTROL_M_ENABLE_LNE_PRESCALER;
}
/* Enable next logic */
registers_->enable_next_reg = 1;
/* Enable counting bit in CSR */
registers_->csr_reg = CONTROL_M_CLEAR_SOFTWARE_DISABLE;
/* Do one software next_clock if enabled */
if (countOnStart != 0)
softwareChannelAdvance();
}
/**
* Initialize DMP inside MPU6050
* @return boolean
*/
uint8_t MPU6050::dmpInitialize() {
//Resetting MPU6050...
reset();
usleep(30000); // wait after reset
//Disabling sleep mode...
setSleepEnabled(false);
// get MPU hardware revision
//Selecting user bank 16...
setMemoryBank(0x10, true, true);
//Selecting memory byte 6...
setMemoryStartAddress(0x06);
//Checking hardware revision...
uint8_t hwRevision __attribute__((__unused__)) = readMemoryByte();
//Revision @ user[16][6] =
//hwRevision, HEX);
//Resetting memory bank selection to 0...
setMemoryBank(0, false, false);
// check OTP bank valid
//Reading OTP bank valid flag...
uint8_t otpValid __attribute__((__unused__)) = getOTPBankValid();
//OTP bank is
//otpValid ? F("valid!") : F("invalid!
// get X/Y/Z gyro offsets
//Reading gyro offset values...
/*
int8_t xgOffset = getXGyroOffset();
int8_t ygOffset = getYGyroOffset();
int8_t zgOffset = getZGyroOffset();
sleep(5);
for(int cnt = 0; cnt < 1000; cnt = cnt + 1)
{
std::cout << "X: " << typeid(xgOffset).name() << "Y: " << typeid(ygOffset).name() << "Z: " << typeid(zgOffset).name() << "\n";
sleep(1);
//xgOffset = (getXGyroOffset() + xgOffset)/2;
//ygOffset = (getYGyroOffset() + ygOffset)/2;
//zgOffset = (getZGyroOffset() + zgOffset)/2;
}
*/
//X gyro offset =
//xgOffset);
//Y gyro offset =
//ygOffset);
//Z gyro offset =
//zgOffset);
// setup weird slave stuff (?)
//Setting slave 0 address to 0x7F...
setSlaveAddress(0, 0x7F);
//Disabling I2C Master mode...
setI2CMasterModeEnabled(false);
//Setting slave 0 address to 0x68 (self)...
setSlaveAddress(0, 0x68);
/*
if (addr == 104)
{
std::cout << "Address: 104";
setSlaveAddress(0, 0x68);
}
else
{
std::cout << "Address: 105";
setSlaveAddress(0, 0x69);
}
* */
//Resetting I2C Master control...
resetI2CMaster();
usleep(20000);
// load DMP code into memory banks
//Writing DMP code to MPU memory banks (
// bytes)
if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
// write DMP configuration
//Writing DMP configuration to MPU memory banks (
// bytes in config def)
if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
//Setting clock source to Z Gyro...
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
//Setting DMP and FIFO_OFLOW interrupts enabled...
setIntEnabled(0x12);
//Setting sample rate to 200Hz...
setRate(4); // 1khz / (1 + 4) = 200 Hz
//Setting external frame sync to TEMP_OUT_L[0]...
setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
//Setting DLPF bandwidth to 42Hz...
setDLPFMode(MPU6050_DLPF_BW_42);
//Setting gyro sensitivity to +/- 2000 deg/sec...
setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
//Setting DMP configuration bytes (function unknown)...
setDMPConfig1(0x03);
setDMPConfig2(0x00);
//Clearing OTP Bank flag...
setOTPBankValid(false);
/*
//Setting X/Y/Z gyro offsets to previous values...
setXGyroOffset(xgOffset);
setYGyroOffset(ygOffset);
setZGyroOffset(zgOffset);
//Setting X/Y/Z gyro user offsets to zero...
setXGyroOffsetUser(0);
setYGyroOffsetUser(0);
setZGyroOffsetUser(0);
*/
//Writing final memory update 1/7 (function unknown)...
uint8_t dmpUpdate[16], j;
uint16_t pos = 0;
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//Writing final memory update 2/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//Resetting FIFO...
resetFIFO();
//Reading FIFO count...
uint8_t fifoCount = getFIFOCount();
uint8_t fifoBuffer[128];
//fifoCount);
if (fifoCount > 0)
getFIFOBytes(fifoBuffer, fifoCount);
//Setting motion detection threshold to 2...
setMotionDetectionThreshold(2);
//Setting zero-motion detection threshold to 156...
setZeroMotionDetectionThreshold(156);
//Setting motion detection duration to 80...
setMotionDetectionDuration(80);
//Setting zero-motion detection duration to 0...
setZeroMotionDetectionDuration(0);
//Resetting FIFO...
resetFIFO();
//Enabling FIFO...
setFIFOEnabled(true);
//Enabling DMP...
setDMPEnabled(true);
//Resetting DMP...
resetDMP();
//Writing final memory update 3/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//Writing final memory update 4/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//Writing final memory update 5/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
while ((fifoCount = getFIFOCount()) < 3);
//Reading FIFO data...
getFIFOBytes(fifoBuffer, fifoCount);
//Reading interrupt status...
uint8_t mpuIntStatus __attribute__((__unused__)) = getIntStatus();
//Current interrupt status= mpuIntStatus, HEX
//Reading final memory update 6/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//Waiting for FIFO count > 2...
while ((fifoCount = getFIFOCount()) < 3);
//Current FIFO count=
//fifoCount);
//Reading FIFO data...
getFIFOBytes(fifoBuffer, fifoCount);
//Reading interrupt status...
mpuIntStatus = getIntStatus();
//Current interrupt status=
//mpuIntStatus, HEX
//Writing final memory update 7/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//DMP is good to go! Finally.
//Disabling DMP (you turn it on later)...
setDMPEnabled(false);
//Setting up internal 42-byte (default) DMP packet buffer...
dmpPacketSize = 42;
/*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
return 3; // TODO: proper error code for no memory
}*/
//Resetting FIFO and clearing INT status one last time...
resetFIFO();
getIntStatus();
} else {
//ERROR! DMP configuration verification failed.
return 2; // configuration block loading failed
}
} else {
//ERROR! DMP code verification failed.
return 1; // main binary block loading failed
}
return 0; // success
}
/*Constructor */
drvSIS3801::drvSIS3801(const char *portName, int baseAddress, int interruptVector, int interruptLevel,
int maxChans, int maxSignals)
: drvSIS38XX(portName, maxChans, maxSignals)
{
int status;
epicsUInt32 controlStatusReg;
epicsUInt32 moduleID;
static const char* functionName="SIS3801";
setIntegerParam(SIS38XXModel_, MODEL_SIS3801);
/* Call devLib to get the system address that corresponds to the VME
* base address of the board.
*/
status = devRegisterAddress("drvSIS3801",
SIS3801_ADDRESS_TYPE,
(size_t)baseAddress,
SIS3801_BOARD_SIZE,
(volatile void **)®isters_);
if (status) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: %s, Can't register VME address %p\n",
driverName, functionName, portName, baseAddress);
return;
}
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: Registered VME address: 0x%lX to local address: %p size: 0x%X\n",
driverName, functionName, (long)baseAddress, registers_, SIS3801_BOARD_SIZE);
/* Probe VME bus to see if card is there */
status = devReadProbe(4, (char *) ®isters_->csr_reg,
(char *) &controlStatusReg);
if (status) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: devReadProbe failure = %d\n",
driverName, functionName, status);
return;
}
/* Get the module info from the card */
moduleID = (registers_->irq_reg & 0xFFFF0000) >> 16;
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: module ID=%x\n",
driverName, functionName, moduleID);
firmwareVersion_ = (registers_->irq_reg & 0x0000F000) >> 12;
setIntegerParam(SIS38XXFirmware_, firmwareVersion_);
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: firmware=%d\n",
driverName, functionName, firmwareVersion_);
// Create the mutex used to lock access to the FIFO
fifoLockId_ = epicsMutexCreate();
/* Reset card */
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: resetting port %s\n",
driverName, functionName, portName);
registers_->key_reset_reg = 1;
/* Clear FIFO */
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: clearing FIFO\n",
driverName, functionName);
resetFIFO();
setControlStatusReg();
/* Initialize board in MCS mode */
setAcquireMode(ACQUIRE_MODE_MCS);
/* Set number of readout channels to maxSignals
* Assumes that the lower channels will be used first, and the only unused
* channels will be at the upper end of the channel range.
* Create a mask with zeros in the rightmost maxSignals bits,
* 1 in all higher order bits. */
registers_->copy_disable_reg = 0xffffffff<<maxSignals_;
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: setting copy disable register=0x%08x\n",
driverName, functionName, 0xffffffff<<maxSignals_);
/* Set up the interrupt service routine */
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: interruptServiceRoutine pointer %p\n",
driverName, functionName, intFuncC);
status = devConnectInterruptVME(interruptVector, intFuncC, this);
if (status) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: Can't connect to vector % d\n",
driverName, functionName, interruptVector);
return;
}
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: Connected interrupt vector: %d\n\n",
driverName, functionName, interruptVector);
/* Write interrupt level to hardware */
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: irq before setting IntLevel= 0x%x\n",
driverName, functionName, registers_->irq_reg);
registers_->irq_reg |= (interruptLevel << 8);
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: irq after setting IntLevel= 0x%x\n",
driverName, functionName, registers_->irq_reg);
registers_->irq_reg |= interruptVector;
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"m%s:%s: irq config register after setting interrupt vector = 0x%08x\n",
driverName, functionName, registers_->irq_reg);
/* Create the thread that reads the FIFO */
if (epicsThreadCreate("SIS3801FIFOThread",
epicsThreadPriorityLow,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC)readFIFOThreadC,
this) == NULL) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: epicsThreadCreate failure\n",
driverName, functionName);
return;
}
else
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: epicsThreadCreate success\n",
driverName, functionName);
erase();
/* Enable interrupts in hardware */
asynPrint(pasynUserSelf, ASYN_TRACE_FLOW,
"%s:%s: irq before enabling interrupts= 0x%08x\n",
driverName, functionName, registers_->irq_reg);
enableInterrupts();
/* Enable interrupt level in EPICS */
status = devEnableInterruptLevel(intVME, interruptLevel);
if (status) {
asynPrint(pasynUserSelf, ASYN_TRACE_ERROR,
"%s:%s: Can't enable enterrupt level %d\n",
driverName, functionName, interruptLevel);
return;
}
exists_ = true;
return;
}
void drvSIS3820::stopScaler()
{
registers_->key_op_disable_reg = 1;
resetFIFO();
}
