1、实现了向惯导设置参数；
2、简化代码：抽象出构造和发送命令函数（constructAndSendInstruction）
This commit is contained in:
tangchao0503
546
witmotiondll.cpp
546
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;
|
||||
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(KEY, KEY_UNLOCK);
|
||||
m_witPrintf("WitmotionDll::unlockInstruction: \n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(SAVE, SAVE_PARAM);
|
||||
m_witPrintf("WitmotionDll::saveInstruction: \n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(AXIS6, algorithm);
|
||||
m_witPrintf("WitmotionDll::algorithm: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(ORIENT, orient);
|
||||
m_witPrintf("WitmotionDll::installationOrientation:\n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(POWONSEND, command);
|
||||
m_witPrintf("WitmotionDll::instructStart: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(CALSW, EXITCAL);
|
||||
m_witPrintf("WitmotionDll::exitCalibration: \n");
|
||||
|
||||
saveInstruction();
|
||||
return 0;
|
||||
@ -155,20 +118,10 @@ int WitmotionDll::exitCalibration()
|
||||
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(CALSW, CALMAG);
|
||||
m_witPrintf("WitmotionDll::magneticCalibration: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
@ -177,20 +130,10 @@ int WitmotionDll::magneticCalibration()
|
||||
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(CALSW, CALALTITUDE);
|
||||
m_witPrintf("WitmotionDll::setHeightToZero: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
@ -200,23 +143,10 @@ int WitmotionDll::setHeightToZero()
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(CALSW, CALANGLEZ);
|
||||
m_witPrintf("WitmotionDll::setZAxisAngleToZero: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
@ -225,20 +155,11 @@ int WitmotionDll::setZAxisAngleToZero()
|
||||
|
||||
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));
|
||||
uint16_t registerContent = 0x08;//询问技术支持才得到的命令
|
||||
int numOfSend = constructAndSendInstruction(CALSW, registerContent);
|
||||
m_witPrintf("WitmotionDll::setAngleReference: \n");
|
||||
|
||||
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];
|
||||
unBuffer[0]=0xFF;
|
||||
unBuffer[1]=0xAA;
|
||||
unBuffer[2]=TIMEZONE & 0xFF;
|
||||
unBuffer[3]=timeZone & 0xff;
|
||||
unBuffer[4]=timeZone >> 8;
|
||||
int numOfSend = constructAndSendInstruction(TIMEZONE, timeZone);
|
||||
m_witPrintf("WitmotionDll::setTimeZone: \n");
|
||||
|
||||
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()
|
||||
{
|
||||
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)
|
||||
int WitmotionDll::clearContent(RETURN_CONTENT_STRUCT 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));
|
||||
int numOfSend = constructAndSendInstruction(BAUD, baudrate);
|
||||
m_witPrintf("WitmotionDll::SetBaudrate: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(RRATE, returnRate);
|
||||
m_witPrintf("WitmotionDll::SetReturnRate: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(IICADDR, deviceAddress);
|
||||
m_witPrintf("WitmotionDll::SetDeviceAddress: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D0MODE, model);
|
||||
m_witPrintf("WitmotionDll::setD0Model: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D1MODE, model);
|
||||
m_witPrintf("WitmotionDll::setD1Model: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D2MODE, model);
|
||||
m_witPrintf("WitmotionDll::setD2Model: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D3MODE, model);
|
||||
m_witPrintf("WitmotionDll::setD3Model: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D0PWMH, PWMH);
|
||||
m_witPrintf("WitmotionDll::setD0HighLevelPulseWidth: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D1PWMH, PWMH);
|
||||
m_witPrintf("WitmotionDll::setD1HighLevelPulseWidth: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D2PWMH, PWMH);
|
||||
m_witPrintf("WitmotionDll::setD2HighLevelPulseWidth: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D3PWMH, PWMH);
|
||||
m_witPrintf("WitmotionDll::setD3HighLevelPulseWidth: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D0PWMT, period);
|
||||
m_witPrintf("WitmotionDll::setD0Period: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D1PWMT, period);
|
||||
m_witPrintf("WitmotionDll::setD1Period: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D2PWMT, period);
|
||||
m_witPrintf("WitmotionDll::setD2Period: \n");
|
||||
|
||||
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];
|
||||
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));
|
||||
int numOfSend = constructAndSendInstruction(D3PWMT, period);
|
||||
m_witPrintf("WitmotionDll::setD3Period: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
|
||||
Reference in New Issue
Block a user