构建新的cmakelist工程
修改了 delay_tc函数 修返回频率为20hz 修改波特率修改后先重新设置通讯波特率在保存
This commit is contained in:
407
source/witmotiondll.cpp
Normal file
407
source/witmotiondll.cpp
Normal file
@ -0,0 +1,407 @@
|
||||
#include "witmotiondll.h"
|
||||
|
||||
//当没有注册 延时函数（m_delayFunction）时，运行会停止在调用延时函数（m_delayFunction）处；
|
||||
//此函数就是为了解决上面说的问题
|
||||
void delay_default(uint32_t millisecond)
|
||||
{
|
||||
;
|
||||
}
|
||||
|
||||
void printf_default(const char* text)
|
||||
{
|
||||
;
|
||||
}
|
||||
|
||||
WitmotionDll::WitmotionDll(SerialPortBase * serialPort)
|
||||
{
|
||||
m_SerialPort = serialPort;
|
||||
|
||||
m_delayFunction = delay_default;
|
||||
|
||||
m_witPrintf = printf_default;
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
int numOfSend = constructAndSendInstruction(KEY, KEY_UNLOCK);
|
||||
m_witPrintf("WitmotionDll::unlockInstruction: \n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::saveInstruction()
|
||||
{
|
||||
int numOfSend = constructAndSendInstruction(SAVE, SAVE_PARAM);
|
||||
m_witPrintf("WitmotionDll::saveInstruction: \n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::algorithm(ALGROITHM_ENUM algorithm)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(AXIS6, algorithm);
|
||||
m_witPrintf("WitmotionDll::algorithm: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::installationOrientation(ORIENT_ENUM orient)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(ORIENT, orient);
|
||||
m_witPrintf("WitmotionDll::installationOrientation:\n");
|
||||
|
||||
saveInstruction();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::instructStart(POWONSEND_ENUM command)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(POWONSEND, command);
|
||||
m_witPrintf("WitmotionDll::instructStart: \n");
|
||||
|
||||
saveInstruction();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::exitCalibration()
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(CALSW, EXITCAL);
|
||||
m_witPrintf("WitmotionDll::exitCalibration: \n");
|
||||
|
||||
saveInstruction();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::magneticCalibration()
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(CALSW, CALMAG);
|
||||
m_witPrintf("WitmotionDll::magneticCalibration: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setHeightToZero()
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(CALSW, CALALTITUDE);
|
||||
m_witPrintf("WitmotionDll::setHeightToZero: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setZAxisAngleToZero()
|
||||
{
|
||||
//需要读取参数，确保处于算法ALGROITHM6下
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(CALSW, CALANGLEZ);
|
||||
m_witPrintf("WitmotionDll::setZAxisAngleToZero: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setAngleReference()
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
uint16_t registerContent = 0x08;//询问技术支持才得到的命令
|
||||
int numOfSend = constructAndSendInstruction(CALSW, registerContent);
|
||||
m_witPrintf("WitmotionDll::setAngleReference: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void WitmotionDll::recordData()
|
||||
{
|
||||
char * receivedData;
|
||||
m_SerialPort->ReadData(receivedData);
|
||||
}
|
||||
|
||||
int WitmotionDll::setTimeZone(TIMEZONE_ENUM timeZone)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(TIMEZONE, timeZone);
|
||||
m_witPrintf("WitmotionDll::setTimeZone: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
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::clearContent(RETURN_CONTENT_STRUCT content)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::SetBaudrate(BAUD_ENUM baudrate)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(BAUD, baudrate);
|
||||
m_witPrintf("WitmotionDll::SetBaudrate: \n");
|
||||
|
||||
// saveInstruction(); //设置完波特率 已经无法通讯 需要重新简历连接
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::SetReturnRate(RRATE_ENUM returnRate)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(RRATE, returnRate);
|
||||
m_witPrintf("WitmotionDll::SetReturnRate: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::SetDeviceAddress(int deviceAddress)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(IICADDR, deviceAddress);
|
||||
m_witPrintf("WitmotionDll::SetDeviceAddress: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD0Model(MODEL_D0_D2_D3_ENUM model)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D0MODE, model);
|
||||
m_witPrintf("WitmotionDll::setD0Model: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD1Model(MODEL_D1_ENUM model)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D1MODE, model);
|
||||
m_witPrintf("WitmotionDll::setD1Model: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD2Model(MODEL_D0_D2_D3_ENUM model)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D2MODE, model);
|
||||
m_witPrintf("WitmotionDll::setD2Model: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD3Model(MODEL_D0_D2_D3_ENUM model)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D3MODE, model);
|
||||
m_witPrintf("WitmotionDll::setD3Model: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD0HighLevelPulseWidth(int PWMH)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D0PWMH, PWMH);
|
||||
m_witPrintf("WitmotionDll::setD0HighLevelPulseWidth: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD1HighLevelPulseWidth(int PWMH)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D1PWMH, PWMH);
|
||||
m_witPrintf("WitmotionDll::setD1HighLevelPulseWidth: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD2HighLevelPulseWidth(int PWMH)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D2PWMH, PWMH);
|
||||
m_witPrintf("WitmotionDll::setD2HighLevelPulseWidth: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD3HighLevelPulseWidth(int PWMH)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D3PWMH, PWMH);
|
||||
m_witPrintf("WitmotionDll::setD3HighLevelPulseWidth: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD0Period(int period)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D0PWMT, period);
|
||||
m_witPrintf("WitmotionDll::setD0Period: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD1Period(int period)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D1PWMT, period);
|
||||
m_witPrintf("WitmotionDll::setD1Period: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD2Period(int period)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D2PWMT, period);
|
||||
m_witPrintf("WitmotionDll::setD2Period: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int WitmotionDll::setD3Period(int period)
|
||||
{
|
||||
unlockInstruction();
|
||||
|
||||
int numOfSend = constructAndSendInstruction(D3PWMT, period);
|
||||
m_witPrintf("WitmotionDll::setD3Period: \n");
|
||||
|
||||
saveInstruction();
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user