Hello everyone,
This is my first post so please tell me if and how I should present my problem better if I m not explaining it not comprehensively enough (also english is not my first language).
So I am currently using the WT901B from Witmotion with my ESP32. I am programming with the Arduino IDE (I am not completely new to it but I haven't used multiple serial ports before) and while I can read all the values from the sensor I couldn't figure out with web browsing and trying out different things how to calibrate the sensor for proper use.
There was this post on this forum (Help calibrating WitMotion inclinometer through TTL with Mega2560) where the user sends data in bytes to the sensor according to the documentation (WIT Standard Communication Protocol - WITMOTION SDK) to unlock the calibration mode, calibrate and save the configuration. So I downloaded the example code (WitStandardProtocol_JY901/Arduino/Arduino_sdk/examples/wit_c_sdk_normal at main · WITMOTION/WitStandardProtocol_JY901 · GitHub) and the libraries (WitStandardProtocol_JY901/Arduino/Arduino_sdk at main · WITMOTION/WitStandardProtocol_JY901 · GitHub) that came with it.
I instantiated a hardware serial with RX (GPIO4) and TX (GPIO2) and tried the calibration code from the before mentioned Forum post and it didn't work. I also looked into the "WitStartAccCali(void)" and "WitStopAccCali(void)" functions provided by the "wit_c_sdk.h" file but those don't work and as far as I understand essentially do the same as the code from the forum post just without the additional data ({0xFF, 0xF0, 0xF0, 0xF0, 0xF0}) OP sent to the sensor to unlock it in the first place (that was the solution he was missing).
Here is the code I have tried (again if I am being unclear just tell me since I don't know if I am even explaining my problem well enough):
#include <REG.h>
#include <wit_c_sdk.h>
#include <HardwareSerial.h>
/*
On SerialPort (UART1)
WT901B ESP32
VCC <---> VIN (5V)
RX <---> (GPIO2) TX1
TX <---> (GPIO4) RX1
GND <---> GND
On Serial2 (UART2)
WT901B ESP32
VCC <---> VIN (5V)
RX <---> (GPI17) TX2
TX <---> (GPI16) RX2
GND <---> GND
*/
#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 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};
//UART1
HardwareSerial SerialPort(1);
void setup() {
Serial.begin(115200);
WitInit(WIT_PROTOCOL_NORMAL, 0x50);
WitSerialWriteRegister(SensorUartSend);
WitRegisterCallBack(SensorDataUpdata);
WitDelayMsRegister(Delayms);
//Contains SerialPort.begin(c_uiBaud[i],SERIAL_8N1,4,2);
AutoScanSensor();
//Calibration Sequence
// Special unlock/enable thing (not documented anywhere!)
byte data0[] = {0xFF, 0xF0, 0xF0, 0xF0, 0xF0};
SerialPort.write(data0, 5);
delay(2);
//Unlock Configuration
byte data1[] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
SerialPort.write(data1, 5);
delay(2);
// Enable calibration mode
byte data2[] = {0xFF, 0xAA, 0x01, 0x01, 0x00};
SerialPort.write(data2, 5);
delay(5000); // Wait for calibration to finish
// Save configuration
byte data3[] = {0xFF, 0xAA, 0x00, 0x00, 0x00};
SerialPort.write(data3, 5);
Serial.print("-------------------------------Program Start-------------------------------\r\n");
Serial.println("");
/*
WitStartAccCali();
delay(5000);
WitStopAccCali();
*/
}
int i;
float fAcc[3], fGyro[3], fAngle[3];
void loop() {
//UART1
while (SerialPort.available())
{
WitSerialDataIn(SerialPort.read());
}
while (Serial.available())
{
CopeCmdData(Serial.read());
}
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)
{
Serial.print("acc:");
Serial.print(fAcc[0], 3);
Serial.print(" ");
Serial.print(fAcc[1], 3);
Serial.print(" ");
Serial.print(fAcc[2], 3);
Serial.print("\r\n");
s_cDataUpdate &= ~ACC_UPDATE;
}
if(s_cDataUpdate & GYRO_UPDATE)
{/*
Serial.print("gyro:");
Serial.print(fGyro[0], 1);
Serial.print(" ");
Serial.print(fGyro[1], 1);
Serial.print(" ");
Serial.print(fGyro[2], 1);
Serial.print("\r\n");
*/
s_cDataUpdate &= ~GYRO_UPDATE;
}
if(s_cDataUpdate & ANGLE_UPDATE)
{
Serial.print("angle:");
Serial.print(fAngle[0], 3);
Serial.print(" ");
Serial.print(fAngle[1], 3);
Serial.print(" ");
Serial.print(fAngle[2], 3);
Serial.print("\r\n");
s_cDataUpdate &= ~ANGLE_UPDATE;
}
if(s_cDataUpdate & MAG_UPDATE)
{/*
Serial.print("mag:");
Serial.print(sReg[HX]);
Serial.print(" ");
Serial.print(sReg[HY]);
Serial.print(" ");
Serial.print(sReg[HZ]);
Serial.print("\r\n");
*/
s_cDataUpdate &= ~MAG_UPDATE;
}
s_cDataUpdate = 0;
}
delay(500);
}
//UART2
/*
while (Serial2.available())
{
WitSerialDataIn(Serial2.read());
}
while (Serial.available())
{
CopeCmdData(Serial.read());
}
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)
{
Serial.print("acc:");
Serial.print(fAcc[0], 3);
Serial.print(" ");
Serial.print(fAcc[1], 3);
Serial.print(" ");
Serial.print(fAcc[2], 3);
Serial.print("\r\n");
s_cDataUpdate &= ~ACC_UPDATE;
}
if(s_cDataUpdate & GYRO_UPDATE)
{
Serial.print("gyro:");
Serial.print(fGyro[0], 1);
Serial.print(" ");
Serial.print(fGyro[1], 1);
Serial.print(" ");
Serial.print(fGyro[2], 1);
Serial.print("\r\n");
s_cDataUpdate &= ~GYRO_UPDATE;
}
if(s_cDataUpdate & ANGLE_UPDATE)
{
Serial.print("angle:");
Serial.print(fAngle[0], 3);
Serial.print(" ");
Serial.print(fAngle[1], 3);
Serial.print(" ");
Serial.print(fAngle[2], 3);
Serial.print("\r\n");
s_cDataUpdate &= ~ANGLE_UPDATE;
}
if(s_cDataUpdate & MAG_UPDATE)
{
Serial.print("mag:");
Serial.print(sReg[HX]);
Serial.print(" ");
Serial.print(sReg[HY]);
Serial.print(" ");
Serial.print(sReg[HZ]);
Serial.print("\r\n");
s_cDataUpdate &= ~MAG_UPDATE;
}
s_cDataUpdate = 0;
}
delay(500);
}
*/
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 CmdProcess(void)
{
switch(s_cCmd)
{
case 'a': if(WitStartAccCali() != WIT_HAL_OK) Serial.print("\r\nSet AccCali Error\r\n");
break;
case 'm': if(WitStartMagCali() != WIT_HAL_OK) Serial.print("\r\nSet MagCali Error\r\n");
break;
case 'e': if(WitStopMagCali() != WIT_HAL_OK) Serial.print("\r\nSet MagCali Error\r\n");
break;
case 'u': if(WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK) Serial.print("\r\nSet Bandwidth Error\r\n");
break;
case 'U': if(WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) Serial.print("\r\nSet Bandwidth Error\r\n");
break;
case 'B': if(WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) Serial.print("\r\nSet Baud Error\r\n");
else
{ //UART1
SerialPort.begin(c_uiBaud[WIT_BAUD_115200]);
//UART2
//Serial2.begin(c_uiBaud[WIT_BAUD_115200]);
Serial.print(" 115200 Baud rate modified successfully\r\n");
}
break;
case 'b': if(WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK) Serial.print("\r\nSet Baud Error\r\n");
else
{ //UART1
SerialPort.begin(c_uiBaud[WIT_BAUD_9600]);
//UART2
//Serial2.begin(c_uiBaud[WIT_BAUD_9600]);
Serial.print(" 9600 Baud rate modified successfully\r\n");
}
break;
case 'r': if(WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK) Serial.print("\r\nSet Baud Error\r\n");
else Serial.print("\r\nSet Baud Success\r\n");
break;
case 'R': if(WitSetOutputRate(RRATE_10HZ) != WIT_HAL_OK) Serial.print("\r\nSet Baud Error\r\n");
else Serial.print("\r\nSet Baud Success\r\n");
break;
case 'C': if(WitSetContent(RSW_ACC|RSW_GYRO|RSW_ANGLE|RSW_MAG) != WIT_HAL_OK) Serial.print("\r\nSet RSW Error\r\n");
break;
case 'c': if(WitSetContent(RSW_ACC) != WIT_HAL_OK) Serial.print("\r\nSet RSW Error\r\n");
break;
case 'h':
break;
default :break;
}
s_cCmd = 0xff;
}
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize)
{
//UART1
SerialPort.write(p_data, uiSize);
SerialPort.flush();
//UART2
// Serial2.write(p_data, uiSize);
// Serial2.flush();
}
static void Delayms(uint16_t ucMs)
{
delay(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++)
{
//UART1
SerialPort.begin(c_uiBaud[i],SERIAL_8N1,4,2);
SerialPort.flush();
//UART2
// Serial2.begin(c_uiBaud[i]);
// Serial2.flush();
iRetry = 2;
s_cDataUpdate = 0;
do
{
WitReadReg(AX, 3);
delay(200);
//UART1
while (SerialPort.available())
//UART2
// while (Serial2.available())
{
//UART1
WitSerialDataIn(SerialPort.read());
//UART2
// WitSerialDataIn(Serial2.read());
}
if(s_cDataUpdate != 0)
{
Serial.print(c_uiBaud[i]);
Serial.print(" baud find sensor\r\n\r\n");
return ;
}
iRetry--;
}while(iRetry);
}
Serial.print("can not find sensor\r\n");
Serial.print("please check your connection\r\n");
}
The serial.print output from this code is just the "-------------------------------Program Start-------------------------------" and then triplets of angle and acceleration data. The problem is, that the sensor is perfectly horizontal the whole time so theoretically if the sensor was calibrated sucessfully the acceleration triplets should be around 0, 0, 1 which they aren't (it's more like -0.7, 0 ,0.7). So I don't really know where the problem is but I sure it's not a problem with my ESP32 since I tested the same program on a ATMEGA2560 and same thing there. One thing I noticed that I didn't understand why it was happening was that the "Serial.print("-------------------------------Program Start-------------------------------" )" doesn't output anything if I put it before the calibration sequence.
Would appreciate any feedback and a general thank you to all the members of this forum which helped me solve problems I had in the past with your posts and answers 
.
.