int
mpu6050_config(struct mpu6050 *mpu, struct mpu6050_cfg *cfg)
{
int rc;
struct sensor_itf *itf;
itf = SENSOR_GET_ITF(&(mpu->sensor));
/* Wake up */
rc = mpu6050_sleep(itf, 0);
if (rc) {
return rc;
}
rc = mpu6050_set_clock_source(itf, cfg->clock_source);
if (rc) {
return rc;
}
mpu->cfg.clock_source = cfg->clock_source;
uint8_t val;
rc = mpu6050_read8(itf, MPU6050_WHO_AM_I, &val);
if (rc) {
return rc;
}
if (val != MPU6050_WHO_AM_I_VAL) {
return SYS_EINVAL;
}
rc = mpu6050_set_lpf(itf, cfg->lpf_cfg);
if (rc) {
return rc;
}
mpu->cfg.lpf_cfg = cfg->lpf_cfg;
rc = mpu6050_set_sample_rate(itf, cfg->sample_rate_div);
if (rc) {
return rc;
}
mpu->cfg.sample_rate_div = cfg->sample_rate_div;
rc = mpu6050_set_gyro_range(itf, cfg->gyro_range);
if (rc) {
return rc;
}
mpu->cfg.gyro_range = cfg->gyro_range;
rc = mpu6050_set_accel_range(itf, cfg->accel_range);
if (rc) {
return rc;
}
mpu->cfg.accel_range = cfg->accel_range;
rc = mpu6050_config_interrupt(itf, cfg->int_cfg);
if (rc) {
return rc;
}
mpu->cfg.int_cfg = cfg->int_cfg;
/* Enable/disable interrupt */
rc = mpu6050_enable_interrupt(itf, cfg->int_enable);
if (rc) {
return rc;
}
mpu->cfg.int_enable = cfg->int_enable;
rc = sensor_set_type_mask(&(mpu->sensor), cfg->mask);
if (rc) {
return rc;
}
mpu->cfg.mask = cfg->mask;
return 0;
}
int main(void) {
/* Configure Watch dog timer as an interval timer. WDTIFG is
* set upon expiration of selected time interval, and PUC is not
* generated, so there is no reset of the device. Also, WDTIE bit
* remains unchanged, so you don't have to reset the WDT interrupt.
*
* WDTCTL is 16 bits and always needs to be accessed with
* the upper 8 bits as the WDT password, WDTPW (0x5A).
* Use ACLK for WDTCNT - selected with the WDTSSEL bit
* Set WDTTMSEL bit to 1 for interval timer mode.
* WDTIS0 and WDTIS1 set the interval.
* 00 = WDT clock source/32768 **This is the PUC value
* 01 = WDT clock source/8192
* 10 = WDT clock source/512
* 11 = WDT clock source/64
* With ACLK = 1.5kHz and dividing it by 32768, we get ~21.8 seconds
* With ACLK = 1.5kHz and dividing it by 64, we get 42.6mS
* between WDT interrupts. */
WDTCTL = WDTPW + WDTHOLD;
initClocks();
/* Configure Bluetooth module */
hc05_init(__baud_to_uca0br(9600));
hc05_transmit("HC05 init\r\n",11);
/* Initialize mpu6050 */
mpu6050_init();
hc05_transmit("mpu6050 init\r\n",14);
/* Now we are ready for application code to run. Enable interrupts */
_BIS_SR(GIE);
while(1) {
if(data_received != 0) {
switch(data_received) {
case 'T':
data_received = 0;
mpu6050_temp();
break;
case 'A':
data_received = 0;
mpu6050_accel();
break;
case 'G':
data_received = 0;
mpu6050_gyro();
break;
case 'g':
data_received = 0;
mpu6050_calibrate_gyros();
break;
case 'M':
data_received = 0;
mpu6050_getAddress();
break;
case 'W':
data_received = 0;
mpu6050_wakeup();
break;
case 'S':
data_received = 0;
mpu6050_sleep();
break;
case 'R':
data_received = 0;
dmp_mode = 0;
motion_detect_mode = 0;
mpu6050_reset();
break;
case 'd':
data_received = 0;
mpu6050_dmpinit();
break;
case 'E':
data_received = 0;
motion_detect_mode = 0;
dmp_mode = 1;
mpu6050_setDMPEnabled(true);
P2DIR &= ~MPU6050_INT; // Input
P2SEL &= ~MPU6050_INT; // Digital IO Psel and psel2 are 0
P2SEL2 &= ~MPU6050_INT;
P2IES &= ~MPU6050_INT; // Edge select 0 = low to high
P2IFG &= ~MPU6050_INT; // Clear the interrupt flag before enabling interrupt
P2IE |= MPU6050_INT; // Interrupt enable
//mpu6050_resetFIFO();
break;
case 'e':
dmp_mode = 0;
data_received = 0;
mpu6050_setDMPEnabled(false);
break;
case 'm': /* Itseems that I can have motion detect interrupts if I first call the dmpinit() function, then this code is run. I can probably narrow it down
to a certain function call in the dmpinit() it will just take time */
sendAck();
motion_detect_mode = 1;
dmp_mode = 0;
mpu6050_setDMPEnabled(false);
i2c_write_reg(MPU6050_RA_INT_PIN_CFG,0x10);//interrupt status cleared on any read
//i2c_write_reg(MPU6050_RA_MOT_DETECT_CTRL,0x30); // add the 3 ms delay to accel put
mpu6050_setMotionDetectionThreshold(threshold);//not sure... but I'm told it's 2mg per LSB so 0xFF would only be about 0.512g
mpu6050_setMotionDetectionDuration(threshold_duration); // This duration will really change the snappiness and responsiveness of the motion detect (duh) so
// it should be set to as low as possible, then set detection threshold to the appropriate value for punches or whatever
mpu6050_setIntEnabled(0x40);//motion detect... based on the product specification document, I don't think motion detect can generate an interrupt on INT pin,
// so we also set the data ready interrupt.
mpu6050_configAccel(MPU6050_ACCEL_FS_16<<(MPU6050_ACONFIG_AFS_SEL_BIT-1));
data_received = 0;
P2DIR &= ~MPU6050_INT; // Input
P2SEL &= ~MPU6050_INT; // Digital IO Psel and psel2 are 0
P2SEL2 &= ~MPU6050_INT;
P2IES &= ~MPU6050_INT; // Edge select 0 = low to high
P2IFG &= ~MPU6050_INT; // Clear the interrupt flag before enabling interrupt
P2IE |= MPU6050_INT; // Interrupt enable
/*while(1) {
if(mpu6050_getIntStatus() & 0x40) {
//motion interrupt
hc05_transmit("Motion\r\n",8);
}
}*/
break;
case 'l':
threshold = threshold - 1;
mpu6050_setMotionDetectionThreshold(threshold);
break;
case 'p':
threshold = threshold + 1;
mpu6050_setMotionDetectionThreshold(threshold);
break;
case 'L':
threshold_duration = threshold_duration - 5;
mpu6050_setMotionDetectionDuration(threshold_duration);
break;
case 'P':
threshold_duration = threshold_duration + 5;
mpu6050_setMotionDetectionDuration(threshold_duration);
break;
// case 'h':
// hc05_setspeed(115200);
// data_received = 0;
// break;
//case 'k':
// hc05_key();
// data_received = 0;
// break;
default:
sendAck();
data_received = 0;
break;
}
}
if(mpu6050_interrupt) {
if(dmp_mode) {
mpuIntStatus = mpu6050_getIntStatus();
fifoCount = mpu6050_getFIFOCount();
if(fifoCount > 16) {
fifoCount =16;
}
mpu6050_getFIFOBytes(mpu6050_buffer,fifoCount);
/* This seems to keep the fifo operating. I probably need to read the fifo faster so it doesn't 'die' on me */
mpu6050_resetFIFO();
/* From J.Rowberg's library, the dmp packet output is:
bytes 0-15 quaternion (32 bits) (w,x,y,z) but just use the first two bytes as 16 bit number
bytes 16-27 gyro (32 bits) (gx,gy,gz) but just use the first two bytes as 16 bit number
bytes 28-39 acceleration (32 bits) (ax,ay,az) but just use the first two bytes as 16 bit number
*/
teapotPacket[2] = mpu6050_buffer[0];
teapotPacket[3] = mpu6050_buffer[1];
teapotPacket[4] = mpu6050_buffer[4];
teapotPacket[5] = mpu6050_buffer[5];
teapotPacket[6] = mpu6050_buffer[8];
teapotPacket[7] = mpu6050_buffer[9];
teapotPacket[8] = mpu6050_buffer[12];
teapotPacket[9] = mpu6050_buffer[13];
teapotPacket[10] = mpuIntStatus; // I modified the packet to sent the interrupt status in this byte
hc05_transmit(teapotPacket,14);
teapotPacket[11]++;
mpu6050_interrupt = 0;
} else if (motion_detect_mode) {
if(mpu6050_getIntStatus() & 0x40) {
// Disable motion interrupts, get accel and gyro values until they aren't interesting
// anymore, then quit and enable interrupt.
mpu6050_setIntEnabled(0x00);
//motion interrupt
// hc05_transmit("Motion\r\n",8);
for(uint8_t j = 0; j<100; j++) {
mpu6050_accel();
delay_ms(2);
// i2c_tx_buffer[0] = 0x3B;
// i2c_tx_buffer_counter = 1;
// i2c_transmit_to_receive();
// i2c_transmit();
// i2c_multireceive(6);
// for(j = 0; j< 6; j++) {
// TXData = i2c_rx_buffer[j];
/* These are div 16384 if +/-2g, 8192 if +/-4g, 4096 if +/-8g and 2048 if +/-16g*/
//accelX[j] = (i2c_rx_buffer[0]<<8 | i2c_rx_buffer[1]);
//accelY[j] = (i2c_rx_buffer[2]<<8 | i2c_rx_buffer[3]);
//accelZ[j] = (i2c_rx_buffer[4]<<8 | i2c_rx_buffer[5]);
//sprintf(tempbuf,"%d %d %d\r\n",ax,ay,az);
//sprintf(tempbuf,"E%d,%d,%d\r\n",ax,ay,az);
// hc05_transmit(tempbuf,strlen(tempbuf));
// hc05_transmit((char*)ax,1);
//hc05_transmit((char*)ay,1);
//hc05_transmit((char*)az,1);
//}
//__delay_us(1000);
//delay_ms(2);
}
// hc05_transmit((char*)accelX,2*ACCELBUFSIZE);
// hc05_transmit((char*)accelY,2*ACCELBUFSIZE);
// hc05_transmit((char*)accelZ,2*ACCELBUFSIZE);
// mpu6050_getIntStatus();
mpu6050_setIntEnabled(0x40);
} else {
// sprintf(tempbuf,"Unknown interrupt\r\n");
// hc05_transmit(tempbuf,strlen(tempbuf));
}
} else {
// mpu6050_getIntStatus(); // Clear any other interrupts
// sprintf(tempbuf,"Unknown mode & interrupt\r\n");
// hc05_transmit(tempbuf,strlen(tempbuf));
}
}
__bis_SR_register(LPM0_bits + GIE);
}
}