1、实现了向惯导设置参数；

2、简化代码：抽象出构造和发送命令函数（constructAndSendInstruction）

register.h

@@ -127,6 +127,7 @@ enum TIMEZONE_ENUM {
 #define ClrBit(VAR, Place)    (VAR &= ((1 << Place) ^ 255))
 
 enum RETURN_CONTENT_ENUM {
+    //低位
     TIME = 0,
     ACCELERATION,
     ANGULAR_VELOCITY,
@@ -136,11 +137,31 @@ enum RETURN_CONTENT_ENUM {
     ATMOSPHERIC_PRESSURE_ALTITUDE,
     LATITUDE_LONGITUDE,
 
+    //高位
     GROUND_VELOCITY,
     QUATERNION,//四元数
     SATELLITE_ACCURACY
 };
 
+struct RETURN_CONTENT_STRUCT
+{
+    //低位
+    bool time = false;
+    bool acceleration = false;
+    bool angular_velocity = false;
+    bool euler_angle = false;
+    bool magnetic_field = false;
+    bool port_status = false;
+    bool atmospheric_pressure_altitude = false;
+    bool latitude_longitude = false;
+
+    //高位
+    bool ground_velocity = false;
+    bool quaternion = false;
+    bool satellite_accuracy = false;
+};
+
+
 /* BAUD */
 enum BAUD_ENUM {
     WIT_BAUD_2400 = 0,

witmotiondll.cpp

@@ -1,85 +1,82 @@
 #include "witmotiondll.h"
 
+//当没有注册 延时函数（m_delayFunction）时，运行会停止在调用延时函数（m_delayFunction）处；
+//此函数就是为了解决上面说的问题
+void delay_tc(uint32_t millisecond)
+{
+    ;
+}
+
+void printf_tc(const char* text)
+{
+    ;
+}
+
 WitmotionDll::WitmotionDll(SerialPortBase * serialPort)
 {
     m_SerialPort = serialPort;
+
+    m_delayFunction = delay_tc;
+
+    m_witPrintf = printf_tc;
+}
+
+int WitmotionDll::delayMsRegister(delay delayFunction)
+{
+    m_delayFunction = delayFunction;
+
+    return 0;
+}
+
+int WitmotionDll::printfRegister(witPrintf printfFunction)
+{
+    m_witPrintf = printfFunction;
+
+    return 0;
+}
+
+void WitmotionDll::setDelayTimeMs(uint32_t millisecond)
+{
+    TIME_TO_SLEEP = millisecond;
+}
+
+int WitmotionDll::constructAndSendInstruction(int registerAddress, int registerContent)
+{
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    unBuffer[1]=0xAA;
+    unBuffer[2]=registerAddress & 0xFF;
+    unBuffer[3]=registerContent & 0xff;
+    unBuffer[4]=registerContent >> 8;
+
+    int num = m_SerialPort->SendData1((char *)unBuffer,5);
+    m_delayFunction(TIME_TO_SLEEP);
+
+    return num;
 }
 
 int WitmotionDll::unlockInstruction()
 {
-    using std::this_thread::sleep_for;
+    int numOfSend = constructAndSendInstruction(KEY, KEY_UNLOCK);
-
+    m_witPrintf("WitmotionDll::unlockInstruction: \n");
-    uint8_t unBuffer[8];
-    unBuffer[0]=0xFF;
-    unBuffer[1]=0xAA;
-    unBuffer[2]=KEY & 0xFF;
-    unBuffer[3]=KEY_UNLOCK & 0xff;
-    unBuffer[4]=KEY_UNLOCK >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    std::cout<<"WitmotionDll::unlockInstruction: "<< num <<std::endl;
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     return 0;
 }
 
 int WitmotionDll::saveInstruction()
 {
-    using std::this_thread::sleep_for;
+    int numOfSend = constructAndSendInstruction(SAVE, SAVE_PARAM);
-
+    m_witPrintf("WitmotionDll::saveInstruction: \n");
-    uint8_t unBuffer[8];
-    unBuffer[0]=0xFF;
-    unBuffer[1]=0xAA;
-    unBuffer[2]=SAVE & 0xFF;
-    unBuffer[3]=SAVE_PARAM & 0xff;
-    unBuffer[4]=SAVE_PARAM >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    std::cout<<"WitmotionDll::saveInstruction: "<< num <<std::endl;
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     return 0;
 }
 
 int WitmotionDll::algorithm(ALGROITHM_ENUM algorithm)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(AXIS6, algorithm);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::algorithm: \n");
-    unBuffer[1]=0xAA;
-
-    switch (algorithm)
-    {
-    case ALGROITHM6:
-    {
-        unBuffer[2]=AXIS6 & 0xFF;
-        unBuffer[3]=ALGROITHM6 & 0xff;
-        unBuffer[4]=ALGROITHM6 >> 8;
-
-        break;
-    }
-    case ALGROITHM9:
-    {
-        unBuffer[2]=AXIS6 & 0xFF;
-        unBuffer[3]=ALGROITHM9 & 0xff;
-        unBuffer[4]=ALGROITHM9 >> 8;
-
-        break;
-    }
-    default:
-        break;
-    }
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -88,22 +85,10 @@ int WitmotionDll::algorithm(ALGROITHM_ENUM algorithm)
 
 int WitmotionDll::installationOrientation(ORIENT_ENUM orient)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(ORIENT, orient);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::installationOrientation:\n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=ORIENT & 0xFF;
-    unBuffer[3]=orient & 0xff;
-    unBuffer[4]=orient >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    std::cout<<"WitmotionDll::installationDirection: "<< num <<std::endl;
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
     return 0;
@@ -111,22 +96,10 @@ int WitmotionDll::installationOrientation(ORIENT_ENUM orient)
 
 int WitmotionDll::instructStart(POWONSEND_ENUM command)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(POWONSEND, command);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::instructStart: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=POWONSEND & 0xFF;
-    unBuffer[3]=command & 0xff;
-    unBuffer[4]=command >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    std::cout<<"WitmotionDll::installationDirection: "<< num <<std::endl;
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
     return 0;
@@ -134,20 +107,10 @@ int WitmotionDll::instructStart(POWONSEND_ENUM command)
 
 int WitmotionDll::exitCalibration()
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(CALSW, EXITCAL);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::exitCalibration: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=CALSW & 0xFF;
-    unBuffer[3]=EXITCAL & 0xff;
-    unBuffer[4]=EXITCAL >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
     return 0;
@@ -155,20 +118,10 @@ int WitmotionDll::exitCalibration()
 
 int WitmotionDll::magneticCalibration()
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(CALSW, CALMAG);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::magneticCalibration: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=CALSW & 0xFF;
-    unBuffer[3]=CALMAG & 0xff;
-    unBuffer[4]=CALMAG >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -177,20 +130,10 @@ int WitmotionDll::magneticCalibration()
 
 int WitmotionDll::setHeightToZero()
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(CALSW, CALALTITUDE);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setHeightToZero: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=CALSW & 0xFF;
-    unBuffer[3]=CALALTITUDE & 0xff;
-    unBuffer[4]=CALALTITUDE >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -200,23 +143,10 @@ int WitmotionDll::setHeightToZero()
 int WitmotionDll::setZAxisAngleToZero()
 {
     //需要读取参数，确保处于算法ALGROITHM6下
-
-
-
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(CALSW, CALANGLEZ);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setZAxisAngleToZero: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=CALSW & 0xFF;
-    unBuffer[3]=CALANGLEZ & 0xff;
-    unBuffer[4]=CALANGLEZ >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -225,20 +155,11 @@ int WitmotionDll::setZAxisAngleToZero()
 
 int WitmotionDll::setAngleReference()
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    uint16_t registerContent = 0x08;//询问技术支持才得到的命令
-    unBuffer[0]=0xFF;
+    int numOfSend = constructAndSendInstruction(CALSW, registerContent);
-    unBuffer[1]=0xAA;
+    m_witPrintf("WitmotionDll::setAngleReference: \n");
-    unBuffer[2]=CALSW & 0xFF;
-    unBuffer[3]=0x08;
-    unBuffer[4]=0x00;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -252,113 +173,64 @@ void WitmotionDll::recordData()
 
 int WitmotionDll::setTimeZone(TIMEZONE_ENUM timeZone)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(TIMEZONE, timeZone);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setTimeZone: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=TIMEZONE & 0xFF;
-    unBuffer[3]=timeZone & 0xff;
-    unBuffer[4]=timeZone >> 8;
 
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
+    saveInstruction();
 
-    std::cout<<"WitmotionDll::installationDirection: "<< num <<std::endl;
+    return 0;
+}
 
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
+int WitmotionDll::setContent(RETURN_CONTENT_STRUCT content)
+{
+    unlockInstruction();
+
+    uint16_t registerContent = 0;
+
+    if(content.time)
+        SetBit(registerContent, TIME);
+    if(content.acceleration)
+        SetBit(registerContent, ACCELERATION);
+    if(content.angular_velocity)
+        SetBit(registerContent, ANGULAR_VELOCITY);
+    if(content.euler_angle)
+        SetBit(registerContent, EULER_ANGLE);
+    if(content.magnetic_field)
+        SetBit(registerContent, MAGNETIC_FIELD);
+    if(content.port_status)
+        SetBit(registerContent, PORT_STATUS);
+    if(content.atmospheric_pressure_altitude)
+        SetBit(registerContent, ATMOSPHERIC_PRESSURE_ALTITUDE);
+    if(content.latitude_longitude)
+        SetBit(registerContent, LATITUDE_LONGITUDE);
+
+    if(content.ground_velocity)
+        SetBit(registerContent, GROUND_VELOCITY);
+    if(content.quaternion)
+        SetBit(registerContent, QUATERNION);
+    if(content.satellite_accuracy)
+        SetBit(registerContent, SATELLITE_ACCURACY);
+
+    int numOfSend = constructAndSendInstruction(RSW, registerContent);
+    m_witPrintf("WitmotionDll::setContent: \n");
 
     saveInstruction();
     return 0;
 }
 
-int WitmotionDll::latitudeLongitude()
+int WitmotionDll::clearContent(RETURN_CONTENT_STRUCT content)
-{
-    using std::this_thread::sleep_for;
-
-    unlockInstruction();
-
-    uint8_t unBuffer[8];
-    unBuffer[0]=0xFF;
-    unBuffer[1]=0xAA;
-    unBuffer[2]=RSW & 0xFF;
-
-    //设置回传内容
-    uint8_t RSWL = 0;
-    uint8_t RSWH = 0;
-
-    SetBit(RSWL, LATITUDE_LONGITUDE);
-
-    unBuffer[3] = RSWL;
-    unBuffer[4] = RSWH;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
-
-    saveInstruction();
-
-    exitCalibration();
-    return 0;
-}
-
-int WitmotionDll::eulerAngle()
-{
-    using std::this_thread::sleep_for;
-
-    unlockInstruction();
-
-    uint8_t unBuffer[8];
-    unBuffer[0]=0xFF;
-    unBuffer[1]=0xAA;
-    unBuffer[2]=RSW & 0xFF;
-
-    //设置回传内容
-    uint8_t RSWL = 0;
-    uint8_t RSWH = 0;
-
-    SetBit(RSWL, EULER_ANGLE);
-
-    unBuffer[3] = RSWL;
-    unBuffer[4] = RSWH;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
-
-    saveInstruction();
-
-    exitCalibration();
-    return 0;
-}
-
-int WitmotionDll::setContent(RETURN_CONTENT_ENUM content)
-{
-    return 0;
-}
-
-int WitmotionDll::clearContent(RETURN_CONTENT_ENUM content)
 {
     return 0;
 }
 
 int WitmotionDll::SetBaudrate(BAUD_ENUM baudrate)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(BAUD, baudrate);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::SetBaudrate: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=BAUD & 0xFF;
-    unBuffer[3]=baudrate & 0xff;
-    unBuffer[4]=baudrate >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -367,20 +239,10 @@ int WitmotionDll::SetBaudrate(BAUD_ENUM baudrate)
 
 int WitmotionDll::SetReturnRate(RRATE_ENUM returnRate)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(RRATE, returnRate);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::SetReturnRate: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=RRATE & 0xFF;
-    unBuffer[3]=returnRate & 0xff;
-    unBuffer[4]=returnRate >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -389,20 +251,10 @@ int WitmotionDll::SetReturnRate(RRATE_ENUM returnRate)
 
 int WitmotionDll::SetDeviceAddress(int deviceAddress)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(IICADDR, deviceAddress);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::SetDeviceAddress: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=IICADDR & 0xFF;
-    unBuffer[3]=deviceAddress & 0xff;
-    unBuffer[4]=deviceAddress >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -411,20 +263,10 @@ int WitmotionDll::SetDeviceAddress(int deviceAddress)
 
 int WitmotionDll::setD0Model(MODEL_D0_D2_D3_ENUM model)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D0MODE, model);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD0Model: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D0MODE & 0xFF;
-    unBuffer[3]=model & 0xff;
-    unBuffer[4]=model >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -433,20 +275,10 @@ int WitmotionDll::setD0Model(MODEL_D0_D2_D3_ENUM model)
 
 int WitmotionDll::setD1Model(MODEL_D1_ENUM model)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D1MODE, model);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD1Model: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D1MODE & 0xFF;
-    unBuffer[3]=model & 0xff;
-    unBuffer[4]=model >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -455,20 +287,10 @@ int WitmotionDll::setD1Model(MODEL_D1_ENUM model)
 
 int WitmotionDll::setD2Model(MODEL_D0_D2_D3_ENUM model)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D2MODE, model);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD2Model: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D2MODE & 0xFF;
