static void print_device_info()
{
unsigned char id[4] = {0};
uint8_t ctrl_a, ctrl_b;
uint8_t i;
int8_t rc;
for (i = 0; i < DEV_ID_LEN; ++i) {
rc = twi_read(COMPASS_ADDR, REG_ID_A + i, &id[i]);
if (rc != NRK_OK)
ABORT("failed to read ID\r\n");
}
rc = twi_read(COMPASS_ADDR, REG_CTRL_A, &ctrl_a);
if (rc != NRK_OK)
ABORT("failed to read ctrl a reg\r\n");
rc = twi_read(COMPASS_ADDR, REG_CTRL_B, &ctrl_b);
if (rc != NRK_OK)
ABORT("failed to read ctrl b reg\r\n");
LOG("device info: ");
LOGA(" id: "); LOGP("%s ", id);
LOGA(" crl a: "); LOGP("0x%x ", ctrl_a);
LOGA(" ctrl b: "); LOGP("0x%x ", ctrl_b);
LOGA("\r\n");
}
uint16_t twi_read_2byte(uint8_t addr)
{
uint32_t tmp;
twi_start();
twi_write(addr << 1); /* slave addr */
twi_write(0x00); /* register addr */
twi_stop();
twi_start();
twi_write((addr << 1) | 0x1);/* slave addr + read */
tmp = twi_read();
tmp = (tmp << 8) | twi_read();
twi_stop();
return (tmp & 0xffff);
}
void twi_adc_query(void) {
/* keep sampling adc data */
twi_start(TWI_ADC_ADDR<<1);
/* start at AIN0, autoincrement through channels */
twi_write(1<<2 | (0 & 0x03));
twi_stop();
twi_start((TWI_ADC_ADDR<<1) | 1);
twi_read(1); /* discard result code of precious cycle */
for (uint8_t adc = 0; adc < TWI_ADC_CHANNELS; adc++) {
twi_adc_raw[adc] = twi_read(adc < TWI_ADC_CHANNELS-1);
/* scale to 10 bit values */
twi_adc_values[adc] = twi_adc_raw[adc]<<2;
}
}
int main()
{
DDRB = 0x20;
PORTC = 1 << 4 | 1 << 5;
stdout = &mystdout;
stdin = &mystdin;
uart_init();
twi_master_init();
puts("Master Receive!");
while (twi_mr_start(0x08) != TWST_OK)
{
printf("Failed: %x\n", TW_STATUS);
twi_stop();
puts("No ACK (Enter to continue)");
getchar();
}
while (1)
{
char c = twi_read();
printf("Char: %c, Status: %x\n", c, TW_STATUS);
if (TW_STATUS != TW_MR_DATA_ACK)
break;
PINB = 0x20;
}
puts("Disconnected");
return 0;
}
static DS1307 read_ds1307()
{
if (twi_mt_start(DS1307_CTRL_ID) != TWST_OK)
return;
twi_write(0x00); // Set pointer address to seconds
if (twi_mr_start(DS1307_CTRL_ID) != TWST_OK)
return;
DS1307 time;
//Reading values
time.second = bcd2dec(twi_read() & 0x7f);
time.minute = bcd2dec(twi_read());
time.hour = bcd2dec(twi_read() & 0x3f);
twi_read(); // week number not needed
time.day = bcd2dec(twi_read());
time.month = bcd2dec(twi_read());
time.year = bcd2dec(twi_read());
twi_stop();
return time;
}
void receiveEvent(int bytes) {
uint8_t v, i;
i = 0;
while (available()) {
v = twi_read();
if (i >= sizeof rx_buff) {
continue;
}
rx_buff[i++] = v;
}
bytes = i;
// i2c read
if (bytes == 1) {
tx_buff[1] = '\0';
switch(rx_buff[0]) {
case REG_GET_PID:
dummy = DEVICE_PID;
memcpy(tx_buff, &dummy, _REG_WIDTH);
break;
case REG_GET_VID:
dummy = DEVICE_VID;
memcpy(tx_buff, &dummy, _REG_WIDTH);
break;
case REG_GET_VER:
tx_buff[0] = FW_VER;
break;
case REG_STP_EN:
tx_buff[0] = step_dir;
tx_buff[1] = step_speed;
break;
case REG_STP_RUN:
memcpy(tx_buff, (uint8_t*)&pulse_cnt, _REG_WIDTH);
break;
case REG_STP_INTERVAL:
memcpy(tx_buff, &step_interval, _REG_WIDTH);
break;
case REG_SEQ_LEN:
tx_buff[0] = seq_size;
seq_r_iter = 0;
break;
case REG_SEQ_XET:
if (seq_r_iter < seq_size) {
tx_buff[0] = seq_list[seq_r_iter++] >> 4;
} else {
/** \brief This function acquires data from the LSM303DLHC accelerometer module.
*/
void imu::mag_position(void)
{
uint8_t x_l = 0;
uint8_t x_h = 0;
uint8_t z_l = 0;
uint8_t z_h = 0;
uint8_t y_l = 0;
uint8_t y_h = 0;
twi_start(); // Start Condition
twi_write(0b00111100); // SLA + W = 0x29 + w(0)
twi_write(0x03 | 0x80); //Set Address Pointer to data register location. Setting the MSB makes sure the data autoincrements
twi_start();
twi_write(0b00111101); //SLA + R = 0x29 + w(1)
x_l = twi_read(1);
x_h = twi_read(1);
y_l = twi_read(1);
y_h = twi_read(1);
z_l = twi_read(1);
z_h = twi_read(0);
twi_stop();
x_mag = (x_h<<8)|(x_l) ;
y_mag = (y_h<<8)|(y_l) ;
z_mag = (z_h<<8)|(z_l) ;
}
/** \brief This function acquires data from the L3GD20 Gyro module, via i2c communication.
*/
void imu::gyro_position(void)
{
uint8_t x_l = 0;
uint8_t x_h = 0;
uint8_t y_l = 0;
uint8_t y_h = 0;
uint8_t z_l = 0;
uint8_t z_h = 0;
twi_start(); // Start Condition
twi_write(0b11010110); // SLA + W = 0x29 + w(0)
twi_write(0x28 | 0x80); //Set Address Pointer to data register location. Setting the MSB makes sure the data autoincrements
twi_start();
twi_write(0b11010111); //SLA + R = 0x29 + w(1)
x_l = twi_read(1);
x_h = twi_read(1);
y_l = twi_read(1);
y_h = twi_read(1);
z_l = twi_read(1);
z_h = twi_read(0);
twi_stop();
x_gyro = ( (uint16_t) x_h<<8)|(x_l) ; // HUGE PROBLEM HERE. DOUBLE CHECK
y_gyro = ( (uint16_t) y_h<<8)|(y_l) ;
z_gyro = ( (uint16_t) z_h<<8)|(z_l) ;
}
static int act8865_read(unsigned char reg_addr, unsigned char *data)
{
unsigned int bus;
int ret;
bus = act8865_get_twi_bus();
ret = twi_read(bus, ACT8865_ADDR, reg_addr, 1, data, 1);
if (ret)
return -1;
return 0;
}
void acc_query(void) {
uint8_t buf[6];
twi_start(ACC_ADDRESS<<1);
twi_write(0x32);
twi_start(ACC_ADDRESS<<1 | 1);
for (uint8_t i=0; i<sizeof(buf); i++) {
buf[i] = twi_read(i<sizeof(buf)-1);
}
twi_stop();
ACC_ORIENTATION(buf[1]<<8 | buf[0],
buf[3]<<8 | buf[2],
buf[5]<<8 | buf[4]);
}
static int at24_read(unsigned char device_addr, unsigned char internal_addr,
unsigned char *buff, unsigned int length)
{
unsigned char iaddr_size = 1;
unsigned int bus;
int ret = 0;
bus = at24_get_twi_bus();
ret = twi_read(bus, device_addr, internal_addr, iaddr_size, buff, length);
if (ret)
dbg_loud("EEPROM: Failed to read\n");
return ret;
}
static int SiI9022_read(unsigned char reg_addr, unsigned char *data)
{
unsigned int bus;
int ret;
bus = SiI9022_get_twi_bus();
ret = twi_read(bus, SiI9022_ADDR,
reg_addr, 1, (unsigned char *)data, 1);
if (ret) {
dbg_info("SiI9022: Failed to read on TWI #%d\n", bus);
return -1;
}
return 0;
}
int8_t cmd_twi(uint8_t argc, char **argv)
{
int8_t rc;
char op;
uint8_t addr, reg, val;
if (!(argc == 4 || argc == 5)) {
OUT("usage: twi r|w <addr> <reg> [<val>]\r\n");
return NRK_ERROR;
}
op = argv[1][0];
addr = strtol(argv[2], NULL, 0);
reg = strtol(argv[3], NULL, 0);
switch (op) {
case 'r': /* read */
rc = twi_read(addr, reg, &val);
if (rc == NRK_OK) {
OUTP("0x%x ", val);
OUT("\r\n");
}
break;
case 'w':
if (argc != 5) {
OUT("invalid cmd args\r\n");
return NRK_ERROR;
}
val = strtol(argv[4], NULL, 0);
rc = twi_write(addr, reg, val);
break;
default:
OUT("ERROR: invalid op\r\n");
return NRK_ERROR;
}
return rc;
}
int8_t get_heading(int16_t *heading)
{
uint8_t status = 0;
nrk_time_t start, elapsed, now;
int8_t rc;
uint8_t i;
uint8_t lsb, msb;
float heading_rad;
if (!have_compass) {
LOG("ERROR: no compass\r\n");
return NRK_ERROR;
}
/* trigger a measurement */
rc = twi_write(COMPASS_ADDR, REG_MODE, 0x01);
if (rc != NRK_OK) {
LOG("ERROR: failed to set mode\r\n");
return rc;
}
/* wait for data ready */
nrk_time_get(&start);
do {
nrk_wait(compass_poll_interval);
status = 0;
rc = twi_read(COMPASS_ADDR, REG_STATUS, &status);
if (rc != NRK_OK) {
LOG("ERROR: failed to read status\r\n");
return rc;
}
nrk_time_get(&now);
nrk_time_sub(&elapsed, now, start);
} while (!(status & BIT_STATUS_READY) &&
time_cmp(&elapsed, &compass_measure_timeout) < 0);
if (!(status & BIT_STATUS_READY)) {
LOG("ERROR: compass measure timed out\r\n");
return NRK_ERROR;
}
for (i = 0; i < NUM_AXES; ++i) {
rc = twi_read(COMPASS_ADDR, REG_DATA_OUT_X_MSB + 2 * i, &msb);
if (rc != NRK_OK) break;
rc = twi_read(COMPASS_ADDR, REG_DATA_OUT_X_LSB + 2 * i, &lsb);
if (rc != NRK_OK) break;
mag[i] = (msb << 8) | lsb;
}
if (rc != NRK_OK) {
LOG("ERROR: compass read failed\r\n");
return rc;
}
/* Re-order to (X, Y, Z) and cast to float */
mag_uT[AXIS_X] = mag[RAW_AXIS_X];
mag_uT[AXIS_Y] = mag[RAW_AXIS_Y];
mag_uT[AXIS_Z] = mag[RAW_AXIS_Z];
/* Convert to uT units */
for (i = 0; i < NUM_AXES; ++i)
mag_uT[i] = mag_uT[i] / GAUSS_LSB * GAUSS_TO_MICROTESLA;
heading_rad = atan2(mag_uT[AXIS_Y], mag_uT[AXIS_X]);
if (heading_rad < 0)
heading_rad += 2.0 * M_PI;
heading_rad *= 180.0 / M_PI;
*heading = heading_rad;
LOG("raw: ");
for (i = 0; i < NUM_AXES; ++i)
LOGP("%x:%x ", (mag[i] >> 8) & 0xFF, mag[i] & 0xFF);
LOGA("dec: ");
for (i = 0; i < NUM_AXES; ++i)
LOGP("%d ", mag[i]);
LOGA("\r\n");
LOG("uT: ");
for (i = 0; i < NUM_AXES; ++i)
LOGP("%d ", (int16_t)mag_uT[i]);
LOGP("=> %d deg\r\n", *heading);
return NRK_OK;
}
int main(void)
{
twi_init(); // Initalize the TWI.
sei(); // Enable interrupts.
uint8_t sla = 0x58; // slave address
uint8_t tx[128];
uint8_t rx[128];
tx[0] = 0x30;
tx[1] = 0x01;
twi_write(sla, &tx[0], 2);
_delay_ms(100);
tx[0] = 0x00;
tx[1] = 0x00;
tx[2] = 0x00;
tx[3] = 0x00;
tx[4] = 0x00;
tx[5] = 0x00;
tx[6] = 0x00;
tx[7] = 0x90;
twi_write(sla, &tx[0], 8);
_delay_ms(100);
tx[0] = 0x07;
tx[1] = 0x00;
tx[2] = 0x41;
twi_write(sla, &tx[0], 3);
_delay_ms(100);
tx[0] = 0x1A;
tx[1] = 0x40;
tx[2] = 0x00;
twi_write(sla, &tx[0], 3);
_delay_ms(100);
tx[0] = 0x33;
tx[1] = 0x03;
twi_write(sla, &tx[0], 2);
_delay_ms(100);
tx[0] = 0x30;
tx[1] = 0x08;
twi_write(sla, &tx[0], 2);
_delay_ms(100);
while(1)
{
tx[0] = 0x37;
twi_write(sla, &tx[0], 1);
_delay_us(25);
twi_read(sla, &rx[0], 8);
_delay_us(380);
twi_read(sla, &rx[0], 4);
_delay_us(380);
}
return 0;
}
