UNO 3 --I2C-- DFRobot IMU not getting past START

Go To Last Post
22 posts / 0 new
Author
Message
#1
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I have chinese UNO 3 w/ mega328p connected to SEN0142 from DFRobot with a MPC6050 IMU (3 DOF gyro + 3 DOF accel).

UNO 3 <-->  IMU

+5V    <---> Vin

GNC  <---> GND

SCL  <---> SCL

SDA <---> SDA

 

I'm using command line on Linux.  Upload program with avrdude.   Reading output using minicom.

I have a "dump" routine that outputs the "data" array of 16 bytes at each stage.

I initialize "data" to all 0xff.   When the program runs, I get one line of all FF's, so I conclude that TWINT is never set after the START is sent.

The documentation and example code both indicate address can be 0x68 or 0x69. pEDIT: I get same result with both addresses.]

I'd appreciate any feedback on the initialization etc.

#include <stdint.h>
#include <avr/io.h>
#include <util/delay.h>
#include <util/twi.h>

#include "../uart.c"

void dump_data(uint16_t addr) {
  dump_data16(addr);
  uart_putchar('\r');
  uart_putchar('\n');
}

uint8_t data[16];

/*
 * TWBR bit rate
 * TWCR control reg: TWINT | TWEA | TWSTA | TWSTO | TWWC | TWEN | | TWIE
 * TWSR	status reg
 * TWDR	data register
 * TWAR	slave addr reg
 * TWAMR slave addr mask reg
 */

/*
 * loop_until... means wait for ACK (or NACK -- deal with later)
 */

uint8_t i2c_read(uint8_t sla_, uint8_t reg) {
  uint8_t val;
  uint8_t i = 0;

  dump_data((uint16_t) data);

  /* send START and wait */
  TWCR = TWINT | TWSTA | TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send device address */
  TWDR = sla_ | 1;
  TWCR = TWINT | TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send register address */
  TWDR = reg;
  TWCR = TWINT | TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send START */
  TWCR = TWINT | TWSTA | TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send device address again */
  TWDR = sla_ | 0;
  TWCR = TWINT | TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* recv register contents */
  val = TWDR;
  TWCR = TWINT | TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send STOP */
  TWCR |= TWINT | TWSTO | TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  return val;
}

void twi_init(void) {
  TWBR = 12;				/* to get 400 kHz bit rate */
  TWSR = 0;				/* prescalar of 1 */
}

#define SLA_ (0x68 << 1)		/* 7-bit IMU address */

int main(void) {
  uart_init();
  twi_init();
  for (uint8_t i = 0; i < 16; i++) data[i] = 0xff;

  _delay_ms(500);
  _delay_ms(500);
  _delay_ms(500);

  data[8] = i2c_read(SLA_, 67);	/* X.hi */

  DDRB |= _BV(DDB5);
  for (;;) {
    PINB = _BV(PINB5);
    _delay_ms(100);
  }
  return 0;
}

 

This topic has a solution.
Last Edited: Fri. May 24, 2019 - 05:48 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Peter Fleury has an excellent TWI library that is super easy to use. Might want to look into it. Could save you some aspirin

Jim

I would rather attempt something great and fail, than attempt nothing and succeed - Fortune Cookie

 

"The critical shortage here is not stuff, but time." - Johan Ekdahl

 

"Step N is required before you can do step N+1!" - ka7ehk

 

"If you want a career with a known path - become an undertaker. Dead people don't sue!" - Kartman

"Why is there a "Highway to Hell" and only a "Stairway to Heaven"? A prediction of the expected traffic load?"  - Lee "theusch"

 

Speak sweetly. It makes your words easier to digest when at a later date you have to eat them ;-)  - Source Unknown

Please Read: Code-of-Conduct

Atmel Studio6.2/AS7, DipTrace, Quartus, MPLAB, RSLogix user

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

The nice thing about I2C is it will tell you what is wrong, but you have to look at the status to see it, your code assumes everything is perfect and tries to move on, or gets stuck waiting for perfection that never comes.

