133 lines
3.3 KiB
C
133 lines
3.3 KiB
C
#include <wit_c_sdk.h>
|
|
#include <app.h>
|
|
#include "cmsis_os2.h"
|
|
#include "sensor_i2c_port.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;
|
|
|
|
static void CopeSensorData(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 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:h\\r\\n help.\r\n");
|
|
printf("******************************************************************************\r\n");
|
|
}
|
|
static void AutoScanSensor(void)
|
|
{
|
|
int i, iRetry;
|
|
|
|
for(i = 0; i < 0x7F; i++)
|
|
{
|
|
WitInit(WIT_PROTOCOL_I2C, i);
|
|
iRetry = 2;
|
|
do
|
|
{
|
|
s_cDataUpdate = 0;
|
|
WitReadReg(AX, 3);
|
|
osDelay(5);
|
|
if(s_cDataUpdate != 0)
|
|
{
|
|
printf("find %02X addr sensor\r\n", i);
|
|
ShowHelp();
|
|
return ;
|
|
}
|
|
iRetry--;
|
|
}while(iRetry);
|
|
}
|
|
printf("can not find sensor\r\n");
|
|
printf("please check your connection\r\n");
|
|
}
|
|
|
|
static void Delayms(uint16_t ucMs)
|
|
{
|
|
osDelay(pdMS_TO_TICKS(ucMs));
|
|
}
|
|
|
|
float fAcc[3], fGyro[3], fAngle[3];
|
|
void sensor_task(void)
|
|
{
|
|
int i;
|
|
WitInit(WIT_PROTOCOL_I2C, 0x50);
|
|
WitI2cFuncRegister(IICwriteBytes, IICreadBytes);
|
|
WitRegisterCallBack(CopeSensorData);
|
|
WitDelayMsRegister(Delayms);
|
|
AutoScanSensor();
|
|
while (1) {
|
|
WitReadReg(AX, 12);
|
|
vTaskDelay(pdMS_TO_TICKS(500));
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
} |