////////////////////²»ÍêÕû´úÂë #pragma sfr #pragma DI #pragma EI #pragma NOP #pragma HALT #pragma STOP /////////////////////// struct mpu6050st { S16 accel_xout; S16 accel_yout; S16 accel_zout; S16 temp_out; S16 gyro_xout; S16 gyro_yout; S16 gyro_zout; }; struct mpu6050st mpudat; #define GYRO_GAIN_RAD ((2000*3.1415926)/(32768*180)) U8 mputaskstate=TASK_INIT; const U8 mpu6050initdat[][2]= { {MPU6050_REG_PM1 ,0b00000001}, /* |||||***------CLKSEL = 001:PLL with X axis gyroscope reference ||||*---------TEMP_DIS 0 :enable the temperature sensor |||*----------always 0 ||*-----------CYCLE = 0 :disable cycle |*------------SLEEP = 0 :out sleep mode *-------------DEVICE_RESET = 0: */ {MPU6050_REG_USERC ,0b00000000}, /* |||||||*------SIG_COND_RESET = 0:don't reset the sensor ||||||*-------I2C_MST_RESET = 0:don't reset the iic master |||||*--------FIFO_RESET = 0;don't reset the FIFO |||**---------always 0 ||*-----------I2C_MST_EN = 0 :disable IIC master |*------------FIFO_EN = 0 :disable the FIFO *-------------always 0 */ {MPU6050_REG_CONFIG,0b00000010}, /* |||||***------DLPF_CFG = 0b010 :Accelerometer Bandwidth= 94HZ Gyroscope = 98HZ sample=1Khz ||***---------EXT_SYNC_SET = 0b00:Input disabled **------------always 0 */ {MPU6050_REG_SMPRT ,SAMPLE_RATE}, /* ********------SMPLRT_DIV = 2:Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)=1K/4=250HZ */ {MPU6050_REG_GYROC ,0b00011000}, /* |||||***------always 0 |||**---------FS_SEL = 11:Full Scale Range = ¡À 2000 ¡ã/s ||*-----------ZG_ST = 0 :Z axis gyroscope don't self test |*------------YG_ST = 0 :Y axis gyroscope don't self test *-------------XG_ST = 0 :X axis gyroscope don't self test */ {MPU6050_REG_ACCELC,0b00010000}, /* |||||***------always 0 |||**---------FS_SEL = 11:Full Scale Range = 8g ||*-----------ZA_ST = 0 :Z axis accelerometer don't self test |*------------YA_ST = 0 :Y axis accelerometer don't self test *-------------XA_ST = 0 :X axis accelerometer don't self test */ {MPU6050_REG_INTP_CFG,0b10010000},/*set as pulse and read clear mode*/ /* |||||||*------ 0 ||||||*-------I2C_BYPASS_EN =0 0 disable 1 enable |||||*--------FSYNC_INT_EN =0 0 disable 1 enable ||||*---------FSYNC_INT_LEVEL=0 0 disable 1 enable |||*----------INT_RD_CLEAR =1 0 disable 1 enable ||*-----------LATCH_INT_EN =0 0 pulse interrupt 1 level |*------------INT_OPEN =0 0 push pull 1 open drain *-------------INT_LEVEL =1 0 active h 1 active l */ {MPU6050_REG_INTP_EN, 0b00000001},/*enable data ready interrput*/ /* |||||||*------DATA_RDY_EN =1 0 disable 1 enable ||||||*-------0 |||||*--------0 ||||*---------I2C_MST_INT_EN=0 0 disable 1 enable |||*----------FIFO_OFLOW_EN =0 0 disable 1 enable ||*-----------0 |*------------MOT_EN =0 0 disable 1 enable *-------------0 */ {0xff,0xff} }; #define MPU6050_REG_RAW_DATA 0x3B #define MPU6050_REG_ACC 0x3B #define MPU6050_REG_TEM 0x41 #define MPU6050_REG_GYR 0x43 extern U8 iic_start(void); extern U8 iic_rstart(void); extern U8 iic_stop(void); extern U8 i2c_senddat(U8 dat); extern U8 i2c_recedat(U16 mode); static U8 sel_mpu6050_reg(U8 reg) { if(OK!=iic_start())return ERROR; if(OK!=i2c_senddat(W_MPU6050_ADDRESS)) { iic_stop(); return ERROR; }; if(OK!=i2c_senddat(reg)) { iic_stop(); return ERROR; }; return OK; } static U8 write_mpu6050(U8 reg,U8 *datbuf,U16 datl) { if(OK!=sel_mpu6050_reg(reg))return ERROR; for(;datl!=0;datl--) { if(OK!=i2c_senddat(*datbuf)) { iic_stop(); return ERROR; }; datbuf++; } return iic_stop(); } static U8 read_mpu6050(U8 reg,U8 *datbuf,U16 datl) { if(OK!=sel_mpu6050_reg(reg))return ERROR; if(OK!=iic_rstart())return ERROR; if(OK!=i2c_senddat(R_MPU6050_ADDRESS)) { iic_stop(); return ERROR; }; for(;datl!=0;datl--) { *datbuf=i2c_recedat(datl); datbuf++; } return iic_stop(); } static U8 test_mpu6050(void) { U8 buff[2]={0,0}; read_mpu6050(MPU6050_REG_WHOAMI,buff,1); buff[0]&=0x7e; if(buff[0]==0x68)return OK; return ERROR; } /************************************************************user program***************************/ S16 gyroxzero,gyroyzero,gyrozzero; S32 accel_xout,accel_yout,accel_zout; S32 gyro_xout,gyro_yout,gyro_zout; S32 fabuf[3][16]; float faccx,faccy,faccz,fgyrox,fgyroy,fgyroz; U8 acont=0; extern void R_IICA0_Create(void); static U8 init_mpu6050(void) { U8 i; const U8 *p; R_IICA0_Create(); if(OK!=test_mpu6050())return ERROR; p=&(mpu6050initdat[0][0]); while(*p!=0xff) { if(OK!=write_mpu6050(*p,(p+1),1))return ERROR; p+=2; } return OK; } U8 get_mpu6050_dat_h(struct mpu6050st *datbuf) { U8 i,*p,j; if(OK!=read_mpu6050(MPU6050_REG_SENSOR_DAT,(U8*)(datbuf),sizeof(struct mpu6050st))) { R_IICA0_Create(); return ERROR; } p = (U8*)(datbuf); for(i=0;i599) { cont=0; // mpudat.accel_xout=accel_xout/100; // mpudat.accel_yout=accel_yout/100; // mpudat.accel_zout=accel_zout/100; gyroxzero=gyro_xout/100; gyroyzero=gyro_yout/100; gyrozzero=gyro_zout/100; accel_xout=0; accel_yout=0; accel_zout=0; for(acont=0;acont<16;acont++) { accel_xout+=mpudat.accel_xout; fabuf[0][acont]=mpudat.accel_xout; accel_yout+=mpudat.accel_yout; fabuf[1][acont]=mpudat.accel_yout; accel_zout+=mpudat.accel_zout; fabuf[2][acont]=mpudat.accel_zout; } acont=0; return OK; } } return ERROR; } /*g=9.8015*/ static U8 get_mpu6050_dat(void) { if(OK!=get_mpu6050_dat_h(&mpudat))return ERROR; mpudat.gyro_xout-=gyroxzero; if((mpudat.gyro_xout<4)&&(mpudat.gyro_xout>-4))mpudat.gyro_xout=0; mpudat.gyro_yout-=gyroyzero; if((mpudat.gyro_yout<4)&&(mpudat.gyro_yout>-4))mpudat.gyro_yout=0; mpudat.gyro_zout-=gyrozzero; if((mpudat.gyro_zout<4)&&(mpudat.gyro_zout>-4))mpudat.gyro_zout=0; acont&=0xF; accel_xout-=fabuf[0][acont]; accel_xout+=mpudat.accel_xout; fabuf[0][acont]=mpudat.accel_xout; accel_yout-=fabuf[1][acont]; accel_yout+=mpudat.accel_yout; fabuf[1][acont]=mpudat.accel_yout; accel_zout-=fabuf[2][acont]; accel_zout+=mpudat.accel_zout; fabuf[2][acont]=mpudat.accel_zout; acont++; faccx = accel_xout/16;//mpudat.accel_xout; faccy = accel_yout/16;//mpudat.accel_yout; faccz = accel_zout/16;//mpudat.accel_zout; fgyrox = mpudat.gyro_xout*GYRO_GAIN_RAD; fgyroy = mpudat.gyro_yout*GYRO_GAIN_RAD; fgyroz = mpudat.gyro_zout*GYRO_GAIN_RAD; return OK; } U8 mpu6050updat_fg=ERROR; void task_mpu6050(void) { static U16 temp1=0; switch(mputaskstate) { case TASK_INIT: { if(OK!=init_mpu6050())return; mputaskstate=TASK_WAIT; } break; case TASK_WAIT: { if(!(is_irq_mpu6050()))return; clr_irq_fg(); if(OK!=get_mpu6050_zero())return; mputaskstate=TASK_WORK; mpu6050updat_fg=ERROR; } break; case TASK_WORK: { if(!(is_irq_mpu6050()))return; clr_irq_fg(); get_mpu6050_dat(); mpu6050updat_fg=OK; } break; default: break; } } U8 get_mpu6050_mode(void) { if(mpu6050updat_fg==ERROR)return ERROR; mpu6050updat_fg=ERROR; return OK; } void check_mpu6050(void) { mputaskstate=TASK_WAIT; mpu6050updat_fg=ERROR; } ///////////////////////////////////////////// void atttitude_calculate(unsigned int timer_us,float gx,float gy,float gz) { float a_rsqrt; float qa,qb,qc; float timesec=timer_us*0.000001; gx *= (0.5f * timesec); // pre-multiply common factors gy *= (0.5f * timesec); gz *= (0.5f * timesec); qa = curattX.w; qb = curattX.x; qc = curattX.y; curattX.w += (-qb * gx - qc * gy - curattX.z * gz); curattX.x += (qa * gx + qc * gz - curattX.z * gy); curattX.y += (qa * gy - qb * gz + curattX.z * gx); curattX.z += (qa * gz + qb * gy - qc * gx); a_rsqrt = math_rsqrt(curattX.w*curattX.w + curattX.x*curattX.x + curattX.y*curattX.y + curattX.z*curattX.z); curattX.w *= a_rsqrt; curattX.x *= a_rsqrt; curattX.y *= a_rsqrt; curattX.z *= a_rsqrt; } void q4_to_euler_rpy(Q4 *dat,RPY* ang) { float gx, gy, gz,x_2,w_2; gx = 2 * (dat->x*dat->z - dat->w*dat->y); gy = 2 * (dat->w*dat->x + dat->y*dat->z); x_2 = dat->x*dat->x; w_2 = dat->w*dat->w; gz = w_2 -x_2 - dat->y*dat->y + dat->z*dat->z; gz*= gz; ang->yaw = -1*atan2(2*dat->x*dat->y - 2*dat->w*dat->z, 2*w_2 + 2*x_2 - 1);// * 180 / PI; ang->roll =-1* atan(gx* math_rsqrt(gy*gy + gz));// * 180 / PI; ang->pitch = atan(gy* math_rsqrt(gx*gx + gz));// * 180 / PI; }