I agree with East Coast Jim, get the fluery lib's, check the status they return and move on with your project.

I would also recommend you get one of the low cost logic analyzers too, it will save you many headaches like this...

 

Jim

 

Click Link: Get Free Stock: Retire early! PM for strategy

share.robinhood.com/jamesc3274
get $5 free gold/silver https://www.onegold.com/join/713...

 

 

 

 

This reply has been marked as the solution. 
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Dumb mistake:    REG = FOO | BAR;   vs   REG = _BV(FOO) | _BV(BAR);    where _BV(X) is defined to be (1 << X).

 

here is output of dump_data():


0100 FF FF FF FF FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 FF FF FF FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 40 FF FF FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 40 58 FF FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 40 58 10 FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 40 58 10 18 FF FF FF  FF FF FF FF FF FF FF FF

0100 08 40 58 10 18 28 FF FF  FF FF FF FF FF FF FF FF                           

 

Last Edited: Fri. May 24, 2019 - 05:50 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0
  dump_data((uint16_t) data);

You don't want to do this inside the I2C routine because (I assume) that it does sends four UART chars before continuing, which takes a long time.

 

  dump_data16(addr);

Not defined anywhere.  I assume that it sends two bytes to the UART.

 

/* send device address */
  TWDR = sla_ | 1;

I think that you have read ( | 1)  and write ( | 0) reversed. After sending the eight bit SLA (0x69 << 1 because this board has AD0 tied to logic high), send the register number that you want to read. Then do a re-start with SLA and read (bit 0=set) to get the value of that register from the 6050.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

dump_data() does push data out with UART and will take a long time. 

I did not catch that I2C (TWI) has time constraints.  Does it?  

 

And I did catch the 1/0 flip in SLA+W and SLA+R protocols.  Thanks for catching that.

 

EDIT: dump_data() is only there for debugging

Last Edited: Fri. May 24, 2019 - 09:00 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

There are a couple other bugs in the original.

1) It does not include TWEA bit for the read.

2) It waits for TWINT after sending STOP.

BTW, this board wants to use address 0x68, not 0x69.

Here is the output of data dump:

0100 FF FF FF FF FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 FF FF FF FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 18 FF FF FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 18 28 FF FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 18 28 10 FF FF FF FF  FF FF FF FF FF FF FF FF

0100 08 18 28 10 40 FF FF FF  FF FF FF FF FF FF FF FF

0100 08 18 28 10 40 50 FF FF  FF FF FF FF FF FF FF FF                           

0100 08 18 28 10 40 50 FF FF  D1 FF FF FF FF FF FF FF

Here is the working program:

#include <stdint.h>
#include <avr/io.h>
#include <util/delay.h>
#include <util/twi.h>

#include "../uart.c"

void dump_data(uint16_t addr) {
  dump_data16(addr + 0x00);
  uart_putchar('\r');
  uart_putchar('\n');
}

uint8_t data[16];

#define _TWINT _BV(TWINT)
#define _TWEA _BV(TWEA)
#define _TWSTA _BV(TWSTA)
#define _TWSTO _BV(TWSTO)
#define _TWEN _BV(TWEN)

uint8_t i2c_read(uint8_t sla_, uint8_t reg) {
  uint8_t val;
  uint8_t i = 0;

  dump_data((uint16_t) data);

  /* send START and wait */
  TWCR = _TWINT | _TWSTA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT); 	/* => 0x08 */
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send device address */
  TWDR = sla_ | 0;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);	/* => 0x18 */
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send register address */
  TWDR = reg;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);	/* => 0x28 */
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send START */
  TWCR = _TWINT | _TWSTA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);	/* => 0x10 */
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send device address again */
  TWDR = sla_ | 1;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);	/* => 0x40 */
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* recv register contents */
  val = TWDR;
  TWCR = _TWINT | _TWEA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);	/* => 0x58 */
  data[i++] = TWSR;
  dump_data((uint16_t) data);

  /* send STOP */
  TWCR |= _TWINT | _TWSTO | _TWEN;

  return val;
}

