226 lines
12 KiB
C
226 lines
12 KiB
C
#include "McuTaskImu.h"
|
|
#include "McuJY60Drv.h"
|
|
#include "McuTaskBlueTooth.h"
|
|
#include "BeiDouHtyDrv.h"
|
|
#include "ZthyToolDataTypeSwitch.h"
|
|
#include "McuTaskFlash.h"
|
|
#include "Usr_4GCatDtuDrv.h"
|
|
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
|
|
float gAx;
|
|
float gAy;
|
|
float gAz;
|
|
float gWx;
|
|
float gWy;
|
|
float gWz;
|
|
float gRoll;
|
|
float gPitch;
|
|
float gYaw;
|
|
|
|
uint32_t gAx32;
|
|
uint32_t gAy32;
|
|
uint32_t gAz32;
|
|
uint32_t gWx32;
|
|
uint32_t gWy32;
|
|
uint32_t gWz32;
|
|
uint32_t gRoll32;
|
|
uint32_t gPitch32;
|
|
uint32_t gYaw32;
|
|
|
|
/* 蓝牙发送标志位 */
|
|
static int gMcuTaskImuBlueToothSendFlag = 0;
|
|
static unsigned short gMcuTaskImuBlueToothSerial = 0;
|
|
static unsigned char gMcuTaskImuBlueToothSendBuf[37] = {0};
|
|
static unsigned char gMcuTaskImuBeiDouSerial = 0;
|
|
static unsigned short gMcuTaskImu4gSerial = 0;
|
|
|
|
/* 北斗发送标志位 */
|
|
static int gMcuTaskImuBeiDouSendFlag = 0;
|
|
|
|
int McuTaskImuInit(void)
|
|
{
|
|
int rt;
|
|
rt = McuDrvJy60Init();
|
|
if (rt < 0){
|
|
/* 初始化 */
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/* IMU任务 */
|
|
int McuTaskImuTask(void)
|
|
{
|
|
if (gMcuTaskImuBlueToothSendFlag == 1 || gMcuTaskImuBeiDouSendFlag == 1) {
|
|
/* 获取数据 */
|
|
McuTaskFlashWriteOpt("Imu Join\r\n", strlen("Imu Join\r\n"));
|
|
McuDrvJy60GetData(&gAx, &gAy, &gAz, &gWx, &gWy, &gWz, &gRoll, &gPitch, &gYaw); // 可能会导致假死
|
|
if (gMcuTaskImuBlueToothSendFlag == 1) {
|
|
gMcuTaskImuBlueToothSendBuf[0] = 1;
|
|
ZthyToolFloatToUint32(gAx, &gAx32);
|
|
ZthyToolFloatToUint32(gAy, &gAy32);
|
|
ZthyToolFloatToUint32(gAz, &gAz32);
|
|
ZthyToolFloatToUint32(gWx, &gWx32);
|
|
ZthyToolFloatToUint32(gWy, &gWy32);
|
|
ZthyToolFloatToUint32(gWz, &gWz32);
|
|
ZthyToolFloatToUint32(gRoll, &gRoll32);
|
|
ZthyToolFloatToUint32(gPitch, &gPitch32);
|
|
ZthyToolFloatToUint32(gYaw, &gYaw32);
|
|
gMcuTaskImuBlueToothSendBuf[1] = (uint8_t)((gAx32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[2] = (uint8_t)((gAx32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[3] = (uint8_t)((gAx32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[4] = (uint8_t)(gAx32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[5] = (uint8_t)((gAy32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[6] = (uint8_t)((gAy32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[7] = (uint8_t)((gAy32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[8] = (uint8_t)(gAy32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[9] = (uint8_t)((gAz32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[10] = (uint8_t)((gAz32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[11] = (uint8_t)((gAz32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[12] = (uint8_t)(gAz32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[13] = (uint8_t)((gWx32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[14] = (uint8_t)((gWx32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[15] = (uint8_t)((gWx32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[16] = (uint8_t)(gWx32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[17] = (uint8_t)((gWy32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[18] = (uint8_t)((gWy32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[19] = (uint8_t)((gWy32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[20] = (uint8_t)(gWy32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[21] = (uint8_t)((gWz32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[22] = (uint8_t)((gWz32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[23] = (uint8_t)((gWz32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[24] = (uint8_t)(gWz32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[25] = (uint8_t)((gRoll32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[26] = (uint8_t)((gRoll32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[27] = (uint8_t)((gRoll32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[28] = (uint8_t)(gRoll32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[29] = (uint8_t)((gPitch32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[30] = (uint8_t)((gPitch32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[31] = (uint8_t)((gPitch32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[32] = (uint8_t)(gPitch32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[33] = (uint8_t)((gYaw32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[34] = (uint8_t)((gYaw32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[35] = (uint8_t)((gYaw32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[36] = (uint8_t)(gYaw32 & 0x000000ff);
|
|
McuTaskFlashWriteOpt("Imu BlueTh Send Data\r\n", strlen("Imu BlueTh Send Data\r\n"));
|
|
TaskBlueToothSendMessage(MCU_TASK_IMU_SEND_MESSAGE_ID, &gMcuTaskImuBlueToothSerial, gMcuTaskImuBlueToothSendBuf, sizeof(gMcuTaskImuBlueToothSendBuf));
|
|
gMcuTaskImuBlueToothSerial++;
|
|
gMcuTaskImuBlueToothSendFlag = 0;
|
|
}
|
|
if (gMcuTaskImuBeiDouSendFlag == 1) {
|
|
gMcuTaskImuBeiDouSendFlag = 0;
|
|
if (1 != McuDrv4GCatDtuPowerSta()) {
|
|
unsigned char crc = 0;
|
|
int i = 0;
|
|
unsigned char BeiDouSend[70] = {0};
|
|
|
|
/* 发送加速度 */
|
|
crc = 0;
|
|
sprintf((char *)BeiDouSend, "Imu 01 %03d %0.2f %0.2f %0.2f", gMcuTaskImuBeiDouSerial, gAx, gAy, gAz);
|
|
for (i = 0; i < strlen((char *)BeiDouSend); i++) {
|
|
crc ^= BeiDouSend[i];
|
|
}
|
|
sprintf((char *)&BeiDouSend[strlen((char *)BeiDouSend)], "%x", crc);
|
|
McuDrvBeiDouHtySendPacket(BeiDouSend, strlen((char *)BeiDouSend));
|
|
/* 发送角速度 */
|
|
crc = 0;
|
|
memset(BeiDouSend, 0, sizeof(BeiDouSend));
|
|
sprintf((char *)BeiDouSend, "Imu 02 %03d %0.2f %0.2f %0.2f", gMcuTaskImuBeiDouSerial, gWx, gWy, gWz);
|
|
for (i = 0; i < strlen((char *)BeiDouSend); i++) {
|
|
crc ^= BeiDouSend[i];
|
|
}
|
|
sprintf((char *)&BeiDouSend[strlen((char *)BeiDouSend)], "%x", crc);
|
|
McuDrvBeiDouHtySendPacket(BeiDouSend, strlen((char *)BeiDouSend));
|
|
|
|
/* 发送角度 */
|
|
crc = 0;
|
|
memset(BeiDouSend, 0, sizeof(BeiDouSend));
|
|
sprintf((char *)BeiDouSend, "Imu 03 %03d %0.2f %0.2f %0.2f", gMcuTaskImuBeiDouSerial, gRoll, gPitch, gYaw);
|
|
for (i = 0; i < strlen((char *)BeiDouSend); i++) {
|
|
crc ^= BeiDouSend[i];
|
|
}
|
|
sprintf((char *)&BeiDouSend[strlen((char *)BeiDouSend)], "%x", crc);
|
|
McuDrvBeiDouHtySendPacket(BeiDouSend, strlen((char *)BeiDouSend));
|
|
|
|
gMcuTaskImuBeiDouSerial++;
|
|
gMcuTaskImuBeiDouSerial = gMcuTaskImuBeiDouSerial % 1000;
|
|
McuTaskFlashWriteOpt("Imu BeiDou Send Data\r\n", strlen("Imu BeiDou Send Data\r\n"));
|
|
} else {
|
|
gMcuTaskImuBlueToothSendBuf[0] = 1;
|
|
ZthyToolFloatToUint32(gAx, &gAx32);
|
|
ZthyToolFloatToUint32(gAy, &gAy32);
|
|
ZthyToolFloatToUint32(gAz, &gAz32);
|
|
ZthyToolFloatToUint32(gWx, &gWx32);
|
|
ZthyToolFloatToUint32(gWy, &gWy32);
|
|
ZthyToolFloatToUint32(gWz, &gWz32);
|
|
ZthyToolFloatToUint32(gRoll, &gRoll32);
|
|
ZthyToolFloatToUint32(gPitch, &gPitch32);
|
|
ZthyToolFloatToUint32(gYaw, &gYaw32);
|
|
gMcuTaskImuBlueToothSendBuf[1] = (uint8_t)((gAx32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[2] = (uint8_t)((gAx32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[3] = (uint8_t)((gAx32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[4] = (uint8_t)(gAx32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[5] = (uint8_t)((gAy32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[6] = (uint8_t)((gAy32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[7] = (uint8_t)((gAy32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[8] = (uint8_t)(gAy32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[9] = (uint8_t)((gAz32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[10] = (uint8_t)((gAz32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[11] = (uint8_t)((gAz32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[12] = (uint8_t)(gAz32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[13] = (uint8_t)((gWx32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[14] = (uint8_t)((gWx32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[15] = (uint8_t)((gWx32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[16] = (uint8_t)(gWx32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[17] = (uint8_t)((gWy32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[18] = (uint8_t)((gWy32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[19] = (uint8_t)((gWy32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[20] = (uint8_t)(gWy32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[21] = (uint8_t)((gWz32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[22] = (uint8_t)((gWz32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[23] = (uint8_t)((gWz32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[24] = (uint8_t)(gWz32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[25] = (uint8_t)((gRoll32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[26] = (uint8_t)((gRoll32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[27] = (uint8_t)((gRoll32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[28] = (uint8_t)(gRoll32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[29] = (uint8_t)((gPitch32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[30] = (uint8_t)((gPitch32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[31] = (uint8_t)((gPitch32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[32] = (uint8_t)(gPitch32 & 0x000000ff);
|
|
gMcuTaskImuBlueToothSendBuf[33] = (uint8_t)((gYaw32 & 0xff000000) >> 24);
|
|
gMcuTaskImuBlueToothSendBuf[34] = (uint8_t)((gYaw32 & 0x00ff0000) >> 16);
|
|
gMcuTaskImuBlueToothSendBuf[35] = (uint8_t)((gYaw32 & 0x0000ff00) >> 8);
|
|
gMcuTaskImuBlueToothSendBuf[36] = (uint8_t)(gYaw32 & 0x000000ff);
|
|
McuTaskFlashWriteOpt("Imu 4G Send Data\r\n", strlen("Imu 4G Send Data\r\n"));
|
|
McuDrv4GSendMessage(MCU_TASK_IMU_SEND_MESSAGE_ID, &gMcuTaskImu4gSerial, gMcuTaskImuBlueToothSendBuf, sizeof(gMcuTaskImuBlueToothSendBuf));
|
|
gMcuTaskImu4gSerial++;
|
|
}
|
|
|
|
}
|
|
/* 保存数据到Flash */
|
|
McuTaskFlashWriteImu(gAx, gAy, gAz, gWx, gWy, gWz, gRoll, gPitch, gYaw);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/* 蓝牙请求数据 */
|
|
void McuTaskImuBlueToothAsk(void)
|
|
{
|
|
gMcuTaskImuBlueToothSendFlag = 1;
|
|
}
|
|
|
|
/* 北斗请求数据 */
|
|
void McuTaskImuBeiDouAsk(void)
|
|
{
|
|
gMcuTaskImuBeiDouSendFlag = 1;
|
|
}
|
|
|
|
/* 获取数据请求状态 */
|
|
int McuTaskImuAskStatus(void)
|
|
{
|
|
return ((gMcuTaskImuBlueToothSendFlag * 2) | gMcuTaskImuBeiDouSendFlag);
|
|
}
|