RVB2601 drives MPU6050 gyroscope module

乘简

RVB2601 drives MPU6050 gyroscope module [Copy link]

This post was last edited by Chengjian on 2022-6-6 09:39

The 4 pins of the gyroscope, VCC, GND, SCL, SDA are connected to 3.3, GND, PA8, PA9 of RVB2601 respectively.

The .c code is as follows:

#include "mpu6050.h"

#include <drv/iic.h>

#include <aos/aos.h>

#include <drv/pin.h>

#include <math.h>



#define IIC_IDX 0

static csi_iic_t master_iic;

static aos_timer_t mpu_timer;//定时器



int16_t rawAccX=0,rawAccY=0,rawAccZ=0,rawGyroX=0,rawGyroY=0,rawGyroZ=0;//6项原始数据



float angleX=0, angleY=0, angleZ=0;

float angleAccX=0,angleAccY=0; //由mpu6050_tockn加计算的加速度角度

float gyroX, gyroY, gyroZ;



//float gyroXoffset=0,gyroYoffset=0,gyroZoffset=0;



int iic_init(void)

{

csi_error_t ret;



csi_pin_set_mux(PA8, PA8_IIC0_SCL);

csi_pin_set_mux(PA9, PA9_IIC0_SDA);



ret = csi_iic_init(&master_iic, IIC_IDX);

if (ret != CSI_OK) {

printf("csi_iic_initialize error\n");

return -1;

}

/* config iic master mode */

ret = csi_iic_mode(&master_iic, IIC_MODE_MASTER);

if (ret != CSI_OK) {

printf("csi_iic_set_mode error\n");

return -1;

}

/* config iic 7bit address mode */

ret = csi_iic_addr_mode(&master_iic, IIC_ADDRESS_7BIT);

if (ret != CSI_OK) {

printf("csi_iic_set_addr_mode error\n");

return -1;

}

/* config iic standard/fast speed*/

ret = csi_iic_speed(&master_iic, IIC_BUS_SPEED_FAST);//IIC_BUS_SPEED_STANDARD);

if (ret != CSI_OK) {

printf("csi_iic_set_speed error\n");

return -1;

}

return 0;

}



//写寄存器

void MPU6050_WriteReg(uint8_t REG_Address, uint8_t REG_data)

{

uint8_t buf[2];

buf[0]=REG_Address;

buf[1]=REG_data;

csi_iic_master_send(&master_iic, SLAVE_ADDR, buf, 2, 100);

}



//读寄存器

void MPU6050_ReadReg(uint8_t REG_Address)

{

uint8_t buf;

buf=REG_Address;

csi_iic_master_send(&master_iic, SLAVE_ADDR, &buf, 1, 100);

}





//读数据

int16_t MPU6050_ReadData(uint8_t REG_Address)

{

uint8_t H,L;

uint8_t buf[2]={0,0};



MPU6050_ReadReg(REG_Address);

csi_iic_master_receive(&master_iic, SLAVE_ADDR, buf, 2, 100);

H=buf[0];

L=buf[1];



return (H<<8)|L;

}



void MPU6050_Get_Data()

{

float accX, accY, accZ;

// float homeAccX,homeAccY;



//6项原始数据

rawAccX = MPU6050_ReadData(ACCEL_XOUT_H);

rawAccY = MPU6050_ReadData(ACCEL_YOUT_H);

rawAccZ = MPU6050_ReadData(ACCEL_ZOUT_H);

rawGyroX = MPU6050_ReadData(GYRO_XOUT_H);

rawGyroY = MPU6050_ReadData(GYRO_YOUT_H);

rawGyroZ = MPU6050_ReadData(GYRO_ZOUT_H);



//计算重力加速度的g数

accX = ((float)rawAccX) / 16384.0;//+-2g，65536/4=16384

accY = ((float)rawAccY) / 16384.0;

accZ = ((float)rawAccZ) / 16384.0;



//角速度

gyroX = ((float)rawGyroX) / 65.536;//+-500

gyroY = ((float)rawGyroY) / 65.536;

gyroZ = ((float)rawGyroZ) / 65.536;



//由重力加速度计算X、Y轴的角度，tockn库

angleAccX = atan2(accY, accZ + abs(accX)) * RAD_TO_DEG;//360 / 2.0 / PI;

angleAccY = -atan2(accX, accZ + abs(accY)) * RAD_TO_DEG;

//angleAccY += MPU_OFFSET_Y;



angleAccY += 1.15;//此参数为陀镙仪固定在小车上的角度物理误差，根据经验调整



// gyroX -= gyroXoffset;

// gyroY -= gyroYoffset;

// gyroZ -= gyroZoffset;



// //平衡小车之家的融合算法

// homeAccX = 0;

// homeAccY = -atan2((float)rawAccX, (float)rawAccZ) * RAD_TO_DEG;

// homeAccY+=MPU_OFFSET_Y;

// homeX = 0.02 * homeAccX + 0.98 * (homeX + gyroX * 0.01);

// homeY = 0.02 * homeAccY + 0.98 * (homeY + gyroY * 0.01);



//tockn库角度融合算法，上次的角度+10ms的角速度占98%，本次的角度占2%

angleX = (0.98f * (angleX + gyroX * 0.01)) + (0.02f * angleAccX);

angleY = (0.98f * (angleY + gyroY * 0.01)) + (0.02f * angleAccY);

angleZ = gyroZ*0.01;



}