void twi_init(void) {
  TWSR = 0;				/* prescalar of 1 */
  TWBR = 12;				/* to get 400 kHz bit rate */
}

#define SLA_ (0x68 << 1)		/* 7-bit IMU address */

int main(void) {
  uart_init();
  twi_init();
  for (uint8_t i = 0; i < 16; i++) data[i] = 0xff;

  DDRB |= _BV(DDB5);
  PORTB &= ~_BV(PORTB5);
  _delay_ms(500);
  _delay_ms(500);
  _delay_ms(500);
  PORTB |= _BV(PORTB5);

  data[8] = i2c_read(SLA_, 67);	/* X.hi */
  dump_data((uint16_t)data);

  uart_fini();

  for (;;) {
    PINB = _BV(PINB5);
    _delay_ms(100);
  }
  return 0;
}

 

Last Edited: Fri. May 24, 2019 - 09:46 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Since the AD0 pin on the MPC6050 IC is tied to Vcc through a pull-up resistor (according to the schematic on the DFRobot website), the 7-bit SLA address should be 0x69.

 

Help us poor lost fools:  what does this do? What is it supposed to do?

  for (;;) {
    PINB = _BV(PINB5);
    _delay_ms(100);
  }

 

This is all so much easier with an Arduino library.  There are lots of them for the MPC6050.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

That for() loop is obvious isn't it? It's flashing an LED at 5Hz. Presumably an "alive" signal?

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0
PINB = _BV(PINB5);

This line toggles the pin?

 

The largest known prime number: 282589933-1

It's easy to stop breaking the 10th commandment! Break the 8th instead. 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

For you "lost fools," the PIN and PORT manipulations are to 1) turn off LED at start, 2) turn on LED when starting I2C I/O and 3) blink LED when I2C I/O is complete.

 

I believe you now that the device address is 0x69.   I now don't know why it is not getting futher.

I do have a copy of Fleury's code.  I don't want to use C++.  I want to learn how this works and to make tight C code.

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

So now I'm puzzled.  The schematic shows AD0 tied high but it also shows a jumper J1, which is in.  I am getting something on 0x68, not on 0x69.

I performing a write followed by several reads, but the six reads are all giving me 0xD1, which is the SLA+R byte from TWDR, so I'm not getting the data back for some reason.

 

0x68:
WR 08 18 28 10 18 28
RD 08 18 28 10 40 58

code
08 START sent
10 repeated START sent
18 SLA+W sent, ACK recvd
20 SLA+W sent, NACK recvd
28 data sent, ACK recvd
30 data sent, NACK recvd
38 SLAX|data arb lost
40 SLAR sent, ACK recvd
48 SLAR sent, NACK recvd
50 data recvd, ACK sent
58 data recvd, NACK sent

 

latest code:

 

#include <stdint.h>
#include <avr/io.h>
#include <util/delay.h>
#include <util/twi.h>

#include "../uart.c"

void dump_data(uint16_t addr) {
  dump_data16(addr + 0x00);
  uart_putchar('\r');
  uart_putchar('\n');
}

uint8_t dbug[16];
uint8_t regs[16];

#define _TWINT _BV(TWINT)
#define _TWEA _BV(TWEA)
#define _TWSTA _BV(TWSTA)
#define _TWSTO _BV(TWSTO)
#define _TWEN _BV(TWEN)

uint8_t i2c_read(uint8_t sla_, uint8_t reg) {
  uint8_t val;
  uint8_t i = 8;

  /* send START and wait */
  TWCR = _TWINT | _TWSTA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send device address */
  TWDR = sla_ | 0;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send register address */
  TWDR = reg;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send START */
  TWCR = _TWINT | _TWSTA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send device address again */
  TWDR = sla_ | 1;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* recv register contents, send ACK  */
  val = TWDR;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;
  
  /* send STOP */
  TWCR = _TWINT | _TWSTO | _TWEN;
  loop_until_bit_is_set(TWCR, TWSTO);

  return val;
}

