实现了任工需要设置的所有参数

witmotiondll.cpp

@@ -1,42 +1,674 @@
 #include "witmotiondll.h"
 
-
 WitmotionDll::WitmotionDll(SerialPortBase * serialPort)
 {
     m_SerialPort = serialPort;
 }
 
-int WitmotionDll::algorithm(int algorithm)
+int WitmotionDll::unlockInstruction()
 {
+    using std::this_thread::sleep_for;
+
+    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::installationDirection(int direction)
+int WitmotionDll::saveInstruction()
 {
+    using std::this_thread::sleep_for;
+
+    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::instructStart()
+int WitmotionDll::algorithm(ALGROITHM_ENUM algorithm)
 {
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::installationOrientation(ORIENT_ENUM orient)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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;
+}
+
+int WitmotionDll::instructStart(POWONSEND_ENUM command)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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;
+}
+
+int WitmotionDll::exitCalibration()
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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;
 }
 
 int WitmotionDll::magneticCalibration()
 {
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
     return 0;
 }
 
-int WitmotionDll::heightCalibration()
+int WitmotionDll::setHeightToZero()
 {
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
     return 0;
 }
 
-int WitmotionDll::zAxisAngleCalibration()
+int WitmotionDll::setZAxisAngleToZero()
 {
+    //需要读取参数，确保处于算法ALGROITHM6下
+
+
+
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
     return 0;
 }
 
 int WitmotionDll::setAngleReference()
 {
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    unBuffer[1]=0xAA;
+    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();
+
+    return 0;
+}
+
+void WitmotionDll::recordData()
+{
+    m_SerialPort->ReadData();
+}
+
+int WitmotionDll::setTimeZone(TIMEZONE_ENUM timeZone)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    unBuffer[1]=0xAA;
+    unBuffer[2]=TIMEZONE & 0xFF;
+    unBuffer[3]=timeZone & 0xff;
+    unBuffer[4]=timeZone >> 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;
+}
+
+int WitmotionDll::latitudeLongitude()
+{
+    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];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::SetReturnRate(RRATE_ENUM returnRate)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::SetDeviceAddress(int deviceAddress)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD0Model(MODEL_D0_D2_D3_ENUM model)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD1Model(MODEL_D1_ENUM model)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD2Model(MODEL_D0_D2_D3_ENUM model)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD3Model(MODEL_D0_D2_D3_ENUM model)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD0HighLevelPulseWidth(int PWMH)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD1HighLevelPulseWidth(int PWMH)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD2HighLevelPulseWidth(int PWMH)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD3HighLevelPulseWidth(int PWMH)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD0Period(int period)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD1Period(int period)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD2Period(int period)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
+    return 0;
+}
+
+int WitmotionDll::setD3Period(int period)
+{
+    using std::this_thread::sleep_for;
+
+    unlockInstruction();
+
+    uint8_t unBuffer[8];
+    unBuffer[0]=0xFF;
+    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();
+
     return 0;
 }