//定时器回调函数

static void timer_mpu6050(void *arg1, void* arg2)

{



MPU6050_Get_Data();



//pid_car();



static int t=0;

t++;

t%=10;

if(t==0){

printf("%12f,%12f,%12f\r\n",angleX,angleY,angleZ);

}

}



void mpu6050_init()

{

iic_init();//初始化iic总线

MPU6050_WriteReg(SMPLRT_DIV, 0x00); //陀螺仪125hz

MPU6050_WriteReg(CONFIG, 0x00); //21HZ滤波 延时A8.5ms G8.3ms 此处取值应相当注意，延时与系统周期相近为宜

MPU6050_WriteReg(GYRO_CONFIG, 0x08); //陀螺仪 0x00为+-250 0x08为+-500 0x10为+-1000 0x18为+-2000度

MPU6050_WriteReg(ACCEL_CONFIG, 0x00);//加速度 0x00为+-2g 0x08为+-4g 0x10为+-8g 0x18为+-16g

MPU6050_WriteReg(PWR_MGMT_1, 0x01); //解除休眠状态



aos_timer_new(&mpu_timer, timer_mpu6050, NULL, 10, 1);//10ms定时器

}

The code of the .h file is as follows:

#ifndef _MPU6050_H_

#define _MPU6050_H_

#include <stdio.h>



#define PI 3.141593

#define RAD_TO_DEG 57.295779513082320876798154814105



#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)

#define CONFIG 0x1A //低通滤波频率，典型值：0x06(5Hz)

#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)

//bit3 bit4为量程调节 0：+-250度/s 1：+=500 2：+-1000 3：+-2000度

//0x00为+-250 0x08为+-500 0x10为+-1000 0x18为+-2000度

#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)

#define ACCEL_XOUT_H 0x3B // 存储最近的X轴、Y轴、Z轴加速度感应器的测量值

#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 // 存储最近的X轴、Y轴、Z轴角速度感应器的测量值

#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 SLAVE_ADDR 0x68

#define SLAVE_ADDRWT 0xD0 //IIC写入时的地址字节数据，(0x68<<1)表示写

#define SLAVE_ADDRRD 0xD1 //IIC写入时的地址字节数据，(0x68<<1)|1表示读



extern int16_t rawAccX,rawAccY,rawAccZ,rawGyroX,rawGyroY,rawGyroZ;//6项原始数据

extern float angleX, angleY, angleZ;//+-90.0

extern float gyroX, gyroY, gyroZ;//+-500



void mpu6050_init();

void MPU6050_Get_Data();



#endif

Call mpu6050_init() in the main function;

In this way, your serial port debugging assistant will continuously print out the angle of the gyroscope. . .

Of course, it can also be printed on the OLED screen in the form of a curve.

728fd92fe466af5cfe8033cc689ba53c

lugl4313820

You can play with this for a while.

乘简

lugl4313820 posted on 2022-6-6 17:34 You can play with this for a while.

I didn't expect to see someone who typed in Wubi,,,

小郑要变瘦

Hello, could you provide a driver example? The above code cannot be driven successfully.

乘简

Xiao Zheng wants to become thinner, published on 2022-6-27 16:05 Hello, can you provide a driver example? The code above cannot be driven successfully.

The above code is to be called in the main function

mpu6050_init();

The cables must be connected correctly, and if they are connected in the middle, they need to be restarted.

RVB2601 drives MPU6050 gyroscope module [Copy link]

Latest reply

Comments

Comments