赞
踩
此电路由STM32为主控芯片,NRF24L01、MPU6050为辅,当接受到信号时,处理对应的指令。
- #include "ALL_DATA.h"
- #include "mpu6050.h"
- #include "I2C.h"
- #include "filter.h"
- #include <string.h>
- #include "LED.h"
- #include "myMath.h"
- #include "kalman.h"
- #include "flash.h"
-
-
- #define SMPLRT_DIV 0x19 //ÍÓÂÝÒDzÉÑùÂÊ£¬µäÐÍÖµ£º0x07(125Hz)
- #define CONFIGL 0x1A //µÍͨÂ˲¨ÆµÂÊ£¬µäÐÍÖµ£º0x06(5Hz)
- #define GYRO_CONFIG 0x1B //ÍÓÂÝÒÇ×Լ켰²âÁ¿·¶Î§£¬µäÐÍÖµ£º0x18(²»×Լ죬2000deg/s)
- #define ACCEL_CONFIG 0x1C //¼ÓËÙ¼Æ×Լ졢²âÁ¿·¶Î§¼°¸ßͨÂ˲¨ÆµÂÊ£¬µäÐÍÖµ£º0x01(²»×Լ죬2G£¬5Hz)
- #define ACCEL_ADDRESS 0x3B
- #define ACCEL_XOUT_H 0x3B
- #define ACCEL_XOUT_L 0x3C
- #define ACCEL_YOUT_H 0x3D
- #define ACCEL_YOUT_L 0x3E
- #define ACCEL_ZOUT_H 0x3F
- #define ACCEL_ZOUT_L 0x40
- #define TEMP_OUT_H 0x41
- #define TEMP_OUT_L 0x42
- #define GYRO_XOUT_H 0x43
- #define GYRO_ADDRESS 0x43
- #define GYRO_XOUT_L 0x44
- #define GYRO_YOUT_H 0x45
- #define GYRO_YOUT_L 0x46
- #define GYRO_ZOUT_H 0x47
- #define GYRO_ZOUT_L 0x48
- #define PWR_MGMT_1 0x6B //µçÔ´¹ÜÀí£¬µäÐÍÖµ£º0x00(Õý³£ÆôÓÃ)
- #define WHO_AM_I 0x75 //IICµØÖ·¼Ä´æÆ÷(ĬÈÏÊýÖµ0x68£¬Ö»¶Á)
- #define MPU6050_PRODUCT_ID 0x68
- #define MPU6050_ADDRESS 0xD0//0x68
-
-
-
- int16_t MpuOffset[6] = {0};
-
- static volatile int16_t *pMpu = (int16_t *)&MPU6050;
-
-
-
- /****************************************************************************************
- MPU6050¸´Î»
- *@brief
- *@brief
- *@param[in]
- *****************************************************************************************/
- int8_t mpu6050_rest(void)
- {
- if(IIC_Write_One_Byte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80) == FAILED)
- return FAILED; //¸´Î»
- delay_ms(20);
- return SUCCESS;
- }
- /****************************************************************************************
- MPU6050³õʼ»¯
- *@brief
- *@brief
- *@param[in]
- *****************************************************************************************/
- int8_t MpuInit(void) //³õʼ»¯
- {
- uint8_t date = SUCCESS;
- do
- {
- date = IIC_Write_One_Byte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); //¸´Î»
- delay_ms(30);
- date += IIC_Write_One_Byte(MPU6050_ADDRESS, SMPLRT_DIV, 0x02); //ÍÓÂÝÒDzÉÑùÂÊ£¬0x00(500Hz)
- date += IIC_Write_One_Byte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); //ÉèÖÃÉ豸ʱÖÓÔ´£¬ÍÓÂÝÒÇZÖá
- date += IIC_Write_One_Byte(MPU6050_ADDRESS, CONFIGL, 0x03); //µÍͨÂ˲¨ÆµÂÊ£¬0x03(42Hz)
- date += IIC_Write_One_Byte(MPU6050_ADDRESS, GYRO_CONFIG, 0x18);//+-2000deg/s
- date += IIC_Write_One_Byte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x09);//+-4G
- }
- while(date != SUCCESS); //Èç¹ûÍÓÂÝÒÇÕý³£
- date = IIC_Read_One_Byte(MPU6050_ADDRESS, 0x75); //ÅжÏMPU6050µØÖ·
- if(date!= MPU6050_PRODUCT_ID) //Èç¹ûµØÖ·ÕýÈ·
- return FAILED;
- FLASH_read(MpuOffset,6);//´ÓmcuµÄFLASHÖжÁÈ¡MPU6050µÄˮƽ¾²Ö¹±ê¶¨Ð£×¼Öµ
- return SUCCESS;
- }
- /****************************************************************************************
- »ñÈ¡MPU6050Êý¾Ý
- *@brief
- *@brief
- *@param[in]
- *****************************************************************************************/
-
- #define Acc_Read() IIC_read_Bytes(MPU6050_ADDRESS, 0X3B,buffer,6) //¶ÁÈ¡¼ÓËÙ¶È
- #define Gyro_Read() IIC_read_Bytes(MPU6050_ADDRESS, 0x43,&buffer[6],6) // ¶ÁÈ¡½ÇËÙ¶È
-
- void MpuGetData(void) //¶ÁÈ¡ÍÓÂÝÒÇÊý¾Ý¼ÓÂ˲¨
- {
- uint8_t i;
- uint8_t buffer[12];
-
- Acc_Read();
- Gyro_Read();
-
- for(i=0;i<6;i++)
- {
- pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];
- if(i < 3) //´Ë´¦¶Ô¼ÓËÙ¶È×öһά¿¨¶ûÂüÂ˲¨
- {
- {
- static struct _1_ekf_filter ekf[3] = {{0.02,0,8192,0,0.001,0.543},{0.02,0,8192,0,0.001,0.543},{0.02,0,8192,0,0.001,0.543}};
- kalman_1(&ekf[i],(float)pMpu[i]); //һά¿¨¶ûÂü
- pMpu[i] = (int16_t)ekf[i].out;
- }
- }
- if(i > 2) //´Ë´¦¶Ô½ÇËÙ¶È×öÒ»½éµÍͨÂ˲¨
- {
- uint8_t k=i-3;
- const float factor = 0.25f; //Â˲¨ÒòËØ
- static float tBuff[3];
-
- pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
- }
- }
- }
-
- /****************************************************************************************
- *@brief get mpu offset
- *@brief initial and cmd call this
- *@param[in]
- *****************************************************************************************/
- void MpuGetOffset(void) //У׼
- {
- int32_t buffer[6]={0};
- int16_t i;
- uint8_t k=30;
- const int8_t MAX_GYRO_QUIET = 5;
- const int8_t MIN_GYRO_QUIET = -5;
- /* wait for calm down */
- int16_t LastGyro[3] = {0};
- int16_t ErrorGyro[3];
- /* set offset initial to zero */
-
- memset(MpuOffset,0,12);
- MpuOffset[2] = 8192; //¸ù¾ÝÊÖ²áÁ¿³ÌÉ趨¼ÓËٶȱ궨ֵ
-
- TIM_ITConfig( //ʹÄÜ»òÕßʧÄÜÖ¸¶¨µÄTIMÖжÏ
- TIM3, //TIM2
- TIM_IT_Update ,
- DISABLE //ʹÄÜ
- );
- while(k--) //ÅжϷɿØÊÇ·ñ´¦ÓÚ¾²Ö¹×´Ì¬
- {
- do
- {
- delay_ms(10);
- MpuGetData();
- for(i=0;i<3;i++)
- {
- ErrorGyro[i] = pMpu[i+3] - LastGyro[i];
- LastGyro[i] = pMpu[i+3];
- }
- }while ((ErrorGyro[0] > MAX_GYRO_QUIET )|| (ErrorGyro[0] < MIN_GYRO_QUIET)
- ||(ErrorGyro[1] > MAX_GYRO_QUIET )|| (ErrorGyro[1] < MIN_GYRO_QUIET)
- ||(ErrorGyro[2] > MAX_GYRO_QUIET )|| (ErrorGyro[2] < MIN_GYRO_QUIET)
- );
- }
-
- /* throw first 100 group data and make 256 group average as offset */
- for(i=0;i<356;i++) //È¡µÚ100µ½µÚ356×éµÄƽ¾ùÖµ×öΪУ׼ֵ
- {
- MpuGetData();
- if(100 <= i)
- {
- uint8_t k;
- for(k=0;k<6;k++)
- {
- buffer[k] += pMpu[k];
- }
- }
- }
-
- for(i=0;i<6;i++) //±£´æУ׼ֵ
- {
- MpuOffset[i] = buffer[i]>>8;
- }
- TIM_ITConfig( //ʹÄÜ»òÕßʧÄÜÖ¸¶¨µÄTIMÖжÏ
- TIM3, //TIM2
- TIM_IT_Update ,
- ENABLE //ʹÄÜ
- );
- FLASH_write(MpuOffset,6);//½«Êý¾Ýдµ½FLASHÖУ¬Ò»¹²ÓÐ6¸öint16Êý¾Ý
- }
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。