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);
}
}
