/** \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) ;
}
uint8_t twi_read_byte(void)
{
//uint8_t databyte;
twi_start();
if (twi_get_status() != 0x08)
return ERROR;
//select devise and send A2 A1 A0 address bits
twi_write(0xD0);
if (twi_get_status() != 0x18)
return ERROR;
twi_write(0x01);
if (twi_get_status() != 0x18)
return ERROR;
twi_read_ack();
twi_write(0xD1);
if (twi_get_status() != 0x18)
return ERROR;
twi_start();
twi_read_nack();
if (twi_get_status() != 0x58)
return ERROR;
twi_stop();
return SUCCESS;
}
/** \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) ;
}
void twi_write_2byte(uint16_t buf, uint8_t addr)
{
twi_start();
twi_write(addr << 1); /* slave addr */
twi_write(0x00); /* register addr */
twi_write(buf);
twi_write(buf >> 8);
twi_stop();
}
/** \brief This function initializes the LSM303DLHC accelerometer module, via i2c communication.
*/
void imu::accl_init(void)
{
_delay_us(1);
twi_start();
twi_write(0b00110010); //Write SLA + W.
twi_write(0x20); // Write SUB address to CTRL_REG1.
twi_write(0x47); // Set data rate to 95Hz.
twi_stop();
}
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);
}
int main()
{
DDRB = 0x20;
PORTC = 1 << 4 | 1 << 5;
stdout = &mystdout;
stdin = &mystdin;
uart_init();
puts("Slave Transmit!");
twi_slave_init(0x08);
printf("Enter char to send: ");
while (1)
{
char c = getchar();
twi_write(c);
printf("Status: %x\n", TW_STATUS);
if (TW_STATUS != TW_ST_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;
}
uint8_t init_compass(uint8_t priority)
{
uint8_t num_tasks = 0;
int8_t rc;
LOG("init: prio "); LOGP("%u\r\n", priority);
// 1 sample per reading and 15Hz frequency
rc = twi_write(COMPASS_ADDR, REG_CTRL_A, 0x10);
if (rc == NRK_OK) {
have_compass = true;
print_device_info();
} else {
have_compass = false;
WARN("device init failed\r\n");
/* continue (heading RPC needs to be supported) */
}
#if ENABLE_COMPASS_RPC
rpc_init_endpoint(&compass_endpoint);
#endif
ASSERT(num_tasks == NUM_TASKS(COMPASS));
return num_tasks;
}
/* Read num bytes starting from regAddrStart. Each subsequent byte will be from the register above (newRegPtr = oldRegPtr + 1).
*
* Input: regAddrStart The register address to start reading bytes from
* data Location to store read data
* num The number of registers to be read
* Return: 0 Succeeded
* >0 Failed, see handle_error() for relavant error codes
*/
uint8_t twi_read_bytes(uint8_t regAddrStart, uint8_t *data, uint8_t num) {
uint8_t i, error;
/* Transmit start condition. */
if((error = twi_start())) return error;
/* Transmit slave address and write bit. */
if((error = twi_sla_w(IMU_ADDR))) return error;
/* Transmit to slave the register to be read. */
if((error = twi_write(regAddrStart))) return error;
/* Transmit repeated start condition. */
if((error = twi_start_r())) return error;
/* Transmit slave address and read bit. */
if((error = twi_sla_r(IMU_ADDR))) return error;
/* Read data from slave. */
for(i = 0; i < num; i++) {
if(i == num - 1) {
/* Last byte, return NACK. */
if((error = twi_read_nack(&data[i]))) return error;
} else {
if((error = twi_read_ack(&data[i]))) return error;
}
}
/* Transmit stop condition. */
twi_stop();
return 0;
}
static int act8865_write(unsigned char reg_addr, unsigned char data)
{
unsigned int bus;
int ret;
bus = act8865_get_twi_bus();
ret = twi_write(bus, ACT8865_ADDR, reg_addr, 1, &data, 1);
if (ret)
return -1;
return 0;
}
/* Writes num bytes starting from regAddrStart. Each subsequent byte will be written to the register above (newRegPtr = oldRegPtr + 1).
*
* Input: regAddrStart The register address to start reading bytes from
* data Pointer to data to be written
* num The number of registers to be read
* Return: 0 Succeeded
* >0 Failed, see handle_error() for relavant error codes
*/
uint8_t twi_write_bytes(uint8_t regAddrStart, uint8_t *data, uint8_t num) {
uint8_t i, error;
/* Transmit start condition. */
if((error = twi_start())) return error;
/* Transmit slave address and write bit. */
if((error = twi_sla_w(IMU_ADDR))) return error;
/* Transmit to slave the register to write to. */
if((error = twi_write(regAddrStart))) return error;
/* Write data to slave. */
for(i = 0; i < num; i++) {
if((error = twi_write(data[i]))) return error;
}
/* Transmit stop condition. */
twi_stop();
return 0;
}
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;
}
}
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 SiI9022_write(unsigned char reg_addr, unsigned char data)
{
unsigned int bus;
int ret;
bus = SiI9022_get_twi_bus();
ret = twi_write(bus, SiI9022_ADDR, reg_addr, 1, &data, 1);
if (ret) {
dbg_info("SiI9022: Failed to write on TWI #%d\n", bus);
return -1;
}
return 0;
}
/** \brief This function initializes the LSM303DLHC magnetometer module, via i2c communication.
*/
void imu::mag_init(void)
{
_delay_us(1);
twi_start();
twi_write(0b00111100); //Write SLA + W.
twi_write(0x00);
twi_write(0x14);
twi_stop();
_delay_us(1);
twi_start();
twi_write(0b00111100); //Write SLA + W.
twi_write(0x01);
twi_write(0x40);
twi_stop();
}
/** \brief Initialize the L3GD20 Gyro module, via i2c communication protocol.
* \details This function initializes the L3GD20 Gyro to acquire data at 95Hz, over
* a range of (+-)500 degrees per second.
*/
void imu::gyro_init(void)
{
_delay_us(1);
twi_start();
twi_write(0b11010110); //Write SLA + W.
twi_write(0x20); // Write SUB address to CTRL_REG1.
twi_write(0x0F); // Set data rate to 95Hz.
twi_stop();
_delay_ms(1); //_delay_us(1);
twi_start();
twi_write(0b11010110); //Write SLA + W.
twi_write(0x23); // Write SUB address to CTRL_REG4.
twi_write(0x10); // Set range to 500degrees/second
twi_stop();
}
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;
}
