static PyObject *I2CDev_read(I2CDev *self, PyObject *args, PyObject *kwds) {
uint32_t n_bytes, i, addr;
PyObject *data, *byte_obj;
uint8_t *rxbuf;
if(!PyArg_ParseTuple(args, "II", &addr, &n_bytes)) {
return NULL;
}
if (self->slave_addr != addr) {
if (setSlaveAddress(self->i2c_fd, addr) < 0) {
PyErr_SetString(PyExc_IOError, "could not configure I2C interface");
return NULL;
}
self->slave_addr = addr;
}
rxbuf = malloc(n_bytes);
if (I2C_read(self->i2c_fd, (void *) rxbuf, n_bytes) < 0) {
PyErr_SetString(PyExc_IOError, "could not read from I2C device");
free(rxbuf);
return NULL;
}
data = PyList_New(0);
for (i=0; i<n_bytes; i++) {
byte_obj = PyInt_FromLong((long) rxbuf[i]);
PyList_Append(data, byte_obj);
Py_DECREF(byte_obj);
}
free(rxbuf);
return data;
}
int main()
{
//Step 1
//Enalbe and provide a clock to I2C module 1 in Run mode.
SYSCTL->RCGCI2C = (1<<1); //using I2C Module 1
//Step 2
SYSCTL->RCGCGPIO = (1<<0); //Module 1 uses port A => we must enable the clock on Port A
//Step 3
GPIOA->AFSEL = (1<<6)|(1<<7); //Enabling the alternate functions for pin 6 and 7
GPIOA->DEN = (1<<6)|(1<<7); //They are digital signals
//Step 4
//Setting a bit in this register enables the open-drain configuration of the corresponding GPIO pad
//When this bit is enabled, it should also be set in the GPIODEN register.
//PA7 corresponds to SDA.
GPIOA->ODR = (1<<7);
//Step 5
//GPIOA->PCTL &= ~0xFF000000; //This line is not required
//Specify which alternate function you want to use, see Table 21-5
GPIOA->PCTL = (3<<28)|(3<<24); //put a 3 in PMC7 and PMC6 (pins PA7 and PA6 respectively)
//Step 6
//Initialize I2C as a Master
I2C1->MCR = (1<<4); //Master mode is enabled
//Step 7
//Set the desired SCL clock speed of 100 Kbps
//The TPR (for us is 7) is found using the formula on pg. 972
I2C1->MTPR = (7<<0);
//1 byte must be sent
setSlaveAddress(0x20); //last 7 bits (7-1)
setRW(0); //first bit (0)
//writeByte(BYTE ONE, BYTE TWO)
//send IODIR register address
writeByte(0x00, (1<<0)|(1<<1)); //BYTE TWO: conditions = RUN and START
//send IODIR value to make GPIO pins outputs
writeByte(0x00, (1<<0)|(1<<2)); //BTYE TWO: conditions = RUN, STOP
while(1)
{
writeByte(0x09, (1<<0)|(1<<1));
writeByte(leds++, (1<<0)|(1<<2));
for(int i = 0; i < 100000; i++){};
}
return 0;
}
static PyObject *I2CDev_write(I2CDev *self, PyObject *args, PyObject *kwds) {
uint32_t n_bytes, i, addr;
long byte;
PyObject *data, *byte_obj;
uint8_t *txbuf;
if(!PyArg_ParseTuple(args, "IO!", &addr, &PyList_Type, &data)) {
return NULL;
}
if (self->slave_addr != addr) {
if (setSlaveAddress(self->i2c_fd, addr) < 0) {
PyErr_SetString(PyExc_IOError, "could not configure I2C interface");
return NULL;
}
self->slave_addr = addr;
}
n_bytes = PyList_Size(data);
txbuf = malloc(n_bytes);
for (i=0; i<n_bytes; i++) {
byte_obj = PyList_GetItem(data, i);
if (!PyInt_Check(byte_obj)) {
PyErr_SetString(PyExc_ValueError,
"data list to transmit can only contain integers");
free(txbuf);
return NULL;
}
byte = PyInt_AsLong(byte_obj);
if (byte < 0) {
// Check for error from PyInt_AsLong:
if (PyErr_Occurred() != NULL) return NULL;
// Negative numbers are set to 0:
byte = 0;
}
// Just send the LSB if value longer than 1 byte:
byte &= 255;
txbuf[i] = (uint8_t) byte;
}
if (I2C_write(self->i2c_fd, (void *) txbuf, n_bytes) < 0) {
PyErr_SetString(PyExc_IOError, "could not write to I2C device");
free(txbuf);
return NULL;
}
free(txbuf);
Py_INCREF(Py_None);
return Py_None;
}
/**
* 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
}
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
}