void i2c_write(uint8_t sla_, uint8_t reg, uint8_t val) {
  uint8_t i = 0;

  /* send START and wait */
  TWCR = _TWINT | _TWSTA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send device address */
  TWDR = sla_ | 0;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send register address */
  TWDR = reg;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send START */
  TWCR = _TWINT | _TWSTA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send device address again */
  TWDR = sla_ | 0;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send register value */
  TWDR = val;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  dbug[i++] = TWSR;

  /* send STOP */
  TWCR = _TWINT | _TWSTO | _TWEN;
  loop_until_bit_is_set(TWCR, TWSTO);
}

void twi_init(void) {
  TWSR = 0;				/* prescalar of 1 */
  TWBR = 12;				/* to get 400 kHz bit rate */
}

#define SLA_ (0x68 << 1)		/* 7-bit IMU address */

int main(void) {
  uart_init();
  twi_init();
  for (uint8_t i = 0; i < 16; i++) dbug[i] = 0xff;
  for (uint8_t i = 0; i < 16; i++) regs[i] = 0xff;
  
  DDRB |= _BV(DDB5);
  PORTB &= ~_BV(PORTB5);		/* LED off */
  _delay_ms(500);
  PORTB |= _BV(PORTB5);			/* LED on */
  
  i2c_write(SLA_, 25, 80);		/* 100 Hz sample rate */
  regs[0] = i2c_read(SLA_, 25);
  regs[1] = i2c_read(SLA_, 26);
  regs[2] = i2c_read(SLA_, 27);
  regs[3] = i2c_read(SLA_, 28);
  regs[4] = i2c_read(SLA_, 29);
  regs[5] = i2c_read(SLA_, 30);
  
  dump_data16((uint16_t) dbug);
  dump_data16((uint16_t) regs);

  uart_fini();
  
  for (;;) { PINB = _BV(PINB5); _delay_ms(100); } /* LED blink */

  return 0;
}

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

MattRW wrote:

/* recv register contents */
  val = TWDR;
  TWCR = _TWINT | _TWEA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);	/* => 0x58 */
  data[i++] = TWSR;
  dump_data((uint16_t) data);

You are reading TWDR before the slave has actually returned the data.

Read TWDR after the loop_until_bit_is_set.

 

Also, you need to do read-ack (ie. with _TWEA set ) if you are going to read more, and do read-nak (ie. without _TWEA set) for the last read.

 

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

MrKendo wrote:

You are reading TWDR before the slave has actually returned the data.

Read TWDR after the loop_until_bit_is_set.

/quote]

 

Thanks.  I am getting zeros now, but I'm guessing I need to configure the board still.

 

(I got the NACK for last read part.  Thanks for spotting that.)

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I'll mention this since you're still having problems

even though it might not apply.  Sometimes the

I2C address is specified as two (consecutive) 8-bit

numbers, one for reading and one for writing.

The difference is just the R/W bit.  So you may not

need to shift the 0x68 up, just use it directly for a

write and use address 0x69 to read.

 

--Mike

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Thanks Mike.  In this case the board can be configured via Jumper J1 for two addresses 0x68 and 0x69.  And the SLA+R value is 0xD1 and theSLA+W value is 0xD0.

 

EDIT: but there are registers (decimal 37-39) for setting up slave I2C addresses.  Reading it now ...

Last Edited: Sat. May 25, 2019 - 03:22 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

MattRW wrote:

regs[0] = i2c_read(SLA_, 25);
  regs[1] = i2c_read(SLA_, 26);
  regs[2] = i2c_read(SLA_, 27);
  regs[3] = i2c_read(SLA_, 28);
  regs[4] = i2c_read(SLA_, 29);
  regs[5] = i2c_read(SLA_, 30);

