void accel_calibrate(int * offsets) {
int xcal, ycal, zcal, z;
accel_write(XOFFL, 0);
accel_write(XOFFH, 0);
accel_write(YOFFL, 0);
accel_write(YOFFH, 0);
accel_write(ZOFFL, 0);
accel_write(ZOFFH, 0);
accel_read_xyz((int *)accel_xyz);
xcal = -2 * accel_xyz[0];
xcal = xcal < 0 ? xcal + 2048 : xcal;
ycal = -2 * accel_xyz[1];
ycal = ycal < 0 ? ycal + 2048 : ycal;
zcal = -2 * (accel_xyz[2] - 64);
zcal = zcal < 0 ? zcal + 2048 : zcal;
accel_write(XOFFL, xcal&0x00FF);
accel_write(XOFFH, xcal>>8);
accel_write(YOFFL, ycal&0x00FF);
accel_write(YOFFH, ycal>>8);
accel_write(ZOFFL, zcal&0x00FF);
accel_write(ZOFFH, zcal>>8);
//printf("Calibration values: %d\t%d\t%d\n", xcal, ycal, zcal);
offsets[0] = xcal;
offsets[1] = ycal;
offsets[2] = zcal;
}
void accel_init() {
//CS in low
ACCEL_CS_SEL &= ~ACCEL_CS_BIT;
ACCEL_CS_DIR |= ACCEL_CS_BIT;
ACCEL_CS_OUT &= ~ACCEL_CS_BIT;
//Power on 3.6
ACCEL_PWR_SEL &= ~ACCEL_PWR_BIT;
ACCEL_PWR_DIR |= ACCEL_PWR_BIT;
ACCEL_PWR_OUT |= ACCEL_PWR_BIT;
//Setup 3.3, 3.4 to simo, somi
ACCEL_SOMI_SEL |= ACCEL_SOMI_BIT;
ACCEL_SIMO_SEL |= ACCEL_SIMO_BIT;
//Setup 2.7 to UCA0CLK
ACCEL_SCK_SEL |= ACCEL_SCK_BIT;
// Initialize USCI_A0 for SPI Master operation
// Put state machine in reset
UCA0CTL1 |= UCSWRST;
//3-pin, 8-bit SPI master
UCA0CTL0 = UCCKPH | UCMSB | UCMST | UCMODE_0 | UCSYNC;
// Clock phase - data captured first edge, change second edge
// MSB
// Use SMCLK, keep RESET
UCA0CTL1 = UCSSEL_2 | UCSWRST;
UCA0BR0 = 20;
UCA0BR1 = 0;
// Release USCI state machine
UCA0CTL1 &= ~UCSWRST;
UCA0IFG &= ~UCRXIFG;
__delay_cycles(10000);
accel_write(0x02, 0x92);
ACCEL_CS_OUT |= ACCEL_CS_BIT;
}
void imu_init(void) {
pin_init(&IMU_MOSI, (unsigned int *)&PORTB, (unsigned int *)&TRISB,
(unsigned int *)NULL, 8, -1, 0, 8, (unsigned int *)&RPOR4);
pin_init(&IMU_SCK, (unsigned int *)&PORTB, (unsigned int*)&TRISB,
(unsigned int *)NULL, 9, -1, 8, 9, (unsigned int*)&RPOR4);
pin_init(&IMU_MISO, (unsigned int *)&PORTB, (unsigned int *)&TRISB,
(unsigned int *)NULL, 14, -1, 0, 14, (unsigned int *)&RPOR7);
pin_init(&ACCEL_CS, (unsigned int*)&PORTB, (unsigned int *)&TRISB,
(unsigned int *)NULL, 13, -1, 0, -1, (unsigned int *)NULL);
pin_init(&GYRO_CS, (unsigned int *)&PORTB, (unsigned int *)&TRISB,
(unsigned int *)NULL, 11, -1, 0, -1, (unsigned int *)NULL);
pin_digitalOut(&ACCEL_CS);
pin_digitalOut(&GYRO_CS);
pin_set(&GYRO_CS);
pin_set(&ACCEL_CS);
spi_open(&spi1, &IMU_MISO, &IMU_MOSI, &IMU_SCK, 2e6);
accel_write(I2CADD, 0x80); //Disable I2C
}
void accel_set_measure_mode(void) {
accel_write(MCTL, 0x05); //2g measurement mode
accel_write(CTL1, 0x80); //set absolute condition, 125Hz BW, all axes enabled
}
static int
accel_sleep(void)
{
return accel_write(0x16, 0);
}
static void
accel_wakeup(void)
{
/* Set the mode to "measurement", measuring 2g */
accel_write(0x16, 0x04 | 0x01);
}
static int
accel_deinit(struct device *dev)
{
accel_write(0x16, 0);
return 0;
}
void accel_modify(uint8 reg, uint8 value, uint8 mask) {
uint8 curr = accel_read(reg);
curr &= ~mask;
curr |= value & mask;
accel_write(reg, curr);
}
