I want to use IMU with NUCLEO-f446RE, IMU is HWT901B-232.
I have an error in the function “SensorUartSend” and have not been able to resolve it.
You can get the code by searching “wit standard protocol github”. (Unfortunately, there is no mbed).
I am using OS 5.
====================
#include <REG.h>
#include <wit_c_sdk.h>
#include <mbed.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;
Serial uart_usb(USBTX, USBRX,115200);
Serial imu(PA_3,PA_2,115200);
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);
const uint32_t c_uiBaud[8] = {0,4800, 9600, 19200, 38400, 57600, 115200, 230400};
int main()
{
// put your setup code here, to run once:
//Serial.begin(115200);
WitInit(WIT_PROTOCOL_NORMAL, 0x50);
WitSerialWriteRegister(SensorUartSend);
WitRegisterCallBack(SensorDataUpdata);
WitDelayMsRegister(Delayms);
uart_usb.printf(“\r\n********************** wit-motion normal example ************************\r\n”);
AutoScanSensor();
int i;
float fAcc[3];
float fGyro[3];
float fAngle[3];
while(true)
{
while (imu.readable())
{
WitSerialDataIn(imu.getc());
}
while (uart_usb.readable())
{
//CopeCmdData(uart_usb.getc());
}
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)
{
uart_usb.printf(“Acc:%.2f %.2f %.2f\r\n”,fAcc[0],fAcc[1],fAcc[2]);
s_cDataUpdate &= ~ACC_UPDATE;
}
if(s_cDataUpdate & GYRO_UPDATE)
{
uart_usb.printf(“Gyro:%.2f %.2f %.2f\r\n”,fGyro[0],fGyro[1],fGyro[2]);
s_cDataUpdate &= ~GYRO_UPDATE;
}
if(s_cDataUpdate & ANGLE_UPDATE)
{
uart_usb.printf(“Angle:%.2f %.2f %.2f\r\n”,fAngle[0],fAngle[1],fAngle[2]);
s_cDataUpdate &= ~ANGLE_UPDATE;
}
if(s_cDataUpdate & MAG_UPDATE)
{
//uart_usb.printf(“Mag:%.2f %.2f %.2f\r\n”,sReg[0],sReg[1],sReg[2]);
s_cDataUpdate &= ~MAG_UPDATE;
}
s_cDataUpdate = 0;
}
}
}
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;
}
}
}
static void ShowHelp(void)
{
uart_usb.printf(“\r\n************************ WIT_SDK_DEMO “);
uart_usb.printf(”\r\n HELP \r\n");
uart_usb.printf(“UART SEND:a\r\n Acceleration calibration.\r\n”);
uart_usb.printf(“UART SEND:m\r\n Magnetic field calibration,After calibration send: e\r\n to indicate the end\r\n”);
uart_usb.printf(“UART SEND:U\r\n Bandwidth increase.\r\n”);
uart_usb.printf(“UART SEND:u\r\n Bandwidth reduction.\r\n”);
uart_usb.printf(“UART SEND:B\r\n Baud rate increased to 115200.\r\n”);
uart_usb.printf(“UART SEND:b\r\n Baud rate reduction to 9600.\r\n”);
uart_usb.printf(“UART SEND:R\r\n The return rate increases to 10Hz.\r\n”);
uart_usb.printf(“UART SEND:r\r\n The return rate reduction to 1Hz.\r\n”);
uart_usb.printf(“UART SEND:C\r\n Basic return content: acceleration, angular velocity, angle, magnetic field.\r\n”);
uart_usb.printf(“UART SEND:c\r\n Return content: acceleration.\r\n”);
uart_usb.printf(“UART SEND:h\r\n help.\r\n”);
uart_usb.printf("******************************************************\r\n”);
}
static void CmdProcess(void)
{
switch(s_cCmd)
{
case ‘a’: if(WitStartAccCali() != WIT_HAL_OK) uart_usb.printf(“\r\nSet AccCali Error\r\n”);
break;
case ‘m’: if(WitStartMagCali() != WIT_HAL_OK) uart_usb.printf(“\r\nSet MagCali Error\r\n”);
break;
case ‘e’: if(WitStopMagCali() != WIT_HAL_OK) uart_usb.printf(“\r\nSet MagCali Error\r\n”);
break;
case ‘u’: if(WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK) uart_usb.printf(“\r\nSet Bandwidth Error\r\n”);
break;
case ‘U’: if(WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) uart_usb.printf(“\r\nSet Bandwidth Error\r\n”);
break;
case ‘B’: if(WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) uart_usb.printf(“\r\nSet Baud Error\r\n”);
else
{
imu.baud(115200);
uart_usb.printf(" 115200 Baud rate modified successfully\r\n");
}
break;
case ‘b’: if(WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK) uart_usb.printf(“\r\nSet Baud Error\r\n”);
else
{
imu.baud(9600);
uart_usb.printf(" 9600 Baud rate modified successfully\r\n");
}
break;
case ‘r’: if(WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK) uart_usb.printf(“\r\nSet Baud Error\r\n”);
else uart_usb.printf(“\r\nSet Baud Success\r\n”);
break;
case ‘R’: if(WitSetOutputRate(RRATE_10HZ) != WIT_HAL_OK) uart_usb.printf(“\r\nSet Baud Error\r\n”);
else uart_usb.printf(“\r\nSet Baud Success\r\n”);
break;
case ‘C’: if(WitSetContent(RSW_ACC|RSW_GYRO|RSW_ANGLE|RSW_MAG) != WIT_HAL_OK) uart_usb.printf(“\r\nSet RSW Error\r\n”);
break;
case ‘c’: if(WitSetContent(RSW_ACC) != WIT_HAL_OK) uart_usb.printf(“\r\nSet RSW Error\r\n”);
break;
case ‘h’: ShowHelp();
break;
default :break;
}
s_cCmd = 0xff;
}
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize)
{
imu.write(p_data, uiSize);
imu.sync();
}
static void Delayms(uint16_t ucMs)
{
thread_sleep_until(ucMs);
}
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
int i;
for(i = 0; i < uiRegNum; i++)
{
switch(uiReg)
{
case AZ:
s_cDataUpdate |= ACC_UPDATE;
break;
case GZ:
s_cDataUpdate |= GYRO_UPDATE;
break;
case HZ:
s_cDataUpdate |= MAG_UPDATE;
break;
case Yaw:
s_cDataUpdate |= ANGLE_UPDATE;
break;
default:
s_cDataUpdate |= READ_UPDATE;
break;
}
uiReg++;
}
}
static void AutoScanSensor(void)
{
int i, iRetry;
for(i = 0; i < sizeof(c_uiBaud)/sizeof(c_uiBaud[0]); i++)
{
imu.baud(c_uiBaud[i]);
imu.sync();
iRetry = 2;
s_cDataUpdate = 0;
do
{
WitReadReg(AX, 3);
thread_sleep_until(200);
while (imu.readable())
{
WitSerialDataIn(imu.getc());
}
if(s_cDataUpdate != 0)
{
uart_usb.printf("%.i",c_uiBaud[i]);
uart_usb.printf(" baud find sensor\r\n\r\n");
ShowHelp();
return ;
}
iRetry--;
}while(iRetry);
}
uart_usb.printf("can not find sensor\r\n");
uart_usb.printf("please check your connection\r\n");
}