1、实现了向惯导设置参数；
2、简化代码：抽象出构造和发送命令函数（constructAndSendInstruction）
This commit is contained in:
tangchao0503
21
register.h
21
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,
|
||||
|
||||
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();
|
||||
|
||||
|
||||
@ -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 eulerAngle();
|
||||
int setContent(RETURN_CONTENT_ENUM content);
|
||||
int clearContent(RETURN_CONTENT_ENUM content);
|
||||
//设置回传内容
|
||||
int setContent(RETURN_CONTENT_STRUCT content);
|
||||
int clearContent(RETURN_CONTENT_STRUCT 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;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user