int drotek_marg2_read(marg_data_t *data, drotek_marg2_t *marg2) { THROW_BEGIN(); THROW_ON_ERR(mpu6050_read(&marg2->mpu, &data->gyro, &data->acc, NULL)); THROW_ON_ERR(hmc5883_read_mag(&data->mag, &marg2->hmc)); THROW_END(); }
void mpu6050_init() { uint8_t send_rec[2]; mpu6050_read(MPU6050_ADDR0, WHO_AM_I, send_rec, 1); if(send_rec[0] != 0x68) { xprintf("mpu6050 address error!\r\n"); } xprintf("mpu6050 ID:%X \r\n", send_rec[0]); send_rec[0] = 0x00; mpu6050_write(MPU6050_ADDR0, PWR_MGMT_1, send_rec, 1); // 0x6B send_rec[0] = 0x07; mpu6050_write(MPU6050_ADDR0, SMPRT_DIV, send_rec, 1); // 0x19 send_rec[0] = 0x06; mpu6050_write(MPU6050_ADDR0, CONFIG, send_rec, 1); // // 0x1A send_rec[0] = 0x18; mpu6050_write(MPU6050_ADDR0, GYRO_CFG, send_rec, 1); // 000 11 000 // 0x1B send_rec[0] = 0x08; mpu6050_write(MPU6050_ADDR0, ACCEL_CFG, send_rec, 1); // 000 11 000 4g // 0x1C }
mpu6050_temp mpu6050_get_temp() { uint8_t send_rec[2]; short temp; mpu6050_read(MPU6050_ADDR0, TEMP_OUT_H, send_rec, 2); temp = (send_rec[0] << 8) | send_rec[0]; return ((temp*1.0)/340 + 36.53); }
mpu6050_gyro mpu6050_get_gyro() { uint8_t send_rec[6]; mpu6050_gyro gyro; mpu6050_read(MPU6050_ADDR0, GYRO_XOUT_H, send_rec, 6); gyro.X = (uint16_t)((send_rec[0] << 8) | send_rec[1]); gyro.Y = (uint16_t)((send_rec[2] << 8) | send_rec[3]); gyro.Z = (uint16_t)((send_rec[4] << 8) | send_rec[5]); return gyro; }
mpu6050_accel mpu6050_get_accel() { uint8_t send_rec[6]; mpu6050_accel accel; mpu6050_read(MPU6050_ADDR0, ACCEL_XOUT_H, send_rec, 6); accel.X = (uint16_t)((send_rec[0] << 8) | send_rec[1]); accel.Y = (uint16_t)((send_rec[2] << 8) | send_rec[3]); accel.Z = (uint16_t)((send_rec[4] << 8) | send_rec[5]); return accel; }
void sensor_read(float basic[9], unsigned int loop_count, float bmp085[2]) { short temp_mpu[6]; short temp_acc[3] = {0}; short temp_gyr[3] = {0}; short temp_hmc[3]; int i; unsigned char filter_cnt=0; unsigned char bmp085_state = loop_count % 10; mpu6050_read(temp_mpu); hmc5883l_read(temp_hmc); bmp085_read(bmp085_state,bmp085); ACC_X_BUF[filter_cnt] = temp_mpu[0];//更新滑动窗口数组 ACC_Y_BUF[filter_cnt] = temp_mpu[1]; ACC_Z_BUF[filter_cnt] = temp_mpu[2]; GYR_X_BUF[filter_cnt] = temp_mpu[3]; GYR_Y_BUF[filter_cnt] = temp_mpu[4]; GYR_Z_BUF[filter_cnt] = temp_mpu[5]; if((filter_cnt++) == FILTER_NUM) { filter_cnt=0; filter_full = 1; } if(filter_full == 1) { for(i=0;i<FILTER_NUM;i++) { temp_acc[0] += ACC_X_BUF[i]; temp_acc[1] += ACC_Y_BUF[i]; temp_acc[2] += ACC_Z_BUF[i]; temp_gyr[0] += GYR_X_BUF[i]; temp_gyr[1] += GYR_Y_BUF[i]; temp_gyr[2] += GYR_Z_BUF[i]; } for(i=0;i<3;i++) { temp_mpu[i] = temp_acc[i] / FILTER_NUM; temp_mpu[i+3] = temp_gyr[i] / FILTER_NUM; } } for(i=0;i<3;i++) basic[i] = (temp_mpu[i])*0.0001220703125; for(i=3;i<6;i++) basic[i] = (temp_mpu[i])*0.00762939453125 - mpu6050_OFFSET[i]; for(i=6;i<9;i++) basic[i] = (temp_hmc[i-6])*1.0; if(my_abs(basic[5])<1)basic[5] = 0.0; }
void PIT0_IRQHandler(void) { if(aa<1000) aa++; delayms(); mpu6050_read(); angle_calculate(); angle_calculate1(); if (flag==2||flag==1) set_gyro(); if (flag==3&&active==1) { set_gyro1_3(); set_gyro_3(); } // oledplay(); if(flag==1) { motor_control(); SetMotorVoltage(0.05,0); MotorSpeedOut(); } if(qd==0) { motor_control(); SetMotorVoltage(0.05,0); MotorSpeedOut(); } if (flag==3&&active==1) { motor_control_3(); // SetMotorVoltage(0,0.5); MotorSpeedOut_3(); } if (flag==4) { motor_control_4(); SetMotorVoltage(0.05,0); MotorSpeedOut(); } if(flag==5) { set_gyro1(); motor_control1(); SetMotorVoltage(0,0.05); MotorSpeedOut1(); angle_set1=16; } PIT_Flag_Clear(PIT0); //清中断标志位 }
int main(void) { i2c_bus_t bus; int ret = i2c_bus_open(&bus, "/dev/i2c-0"); if (ret < 0) { printf("could not open i2c bus", ret); return EXIT_FAILURE; } mpu6050_dev_t mpu; mpu6050_init(&mpu, &bus, MPU6050_DLPF_CFG_94_98Hz, MPU6050_FS_SEL_500, MPU6050_AFS_SEL_4G); while (1) { mpu6050_read(&mpu); } return 0; }
void main() { OLED_Init(); //初始化oled qd=1; ftm_pwm_init(FTM0,FTM_CH3,10000,0); ftm_pwm_init(FTM0,FTM_CH4,10000,0); ftm_pwm_init(FTM2,FTM_CH0,10000,0); ftm_pwm_init(FTM2,FTM_CH1,10000,0); adc_init (ADC1_SE10); adc_init (ADC1_SE11); adc_init (ADC1_SE12); adc_init (ADC1_SE13); //按键初始化 gpio_init (PTA13, GPI,HIGH);//拨码开关初始化 gpio_init (PTA19, GPI,HIGH); gpio_init (PTA24, GPI,HIGH); gpio_init (PTA25, GPI,HIGH); gpio_init (PTA26, GPI,HIGH); gpio_init (PTA27, GPI,HIGH); gpio_init (PTA28, GPI,HIGH); gpio_init (PTA29, GPI,HIGH); led_init (LED0); mpu6050_init(); lptmr_delay_ms(1000); gyro_zero=ad_ave(100); gyro_zero1=ad_ave1(100); mpu6050_read(); accel_accel=(accel_x-accel_zero)/16384.0; if(accel_accel>1) accel_accel=1; if(accel_accel<-1) accel_accel=-1; angle_fuse=180/pi*(asin(accel_accel)); accel_accel1=(accel_y-accel_zero1)/16384.0; if(accel_accel1>1) accel_accel1=1; if(accel_accel1<-1) accel_accel1=-1; angle_fuse1=180/3.1415926*(asin(accel_accel1)); pit_init_ms(PIT0, 5); //初始化PIT0,定时时间为: 5ms set_vector_handler(PIT0_VECTORn ,PIT0_IRQHandler); //设置PIT0的中断服务函数为 PIT0_IRQHandler enable_irq (PIT0_IRQn); //使能PIT0中断 uart_init(UART3, 115200); while(aa<200); //初始化 1秒 DIP_switch(); while(1) { //display[0]=angle_fuse; //display[1]=angle_fuse1; display[0]=angle_fuse3; oledplay(); if(flag==1) mode1(); else if (flag==2) mode2(); else if (flag==3) mode3(); else if (flag==4) mode4(); vcan_sendware((unsigned char *)display, 20); } }