435 lines
9.9 KiB
C++
435 lines
9.9 KiB
C++
#include "pch.h"
|
||
#include "WDACalibration.h"
|
||
#include <QRegularExpression>
|
||
#include <QThread>
|
||
|
||
WDACalibration::WDACalibration()
|
||
{
|
||
}
|
||
|
||
WDACalibration::~WDACalibration()
|
||
{
|
||
}
|
||
|
||
int WDACalibration::ExecuteCalibration(QSerialPort* pSerialPort, double dTemperature)
|
||
{
|
||
if (!pSerialPort || !pSerialPort->isOpen())
|
||
{
|
||
qDebug() << "Err:ExecuteCalibration - Serial port not open.Exit Code:1";
|
||
return 1;
|
||
}
|
||
|
||
// 将温度转换为5位数字格式，例如24.0 -> 00240
|
||
int iTempValue = static_cast<int>(dTemperature * 10);
|
||
char szTemp[16];
|
||
snprintf(szTemp, sizeof(szTemp), "%05d", iTempValue);
|
||
std::string sTempValue = szTemp;
|
||
|
||
qDebug() << QString("开始WDA校准流程，校准温度: %1℃ (编码: %2)").arg(dTemperature).arg(sTempValue.c_str());
|
||
|
||
CalibrationState eState = Calib_Step1_TC0;
|
||
int iRetryCount = 0;
|
||
int iTryCount = 0;
|
||
|
||
// 执行校准流程
|
||
while (eState != Calib_Idle)
|
||
{
|
||
// 执行当前步骤
|
||
std::string sResponse;
|
||
int iResult = ExecuteCalibrationStep(pSerialPort, eState, sTempValue, iRetryCount, iTryCount, sResponse);
|
||
|
||
if (iResult != 0)
|
||
{
|
||
// 步骤执行失败
|
||
iRetryCount++;
|
||
if (iRetryCount > 3)
|
||
{
|
||
// 错误码映射：ExecuteCalibrationStep返回1,2,3,4 -> ExecuteCalibration返回2,3,4,5
|
||
// (因为错误码1是串口未打开，错误码2由IrisSensor_WDA_P0返回表示WDA正在工作)
|
||
qDebug() << "Err:ExecuteCalibration - Too many retries.Exit Code:" << (iResult + 1);
|
||
return (iResult + 1);
|
||
}
|
||
qDebug() << "Warning:ExecuteCalibration - Step failed, retrying...";
|
||
continue;
|
||
}
|
||
|
||
// 步骤执行成功，重置重试计数
|
||
iRetryCount = 0;
|
||
|
||
// 特殊处理：步骤5需要检查校准是否完成
|
||
if (eState == Calib_Step5_AC_Second)
|
||
{
|
||
if (CheckCalibrationComplete(sResponse))
|
||
{
|
||
qDebug() << "WDA校准完成！所有值均小于10";
|
||
// 继续到下一步
|
||
eState = Calib_Step6_Restore_KY18;
|
||
}
|
||
else
|
||
{
|
||
iTryCount++;
|
||
if (iTryCount > 1)
|
||
{
|
||
qDebug() << "Warning:ExecuteCalibration - Calibration not complete after retries, continuing...";
|
||
eState = Calib_Step6_Restore_KY18;
|
||
}
|
||
else
|
||
{
|
||
qDebug() << "WDA校准未完成，需要重复步骤2-3，等待5秒后重试...";
|
||
// 等待5秒
|
||
QThread::msleep(5000);
|
||
// 重新开始步骤2
|
||
eState = Calib_Step2_KY18_First;
|
||
iRetryCount = 0;
|
||
}
|
||
continue;
|
||
}
|
||
}
|
||
|
||
// 更新到下一步状态
|
||
switch (eState)
|
||
{
|
||
case Calib_Step1_TC0:
|
||
eState = Calib_Step2_KY18_First;
|
||
break;
|
||
case Calib_Step2_KY18_First:
|
||
eState = Calib_Step3_AC_First;
|
||
break;
|
||
case Calib_Step3_AC_First:
|
||
eState = Calib_Step4_KY18_Second;
|
||
break;
|
||
case Calib_Step4_KY18_Second:
|
||
eState = Calib_Step5_AC_Second;
|
||
break;
|
||
case Calib_Step5_AC_Second:
|
||
eState = Calib_Step6_Restore_KY18;
|
||
break;
|
||
case Calib_Step6_Restore_KY18:
|
||
eState = Calib_Step7_Restore_TC;
|
||
break;
|
||
case Calib_Step7_Restore_TC:
|
||
qDebug() << "WDA校准流程全部完成！";
|
||
return 0;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
// 步骤之间等待500ms
|
||
QThread::msleep(500);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int WDACalibration::ExecuteCalibrationStep(QSerialPort* pSerialPort, CalibrationState eStep,
|
||
const std::string& sTempValue, int& iRetryCount, int& iTryCount,
|
||
std::string& sResponse)
|
||
{
|
||
// 发送当前步骤的命令
|
||
std::string sCommand;
|
||
switch (eStep)
|
||
{
|
||
case Calib_Step1_TC0:
|
||
sCommand = "$01,TC0\r\n";
|
||
break;
|
||
case Calib_Step2_KY18_First:
|
||
sCommand = "$01,KY18\r\n";
|
||
break;
|
||
case Calib_Step3_AC_First:
|
||
{
|
||
char szCmd[64];
|
||
snprintf(szCmd, sizeof(szCmd), "$01,AC%s^**\r\n", sTempValue.c_str());
|
||
sCommand = szCmd;
|
||
}
|
||
break;
|
||
case Calib_Step4_KY18_Second:
|
||
sCommand = "$01,KY18\r\n";
|
||
break;
|
||
case Calib_Step5_AC_Second:
|
||
{
|
||
char szCmd[64];
|
||
snprintf(szCmd, sizeof(szCmd), "$01,AC%s^**\r\n", sTempValue.c_str());
|
||
sCommand = szCmd;
|
||
}
|
||
break;
|
||
case Calib_Step6_Restore_KY18:
|
||
sCommand = "$01,KY18\r\n";
|
||
break;
|
||
case Calib_Step7_Restore_TC:
|
||
sCommand = "$01,TC00001^**\r\n";
|
||
break;
|
||
default:
|
||
return 1;
|
||
}
|
||
|
||
if (SendCalibrationCommand(pSerialPort, sCommand) != 0)
|
||
{
|
||
qDebug() << "Err:ExecuteCalibrationStep - SendCalibrationCommand failed.Exit Code:2";
|
||
return 2;
|
||
}
|
||
|
||
// 接收响应
|
||
sResponse.clear();
|
||
if (RecvCalibrationResponse(pSerialPort, sResponse, 5000) != 0)
|
||
{
|
||
qDebug() << "Err:ExecuteCalibrationStep - RecvCalibrationResponse timeout.Exit Code:3";
|
||
return 3;
|
||
}
|
||
|
||
// 检查响应
|
||
if (!CheckCalibrationResponse(sResponse, eStep))
|
||
{
|
||
qDebug() << "Err:ExecuteCalibrationStep - Invalid response.Exit Code:4";
|
||
return 4;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int WDACalibration::SendCalibrationCommand(QSerialPort* pSerialPort, const std::string& sCommand)
|
||
{
|
||
if (!pSerialPort || !pSerialPort->isOpen())
|
||
{
|
||
return 1;
|
||
}
|
||
|
||
QByteArray qbaSend(sCommand.c_str(), (int)sCommand.length());
|
||
qint64 qi64Write = pSerialPort->write(qbaSend);
|
||
pSerialPort->waitForBytesWritten(50);
|
||
|
||
if (qi64Write != qbaSend.size())
|
||
{
|
||
qDebug() << "Err:SendCalibrationCommand - Write failed";
|
||
return 1;
|
||
}
|
||
|
||
qDebug() << QString("发送: %1").arg(sCommand.c_str());
|
||
return 0;
|
||
}
|
||
|
||
int WDACalibration::RecvCalibrationResponse(QSerialPort* pSerialPort, std::string& sResponse, int iTimeoutMs)
|
||
{
|
||
if (!pSerialPort || !pSerialPort->isOpen())
|
||
{
|
||
return 1;
|
||
}
|
||
|
||
sResponse.clear();
|
||
QByteArray qbData;
|
||
qbData.clear();
|
||
|
||
// 清空接收缓冲区
|
||
pSerialPort->readAll();
|
||
|
||
// 等待数据到达
|
||
int iElapsed = 0;
|
||
while (iElapsed < iTimeoutMs)
|
||
{
|
||
if (pSerialPort->waitForReadyRead(100))
|
||
{
|
||
QByteArray qbTemp = pSerialPort->readAll();
|
||
qbData.append(qbTemp);
|
||
|
||
// 检查是否收到完整的行（以\r\n或\n结尾）
|
||
if (qbData.contains("\r\n") || qbData.contains('\n') || qbData.contains('\r'))
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
iElapsed += 100;
|
||
}
|
||
|
||
if (qbData.isEmpty())
|
||
{
|
||
return 1;
|
||
}
|
||
|
||
// 提取一行数据
|
||
int iLineEnd = -1;
|
||
int iLineLength = 0;
|
||
|
||
if (qbData.contains("\r\n"))
|
||
{
|
||
iLineEnd = qbData.indexOf("\r\n");
|
||
iLineLength = 2;
|
||
}
|
||
else if (qbData.contains('\n'))
|
||
{
|
||
iLineEnd = qbData.indexOf('\n');
|
||
iLineLength = 1;
|
||
}
|
||
else if (qbData.contains('\r'))
|
||
{
|
||
iLineEnd = qbData.indexOf('\r');
|
||
iLineLength = 1;
|
||
}
|
||
else
|
||
{
|
||
// 没有换行符，使用全部数据
|
||
iLineEnd = qbData.size();
|
||
iLineLength = 0;
|
||
}
|
||
|
||
QByteArray qbLine = qbData.left(iLineEnd);
|
||
QString qstrResponse = QString::fromUtf8(qbLine).trimmed();
|
||
sResponse = qstrResponse.toStdString();
|
||
|
||
if (!sResponse.empty())
|
||
{
|
||
qDebug() << QString("接收: %1").arg(sResponse.c_str());
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
bool WDACalibration::CheckCalibrationResponse(const std::string& sResponse, CalibrationState eStep)
|
||
{
|
||
QString qstrResponse = QString::fromStdString(sResponse);
|
||
|
||
switch (eStep)
|
||
{
|
||
case Calib_Step1_TC0:
|
||
if (qstrResponse.contains("$WI,TC=0") || qstrResponse.contains("TC=0"))
|
||
{
|
||
qDebug() << "步骤1完成: TC0设置成功";
|
||
return true;
|
||
}
|
||
break;
|
||
|
||
case Calib_Step2_KY18_First:
|
||
if (qstrResponse.contains("$WI,KY=18") || qstrResponse.contains("KY=18"))
|
||
{
|
||
qDebug() << "步骤2完成: KY18设置成功";
|
||
return true;
|
||
}
|
||
break;
|
||
|
||
case Calib_Step3_AC_First:
|
||
if (qstrResponse.contains("$WI,AC=") || qstrResponse.contains("AC="))
|
||
{
|
||
std::string sACValues = ExtractACValues(sResponse);
|
||
qDebug() << QString("步骤3完成: 第一次AC校准，返回值: %1").arg(sACValues.c_str());
|
||
return true;
|
||
}
|
||
break;
|
||
|
||
case Calib_Step4_KY18_Second:
|
||
if (qstrResponse.contains("$WI,KY=18") || qstrResponse.contains("KY=18"))
|
||
{
|
||
qDebug() << "步骤4完成: 第二次KY18设置成功";
|
||
return true;
|
||
}
|
||
break;
|
||
|
||
case Calib_Step5_AC_Second:
|
||
if (qstrResponse.contains("$WI,AC=") || qstrResponse.contains("AC="))
|
||
{
|
||
std::string sACValues = ExtractACValues(sResponse);
|
||
qDebug() << QString("步骤5: 第二次AC校准，返回值: %1").arg(sACValues.c_str());
|
||
return true;
|
||
}
|
||
break;
|
||
|
||
case Calib_Step6_Restore_KY18:
|
||
if (qstrResponse.contains("$WI,KY=18") || qstrResponse.contains("KY=18"))
|
||
{
|
||
qDebug() << "步骤6完成: 恢复模式KY18设置成功";
|
||
return true;
|
||
}
|
||
break;
|
||
|
||
case Calib_Step7_Restore_TC:
|
||
if (qstrResponse.contains("$WI,TC=00001") || qstrResponse.contains("TC=00001"))
|
||
{
|
||
qDebug() << "步骤7完成: 恢复模式TC设置成功";
|
||
return true;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
bool WDACalibration::CheckCalibrationComplete(const std::string& sResponse)
|
||
{
|
||
std::string sACValues = ExtractACValues(sResponse);
|
||
return AllValuesLessThan10(sACValues);
|
||
}
|
||
|
||
std::string WDACalibration::ExtractACValues(const std::string& sResponse)
|
||
{
|
||
QString qstrResponse = QString::fromStdString(sResponse);
|
||
QRegularExpression re(R"(\$WI,AC=(.+))");
|
||
QRegularExpressionMatch match = re.match(qstrResponse);
|
||
|
||
if (!match.hasMatch())
|
||
{
|
||
re.setPattern(R"(AC=(.+))");
|
||
match = re.match(qstrResponse);
|
||
}
|
||
|
||
if (match.hasMatch())
|
||
{
|
||
return match.captured(1).toStdString();
|
||
}
|
||
|
||
return "";
|
||
}
|
||
|
||
bool WDACalibration::AllValuesLessThan10(const std::string& sACValues)
|
||
{
|
||
if (sACValues.empty())
|
||
{
|
||
return false;
|
||
}
|
||
|
||
QString qstrACValues = QString::fromStdString(sACValues);
|
||
QStringList values = qstrACValues.split(',');
|
||
|
||
if (values.isEmpty())
|
||
{
|
||
return false;
|
||
}
|
||
|
||
// 检查从第5个值（索引4）到最后一个值
|
||
int iStartIndex = 4;
|
||
|
||
if (iStartIndex >= values.size())
|
||
{
|
||
qDebug() << QString("值数量不足5个，无法检查");
|
||
return false;
|
||
}
|
||
|
||
QStringList checkedValues;
|
||
for (int i = iStartIndex; i < values.size(); i++)
|
||
{
|
||
QString valueStr = values[i].trimmed();
|
||
bool ok;
|
||
int iValue = valueStr.toInt(&ok);
|
||
if (!ok)
|
||
{
|
||
qDebug() << QString("无法解析值: %1").arg(valueStr);
|
||
return false;
|
||
}
|
||
checkedValues << valueStr;
|
||
if (iValue >= 10)
|
||
{
|
||
qDebug() << QString("检查值: %1 (第%2到最后一个值: %3)，值 %4 >= 10，校准未完成")
|
||
.arg(valueStr)
|
||
.arg(iStartIndex + 1)
|
||
.arg(checkedValues.join(","))
|
||
.arg(iValue);
|
||
return false;
|
||
}
|
||
}
|
||
|
||
qDebug() << QString("第%1到最后一个值均小于10 (值: %2)，校准完成")
|
||
.arg(iStartIndex + 1)
|
||
.arg(checkedValues.join(","));
|
||
return true;
|
||
}
|
||
|