/** * 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(); }