利用STM8驅(qū)動(dòng)的MPU6050陀螺儀源程序

ID:335790 · 發(fā)表于 2018-5-22 16:23

利用STM8驅(qū)動(dòng)的陀螺儀程序，參考其他人寫的32的程序，自己修改接口，完成驅(qū)動(dòng)陀螺儀。

單片機(jī)源程序如下:

#include "MPU6050.h"
#include "IOI2C.h"
#include "usart1.h"
#include <math.h>
#include "inv_mpu_dmp_motion_driver.h"
#include "inv_mpu.h"
#include <stdio.h>
/**************************************************************************/
#define PRINT_ACCEL (0x01)
#define PRINT_GYRO (0x02)
#define PRINT_QUAT (0x04)
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
#define MOTION (0)
#define NO_MOTION (1)
#define DEFAULT_MPU_HZ (200)
#define FLASH_SIZE (512)
#define FLASH_MEM_START ((void*)0x1800)
#define q30 1073741824.0f
static signed char gyro_orientation[9] = {-1, 0, 0,
0,-1, 0,
0, 0, 1};
static unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b;
if (row[0] > 0)
b = 0;
else if (row[0] < 0)
b = 4;
else if (row[1] > 0)
b = 1;
else if (row[1] < 0)
b = 5;
else if (row[2] > 0)
b = 2;
else if (row[2] < 0)
b = 6;
else
b = 7; // error
return b;
}
static unsigned short inv_orientation_matrix_to_scalar(
const signed char *mtx)
{
unsigned short scalar;
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
static void run_self_test(void)
{
int result;
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x7) {
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
printf("setting bias succesfully ......\r\n");
}
}
uint8_t buffer[14];
int16_t MPU6050_FIFO[6][11];
int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
*功　　能: 將新的ADC數(shù)據(jù)更新到 FIFO數(shù)組，進(jìn)行濾波處理
*******************************************************************************/
void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
{
unsigned char i ;
int32_t sum=0;
for(i=1;i<10;i++){ //FIFO 操作
MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][i];
MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3][i];
MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4][i];
MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
}
MPU6050_FIFO[0][9]=ax;//將新的數(shù)據(jù)放置到數(shù)據(jù)的最后面
MPU6050_FIFO[1][9]=ay;
MPU6050_FIFO[2][9]=az;
MPU6050_FIFO[3][9]=gx;
MPU6050_FIFO[4][9]=gy;
MPU6050_FIFO[5][9]=gz;
sum=0;
for(i=0;i<10;i++){ //求當(dāng)前數(shù)組的合，再取平均值
sum+=MPU6050_FIFO[0][i];
}
MPU6050_FIFO[0][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[1][i];
}
MPU6050_FIFO[1][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[2][i];
}
MPU6050_FIFO[2][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[3][i];
}
MPU6050_FIFO[3][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[4][i];
}
MPU6050_FIFO[4][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[5][i];
}
MPU6050_FIFO[5][10]=sum/10;
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setClockSource(uint8_t source)
*功　　能: 設(shè)置 MPU6050 的時(shí)鐘源
* CLK_SEL | Clock Source
* --------+--------------------------------------
* 0 | Internal oscillator
* 1 | PLL with X Gyro reference
* 2 | PLL with Y Gyro reference
* 3 | PLL with Z Gyro reference
* 4 | PLL with external 32.768kHz reference
* 5 | PLL with external 19.2MHz reference
* 6 | Reserved
* 7 | Stops the clock and keeps the timing generator in reset
*******************************************************************************/
void MPU6050_setClockSource(uint8_t source){
IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
}
/** Set full-scale gyroscope range.
* @param range New full-scale gyroscope range value
* @see getFullScaleRange()
* @see MPU6050_GYRO_FS_250
* @see MPU6050_RA_GYRO_CONFIG
* @see MPU6050_GCONFIG_FS_SEL_BIT
* @see MPU6050_GCONFIG_FS_SEL_LENGTH
*/
void MPU6050_setFullScaleGyroRange(uint8_t range) {
IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setFullScaleAccelRange(uint8_t range)
*功　　能: 設(shè)置 MPU6050 加速度計(jì)的最大量程
*******************************************************************************/
void MPU6050_setFullScaleAccelRange(uint8_t range) {
IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setSleepEnabled(uint8_t enabled)
*功　　能: 設(shè)置 MPU6050 是否進(jìn)入睡眠模式
enabled =1 睡覺
enabled =0 工作
*******************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: uint8_t MPU6050_getDeviceID(void)
*功　　能: 讀取 MPU6050 WHO_AM_I 標(biāo)識將返回 0x68
*******************************************************************************/
uint8_t MPU6050_getDeviceID(void) {
IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
return buffer[0];
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: uint8_t MPU6050_testConnection(void)
*功　　能: 檢測MPU6050 是否已經(jīng)連接
*******************************************************************************/
uint8_t MPU6050_testConnection(void) {
if(MPU6050_getDeviceID() == 0x68) //0b01101000;
return 1;
else return 0;
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setI2CMasterModeEnabled(uint8_t enabled)
*功　　能: 設(shè)置 MPU6050 是否為AUX I2C線的主機(jī)
*******************************************************************************/
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_setI2CBypassEnabled(uint8_t enabled)
*功　　能: 設(shè)置 MPU6050 是否為AUX I2C線的主機(jī)
*******************************************************************************/
void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
/**************************實(shí)現(xiàn)函數(shù)********************************************
*函數(shù)原型: void MPU6050_initialize(void)
*功　　能: 初始化 MPU6050 以進(jìn)入可用狀態(tài)。
*******************************************************************************/
void MPU6050_initialize(void) {
MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO); //設(shè)置時(shí)鐘
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺儀最大量程 +-1000度每秒
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //加速度度最大量程 +-2G
MPU6050_setSleepEnabled(0); //進(jìn)入工作狀態(tài)
MPU6050_setI2CMasterModeEnabled(0); //不讓MPU6050 控制AUXI2C
MPU6050_setI2CBypassEnabled(0); //主控制器的I2C與 MPU6050的AUXI2C 直通�？刂破骺梢灾苯釉L問HMC5883L
}
/**************************************************************************
函數(shù)功能：MPU6050內(nèi)置DMP的初始化
入口參數(shù)：無
返回值：無
作者：平衡小車之家
**************************************************************************/
void DMP_Init(void)
{
u8 temp[1]={0};
i2cRead(0x68,0x75,1,temp);
printf("mpu_set_sensor complete ......\r\n");
if(temp[0]!=0x68)NVIC_SystemReset();
if(!mpu_init())
{
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_set_sensor complete ......\r\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_configure_fifo complete ......\r\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
printf("mpu_set_sample_rate complete ......\r\n");
if(!dmp_load_motion_driver_firmware())
printf("dmp_load_motion_driver_firmware complete ......\r\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
printf("dmp_set_orientation complete ......\r\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL))
printf("dmp_enable_feature complete ......\r\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
printf("dmp_set_fifo_rate complete ......\r\n");
run_self_test();
if(!mpu_set_dmp_state(1))
printf("mpu_set_dmp_state complete ......\r\n");
}
}
/**************************************************************************
函數(shù)功能：讀取MPU6050內(nèi)置DMP的姿態(tài)信息
入口參數(shù)：無
返回值：無
作者：平衡小車之家
**************************************************************************/
uint8_t Read_DMP(float* Pitch,float* Roll,float* Yaw)
{
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復(fù)制代碼

所有資料51hei提供下載:

6050測試3.rar (598.63 KB, 下載次數(shù): 301)

ID:158903 · 發(fā)表于 2018-6-21 15:14

謝謝分享

ID:357306 · 發(fā)表于 2018-6-23 16:25

高手！謝謝，學(xué)習(xí)了

ID:361002 · 發(fā)表于 2018-6-28 14:59

謝謝分享

ID:425665 · 發(fā)表于 2018-11-13 14:28

正好有個(gè)項(xiàng)目可以用到這個(gè)程序，感謝樓主。

ID:421518 · 發(fā)表于 2019-4-1 09:59

謝謝分享

。。。。。

ID:430197 · 發(fā)表于 2019-4-2 12:13

學(xué)習(xí)了，謝謝分享

ID:578767 · 發(fā)表于 2019-7-6 01:23

學(xué)習(xí) 學(xué)習(xí) 謝謝

ID:578767 · 發(fā)表于 2019-7-6 01:24

學(xué)習(xí)了，謝謝分享。。

ID:585663 · 發(fā)表于 2019-7-22 20:39

uint8_t buffer[14];

int16_t MPU6050_FIFO[6][11];
int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;

請問這段什么意思？實(shí)現(xiàn)什么功能

ID:196702 · 發(fā)表于 2019-8-3 00:33

感謝樓主。

ID:645237 · 發(fā)表于 2019-11-19 18:25

謝謝分享

ID:754694 · 發(fā)表于 2020-8-25 12:57

好資料，謝謝分享

ID:229821 · 發(fā)表于 2020-9-11 22:47

好資料。謝謝樓主的共享精神。

ID:754694 · 發(fā)表于 2020-10-24 21:24

32到8完美移植，多謝分享

ID:81138 · 發(fā)表于 2021-1-27 15:48

不知道電路是如何設(shè)計(jì)的

ID:928052 · 發(fā)表于 2021-8-8 20:26

為什么重復(fù)下載，需要重復(fù)扣分呢？

ID:767310 · 發(fā)表于 2021-8-26 16:21

編譯提示NVIC_SystemReset沒有定義，并且空間不足，請問樓主是用哪個(gè)型號的STM8芯片

ID:1038866 · 發(fā)表于 2022-7-17 09:05

高手！謝謝，學(xué)習(xí)

ID:189235 · 發(fā)表于 2023-10-14 00:51

#在這里快速回復(fù)#謝謝分享

帳號		自動(dòng)登錄	找回密碼
密碼			立即注冊