-    unBuffer[3]=model & 0xff;
-    unBuffer[4]=model >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -477,20 +299,10 @@ int WitmotionDll::setD2Model(MODEL_D0_D2_D3_ENUM model)
 
 int WitmotionDll::setD3Model(MODEL_D0_D2_D3_ENUM model)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D3MODE, model);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD3Model: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D3MODE & 0xFF;
-    unBuffer[3]=model & 0xff;
-    unBuffer[4]=model >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -499,20 +311,10 @@ int WitmotionDll::setD3Model(MODEL_D0_D2_D3_ENUM model)
 
 int WitmotionDll::setD0HighLevelPulseWidth(int PWMH)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D0PWMH, PWMH);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD0HighLevelPulseWidth: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D0PWMH & 0xFF;
-    unBuffer[3]=PWMH & 0xff;
-    unBuffer[4]=PWMH >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -521,20 +323,10 @@ int WitmotionDll::setD0HighLevelPulseWidth(int PWMH)
 
 int WitmotionDll::setD1HighLevelPulseWidth(int PWMH)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D1PWMH, PWMH);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD1HighLevelPulseWidth: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D1PWMH & 0xFF;
-    unBuffer[3]=PWMH & 0xff;
-    unBuffer[4]=PWMH >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -543,20 +335,10 @@ int WitmotionDll::setD1HighLevelPulseWidth(int PWMH)
 
 int WitmotionDll::setD2HighLevelPulseWidth(int PWMH)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D2PWMH, PWMH);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD2HighLevelPulseWidth: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D2PWMH & 0xFF;
