void imu_init(void) {
// FS = 0x00 (250dps)
imu_write_reg(GYRO_ADDR, GYRO_CTRL4, 0x00);
imu_write_reg(GYRO_ADDR, GYRO_LOW_ODR, 0x00);
imu_write_reg(GYRO_ADDR, GYRO_CTRL1, 0x6F);
// AFS = 0 (+/- 2 g full scale)
imu_write_reg(ACCEL_ADDR, ACCEL_CTRL2, 0x00);
// AODR = 0110 (100 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
imu_write_reg(ACCEL_ADDR, ACCEL_CTRL1, 0x67);
}
void imu_accel_power(unsigned char sel)
{
unsigned char oldval = imu_read_reg(IMU_ACCEL_ADDR, IMU_ACCEL_CTRL1);
if (sel == IMU_GYRO_POWER_ON)
{
imu_write_reg(IMU_ACCEL_ADDR, IMU_GYRO_CTRL1, (oldval & 0x1f) | 0x20);
}
else
{
imu_write_reg(IMU_ACCEL_ADDR, IMU_GYRO_CTRL1, oldval & 0x1f);
}
}
void imu_accel_dump(unsigned char on)
{
if (on)
{
imu_accel_power(IMU_ACCEL_POWER_ON);
_delay_ms(1);
imu_write_reg(IMU_ACCEL_ADDR, IMU_ACCEL_CTRL3, 0x02);
}
else
{
imu_write_reg(IMU_GYRO_ADDR, IMU_ACCEL_CTRL3, 0x00);
imu_accel_power(IMU_ACCEL_POWER_OFF);
}
}
void imu_gyro_dump(unsigned char on)
{
if (on)
{
imu_gyro_power(IMU_GYRO_POWER_ON);
_delay_ms(1);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL3, 0x08);
}
else
{
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL3, 0x00);
imu_gyro_power(IMU_GYRO_POWER_OFF);
}
}
void imu_gyro_power(unsigned char sel)
{
unsigned char oldval = imu_read_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL1);
switch(sel)
{
case IMU_GYRO_POWER_ON:
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL1, oldval | 0x0f);
break;
case IMU_GYRO_POWER_SLEEP:
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL1, oldval & 0xf8);
break;
case IMU_GYRO_POWER_OFF:
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL1, oldval & 0xf0);
break;
default:
break;
}
}
void imu_init(void)
{
twi_master_init(&imu_twi, &IMU_TWI,
TWI_MASTER_INTLVL_LO_gc, TWI_BAUD(F_CPU, 400000));
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL2, 0x37); /* HPF autoreset, cut-off @ 0.1hz when ODR=200 */
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL3, 0x00); /* */
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL4, 0x80); /* block data update, FS = 250dps */
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_REF, 0x00);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_INT1_THS_XH, 0x00);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_INT1_THS_XL, 0x00);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_INT1_THS_YH, 0x00);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_INT1_THS_YL, 0x00);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_INT1_THS_ZH, 0x00);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_INT1_THS_ZL, 0x00);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_INT1_DURATION, 0x00);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_INT1_CFG, 0x00);
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL5, 0x13); /* HPF and LPF2 enable */
imu_write_reg(IMU_GYRO_ADDR, IMU_GYRO_CTRL1, 0x60); /* power off, LPF1->54hz, LPF2->50hz, ODR->200hz */
////////////////
////////////////
imu_write_reg(IMU_ACCEL_ADDR, IMU_ACCEL_CTRL1, 0x0f); /* power off, ODR->100hz, LPF->74hz */
// imu_write_reg(IMU_ACCEL_ADDR, IMU_ACCEL_CTRL2, 0x12); /* enable FDS, HPF cut-off @ 0.5hz */
imu_write_reg(IMU_ACCEL_ADDR, IMU_ACCEL_CTRL2, 0x00); /* disable FDS */
imu_write_reg(IMU_ACCEL_ADDR, IMU_ACCEL_CTRL3, 0x00); /* data ready on INT1 */
imu_write_reg(IMU_ACCEL_ADDR, IMU_ACCEL_CTRL4, 0x80); /* block data update, FS = 2g */
imu_write_reg(IMU_ACCEL_ADDR, IMU_ACCEL_CTRL5, 0x00); /* disable sleep to wake */
imu_write_reg(IMU_ACCEL_ADDR, IMU_ACCEL_INT1_CFG, 0x40); /* 6 direction movement */
}
void uart_process_lb_bt(void)
{
// printf("%s", LB_BT.buf);
P_LIST_t *para_head, *curr;
char *p = NULL;
unsigned char p_count = 0;
unsigned char reg, val;
IMU_GYRO_RESULT_t gyro_rev;
IMU_ACCEL_RESULT_t accel_rev;
unsigned char rev;
para_head = (P_LIST_t *)malloc(sizeof(P_LIST_t));
p = strtok(&LB_BT.buf[0], ",");
curr = para_head;
while (p)
{
p_count++;
curr->para = p;
curr->next = (P_LIST_t *)malloc(sizeof(P_LIST_t));
curr = curr->next;
p = strtok(NULL, ",");
}
curr->next = NULL;
if ( !strcmp(para_head->para, "hi") )
{
printf("hello\n");
}
else if ( !strcmp(para_head->para, "gyro") )
{
if ( !strcmp(para_head->next->para, "readreg") )
{
if (p_count == 3)
{
hexed_to_plain(para_head->next->next->para, ®);
printf("reg(0x%02x): 0x%02x\n", reg, imu_read_reg(IMU_GYRO_ADDR, reg));
}
}
else if ( !strcmp(para_head->next->para, "writereg") )
{
if (p_count == 4)
{
hexed_to_plain(para_head->next->next->para, ®);
hexed_to_plain(para_head->next->next->next->para, &val);
imu_write_reg(IMU_GYRO_ADDR, reg, val);
printf("write reg(0x%02x) to 0x%02x\n", reg, val);
}
}
else if ( !strcmp(para_head->next->para, "read") )
{
imu_gyro_read(&gyro_rev);
printf("x-> %d, y-> %d, z->%d\n", gyro_rev.x, gyro_rev.y, gyro_rev.z);
}
else if ( !strcmp(para_head->next->para, "temp") )
{
printf("temp: %d\n", imu_read_reg(IMU_GYRO_ADDR, IMU_GYRO_TEMP));
}
else if ( !strcmp(para_head->next->para, "power") )
{
if (p_count == 3)
{
if ( !strcmp(para_head->next->next->para, "on") )
{
printf("power on\n");
rev = IMU_GYRO_POWER_ON;
}
else if ( !strcmp(para_head->next->next->para, "sleep") )
{
printf("power sleep\n");
rev = IMU_GYRO_POWER_SLEEP;
}
else
{
printf("power off\n");
rev = IMU_GYRO_POWER_OFF;
}
imu_gyro_power(rev);
}
}
else if ( !strcmp(para_head->next->para, "dump") )
{
if (p_count == 2)
{
printf("start dump..\n");
imu_gyro_dump(1);
}
else
{
printf("stop dump\n");
imu_gyro_dump(0);
}
}
else
{
}
}
else if ( !strcmp(para_head->para, "accel") )
{
if ( !strcmp(para_head->next->para, "readreg") )
{
if (p_count == 3)
{
hexed_to_plain(para_head->next->next->para, ®);
printf("reg(0x%02x): 0x%02x\n", reg, imu_read_reg(IMU_ACCEL_ADDR, reg));
}
}
else if ( !strcmp(para_head->next->para, "writereg") )
{
if (p_count == 4)
{
hexed_to_plain(para_head->next->next->para, ®);
hexed_to_plain(para_head->next->next->next->para, &val);
imu_write_reg(IMU_ACCEL_ADDR, reg, val);
printf("write reg(0x%02x) to 0x%02x\n", reg, val);
}
}
else if ( !strcmp(para_head->next->para, "power") )
{
if (p_count == 3)
{
if ( !strcmp(para_head->next->next->para, "on") )
{
printf("power on\n");
rev = IMU_ACCEL_POWER_ON;
}
else
{
printf("power off\n");
rev = IMU_ACCEL_POWER_OFF;
}
imu_accel_power(rev);
}
}
else if ( !strcmp(para_head->next->para, "read") )
{
imu_accel_read(&accel_rev);
printf("x-> %d, y-> %d, z->%d\n", accel_rev.x>>4, accel_rev.y>>4, accel_rev.z>>4);
}
else if ( !strcmp(para_head->next->para, "dump") )