If you're doing separate reads as above, and each read operation is only reading a single byte, then each read would  be a read- NAK.

You can setup the read and then read multiple consecutive bytes eg. to read 6 consecutive bytes the sequence would be like

        i2c_start()
        i2c_sla_w(MPU6050_ADDR)
        i2c_write(start_addr)
        i2c_rep_start()
        i2c_sla_r(MPU6050_ADDR)
        i2c_read_ack(&val[0])
        i2c_read_ack(&val[1])
        i2c_read_ack(&val[2])
        i2c_read_ack(&val[3])
        i2c_read_ack(&val[4])
        i2c_read_nack(&val[5]);

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I strongly advise you to use a respected library e.g. Fleury.

 

When your project is working 100% with the proven library,  you can replace it with your "home grown" library.

 

You can pick up a lot of tips from just studying the Fleury code.

The most important lesson is:  look at the design of the basic functions e.g. name, arguments, return value

 

choose intuitive names for your project helper functions e.g.

uint8_t i2c_read_reg(uint8_t slave, uint8_t reg);
int i2c_write_reg_val(uint8_t slave, uint8_t reg, uint8_t val);

I have not looked at the MPU6050 data sheet.  But your write_reg_val() sequence does not look normal to me.

 

David.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

david.prentice wrote:

I have not looked at the MPU6050 data sheet.  But your write_reg_val() sequence does not look normal to me.

Oh yes, it does look wrong. You don't need to repeat the start.

More normal would be  :-

start

sla_w

write_adress

write_value

stop

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

You guys are right.  Here are relevant diagrams from the manual:

 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Torby wrote:

PINB = _BV(PINB5);

This line toggles the pin?

 

Err yes. I think all tiny/mega since about 2005 (right up to these new fangled AVR-0/1) have the feature that writing 1 bits to PIN toggles the relevant output. Kind of surprised that "regulars" haven't spotted this in the intervening 14 years?

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

So have the gyro read/write all working.  The chip boots in SLEEP mode by default (i2c comm only).  Once I programmed registers for that and tuned, I started getting output.

 