-    unBuffer[3]=PWMH & 0xff;
-    unBuffer[4]=PWMH >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -565,20 +347,10 @@ int WitmotionDll::setD2HighLevelPulseWidth(int PWMH)
 
 int WitmotionDll::setD3HighLevelPulseWidth(int PWMH)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D3PWMH, PWMH);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD3HighLevelPulseWidth: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D3PWMH & 0xFF;
-    unBuffer[3]=PWMH & 0xff;
-    unBuffer[4]=PWMH >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -587,20 +359,10 @@ int WitmotionDll::setD3HighLevelPulseWidth(int PWMH)
 
 int WitmotionDll::setD0Period(int period)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D0PWMT, period);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD0Period: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D0PWMT & 0xFF;
-    unBuffer[3]=period & 0xff;
-    unBuffer[4]=period >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -609,20 +371,10 @@ int WitmotionDll::setD0Period(int period)
 
 int WitmotionDll::setD1Period(int period)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D1PWMT, period);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD1Period: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D1PWMT & 0xFF;
-    unBuffer[3]=period & 0xff;
-    unBuffer[4]=period >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -631,20 +383,10 @@ int WitmotionDll::setD1Period(int period)
 
 int WitmotionDll::setD2Period(int period)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D2PWMT, period);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD2Period: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D2PWMT & 0xFF;
