使用的是如图所示的这款陀螺仪,相比MPU6050,它可以做到Z轴不漂的效果。
官方给的代码是使用两个串口,一个用来和上位机通信,一个用来于该模块通信。在实际使用中,我们只需要单片机和该模块通信即可,所以我们需要对原有代码进行修改。在这里我会介绍一下具体的修改和移植方法。
在文章末尾有官方的源代码和修改后的代码,大家可以参考。这里只介绍移植方法
1把官方代码的这7个函数复制粘贴的自己的工程之中不需要修改
2在主函数中删除一些函数,这些函数都是用来单片机和电脑通信用的,所以可以删除,我们只留下单片机和模块的通信所需函数即可
删除前的主函数:
/*
@ link : http://wit-motion.cn
@ Function:
1. Power on automatic detection sensor
2. Read acceleration, angular velocity, angle and magnetic field data
3. Set switching baud rate parameters
USB-TTL STM32Core JY901s
VCC ----- VCC ---- VCC
TX ----- RX1 (GPIOA_10)
RX ----- TX1 (GPIOA_9)
GND ----- GND ---- GND
RX2 (GPIOA_3) ---- TX
TX2 (GPIOA_2) ---- RX
------------------------------------
*/
#include <string.h>
#include <stdio.h>
#include "stm32f10x.h"
#include "stm32f10x_gpio.h"
#include "stm32f10x_rcc.h"
#include "misc.h"
#include "wit_c_sdk.h"
#include "UART1.h"
#include "UART2.h"
#include "delay.h"
#define ACC_UPDATE 0x01
#define GYRO_UPDATE 0x02
#define ANGLE_UPDATE 0x04
#define MAG_UPDATE 0x08
#define READ_UPDATE 0x80
static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
const uint32_t c_uiBaud[10] = {0, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600}; //波特率选择
static void CmdProcess(void);
static void AutoScanSensor(void);
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs);
int main(void)
{
float fAcc[3], fGyro[3], fAngle[3];
int i;
SysTick_Init();
Usart1Init(115200); //USB 串口 可以不用
Usart2Init(9600); //模块串口
WitInit(WIT_PROTOCOL_NORMAL, 0x50); //选择正常模式
WitSerialWriteRegister(SensorUartSend); //用串口二发数据的声明
WitRegisterCallBack(SensorDataUpdata); //看哪个数据接收到了,给s_cDataUpdate赋值,从而在while中不断接收数据
WitDelayMsRegister(Delayms); //延迟函数的声明
printf("\r\n********************** wit-motion normal example ************************\r\n"); //可以不用
AutoScanSensor(); //寻找是否连接上模块 可以不用
while (1)
{
CmdProcess(); //根据串口一接收到的数据设置传感器数据 可以不用
if(s_cDataUpdate) //如果有数据
{
for(i = 0; i < 3; i++)
{
fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
}
if(s_cDataUpdate & ACC_UPDATE)
{
printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]);
s_cDataUpdate &= ~ACC_UPDATE;
}
if(s_cDataUpdate & GYRO_UPDATE)
{
printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]);
s_cDataUpdate &= ~GYRO_UPDATE;
}
if(s_cDataUpdate & ANGLE_UPDATE)
{
printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]);
s_cDataUpdate &= ~ANGLE_UPDATE;
}
if(s_cDataUpdate & MAG_UPDATE)
{
printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
s_cDataUpdate &= ~MAG_UPDATE;
}
}
}
}
//用于接收串口数据的函数,USB的串口1 可以不用
void CopeCmdData(unsigned char ucData)
{
static unsigned char s_ucData[50], s_ucRxCnt = 0;
s_ucData[s_ucRxCnt++] = ucData;
if(s_ucRxCnt<3)return; //Less than three data returned
if(s_ucRxCnt >= 50) s_ucRxCnt = 0;
if(s_ucRxCnt >= 3)
{
if((s_ucData[1] == '\r') && (s_ucData[2] == '\n'))
{
s_cCmd = s_ucData[0];
memset(s_ucData,0,50);//
s_ucRxCnt = 0;
}
else
{
s_ucData[0] = s_ucData[1];
s_ucData[1] = s_ucData[2];
s_ucRxCnt = 2;
}
}
}
//用于串口1,告诉发送什么内容 可以不用
static void ShowHelp(void)
{
printf("\r\n************************ WIT_SDK_DEMO ************************");
printf("\r\n************************ HELP ************************\r\n");
printf("UART SEND:a\\r\\n Acceleration calibration.\r\n");
printf("UART SEND:m\\r\\n Magnetic field calibration,After calibration send: e\\r\\n to indicate the end\r\n");
printf("UART SEND:U\\r\\n Bandwidth increase.\r\n");
printf("UART SEND:u\\r\\n Bandwidth reduction.\r\n");
printf("UART SEND:B\\r\\n Baud rate increased to 115200.\r\n");
printf("UART SEND:b\\r\\n Baud rate reduction to 9600.\r\n");
printf("UART SEND:R\\r\\n The return rate increases to 10Hz.\r\n");
printf("UART SEND:r\\r\\n The return rate reduction to 1Hz.\r\n");
printf("UART SEND:C\\r\\n Basic return content: acceleration, angular velocity, angle, magnetic field.\r\n");
printf("UART SEND:c\\r\\n Return content: acceleration.\r\n");
printf("UART SEND:h\\r\\n help.\r\n");
printf("******************************************************************************\r\n");
}
//根据串口一接收到的数据设置传感器数据
static void CmdProcess(void)
{
switch(s_cCmd) //s_cCmd数据来自USB即串口一的内容,有上位机发给单片机
{
case 'a':
if(WitStartAccCali() != WIT_HAL_OK)
printf("\r\nSet AccCali Error\r\n");
break;
case 'm':
if(WitStartMagCali() != WIT_HAL_OK)
printf("\r\nSet MagCali Error\r\n");
break;
case 'e':
if(WitStopMagCali() != WIT_HAL_OK)
printf("\r\nSet MagCali Error\r\n");
break;
case 'u':
if(WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK)
printf("\r\nSet Bandwidth Error\r\n");
break;
case 'U':
if(WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK)
printf("\r\nSet Bandwidth Error\r\n");
break;
case 'B':
if(WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK)
printf("\r\nSet Baud Error\r\n");
else
Usart2Init(c_uiBaud[WIT_BAUD_115200]);
break;
case 'b':
if(WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
printf("\r\nSet Baud Error\r\n");
else
Usart2Init(c_uiBaud[WIT_BAUD_9600]);
break;
case 'R':
if(WitSetOutputRate(RRATE_10HZ) != WIT_HAL_OK)
printf("\r\nSet Rate Error\r\n");
break;
case 'r':
if(WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK)
printf("\r\nSet Rate Error\r\n");
break;
case 'C':
if(WitSetContent(RSW_ACC|RSW_GYRO|RSW_ANGLE|RSW_MAG) != WIT_HAL_OK)
printf("\r\nSet RSW Error\r\n");
break;
case 'c':
if(WitSetContent(RSW_ACC) != WIT_HAL_OK)
printf("\r\nSet RSW Error\r\n");
break;
case 'h':
ShowHelp();
break;
}
s_cCmd = 0xff;
}
//用串口二发数据
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize)
{
Uart2Send(p_data, uiSize);
}
//延迟函数
static void Delayms(uint16_t ucMs)
{
delay_ms(ucMs);
}
//看哪个数据接收到了,给s_cDataUpdate赋值
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
int i;
for(i = 0; i < uiRegNum; i++)
{
switch(uiReg)
{
// case AX:
// case AY:
case AZ:
s_cDataUpdate |= ACC_UPDATE;
break;
// case GX:
// case GY:
case GZ:
s_cDataUpdate |= GYRO_UPDATE;
break;
// case HX:
// case HY:
case HZ:
s_cDataUpdate |= MAG_UPDATE;
break;
// case Roll:
// case Pitch:
case Yaw:
s_cDataUpdate |= ANGLE_UPDATE;
break;
default:
s_cDataUpdate |= READ_UPDATE;
break;
}
uiReg++;
}
}
//寻找模块
static void AutoScanSensor(void)
{
int i, iRetry;
for(i = 1; i < 10; i++)
{
Usart2Init(c_uiBaud[i]);
iRetry = 2;
do
{
s_cDataUpdate = 0;
WitReadReg(AX, 3);
delay_ms(100);
if(s_cDataUpdate != 0)
{
printf("%d baud find sensor\r\n\r\n", c_uiBaud[i]);
ShowHelp();
return ;
}
iRetry--;
}while(iRetry);
}
printf("can not find sensor\r\n");
printf("please check your connection\r\n");
}
修改后的主函数:
#include <string.h>
#include <stdio.h>
#include "stm32f10x.h"
#include "stm32f10x_gpio.h"
#include "stm32f10x_rcc.h"
#include "misc.h"
#include "wit_c_sdk.h"
#include "UART2.h"
#include "delay.h"
#define ACC_UPDATE 0x01
#define GYRO_UPDATE 0x02
#define ANGLE_UPDATE 0x04
#define MAG_UPDATE 0x08
#define READ_UPDATE 0x80
static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
const uint32_t c_uiBaud[10] = {0, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600}; //波特率选择
//static void CmdProcess(void);
static void AutoScanSensor(void);
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs);
int main(void)
{
float fAcc[3], fGyro[3], fAngle[3];
int i;
SysTick_Init();
//Usart1Init(115200); //USB 串口 可以不用
Usart2Init(9600); //模块串口
WitInit(WIT_PROTOCOL_NORMAL, 0x50); //选择正常模式
WitSerialWriteRegister(SensorUartSend); //用串口二发数据的声明
WitRegisterCallBack(SensorDataUpdata); //看哪个数据接收到了,给s_cDataUpdate赋值,从而在while中不断接收数据
WitDelayMsRegister(Delayms); //延迟函数的声明
//printf("\r\n********************** wit-motion normal example ************************\r\n"); //可以不用
AutoScanSensor(); //寻找是否连接上模块 可以不用
while (1)
{
//CmdProcess(); //根据串口一接收到的数据设置传感器数据 可以不用
if(s_cDataUpdate) //如果有数据
{
for(i = 0; i < 3; i++)
{
fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
}
if(s_cDataUpdate & ACC_UPDATE)
{
//printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]);
s_cDataUpdate &= ~ACC_UPDATE;
}
if(s_cDataUpdate & GYRO_UPDATE)
{
//printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]);
s_cDataUpdate &= ~GYRO_UPDATE;
}
if(s_cDataUpdate & ANGLE_UPDATE)
{
//printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]);
s_cDataUpdate &= ~ANGLE_UPDATE;
}
if(s_cDataUpdate & MAG_UPDATE)
{
//printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
s_cDataUpdate &= ~MAG_UPDATE;
}
}
}
}
//用串口二发数据
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize)
{
Uart2Send(p_data, uiSize);
}
//延迟函数
static void Delayms(uint16_t ucMs)
{
delay_ms(ucMs);
}
//看哪个数据接收到了,给s_cDataUpdate赋值
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
int i;
for(i = 0; i < uiRegNum; i++)
{
switch(uiReg)
{
// case AX:
// case AY:
case AZ:
s_cDataUpdate |= ACC_UPDATE;
break;
// case GX:
// case GY:
case GZ:
s_cDataUpdate |= GYRO_UPDATE;
break;
// case HX:
// case HY:
case HZ:
s_cDataUpdate |= MAG_UPDATE;
break;
// case Roll:
// case Pitch:
case Yaw:
s_cDataUpdate |= ANGLE_UPDATE;
break;
default:
s_cDataUpdate |= READ_UPDATE;
break;
}
uiReg++;
}
}
//寻找模块
static void AutoScanSensor(void)
{
int i, iRetry;
for(i = 1; i < 10; i++)
{
Usart2Init(c_uiBaud[i]);
iRetry = 2;
do
{
s_cDataUpdate = 0;
WitReadReg(AX, 3);
delay_ms(100);
if(s_cDataUpdate != 0)
{
//printf("%d baud find sensor\r\n\r\n", c_uiBaud[i]);
//ShowHelp();
return ;
}
iRetry--;
}while(iRetry);
}
printf("can not find sensor\r\n");
printf("please check your connection\r\n");
}
删掉了一些没用的函数,大家对比一下就可以明白
3按照说明正确连接,也可以自己定义串口进行连接
4到这里就已经完全修改好了。最后验证:打开DEBUG,将 fAngle数组加入watch里面,可以看到实时的pitch roll yaw 三个角数据变化
官方代码和修改后的代码链接
通过网盘分享的文件:
维特陀螺仪
链接: https://pan.baidu.com/s/1HG8nYcSanUD3SSVDqoJWwQ?pwd=ndri 提取码: ndri
--来自百度网盘超级会员v3的分享