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") )