-    unBuffer[3]=period & 0xff;
-    unBuffer[4]=period >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 
@@ -653,20 +395,10 @@ int WitmotionDll::setD2Period(int period)
 
 int WitmotionDll::setD3Period(int period)
 {
-    using std::this_thread::sleep_for;
-
     unlockInstruction();
 
-    uint8_t unBuffer[8];
+    int numOfSend = constructAndSendInstruction(D3PWMT, period);
-    unBuffer[0]=0xFF;
+    m_witPrintf("WitmotionDll::setD3Period: \n");
-    unBuffer[1]=0xAA;
-    unBuffer[2]=D3PWMT & 0xFF;
-    unBuffer[3]=period & 0xff;
-    unBuffer[4]=period >> 8;
-
-    int num = m_SerialPort->SendData1((char *)unBuffer,5);
-
-    sleep_for(std::chrono::milliseconds(TIME_TO_SLEEP));
 
     saveInstruction();
 

witmotiondll.h

@@ -4,18 +4,27 @@
 #include "witmotiondll_global.h"
 
 #include <iostream>
-#include <string.h>
+#include <string>
 #include <thread>
 
 #include "serialportbase.h"
 #include "register.h"
 
+typedef void (*delay)(uint32_t millisecond);
+typedef void (*witPrintf)(const char* text);
+
 class WITMOTIONDLLSHARED_EXPORT WitmotionDll
 {
 
 public:
     WitmotionDll(SerialPortBase * serialPort);
 
+    int delayMsRegister(delay delayFunction);
+    int printfRegister(witPrintf printfFunction);
+    void setDelayTimeMs(uint32_t millisecond);
+
+    int constructAndSendInstruction(int registerAddress, int registerContent);
+
     int unlockInstruction();
     int saveInstruction();
 
@@ -24,7 +33,7 @@ public:
     //系统
     int algorithm(ALGROITHM_ENUM algorithm);
     int installationOrientation(ORIENT_ENUM orient);
-    int instructStart(POWONSEND_ENUM command);//是否上电输出数据
+    int instructStart(POWONSEND_ENUM command);//是否上电输出数据，可以防止鼠标乱跳
 
 
     //校准：完成后，需要退出校准模式（ 调用exitCalibration）
@@ -33,17 +42,15 @@ public:
     int magneticCalibration();
     int setHeightToZero();//需要JY901B带有高度数据输出的模块才能进行高度校准，此为相对高度，短时间内有效
     int setZAxisAngleToZero();//Z轴角度归零，只在切换成 6 轴算法下才能成功置零;
-    int setAngleReference();//问的技术支持，文档没有，不建议使用，设置后只能在静态下运动，即原点旋转单轴，其他方式旋转出来会有比较大的误差
+    int setAngleReference();//询问技术支持，文档没有，不建议使用，设置后只能在静态下运动，即原点旋转单轴，其他方式旋转出来会有比较大的误差
 
 
     //范围
     int setTimeZone(TIMEZONE_ENUM timeZone);
 
-    //设置回传内容：操作位？？？？？？？？？？？？？？？？？？？？？？
+    //设置回传内容
-    int latitudeLongitude();
+    int setContent(RETURN_CONTENT_STRUCT content);
-    int eulerAngle();
+    int clearContent(RETURN_CONTENT_STRUCT content);
-    int setContent(RETURN_CONTENT_ENUM content);
-    int clearContent(RETURN_CONTENT_ENUM content);
 
     //通讯
     int SetBaudrate(BAUD_ENUM baudrate);
@@ -66,10 +73,13 @@ public:
     int setD2Period(int period);
     int setD3Period(int period);
 
-
 private:
     SerialPortBase * m_SerialPort;
-    int TIME_TO_SLEEP = 500;//毫秒
+    uint32_t TIME_TO_SLEEP = 500;//毫秒
+
+    delay m_delayFunction;
+    witPrintf m_witPrintf;
+
 
 };