data from Xhi, Xlo, Yhi, Ylo, Zhi, Zlo gyro (started moving 1/2 way through this sequence:

 FF 43 FF EA 00 1F                                                              
 FF 43 FF ED 00 1F                                                              
 FF 43 FF EF 00 1F                                                              
 FF 41 FF F0 00 1E                                                              
 FF 45 FF EE 00 1D                                                              
 FF 4C FF C4 00 23                                                              
 FA B4 2C 96 07 78                                                              
 FA 50 D3 A3 F7 D7                                                              
 11 6D 1D 96 FC 2E                                                              
 F8 2C C4 04 FF D2                                                              
 E7 E3 F4 3B 07 56                                                              
 17 F0 0A A5 F2 E3                                                              
 F0 66 D9 E5 07 9A                                                              
                                                        

The final program:

#include <stdint.h>
#include <avr/io.h>
#include <util/delay.h>
#include <util/twi.h>

#include "../uart.c"

void dump_data6(uint16_t addr) {
  uint8_t *p = (uint8_t*)addr;
  char buf[4];

  for (uint8_t i = 0; i < 6; i++) {
    cnvt_u8(p[i], buf);
    uart_putchar(' ');
    uart_putchar(buf[0]);
    uart_putchar(buf[1]);
  }
  uart_putchar('\r');
  uart_putchar('\n');
}

void dump_data(uint16_t addr) {
  dump_data16(addr + 0x00);
  uart_putchar('\r');
  uart_putchar('\n');
}

uint8_t stat[16];			/* status codes */
uint8_t rega[16];			/* register address */
uint8_t regv[16];			/* register value */

#define _TWINT _BV(TWINT)
#define _TWEA _BV(TWEA)
#define _TWSTA _BV(TWSTA)
#define _TWSTO _BV(TWSTO)
#define _TWEN _BV(TWEN)

uint8_t i2c_read_reg(uint8_t sla_, uint8_t reg) {
  uint8_t val;
  uint8_t i = 8;

  /* send START and wait */
  TWCR = _TWINT | _TWSTA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  stat[i++] = TWSR;

  /* send device address */
  TWDR = sla_ | 0;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  stat[i++] = TWSR;

  /* send register address */
  TWDR = reg;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  stat[i++] = TWSR;

  /* send START */
  TWCR = _TWINT | _TWSTA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  stat[i++] = TWSR;

  /* send device address again */
  TWDR = sla_ | 1;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  stat[i++] = TWSR;

  /* recv register contents, send ACK  */
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  val = TWDR;
  stat[i++] = TWSR;
  
  /* send STOP */
  TWCR = _TWINT | _TWSTO | _TWEN;
  loop_until_bit_is_set(TWCR, TWSTO);

  return val;
}

void i2c_write_reg(uint8_t sla_, uint8_t reg, uint8_t val) {
  uint8_t i = 0;

  /* send START and wait */
  TWCR = _TWINT | _TWSTA | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  stat[i++] = TWSR;

  /* send device address */
  TWDR = sla_ | 0;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  stat[i++] = TWSR;

  /* send register address */
  TWDR = reg;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  stat[i++] = TWSR;

  /* send register value */
  TWDR = val;
  TWCR = _TWINT | _TWEN;
  loop_until_bit_is_set(TWCR, TWINT);
  stat[i++] = TWSR;

  /* send STOP */
  TWCR = _TWINT | _TWSTO | _TWEN;
  loop_until_bit_is_set(TWCR, TWSTO);
}

void twi_init(void) {
  TWSR = 0;				/* prescalar of 1 */
  TWBR = 12;				/* to get 400 kHz bit rate */
}

#define SLA_ (0x68 << 1)		/* 7-bit IMU address */

int main(void) {
  uint8_t i, imu[12];
  
  uart_init();
  twi_init();

  DDRB |= _BV(DDB5);
  PORTB &= ~_BV(PORTB5);		/* LED off */
  _delay_ms(500);

  for (uint8_t i = 0; i < 16; i++) stat[i] = 0xff;
  for (uint8_t i = 0; i < 16; i++) rega[i] = 0xff;
  for (uint8_t i = 0; i < 16; i++) regv[i] = 0xff;

  rega[0] = 0x75;			/* whoami */
  rega[1] = 0x6B;			/* device control */
  rega[2] = 0x19;			/* sample rate divider */
  rega[3] = 0x1A;			/* config */
  rega[4] = 0x1B;			/* gyro config */
  
  PORTB |= _BV(PORTB5);			/* LED on */

  i2c_write_reg(SLA_, 0x6B, 0x00);	/* device control: wake it up */
  i2c_write_reg(SLA_, 0x19, 80);	/* sample rate => 100 Hz */
  i2c_write_reg(SLA_, 0x1A, 0x03);	/* FSYNC=0, 42Hz gyro, 44Hz accel */
  i2c_write_reg(SLA_, 0x1B, 0x08);	/* 27: gyro config */

  for (i = 0; i < 16; i++) {
    if (rega[i] != 0xff) regv[i] = i2c_read_reg(SLA_, rega[i]);
  }
  
  dump_data16((uint16_t) stat);
  dump_data16((uint16_t) rega);
  dump_data16((uint16_t) regv);

  for (;;) {
    PINB = _BV(PINB5);
    imu[0] = i2c_read_reg(SLA_, 0x43);
    imu[1] = i2c_read_reg(SLA_, 0x44);
    imu[2] = i2c_read_reg(SLA_, 0x45);
    imu[3] = i2c_read_reg(SLA_, 0x46);
    imu[4] = i2c_read_reg(SLA_, 0x47);
    imu[5] = i2c_read_reg(SLA_, 0x48);
    dump_data6((uint16_t)imu);
    _delay_ms(500);
  } /* LED blink */

  return 0;
}