static uint8_t accelReadReg(uint8_t reg)
{
/* Enable the TWI peripheral clock */
PRR &= ~_BV(PRTWI);
/* Select the specified register */
twiStart(ACCEL_I2C_ADDR | TW_WRITE);
twiSendByte(reg);
/* Restart as master receiver */
twiStart(ACCEL_I2C_ADDR | TW_READ);
/* Wait for data; send NACK */
TWCR = _BV(TWINT) | _BV(TWEN) | (0<<TWEA);
BUSY_UNTIL(TWCR & _BV(TWINT));
/* Send the "stop" signal */
TWCR = _BV(TWSTO) | _BV(TWEN) | _BV(TWINT);
BUSY_WHILE(TWCR & _BV(TWSTO));
/* Read the received byte */
uint8_t val = TWDR;
/* Disable the peripheral clock */
PRR |= _BV(PRTWI);
return val;
}
uint8_t rdReg(uint8_t reg){
uint8_t dat;
twiStart();
twiWriteByte(OV7670_I2C_ADDRESS<<1,TW_MT_SLA_ACK);
twiWriteByte(reg,TW_MT_DATA_ACK);
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);//send stop
_delay_ms(1);
twiStart();
twiWriteByte((OV7670_I2C_ADDRESS<<1)|1,TW_MR_SLA_ACK);
dat=twiRd(1);
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);//send stop
_delay_ms(1);
return dat;
}
uint8_t rdReg(uint8_t reg){
PORTG|=1<<5;
uint16_t dat;
twiStart();
twiAddr(camAddr_WR,TW_MT_SLA_ACK);
twiWriteByte(reg,TW_MT_DATA_ACK);
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);//send stop
_delay_ms(1);
twiStart();
twiAddr(camAddr_RD,TW_MR_SLA_ACK);
dat=twiRd(1);
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);//send stop
_delay_ms(1);
PORTG&=~(1<<5);
return dat;
}
uint8_t lcdGetChar(void) {
uint8_t ret;
twiStart(LCD_ADDRESS+I2C_READ);
ret = twiReadAck();
twiReadNak();
twiStop();
return ret;
}
void wrReg(uint8_t reg,uint8_t dat){
//send start condition
twiStart();
twiWriteByte(OV7670_I2C_ADDRESS<<1,TW_MT_SLA_ACK);
twiWriteByte(reg,TW_MT_DATA_ACK);
twiWriteByte(dat,TW_MT_DATA_ACK);
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);//send stop
_delay_ms(1);
}
void wrReg(uint8_t reg,uint8_t dat){
//send start condition
PORTG|=1<<5;
twiStart();
twiAddr(camAddr_WR,TW_MT_SLA_ACK);
twiWriteByte(reg,TW_MT_DATA_ACK);
twiWriteByte(dat,TW_MT_DATA_ACK);
TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);//send stop
_delay_ms(1);
PORTG&=~(1<<5);
}
/** Write a Gyroscope Register.
* I2C should aready be initialized!
*
* \param reg Register Address
* \param val New Value
* \returns #TWI_NO_ANSWER, #TWI_WRITE_ERROR or #SUCCESS.
*/
Error gyroWriteByte(uint8_t reg, uint8_t val) {
if (twiStart(GYRO_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(reg)) {
return TWI_WRITE_ERROR;
}
if (twiWrite(val)) {
return TWI_WRITE_ERROR;
}
twiStop();
return SUCCESS;
}
/** Write a Magnetometer Register.
* I2C should aready be initialized!
*
* \param reg Register Address
* \param val New Value
* \returns #TWI_NO_ANSWER, #TWI_WRITE_ERROR or #SUCCESS.
*/
Error magWriteRegister(uint8_t reg, uint8_t val) {
if (twiStart(MAG_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(reg)) {
return TWI_WRITE_ERROR;
}
if (twiWrite(val)) {
return TWI_WRITE_ERROR;
}
twiStop();
return SUCCESS;
}
static void accelWriteReg(uint8_t reg, uint8_t val)
{
/* Enable the TWI peripheral clock */
PRR &= ~_BV(PRTWI);
/* Write the register, then the address */
twiStart(ACCEL_I2C_ADDR | TW_WRITE);
twiSendByte(reg);
twiSendByte(val);
/* Send stop */
TWCR = _BV(TWSTO) | _BV(TWEN) | _BV(TWINT);
BUSY_WHILE(TWCR & _BV(TWSTO));
/* Disable the peripheral clock */
PRR |= _BV(PRTWI);
}
uint8_t twiReadFrom(uint8_t address, uint8_t* data, bool repeated, bool stop){
//send START condition, abort if error
if(!twiStart(repeated))
return 1;
//send ADDRESS
//shift the address to the left and add write bit, send.
twiWriteByte((MPU9150_ADDRESS << 1) + TW_WRITE);
//check for address send f**k-up
if(TW_STATUS != TW_MT_SLA_ACK)
return 2;
twiReadAck(data);
if(stop)
twiStop();
return 0;
}
/*************************************************************************
Issues a repeated start condition and sends address and transfer direction
Input: address and transfer direction of I2C device
Return: 0 device accessible
1 failed to access device
*************************************************************************/
unsigned char twiRepStart(unsigned char address)
{
return twiStart( address );
}/* i2c_rep_start */
Error gyroRead(Vector3f *v) {
// Simple Software Low-Pass
static double gyroSumX = 0, gyroSumY = 0, gyroSumZ = 0;
static double gyroFilterX = 0, gyroFilterY = 0, gyroFilterZ = 0;
if (v == NULL) {
return ARGUMENT_ERROR;
}
if (twiStart(GYRO_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(GYROREG_OUTXL | 0x80)) { // Auto Increment
return TWI_WRITE_ERROR;
}
if (twiRepStart(GYRO_ADDRESS | TWI_READ)) {
return TWI_NO_ANSWER;
}
uint8_t xl = twiReadAck();
uint8_t xh = twiReadAck();
uint8_t yl = twiReadAck();
uint8_t yh = twiReadAck();
uint8_t zl = twiReadAck();
uint8_t zh = twiReadNak();
int16_t x = *(int8_t *)(&xh);
x *= (1 << 8);
x |= xl;
int16_t y = *(int8_t *)(&yh);
y *= (1 << 8);
y |= yl;
int16_t z = *(int8_t *)(&zh);
z *= (1 << 8);
z |= zl;
switch (gyroRange) {
case r250DPS:
v->x = (((double)x) * 250 / 0x8000);
v->y = (((double)y) * 250 / 0x8000);
v->z = (((double)z) * 250 / 0x8000);
break;
case r500DPS:
v->x = (((double)x) * 500 / 0x8000);
v->y = (((double)y) * 500 / 0x8000);
v->z = (((double)z) * 500 / 0x8000);
break;
case r2000DPS:
v->x = (((double)x) * 2000 / 0x8000);
v->y = (((double)y) * 2000 / 0x8000);
v->z = (((double)z) * 2000 / 0x8000);
break;
default:
return ARGUMENT_ERROR;
}
gyroSumX = gyroSumX - gyroFilterX + v->x;
gyroFilterX = gyroSumX / GYROFILTERFACTOR;
v->x = gyroFilterX;
gyroSumY = gyroSumY - gyroFilterY + v->y;
gyroFilterY = gyroSumY / GYROFILTERFACTOR;
v->y = gyroFilterY;
gyroSumZ = gyroSumZ - gyroFilterZ + v->z;
gyroFilterZ = gyroSumZ / GYROFILTERFACTOR;
v->z = gyroFilterZ;
return SUCCESS;
}
Error magRead(Vector3f *v) {
if (v == NULL) {
return ARGUMENT_ERROR;
}
if (twiStart(MAG_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(MAGREG_XH)) {
return TWI_WRITE_ERROR;
}
if (twiRepStart(MAG_ADDRESS | TWI_READ)) {
return TWI_NO_ANSWER;
}
uint8_t xh = twiReadAck();
uint8_t xl = twiReadAck();
uint8_t zh = twiReadAck();
uint8_t zl = twiReadAck();
uint8_t yh = twiReadAck();
uint8_t yl = twiReadNak();
int16_t x = *(int8_t *)(&xh);
x *= (1 << 8);
x |= xl;
int16_t y = *(int8_t *)(&yh);
y *= (1 << 8);
y |= yl;
int16_t z = *(int8_t *)(&zh);
z *= (1 << 8);
z |= zl;
switch (magRange) {
case r1g3:
v->x = (((double)x) * 1.3 / MAG_NORMALIZE);
v->y = (((double)y) * 1.3 / MAG_NORMALIZE);
v->z = (((double)z) * 1.3 / MAG_NORMALIZE);
break;
case r1g9:
v->x = (((double)x) * 1.9 / MAG_NORMALIZE);
v->y = (((double)y) * 1.9 / MAG_NORMALIZE);
v->z = (((double)z) * 1.9 / MAG_NORMALIZE);
break;
case r2g5:
v->x = (((double)x) * 2.5 / MAG_NORMALIZE);
v->y = (((double)y) * 2.5 / MAG_NORMALIZE);
v->z = (((double)z) * 2.5 / MAG_NORMALIZE);
break;
case r4g0:
v->x = (((double)x) * 4.0 / MAG_NORMALIZE);
v->y = (((double)y) * 4.0 / MAG_NORMALIZE);
v->z = (((double)z) * 4.0 / MAG_NORMALIZE);
break;
case r4g7:
v->x = (((double)x) * 4.7 / MAG_NORMALIZE);
v->y = (((double)y) * 4.7 / MAG_NORMALIZE);
v->z = (((double)z) * 4.7 / MAG_NORMALIZE);
break;
case r5g6:
v->x = (((double)x) * 5.6 / MAG_NORMALIZE);
v->y = (((double)y) * 5.6 / MAG_NORMALIZE);
v->z = (((double)z) * 5.6 / MAG_NORMALIZE);
break;
case r8g1:
v->x = (((double)x) * 8.1 / MAG_NORMALIZE);
v->y = (((double)y) * 8.1 / MAG_NORMALIZE);
v->z = (((double)z) * 8.1 / MAG_NORMALIZE);
break;
default:
return ARGUMENT_ERROR;
}
return SUCCESS;
}
/*
* Desc Create a non-repeated start condition on TWI bus and wait until the hardware is finished.
* Output true: start condition has been successfully sent
* false: not so much
*/
bool twiStart(void){
return twiStart(false);
}
