781 lines
21 KiB
C++
781 lines
21 KiB
C++
#include "stdafx.h"
|
||
#include "focusWindow.h"
|
||
|
||
focusWindow::focusWindow(QWidget *parent, ImagerOperationBase* imager)
|
||
{
|
||
ui.setupUi(this);
|
||
|
||
//读取配置文件
|
||
string HPPACfgFile = getPathofEXE() + "\\HPPA.cfg";
|
||
Configfile configfile;
|
||
configfile.setConfigfilePath(HPPACfgFile);
|
||
if (!configfile.isConfigfileExist())
|
||
configfile.createConfigFile();
|
||
|
||
configfile.parseConfigfile();
|
||
configfile.getPositionRestriction(m_iMaxPos, m_iMinPos);
|
||
|
||
|
||
disableBeforeConnect(true);
|
||
|
||
setAttribute(Qt::WA_DeleteOnClose);//设置关闭窗体就调用窗体的析构函数
|
||
|
||
m_Imager = imager;
|
||
m_FocusState = 0;
|
||
m_ctrlFocusMotor = nullptr;
|
||
|
||
m_AutoFocusThread = new QThread();
|
||
|
||
connect(this->ui.connectMotor_btn, SIGNAL(clicked()), this, SLOT(onConnectMotor()));
|
||
connect(this->ui.logicZero_btn, SIGNAL(clicked()), this, SLOT(onMove2MotorLogicZero()));
|
||
connect(this->ui.max_btn, SIGNAL(clicked()), this, SLOT(onMove2MotorMax()));
|
||
connect(this->ui.add_btn, SIGNAL(clicked()), this, SLOT(onAdd()));
|
||
connect(this->ui.subtract_btn, SIGNAL(clicked()), this, SLOT(onSubtract()));
|
||
connect(this->ui.ultrasound_radioButton, SIGNAL(released()), this, SLOT(onUltrasound_radioButton()));
|
||
connect(this->ui.autoFocus_btn, SIGNAL(clicked()), this, SLOT(onAutoFocus()));
|
||
connect(this->ui.manualFocus_btn, SIGNAL(clicked()), this, SLOT(onManualFocus()));
|
||
|
||
connect(this->ui.updateCurrentLocation_btn, SIGNAL(clicked()), this, SLOT(onUpdateCurrentLocation()));
|
||
connect(this->ui.moveto_btn, SIGNAL(clicked()), this, SLOT(onMoveto()));
|
||
|
||
connect(this->ui.rangeMeasurement_btn, SIGNAL(pressed()), this, SLOT(onx_rangeMeasurement()));
|
||
|
||
//查找可用串口，并显示
|
||
foreach(const QSerialPortInfo &info, QSerialPortInfo::availablePorts())
|
||
{
|
||
QSerialPort serial;
|
||
serial.setPort(info);
|
||
if (serial.open(QIODevice::ReadWrite))
|
||
{
|
||
ui.motorPort_comboBox->addItem(serial.portName());
|
||
ui.ultrasoundPort_comboBox->addItem(serial.portName());
|
||
serial.close();
|
||
}
|
||
}
|
||
|
||
//设置自动调焦进度条
|
||
ui.autoFocusProgress_progressBar->setMinimum(0);
|
||
ui.autoFocusProgress_progressBar->setMaximum(100);
|
||
ui.autoFocusProgress_progressBar->reset();
|
||
|
||
m_dSpeed = 1.0;
|
||
|
||
}
|
||
|
||
focusWindow::~focusWindow()
|
||
{
|
||
printf("destroy focusWindow-------------------------\n");
|
||
|
||
emit StartManualFocusSignal(0);//当用户没有点击停止调焦就关闭窗口
|
||
|
||
delete m_ctrlFocusMotor;
|
||
//delete thread1, progressThread;
|
||
|
||
m_motorThread.quit();
|
||
m_motorThread.wait();
|
||
|
||
m_MotionCaptureCoordinatorThread.quit();
|
||
m_MotionCaptureCoordinatorThread.wait();
|
||
}
|
||
|
||
void focusWindow::disableBeforeConnect(bool disable)
|
||
{
|
||
ui.controlMotor_groupBox->setDisabled(disable);
|
||
ui.autoFocus_btn->setDisabled(disable);
|
||
}
|
||
|
||
bool test(void *pCaller, int *x, int *y, void **pvdata)
|
||
{
|
||
focusWindow *p = (focusWindow *)pCaller;
|
||
|
||
p->m_Imager->GetFrameSize(*x, *y);
|
||
USHORT *pusData = new USHORT[(*x)*(*y)];
|
||
|
||
p->m_Imager->imagerStartCollect();
|
||
p->m_Imager->getFrame(pusData);
|
||
p->m_Imager->imagerStopCollect();
|
||
|
||
|
||
BYTE *pbData = (BYTE*)pusData;
|
||
*pvdata = pbData;
|
||
return true;
|
||
}
|
||
|
||
void focusWindow::onConnectMotor()
|
||
{
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
FileOperation* fileOperation = new FileOperation();
|
||
string directory = fileOperation->getDirectoryOfExe();
|
||
QString configFilePath = QString::fromStdString(directory) + "\\oneMotorConfigFile_focus.cfg";
|
||
|
||
m_multiAxisController = new IrisMultiMotorController(configFilePath);
|
||
m_multiAxisController->moveToThread(&m_motorThread);
|
||
connect(&m_motorThread, SIGNAL(finished()), m_multiAxisController, SLOT(deleteLater()));
|
||
connect(this, SIGNAL(rmoveSignal(int, double, double, int)), m_multiAxisController, SLOT(rmove(int, double, double, int)));
|
||
connect(this, SIGNAL(move2LocSignal(int, double, double, int)), m_multiAxisController, SLOT(moveTo(int, double, double, int)));
|
||
connect(this, SIGNAL(rangeMeasurementSignal(int, double, int)), m_multiAxisController, SLOT(rangeMeasurement(int, double, int)));
|
||
connect(this, SIGNAL(zeroStartSignal(int)), m_multiAxisController, SLOT(zeroStart(int)));
|
||
connect(this, SIGNAL(move2MaxLocSignal(int, double, int)), m_multiAxisController, SLOT(moveToMax(int, double, int)));
|
||
connect(m_multiAxisController, SIGNAL(broadcastLocationSignal(std::vector<double>)), this, SLOT(display_x_loc(std::vector<double>)));
|
||
connect(m_multiAxisController, SIGNAL(motorStopSignal(int, double)), this, SLOT(moveAfterAutoFocus(int, double)));
|
||
m_motorThread.start();
|
||
|
||
//归零
|
||
//emit zeroStartSignal(0);
|
||
|
||
//自动调焦逻辑
|
||
m_coordinator = new MotionCaptureCoordinator(m_multiAxisController, m_Imager);
|
||
m_coordinator->moveToThread(&m_MotionCaptureCoordinatorThread);
|
||
connect(&m_MotionCaptureCoordinatorThread, SIGNAL(finished()), m_coordinator, SLOT(deleteLater()));
|
||
connect(this, SIGNAL(startStepMotion(double, int, double, double)), m_coordinator, SLOT(startStepMotion(double, int, double, double)));
|
||
connect(m_coordinator, SIGNAL(progressChanged(int)), this, SLOT(onAutoFocusProgress(int)));
|
||
connect(m_coordinator, SIGNAL(sequenceComplete()), this, SLOT(onAutoFocusFinished()));
|
||
m_MotionCaptureCoordinatorThread.start();
|
||
}
|
||
else
|
||
{
|
||
if (m_ctrlFocusMotor != nullptr)
|
||
{
|
||
printf("不能重复连接-------------------------\n");
|
||
return;
|
||
}
|
||
|
||
bool isUltrasound = ui.ultrasound_radioButton->isChecked();
|
||
|
||
QString motorPortTmp = ui.motorPort_comboBox->currentText();
|
||
QString ultrasoundPortTmp = ui.ultrasoundPort_comboBox->currentText();
|
||
|
||
QRegExp rx("\\d+$");
|
||
rx.indexIn(motorPortTmp, 0);
|
||
int motorPort = rx.cap(0).toInt();
|
||
rx.indexIn(ultrasoundPortTmp, 0);
|
||
int ultrasoundPort = rx.cap(0).toInt();
|
||
|
||
|
||
|
||
if (isUltrasound)
|
||
{
|
||
PortInfo motor;
|
||
motor.iPortType = 0;
|
||
motor.indexParity = 0;
|
||
|
||
motor.iPortNumber = motorPort;
|
||
motor.indexBaudRate = 0x13;
|
||
motor.indexBytesize = 3;
|
||
motor.indexStopBits = 0;
|
||
|
||
PortInfo ultrasound;
|
||
ultrasound.iPortType = 0;
|
||
ultrasound.indexParity = 0;
|
||
ultrasound.iPortNumber = ultrasoundPort;
|
||
ultrasound.indexBaudRate = 0x0C;
|
||
ultrasound.indexBytesize = 3;
|
||
ultrasound.indexStopBits = 0;
|
||
|
||
m_ctrlFocusMotor = new CFocusMotorControl();
|
||
|
||
m_ctrlFocusMotor->SetLogicZero(m_iMinPos);
|
||
m_ctrlFocusMotor->SetLimit(m_iMinPos, m_iMaxPos);
|
||
m_ctrlFocusMotor->InitSystem(motor, ultrasound, test, this);
|
||
//m_ctrlFocusMotor->MoveToLogicZero();
|
||
}
|
||
else
|
||
{
|
||
PortInfo motor;
|
||
motor.iPortType = 0;
|
||
motor.indexParity = 0;
|
||
motor.iPortNumber = motorPort;
|
||
motor.indexBaudRate = 0x13;
|
||
motor.indexBytesize = 3;
|
||
motor.indexStopBits = 0;
|
||
|
||
m_ctrlFocusMotor = new CFocusMotorControl();
|
||
|
||
m_ctrlFocusMotor->SetLogicZero(m_iMinPos);
|
||
m_ctrlFocusMotor->SetLimit(m_iMinPos, m_iMaxPos);
|
||
m_ctrlFocusMotor->InitSystem(motor, test, this);
|
||
m_ctrlFocusMotor->MoveToLogicZero();
|
||
}
|
||
}
|
||
|
||
disableBeforeConnect(false);
|
||
}
|
||
|
||
void focusWindow::display_x_loc(std::vector<double> loc)
|
||
{
|
||
double tmp = round(loc[0] * 100) / 100;
|
||
this->ui.currentLocation_lineEdit->setText(QString::number(tmp));
|
||
}
|
||
|
||
void focusWindow::onx_rangeMeasurement()
|
||
{
|
||
emit rangeMeasurementSignal(0, m_dSpeed, 1000);
|
||
}
|
||
|
||
void focusWindow::onMove2MotorLogicZero()
|
||
{
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
emit zeroStartSignal(0);
|
||
}
|
||
else
|
||
{
|
||
m_ctrlFocusMotor->MoveToLogicZero();
|
||
}
|
||
}
|
||
|
||
void focusWindow::onMove2MotorMax()
|
||
{
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
emit move2MaxLocSignal(0, m_dSpeed, 1000);
|
||
}
|
||
else
|
||
{
|
||
m_ctrlFocusMotor->MoveToPos(m_iMaxPos);
|
||
}
|
||
}
|
||
|
||
void focusWindow::onAdd()
|
||
{
|
||
double rdistance = ui.addStepSize_lineEdit->text().toDouble();
|
||
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
emit rmoveSignal(0, (double)abs(rdistance), m_dSpeed, 1000);
|
||
}
|
||
else
|
||
{
|
||
DriverInfo di;
|
||
m_ctrlFocusMotor->GetDriverStatus(di);
|
||
|
||
m_ctrlFocusMotor->MoveToPos(di.iAbsPosition + rdistance);
|
||
}
|
||
}
|
||
|
||
void focusWindow::onSubtract()
|
||
{
|
||
double rdistance = ui.subtractStepSize_lineEdit->text().toDouble();
|
||
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
emit rmoveSignal(0, (double)abs(rdistance) * -1, m_dSpeed, 1000);
|
||
}
|
||
else
|
||
{
|
||
DriverInfo di;
|
||
m_ctrlFocusMotor->GetDriverStatus(di);
|
||
|
||
m_ctrlFocusMotor->MoveToPos(di.iAbsPosition - rdistance);
|
||
}
|
||
}
|
||
|
||
void focusWindow::onAutoFocus()
|
||
{
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
//先按照一定间隔从负极限到正极限获取一系列（位置，调焦指数）
|
||
m_iStepSize = ui.sample_ratio_lineEdit->text().toInt();
|
||
ui.autoFocusProgress_progressBar->setMinimum(0);
|
||
ui.autoFocusProgress_progressBar->setMaximum(m_iStepSize);
|
||
ui.autoFocusProgress_progressBar->reset();
|
||
|
||
//获取马达最大位置
|
||
std::vector<double> maxRangeLocations = m_multiAxisController->getMaxPos();
|
||
double maxPos = maxRangeLocations[0];
|
||
emit startStepMotion(m_dSpeed, m_iStepSize, 0, maxPos);
|
||
}
|
||
else
|
||
{
|
||
bool isUltrasound = ui.ultrasound_radioButton->isChecked();
|
||
WorkerThread2* thread1 = new WorkerThread2(m_ctrlFocusMotor, isUltrasound);
|
||
|
||
connect(thread1, SIGNAL(AutoFocusFinishedSignal()), this, SLOT(onAutoFocusFinished()));
|
||
|
||
thread1->start();
|
||
|
||
WorkerThread4* progressThread = new WorkerThread4(m_ctrlFocusMotor);
|
||
connect(progressThread, SIGNAL(AutoFocusProgressSignal(int)), this, SLOT(onAutoFocusProgress(int)));
|
||
progressThread->start();
|
||
ui.autoFocusProgress_progressBar->reset();
|
||
}
|
||
this->setDisabled(true);
|
||
}
|
||
|
||
void focusWindow::onManualFocus()
|
||
{
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
|
||
}
|
||
else
|
||
{
|
||
|
||
}
|
||
|
||
m_FocusState += 1;
|
||
std::cout << "点击调焦按钮！" << std::endl;
|
||
|
||
|
||
if (m_FocusState % 2 == 1)
|
||
{
|
||
//开始调焦
|
||
emit StartManualFocusSignal(1);
|
||
ui.manualFocus_btn->setText(QString::fromLocal8Bit("停止调焦"));
|
||
|
||
ui.manualFocus_btn->setStyleSheet("QWidget{background-color:rgb(255,0,0);}");
|
||
|
||
ui.autoFocus_btn->setDisabled(true);
|
||
|
||
//std::cout << "------------------------------------------：" << m_FocusState << std::endl;
|
||
}
|
||
else
|
||
{
|
||
emit StartManualFocusSignal(0);
|
||
m_Imager->setFocusControlState(false);
|
||
|
||
ui.manualFocus_btn->setText(QString::fromLocal8Bit("调焦"));
|
||
|
||
ui.manualFocus_btn->setStyleSheet("QWidget{background-color:rgb(0,255,0);}");
|
||
|
||
ui.autoFocus_btn->setDisabled(false);
|
||
}
|
||
}
|
||
|
||
void focusWindow::onUpdateCurrentLocation()
|
||
{
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
//因为新版的马达控制是实时反馈位置信息，所以不需要做任何事
|
||
}
|
||
else
|
||
{
|
||
DriverInfo di;
|
||
m_ctrlFocusMotor->GetDriverStatus(di);
|
||
ui.currentLocation_lineEdit->setText(QString::number(di.iAbsPosition));
|
||
}
|
||
}
|
||
|
||
void focusWindow::onMoveto()
|
||
{
|
||
double pos = ui.move2_lineEdit->text().toDouble();
|
||
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
emit move2LocSignal(0, (double)pos, m_dSpeed, 1000);
|
||
}
|
||
else
|
||
{
|
||
//需要做参数有效性验证，查看是否在有效范围内
|
||
|
||
m_ctrlFocusMotor->MoveToPos(pos);
|
||
}
|
||
}
|
||
|
||
void focusWindow::onAutoFocusFinished()
|
||
{
|
||
if (ui.is_new_version_radioButton->isChecked())
|
||
{
|
||
//通过高斯拟合获取最佳位置
|
||
QVector<PositionData> positionData = m_coordinator->getAllPositionData();
|
||
|
||
std::vector<double> actualPositions;
|
||
std::vector<double> cameraIndices;
|
||
|
||
actualPositions.reserve(positionData.size());
|
||
cameraIndices.reserve(positionData.size());
|
||
|
||
int scaleFactor = 100;
|
||
for (const auto& data : positionData)
|
||
{
|
||
actualPositions.push_back(data.actualPosition * scaleFactor);
|
||
cameraIndices.push_back(data.cameraIndex);
|
||
}
|
||
double a_init, mu_init, sigma_init, c_init;
|
||
getGaussianInitParam(actualPositions, cameraIndices, a_init, mu_init, sigma_init, c_init);
|
||
|
||
double a = a_init, mu = mu_init, sigma = sigma_init, c = c_init;
|
||
gaussian_fit(actualPositions, cameraIndices, a, mu, sigma, c);
|
||
|
||
mu_init = mu_init / scaleFactor;
|
||
mu = mu / scaleFactor;
|
||
|
||
std::cout << "mu 初值：" << mu_init << std::endl << "mu 拟合值：" << mu << std::endl;
|
||
|
||
//对拟合值进行判断，排除错误拟合情况（地物没有纹理）
|
||
std::vector<double> maxRangeLocations = m_multiAxisController->getMaxPos();
|
||
double maxPos = maxRangeLocations[0];
|
||
if (mu < 0 || mu > maxPos)
|
||
{
|
||
std::cout << "拟合失败！！！！！" << std::endl;
|
||
m_goodPos = mu_init;
|
||
m_isAutoFocusSuccess = false;
|
||
}
|
||
else
|
||
{
|
||
std::cout << "拟合成功！！！！！" << std::endl;
|
||
m_goodPos = mu;
|
||
m_isAutoFocusSuccess = true;
|
||
}
|
||
|
||
FileOperation* fileOperation = new FileOperation();
|
||
string directory = fileOperation->getDirectoryOfExe();
|
||
QDateTime now = QDateTime::currentDateTime();
|
||
QString format1 = "yyyyMMdd_HHmmss";
|
||
QString fileName = now.toString("yyyyMMdd_HHmmss");
|
||
|
||
QString fitDataFile = QDir::cleanPath(QString::fromStdString(directory) + QDir::separator() + fileName + "_" + QString::number(m_iStepSize) + "interval_" + QString::number(mu) + ".csv");
|
||
m_coordinator->saveToCsv(fitDataFile);
|
||
|
||
//由于马达移动准确性较低，自动调焦完成后，直接移动到m_goodPos效果不好，所以先移动到tmpPos，然后移动到m_goodPos
|
||
double tmpPos = m_goodPos - positionData[positionData.size() - 1].actualPosition / 10;
|
||
m_isMoveAfterAutoFocus = true;
|
||
emit move2LocSignal(0, (double)tmpPos, m_dSpeed, 1000);
|
||
}
|
||
else
|
||
{
|
||
onUpdateCurrentLocation();
|
||
}
|
||
|
||
this->setDisabled(false);
|
||
}
|
||
|
||
void focusWindow::moveAfterAutoFocus(int motorID, double location)
|
||
{
|
||
if (!m_isMoveAfterAutoFocus)
|
||
{
|
||
return;
|
||
}
|
||
|
||
std::cout << "\n已经到达位置：" << location << std::endl;
|
||
|
||
//移动马达到最佳位置
|
||
emit move2LocSignal(0, (double)m_goodPos, m_dSpeed, 1000);
|
||
|
||
double tmp = abs(location - m_goodPos) / m_goodPos * 100;
|
||
if (tmp<5)
|
||
{
|
||
if (!m_isAutoFocusSuccess)
|
||
{
|
||
QMessageBox msgBox;
|
||
msgBox.setText(QString::fromLocal8Bit("纹理较弱，自动调焦效果不佳！请使用调焦纸进行自动调焦！"));
|
||
msgBox.exec();
|
||
}
|
||
else
|
||
{
|
||
QMessageBox msgBox;
|
||
msgBox.setText(QString::fromLocal8Bit("自动调焦成功！"));
|
||
msgBox.exec();
|
||
}
|
||
m_isMoveAfterAutoFocus = false;
|
||
}
|
||
}
|
||
|
||
void focusWindow::getGaussianInitParam(const std::vector<double>& pos, const std::vector<double>& index, double& a_init, double& mu_init, double& sigma_init, double& c_init)
|
||
{
|
||
auto minmax_element = std::minmax_element(index.begin(), index.end());
|
||
a_init = *minmax_element.second - *minmax_element.first;
|
||
mu_init = pos[std::distance(index.begin(), minmax_element.second)];
|
||
c_init = *minmax_element.first;
|
||
|
||
//sigma_init
|
||
double half_max = (*minmax_element.second + *minmax_element.first) / 2.0;
|
||
size_t peak_idx = std::distance(index.begin(), minmax_element.second);
|
||
size_t left_idx = peak_idx;// 找左半高点
|
||
while (left_idx > 0 && index[left_idx] > half_max) left_idx--;
|
||
size_t right_idx = peak_idx;// 找右半高点
|
||
while (right_idx < index.size() - 1 && index[right_idx] > half_max) right_idx++;
|
||
double fwhm = pos[right_idx] - pos[left_idx];
|
||
sigma_init = fwhm / 2.3548;
|
||
}
|
||
|
||
// 使用 Gauss-Newton 进行高斯拟合
|
||
void focusWindow::gaussian_fit(const std::vector<double>& x_data,
|
||
const std::vector<double>& y_data,
|
||
double& a, double& mu, double& sigma, double& c)
|
||
{
|
||
const int max_iter = 100;
|
||
for (int iter = 0; iter < max_iter; iter++)
|
||
{
|
||
Eigen::MatrixXd J(x_data.size(), 4);
|
||
Eigen::VectorXd r(x_data.size());
|
||
|
||
for (size_t i = 0; i < x_data.size(); i++)
|
||
{
|
||
double xi = x_data[i];
|
||
double yi = y_data[i];
|
||
double exp_part = std::exp(-(xi - mu) * (xi - mu) / (2 * sigma * sigma));
|
||
double fi = a * exp_part + c;
|
||
|
||
r(i) = yi - fi;
|
||
|
||
// 雅可比
|
||
J(i, 0) = -exp_part; // ∂f/∂a
|
||
J(i, 1) = -a * exp_part * ((xi - mu) / (sigma * sigma)); // ∂f/∂mu
|
||
J(i, 2) = -a * exp_part * ((xi - mu) * (xi - mu) / (sigma * sigma * sigma)); // ∂f/∂sigma
|
||
J(i, 3) = -1.0; // ∂f/∂c
|
||
}
|
||
|
||
Eigen::VectorXd delta = (J.transpose() * J).ldlt().solve(-J.transpose() * r);
|
||
|
||
a += delta(0);
|
||
mu += delta(1);
|
||
sigma += delta(2);
|
||
c += delta(3);
|
||
|
||
if (delta.norm() < 1e-8) break;
|
||
}
|
||
}
|
||
|
||
void focusWindow::onAutoFocusProgress(int progress)
|
||
{
|
||
//std::cout << "进度：" << progress << std::endl;
|
||
|
||
ui.autoFocusProgress_progressBar->setValue(progress);
|
||
}
|
||
|
||
void focusWindow::onUltrasound_radioButton()
|
||
{
|
||
bool isUltrasound = ui.ultrasound_radioButton->isChecked();
|
||
|
||
if (isUltrasound)
|
||
{
|
||
ui.ultrasoundPort_comboBox->setEnabled(true);
|
||
}
|
||
else
|
||
{
|
||
ui.ultrasoundPort_comboBox->setEnabled(false);
|
||
}
|
||
}
|
||
|
||
WorkerThread2::WorkerThread2(CFocusMotorControl * ctrlFocusMotor, bool isUltrasound)
|
||
{
|
||
m_ctrlFocusMotor = ctrlFocusMotor;
|
||
m_bIsUltrasound = isUltrasound;
|
||
}
|
||
|
||
void WorkerThread2::run()
|
||
{
|
||
//配置文件
|
||
string HPPACfgFile = getPathofEXE() + "\\HPPA.cfg";
|
||
Configfile configfile;
|
||
configfile.setConfigfilePath(HPPACfgFile);
|
||
if (!configfile.isConfigfileExist())
|
||
configfile.createConfigFile();
|
||
configfile.parseConfigfile();
|
||
|
||
int coarse, fine;
|
||
configfile.getTuningStepSize(coarse, fine);
|
||
float fa, fb;
|
||
configfile.getFitParams(fa, fb);
|
||
int max_FocusRange, min_FocusRange;
|
||
configfile.getAutoFocusRange(max_FocusRange, min_FocusRange);
|
||
|
||
if (m_bIsUltrasound)
|
||
{
|
||
m_ctrlFocusMotor->SetFitParams(fa, fb);
|
||
m_ctrlFocusMotor->StartAutoFocus(5, 2);
|
||
}
|
||
else
|
||
{
|
||
m_ctrlFocusMotor->StartAutoFocus(min_FocusRange, max_FocusRange, coarse, fine);
|
||
}
|
||
|
||
emit AutoFocusFinishedSignal();
|
||
}
|
||
|
||
WorkerThread4::WorkerThread4(CFocusMotorControl * ctrlFocusMotor)
|
||
{
|
||
m_ctrlFocusMotor = ctrlFocusMotor;
|
||
}
|
||
|
||
void WorkerThread4::run()
|
||
{
|
||
while (true)
|
||
{
|
||
int progress = m_ctrlFocusMotor->GetProgressIndex();
|
||
|
||
//std::cout << "WorkerThread4::run----自动调焦进度：" << progress << std::endl;
|
||
|
||
emit AutoFocusProgressSignal(progress);
|
||
if (progress == 100)
|
||
{
|
||
//std::cout << "自动调焦完成！" << std::endl;
|
||
break;
|
||
}
|
||
|
||
msleep(200);
|
||
}
|
||
}
|
||
|
||
|
||
//-------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||
|
||
MotionCaptureCoordinator::MotionCaptureCoordinator(
|
||
IrisMultiMotorController* motorCtrl,
|
||
ImagerOperationBase* cameraCtrl,
|
||
QObject* parent)
|
||
: QObject(parent)
|
||
, m_motorCtrl(motorCtrl)
|
||
, m_cameraCtrl(cameraCtrl)
|
||
, m_currentPos(0)
|
||
, m_endPos(0)
|
||
, m_isRunning(false)
|
||
{
|
||
//这些信号槽是按照逻辑顺序的
|
||
connect(this, SIGNAL(moveTo(int, double, double, int)),
|
||
m_motorCtrl, SLOT(moveTo(int, double, double, int)));
|
||
|
||
connect(m_motorCtrl, &IrisMultiMotorController::motorStopSignal,
|
||
this, &MotionCaptureCoordinator::handlePositionReached);
|
||
//connect(m_motorCtrl, &IrisMultiMotorController::moveFailed,
|
||
// this, &MotionCaptureCoordinator::handleError);
|
||
|
||
connect(this, SIGNAL(getFocusIndexSobel()),
|
||
m_cameraCtrl, SLOT(getFocusIndexSobel()));
|
||
|
||
connect(m_cameraCtrl, &ImagerOperationBase::FocusIndexSobelSignal,
|
||
this, &MotionCaptureCoordinator::handleCaptureComplete);
|
||
//connect(m_cameraCtrl, &ImagerOperationBase::captureFailed,
|
||
// this, &MotionCaptureCoordinator::handleError);
|
||
}
|
||
|
||
MotionCaptureCoordinator::~MotionCaptureCoordinator()
|
||
{
|
||
|
||
}
|
||
|
||
void MotionCaptureCoordinator::startStepMotion(double speed, int stepInterval, double startPos, double endPos)
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
|
||
if (m_isRunning)
|
||
{
|
||
emit errorOccurred("Sequence already running");
|
||
return;
|
||
}
|
||
|
||
m_counter = 0;
|
||
|
||
m_positionData.clear();
|
||
|
||
m_speed = speed;
|
||
m_iStepInterval = stepInterval;
|
||
m_iStepIntervalRealTime = 1;
|
||
m_currentPos = startPos;
|
||
m_endPos = endPos;
|
||
m_posInternal = (endPos - startPos) / stepInterval;
|
||
|
||
m_isRunning = true;
|
||
|
||
processNextPosition();
|
||
}
|
||
|
||
void MotionCaptureCoordinator::stopStepMotion()
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
m_isRunning = false;
|
||
emit sequenceStopped();
|
||
}
|
||
|
||
QVector<PositionData> MotionCaptureCoordinator::getAllPositionData() const
|
||
{
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
return m_positionData;
|
||
}
|
||
|
||
bool MotionCaptureCoordinator::saveToCsv(const QString& filename)
|
||
{
|
||
//QMutexLocker locker(&m_dataMutex);
|
||
|
||
QFile file(filename);
|
||
if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
QTextStream out(&file);
|
||
out << "Timestamp,targetPosition,ActualPosition,FocusIndex\n";
|
||
|
||
for (const auto& data : m_positionData)
|
||
{
|
||
out << data.timestamp.toString("yyyy-MM-dd HH:mm:ss.zzz") << ","
|
||
<< QString::number(data.targetPosition, 'f', 4) << ","
|
||
<< QString::number(data.actualPosition, 'f', 4) << ","
|
||
<< QString::number(data.cameraIndex, 'f', 4) << "\n";
|
||
}
|
||
|
||
file.close();
|
||
return true;
|
||
}
|
||
|
||
void MotionCaptureCoordinator::handlePositionReached(int motorID, double pos)
|
||
{
|
||
if (!m_isRunning) return;
|
||
|
||
QMutexLocker locker(&m_dataMutex);
|
||
|
||
//验证马达运动位置是否到达指定位置
|
||
//if (pos != m_currentPos) return;
|
||
|
||
// 记录位置信息
|
||
PositionData data;
|
||
data.targetPosition = m_currentPos;
|
||
data.actualPosition = pos;
|
||
data.timestamp = QDateTime::currentDateTime();
|
||
m_positionData.append(data);
|
||
|
||
// 开始采集
|
||
emit getFocusIndexSobel();
|
||
}
|
||
|
||
void MotionCaptureCoordinator::handleCaptureComplete(double index)
|
||
{
|
||
if (!m_isRunning) return;
|
||
|
||
QMutexLocker locker(&m_dataMutex);
|
||
|
||
// 更新最近一条记录的相机指数
|
||
//if (!m_positionData.isEmpty() &&
|
||
// m_positionData.last().targetPosition == m_positionData.last().actualPosition)
|
||
//{
|
||
// m_positionData.last().cameraIndex = index;
|
||
//}
|
||
m_positionData.last().cameraIndex = index;
|
||
|
||
m_currentPos += m_posInternal;
|
||
m_iStepIntervalRealTime++;
|
||
emit progressChanged(m_iStepIntervalRealTime);
|
||
|
||
m_counter += 1;
|
||
|
||
std::cout << "第" << m_counter << "次采集：" << std::endl;
|
||
std::cout << "目标位置：" << m_positionData.last().targetPosition << std::endl;
|
||
std::cout << "实际位置：" << m_positionData.last().actualPosition << std::endl;
|
||
|
||
processNextPosition();
|
||
}
|
||
|
||
void MotionCaptureCoordinator::handleError(const QString& error)
|
||
{
|
||
QMutexLocker locker(&m_dataMutex);
|
||
m_isRunning = false;
|
||
emit errorOccurred(error);
|
||
}
|
||
|
||
void MotionCaptureCoordinator::processNextPosition()
|
||
{
|
||
if (!m_isRunning) return;
|
||
|
||
if (m_currentPos > m_endPos)
|
||
{
|
||
m_isRunning = false;
|
||
emit sequenceComplete();
|
||
|
||
return;
|
||
}
|
||
|
||
emit moveTo(0, m_currentPos, m_speed, 1000);
|
||
}
|