291 lines
7.4 KiB
C
291 lines
7.4 KiB
C
|
#include "McuJY60Drv.h"
|
||
|
|
#include <string.h>
|
||
|
|
|
||
|
|
#define JY60_RXBUF_LEN 60
|
||
|
|
|
||
|
|
static void McuDrvJy60Recv(unsigned char data);
|
||
|
|
/* 读取一个字符 */
|
||
|
|
static unsigned char McuDrvJy60ReadByte(void);
|
||
|
|
static void McuDrvJy60Handle(void);
|
||
|
|
|
||
|
|
static char gJy60RxBuf[JY60_RXBUF_LEN] = {0}; // 能够存放住两帧数据，主要为了容错
|
||
|
|
static unsigned char gJy60RxBufLocal = 0; //当前读取指针所在位置
|
||
|
|
static unsigned char gJy60RxBufTemp = 0; // 当前接收数据的末期所在位置
|
||
|
|
static unsigned char gJy60AngleCalibration[3] = {0xFF, 0xAA, 0x52}; // 角速度初始化
|
||
|
|
static unsigned char gJy60AccCalibration[3] = {0xFF, 0xAA, 0x67}; // 加速度初始化
|
||
|
|
//static unsigned char gJy60Sleep[3] = {0xFF, 0xAA, 0x60}; //进入睡眠模式
|
||
|
|
//static unsigned char gJy60WakeUp[1] = {0x00}; // 由于输入任何信号就会使得芯片唤醒。
|
||
|
|
static unsigned char gJy60Horizon[3] = {0xFF, 0xAA, 0x65}; //水平安装
|
||
|
|
static unsigned char gJy60Vertical[3] = {0xFF, 0xAA, 0x66}; //垂直安装
|
||
|
|
|
||
|
|
static float gAx = 0.0f;
|
||
|
|
static float gAy = 0.0f;
|
||
|
|
static float gAz = 0.0f;
|
||
|
|
|
||
|
|
static float gWx = 0.0f;
|
||
|
|
static float gWy = 0.0f;
|
||
|
|
static float gWz = 0.0f;
|
||
|
|
|
||
|
|
static float gRoll = 0.0f;
|
||
|
|
static float gPitch = 0.0f;
|
||
|
|
static float gYaw = 0.0f;
|
||
|
|
|
||
|
|
/* JY60初始化 */
|
||
|
|
int McuDrvJy60Init(void)
|
||
|
|
{
|
||
|
|
/* 初始化串口 */
|
||
|
|
MCU_BSP_UART_CONF UartConfig = {0};
|
||
|
|
UartConfig.BaudRate = MCU_DRV_JY60_UART_BAUDRATE;
|
||
|
|
UartConfig.WordLength = MCU_DRV_JY60_UART_WORDLEN;
|
||
|
|
UartConfig.StopBits = MCU_DRV_JY60_UART_STOPBITS;
|
||
|
|
UartConfig.Parity = MCU_DRV_JY60_UART_PARITY;
|
||
|
|
UartConfig.Mode = MCU_DRV_JY60_UART_MODE;
|
||
|
|
if (McuBspUartInit(MCU_DRV_JY60_UART_ID, &UartConfig, NULL, McuDrvJy60Recv) < 0) {
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
McuBspUartStartRecv(MCU_DRV_JY60_UART_ID);
|
||
|
|
/* 电源控制 */
|
||
|
|
McuBspGpioInit(MCU_DRV_JY60_CTRL_POWER_GPIO_PORT, MCU_DRV_JY60_CTRL_POWER_GPIO_PIN,
|
||
|
|
MCU_DRV_JY60_CTRL_POWER_GPIO_MODE, MCU_DRV_JY60_CTRL_POWER_GPIO_SPEED,
|
||
|
|
MCU_DRV_JY60_CTRL_POWER_GPIO_PULL, 0, NULL);
|
||
|
|
McuBspGpioSetLevel(MCU_DRV_JY60_CTRL_POWER_GPIO_PORT, MCU_DRV_JY60_CTRL_POWER_GPIO_PIN,
|
||
|
|
MCU_BSP_GPIO_OUT_LOW);
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Jy60校准 */
|
||
|
|
int McuDrvJy60Calibration(void)
|
||
|
|
{
|
||
|
|
/* 发送角度初始化 */
|
||
|
|
if (McuBspUartSend(MCU_DRV_JY60_UART_ID, gJy60AngleCalibration, sizeof(gJy60AngleCalibration)) < 0) {
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
/* 发送加速读校准 */
|
||
|
|
if (McuBspUartSend(MCU_DRV_JY60_UART_ID, gJy60AccCalibration, sizeof(gJy60AccCalibration)) < 0) {
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 垂直或者水平安装 */
|
||
|
|
int McuDrvJy60InstallType(unsigned char Type)
|
||
|
|
{
|
||
|
|
if (Type == MCU_DRV_JY60_HORIZON_TYPE) {
|
||
|
|
if (McuBspUartSend(MCU_DRV_JY60_UART_ID, gJy60Horizon, sizeof(gJy60Horizon)) < 0) {
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
if (McuBspUartSend(MCU_DRV_JY60_UART_ID, gJy60Vertical, sizeof(gJy60Vertical)) < 0) {
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 获取最新一组数据 */
|
||
|
|
void McuDrvJy60GetData(float *Ax, float *Ay, float *Az,
|
||
|
|
float *Wx, float *Wy, float *Wz,
|
||
|
|
float *Roll, float *Pitch, float *Yaw)
|
||
|
|
{
|
||
|
|
/* 处理一组数据 */
|
||
|
|
McuBspGpioSetLevel(MCU_DRV_JY60_CTRL_POWER_GPIO_PORT, MCU_DRV_JY60_CTRL_POWER_GPIO_PIN,
|
||
|
|
MCU_BSP_GPIO_OUT_HIGH);
|
||
|
|
McuDrvJy60Handle();
|
||
|
|
McuBspGpioSetLevel(MCU_DRV_JY60_CTRL_POWER_GPIO_PORT, MCU_DRV_JY60_CTRL_POWER_GPIO_PIN,
|
||
|
|
MCU_BSP_GPIO_OUT_LOW);
|
||
|
|
/* 将处理完的数据输出 */
|
||
|
|
*Ax = gAx;
|
||
|
|
*Ay = gAy;
|
||
|
|
*Az = gAz;
|
||
|
|
|
||
|
|
*Wx = gWx;
|
||
|
|
*Wy = gWy;
|
||
|
|
*Wz = gWz;
|
||
|
|
|
||
|
|
*Roll = gRoll;
|
||
|
|
*Pitch = gPitch;
|
||
|
|
*Yaw = gYaw;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 处理最新一帧数据 */
|
||
|
|
static void McuDrvJy60Handle(void)
|
||
|
|
{
|
||
|
|
short Ax;
|
||
|
|
short Ay;
|
||
|
|
short Az;
|
||
|
|
|
||
|
|
short Wx;
|
||
|
|
short Wy;
|
||
|
|
short Wz;
|
||
|
|
|
||
|
|
short Roll;
|
||
|
|
short Pitch;
|
||
|
|
short Yaw;
|
||
|
|
uint32_t i = 0;
|
||
|
|
|
||
|
|
unsigned char sum = 0;
|
||
|
|
|
||
|
|
unsigned char AxL = 0;
|
||
|
|
unsigned char AxH = 0;
|
||
|
|
unsigned char AyL = 0;
|
||
|
|
unsigned char AyH = 0;
|
||
|
|
unsigned char AzL = 0;
|
||
|
|
unsigned char AzH = 0;
|
||
|
|
|
||
|
|
unsigned char WxL = 0;
|
||
|
|
unsigned char WxH = 0;
|
||
|
|
unsigned char WyL = 0;
|
||
|
|
unsigned char WyH = 0;
|
||
|
|
unsigned char WzL = 0;
|
||
|
|
unsigned char WzH = 0;
|
||
|
|
|
||
|
|
unsigned char RollL = 0;
|
||
|
|
unsigned char RollH = 0;
|
||
|
|
unsigned char PitchL = 0;
|
||
|
|
unsigned char PitchH = 0;
|
||
|
|
unsigned char YawL = 0;
|
||
|
|
unsigned char YawH = 0;
|
||
|
|
|
||
|
|
/* 清空整个接收空间 */
|
||
|
|
memset(gJy60RxBuf, 0, sizeof(gJy60RxBuf));
|
||
|
|
gJy60RxBufTemp = 0;
|
||
|
|
gJy60RxBufLocal = 0;
|
||
|
|
while (1) {
|
||
|
|
i++;
|
||
|
|
if (i > 2000) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
if (McuDrvJy60ReadByte() == 0x55) {
|
||
|
|
if (McuDrvJy60ReadByte() == 0x51) {
|
||
|
|
sum = 0;
|
||
|
|
sum += 0x55;
|
||
|
|
sum += 0x51;
|
||
|
|
AxL = McuDrvJy60ReadByte();
|
||
|
|
sum += AxL;
|
||
|
|
AxH = McuDrvJy60ReadByte();
|
||
|
|
sum += AxH;
|
||
|
|
AyL = McuDrvJy60ReadByte();
|
||
|
|
sum += AyL;
|
||
|
|
AyH = McuDrvJy60ReadByte();
|
||
|
|
sum += AyH;
|
||
|
|
AzL = McuDrvJy60ReadByte();
|
||
|
|
sum += AzL;
|
||
|
|
AzH = McuDrvJy60ReadByte();
|
||
|
|
sum += AzH;
|
||
|
|
sum += McuDrvJy60ReadByte(); // 模块温度低不获取
|
||
|
|
sum += McuDrvJy60ReadByte(); // 模块温度高不获取
|
||
|
|
if (sum == McuDrvJy60ReadByte()) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
HAL_Delay(1);
|
||
|
|
}
|
||
|
|
i = 0;
|
||
|
|
while (1) {
|
||
|
|
i++;
|
||
|
|
if (i > 2000) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
if (McuDrvJy60ReadByte() == 0x55) {
|
||
|
|
if (McuDrvJy60ReadByte() == 0x52) {
|
||
|
|
sum = 0;
|
||
|
|
sum += 0x55;
|
||
|
|
sum += 0x52;
|
||
|
|
WxL = McuDrvJy60ReadByte();
|
||
|
|
sum += WxL;
|
||
|
|
WxH = McuDrvJy60ReadByte();
|
||
|
|
sum += WxH;
|
||
|
|
WyL = McuDrvJy60ReadByte();
|
||
|
|
sum += WyL;
|
||
|
|
WyH = McuDrvJy60ReadByte();
|
||
|
|
sum += WyH;
|
||
|
|
WzL = McuDrvJy60ReadByte();
|
||
|
|
sum += WzL;
|
||
|
|
WzH = McuDrvJy60ReadByte();
|
||
|
|
sum += WzH;
|
||
|
|
sum += McuDrvJy60ReadByte();
|
||
|
|
sum += McuDrvJy60ReadByte();
|
||
|
|
if (sum == McuDrvJy60ReadByte()) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
HAL_Delay(1);
|
||
|
|
}
|
||
|
|
i = 0;
|
||
|
|
while (1) {
|
||
|
|
i++;
|
||
|
|
if (i > 2000) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
if (McuDrvJy60ReadByte() == 0x55) {
|
||
|
|
if (McuDrvJy60ReadByte() == 0x53) {
|
||
|
|
sum = 0;
|
||
|
|
sum += 0x55;
|
||
|
|
sum += 0x53;
|
||
|
|
RollL = McuDrvJy60ReadByte();
|
||
|
|
sum += RollL;
|
||
|
|
RollH = McuDrvJy60ReadByte();
|
||
|
|
sum += RollH;
|
||
|
|
PitchL = McuDrvJy60ReadByte();
|
||
|
|
sum += PitchL;
|
||
|
|
PitchH = McuDrvJy60ReadByte();
|
||
|
|
sum += PitchH;
|
||
|
|
YawL = McuDrvJy60ReadByte();
|
||
|
|
sum += YawL;
|
||
|
|
YawH = McuDrvJy60ReadByte();
|
||
|
|
sum += YawH;
|
||
|
|
sum += McuDrvJy60ReadByte();
|
||
|
|
sum += McuDrvJy60ReadByte();
|
||
|
|
if (sum == McuDrvJy60ReadByte()) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
HAL_Delay(1);
|
||
|
|
}
|
||
|
|
Ax = ((((short)AxH) << 8) | AxL);
|
||
|
|
Ay = ((((short)AyH) << 8) | AyL);
|
||
|
|
Az = ((((short)AzH) << 8) | AzL);
|
||
|
|
|
||
|
|
Wx = ((((short)WxH) << 8) | WxL);
|
||
|
|
Wy = ((((short)WyH) << 8) | WyL);
|
||
|
|
Wz = ((((short)WzH) << 8) | WzL);
|
||
|
|
|
||
|
|
Roll = ((((short)RollH) << 8) | RollL);
|
||
|
|
Pitch = ((((short)PitchH) << 8) | PitchL);
|
||
|
|
Yaw = ((((short)YawH) << 8) | YawL);
|
||
|
|
|
||
|
|
/* 计算 */
|
||
|
|
gAx = Ax / 32768.0f * 16 * 9.8f;
|
||
|
|
gAy = Ay / 32768.0f * 16 * 9.8f;
|
||
|
|
gAz = Az / 32768.0f * 16 * 9.8f;
|
||
|
|
|
||
|
|
gWx = Wx / 32768.0f * 2000.0f;
|
||
|
|
gWy = Wy / 32768.0f * 2000.0f;
|
||
|
|
gWz = Wz / 32768.0f * 2000.0f;
|
||
|
|
|
||
|
|
gRoll = Roll / 32768.0f * 180.0f;
|
||
|
|
gPitch = Pitch / 32768.0f * 180.0f;
|
||
|
|
gYaw = Yaw / 32768.0f * 180.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 读取一个字符 */
|
||
|
|
static unsigned char McuDrvJy60ReadByte(void)
|
||
|
|
{
|
||
|
|
while (gJy60RxBufLocal == gJy60RxBufTemp){
|
||
|
|
}
|
||
|
|
gJy60RxBufLocal++;
|
||
|
|
gJy60RxBufLocal = gJy60RxBufLocal % JY60_RXBUF_LEN;
|
||
|
|
return gJy60RxBuf[gJy60RxBufLocal];
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 接收函数 */
|
||
|
|
static void McuDrvJy60Recv(unsigned char data)
|
||
|
|
{
|
||
|
|
gJy60RxBuf[gJy60RxBufTemp] = data;
|
||
|
|
gJy60RxBufTemp ++;
|
||
|
|
gJy60RxBufTemp = gJy60RxBufTemp % JY60_RXBUF_LEN;
|
||
|
|
}
|