add，山地所贡嘎山9：
1、解析gps数据； 2、帧率与旋转速度计算器：给定帧率、高度、FOV、传感器像元数，计算旋转速度； 3、修改采集逻辑：（1）采集gps（2）高光谱曝光：以10hz为下限，在保证曝光质量的前提下提高帧率；（3）fodis曝光；（4）以步骤2中的帧率为基础，结合高度FOV等信息计算旋转速度；（5）采集数据；
This commit is contained in:
tangchao0503
@ -1,4 +1,5 @@
|
||||
#include "CaptureCoordinator.h"
|
||||
#include <algorithm>
|
||||
|
||||
TwoMotionCaptureCoordinator::TwoMotionCaptureCoordinator(
|
||||
IrisMultiMotorController* motorCtrl,
|
||||
@ -1021,11 +1022,11 @@ OneMotorMultiPosCoordinator::OneMotorMultiPosCoordinator(
|
||||
//connect(m_motorCtrl, &IrisMultiMotorController::moveFailed,
|
||||
// this, &OneMotorMultiPosCoordinator::handleError);
|
||||
|
||||
connect(this, &OneMotorMultiPosCoordinator::getFocusIndexSobel,
|
||||
connect(this, &OneMotorMultiPosCoordinator::startAutoExposureSignal,
|
||||
m_cameraCtrl, &ImagerOperationBase::auto_exposure);
|
||||
|
||||
connect(m_cameraCtrl, &ImagerOperationBase::autoExposureSignal,
|
||||
this, &OneMotorMultiPosCoordinator::handleCaptureComplete);
|
||||
this, &OneMotorMultiPosCoordinator::onAutoExposureFinished);
|
||||
//connect(m_cameraCtrl, &ImagerOperationBase::captureFailed,
|
||||
// this, &OneMotorMultiPosCoordinator::handleError);
|
||||
}
|
||||
@ -1146,31 +1147,49 @@ void OneMotorMultiPosCoordinator::handlePositionReached(int motorID, double pos)
|
||||
data.targetPosition = m_currentPos;
|
||||
data.actualPosition = pos;
|
||||
data.timestamp = QDateTime::currentDateTime();
|
||||
data.frameRate = 10;
|
||||
m_positionData.append(data);
|
||||
|
||||
// 开始采集
|
||||
emit getFocusIndexSobel();
|
||||
// 开始自动曝光
|
||||
m_cameraCtrl->setFramerate(m_positionData.last().frameRate);
|
||||
emit startAutoExposureSignal();
|
||||
}
|
||||
|
||||
void OneMotorMultiPosCoordinator::handleCaptureComplete(double index)
|
||||
void OneMotorMultiPosCoordinator::onAutoExposureFinished(double exposureTime)
|
||||
{
|
||||
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().exposureTime = index;
|
||||
// 在保证曝光质量的前提下，尽量提高帧率，帧率下限为10hz
|
||||
|
||||
m_positionData.last().exposureTime = exposureTime;
|
||||
|
||||
std::cout << "第" << m_counter << "次曝光：" << std::endl;
|
||||
std::cout << "目标位置：" << m_positionData.last().targetPosition << std::endl;
|
||||
std::cout << "实际位置：" << m_positionData.last().actualPosition << std::endl;
|
||||
std::cout << "曝光时间：" << m_positionData.last().exposureTime << std::endl;
|
||||
|
||||
// 如果曝光时间过低（< 2ms），基于曝光时间计算新的帧率并重新进行自动曝光
|
||||
if (exposureTime < 2.0) {
|
||||
int currentFrameRate = m_positionData.last().frameRate;
|
||||
|
||||
// 基于曝光时间计算理论最大帧率：fps = 1000 / exposureTime(ms)
|
||||
// 乘以0.8作为安全系数，确保不会达到极限
|
||||
int newFrameRate = static_cast<int>((1000.0 / exposureTime) * 0.8);
|
||||
|
||||
// 设置合理的帧率范围：[currentFrameRate + 10, min(200, newFrameRate)]
|
||||
newFrameRate = std::max(currentFrameRate + 10, newFrameRate);
|
||||
newFrameRate = std::min(newFrameRate, 200);
|
||||
|
||||
m_positionData.last().frameRate = newFrameRate;
|
||||
|
||||
std::cout << "曝光时间过低，基于曝光时间计算新帧率：" << currentFrameRate << " -> " << newFrameRate << std::endl;
|
||||
std::cout << "（曝光时间 " << exposureTime << "ms -> 理论最大帧率 " << (1000.0 / exposureTime) << "fps）" << std::endl;
|
||||
|
||||
m_cameraCtrl->setFramerate(newFrameRate);
|
||||
}
|
||||
|
||||
processNextPosition();
|
||||
}
|
||||
|
||||
@ -1190,17 +1209,23 @@ void OneMotorMultiPosCoordinator::processNextPosition()
|
||||
m_isRunning = false;
|
||||
emit sequenceComplete(0);
|
||||
|
||||
// 计算平均曝光时间
|
||||
double avgExposureTime = 0.0;
|
||||
// 找到曝光时间最大的那组数据
|
||||
double maxExposureTime = 0.0;
|
||||
double maxFrameRate = 10;
|
||||
for (const auto& data : m_positionData) {
|
||||
avgExposureTime += data.exposureTime;
|
||||
}
|
||||
if (!m_positionData.isEmpty()) {
|
||||
avgExposureTime /= m_positionData.size();
|
||||
if (data.exposureTime > maxExposureTime) {
|
||||
maxExposureTime = data.exposureTime;
|
||||
maxFrameRate = data.frameRate;
|
||||
}
|
||||
}
|
||||
|
||||
emit hyperAutoExposureDoneSignal(avgExposureTime);
|
||||
m_cameraCtrl->setIntegrationTime(avgExposureTime);
|
||||
std::cout << "自动曝光完成，使用最大曝光时间参数：" << std::endl;
|
||||
std::cout << " 曝光时间：" << maxExposureTime << "ms" << std::endl;
|
||||
std::cout << " 帧率：" << maxFrameRate << "Hz" << std::endl;
|
||||
|
||||
emit hyperAutoExposureDoneSignal(maxExposureTime, maxFrameRate);
|
||||
m_cameraCtrl->setIntegrationTime(maxExposureTime);
|
||||
m_cameraCtrl->setFramerate(maxFrameRate);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
@ -308,11 +308,12 @@ struct PositionsLogData
|
||||
{
|
||||
double targetPosition; // 目标位置
|
||||
double actualPosition; // 实际马达位置
|
||||
double frameRate; // 帧率
|
||||
double exposureTime; //
|
||||
QDateTime timestamp; // 时间戳
|
||||
|
||||
PositionsLogData(double target = 0, double actual = 0.0, double exposure = 0.0)
|
||||
: targetPosition(target), actualPosition(actual),
|
||||
PositionsLogData(double target = 0, double actual = 0.0, double exposure = 0.0, double frameRate = 10.0)
|
||||
: targetPosition(target), actualPosition(actual), frameRate(frameRate),
|
||||
exposureTime(exposure ), timestamp(QDateTime::currentDateTime()) {
|
||||
}
|
||||
};
|
||||
@ -340,14 +341,14 @@ signals:
|
||||
void sequenceStopped();
|
||||
void errorOccurred(const QString& error);
|
||||
void moveTo(int, double, double, int);
|
||||
void getFocusIndexSobel();
|
||||
void startAutoExposureSignal();
|
||||
void zeroStart(int motorID);
|
||||
|
||||
void hyperAutoExposureDoneSignal(double exposureTime);
|
||||
void hyperAutoExposureDoneSignal(double exposureTime, double frameRate);
|
||||
|
||||
private slots:
|
||||
void handlePositionReached(int motorID, double pos);
|
||||
void handleCaptureComplete(double index);
|
||||
void onAutoExposureFinished(double exposureTime);
|
||||
void handleError(const QString& error);
|
||||
void handleZeroComplete(int motorID, double pos);
|
||||
|
||||
|
||||
@ -1,60 +1,9 @@
|
||||
#include "GonggaShanRecordCtl.h"
|
||||
|
||||
GonggashanTaskScheduler::GonggashanTaskScheduler(QObject* parent)
|
||||
: QObject(parent)
|
||||
, m_taskState(TaskState::Idle)
|
||||
{
|
||||
}
|
||||
|
||||
GonggashanTaskScheduler::~GonggashanTaskScheduler()
|
||||
{
|
||||
}
|
||||
|
||||
bool GonggashanTaskScheduler::isTaskRunning() const
|
||||
{
|
||||
return m_taskState == TaskState::Running;
|
||||
}
|
||||
|
||||
void GonggashanTaskScheduler::setTaskRunning(bool running)
|
||||
{
|
||||
m_taskState = running ? TaskState::Running : TaskState::Idle;
|
||||
Q_EMIT taskStateChanged(m_taskState);
|
||||
}
|
||||
|
||||
void GonggaShanRecordCtl::logStatus(const QString& message, bool isHearderBlankLine, bool isTailBlankLine)
|
||||
{
|
||||
QString timestamp = QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss");
|
||||
|
||||
if (isHearderBlankLine)
|
||||
{
|
||||
ui.status_textEdit->append("\n");
|
||||
}
|
||||
ui.status_textEdit->append(QString("[%1] %2").arg(timestamp, message));
|
||||
if (isTailBlankLine)
|
||||
{
|
||||
ui.status_textEdit->append("\n");
|
||||
}
|
||||
|
||||
if (m_logStream.device())
|
||||
{
|
||||
if (isHearderBlankLine)
|
||||
{
|
||||
m_logStream << "\n";
|
||||
}
|
||||
m_logStream << "[" << timestamp << "] " << message << "\n";
|
||||
if (isTailBlankLine)
|
||||
{
|
||||
m_logStream << "\n";
|
||||
}
|
||||
|
||||
m_logStream.flush();
|
||||
m_logFile.flush();
|
||||
}
|
||||
}
|
||||
#include <cmath>
|
||||
|
||||
GonggaShanRecordCtl::GonggaShanRecordCtl(QWidget* parent)
|
||||
: QDialog(parent)
|
||||
, m_taskScheduler(new GonggashanTaskScheduler(this))
|
||||
, m_taskExecutor(new GonggashanTaskExecutor(this))
|
||||
{
|
||||
ui.setupUi(this);
|
||||
@ -68,19 +17,40 @@ GonggaShanRecordCtl::GonggaShanRecordCtl(QWidget* parent)
|
||||
AppSettings::instance().setGonggaShanRecordPort(value);
|
||||
});
|
||||
|
||||
// 初始化GPS读取器（内部包含解析器）
|
||||
m_gpsReader = new GsbGpsReader(this);
|
||||
m_gpsReader->gpsParser()->setMinSatelliteCount(4);
|
||||
|
||||
// 连接 GonggashanTaskExecutor 信号
|
||||
connect(m_taskExecutor, &GonggashanTaskExecutor::finished,
|
||||
this, &GonggaShanRecordCtl::onTaskExecutorFinished);
|
||||
connect(m_taskExecutor, &GonggashanTaskExecutor::gpsAcquisitionSignal,
|
||||
this, &GonggaShanRecordCtl::onGpsAcquisition);
|
||||
connect(this, &GonggaShanRecordCtl::gpsAcquisitionDoneSignal_gonggashan,
|
||||
m_taskExecutor, &GonggashanTaskExecutor::onGpsAcquired);
|
||||
|
||||
connect(m_taskExecutor, &GonggashanTaskExecutor::hyperAutoExposureSignal_gonggashan,
|
||||
this, &GonggaShanRecordCtl::hyperAutoExposureSignal_gonggashan);
|
||||
connect(this, &GonggaShanRecordCtl::hyperAutoExposureDoneSignal_gonggashan,
|
||||
m_taskExecutor, &GonggashanTaskExecutor::onHyperExposureComplete);
|
||||
|
||||
connect(m_taskExecutor, &GonggashanTaskExecutor::fiberExposureSignal,
|
||||
this, &GonggaShanRecordCtl::fiberExposureSignal_gonggashan);
|
||||
connect(m_taskExecutor, &GonggashanTaskExecutor::fiberExposureRecordSignal,
|
||||
this, &GonggaShanRecordCtl::fiberExposureRecordSignal_gonggashan);
|
||||
connect(this, &GonggaShanRecordCtl::fiberExposureDoneSignal_gonggashan,
|
||||
m_taskExecutor, &GonggashanTaskExecutor::onFiberExposureComplete);
|
||||
|
||||
connect(m_taskExecutor, &GonggashanTaskExecutor::calculateMotorSpeedSignal,
|
||||
this, &GonggaShanRecordCtl::onCalculateMotorSpeed);
|
||||
connect(this, &GonggaShanRecordCtl::calculateMotorSpeedDoneSignal_gonggashan,
|
||||
m_taskExecutor, &GonggashanTaskExecutor::onMotorSpeedCalculated);
|
||||
|
||||
connect(m_taskExecutor, &GonggashanTaskExecutor::startCollectionSignal,
|
||||
this, &GonggaShanRecordCtl::startRcordSignal_gonggashan);
|
||||
connect(this, &GonggaShanRecordCtl::recordFinishedSignal_gonggashan,
|
||||
m_taskExecutor, &GonggashanTaskExecutor::onRcordComplete);
|
||||
|
||||
connect(m_taskExecutor, &GonggashanTaskExecutor::finished,
|
||||
this, &GonggaShanRecordCtl::onTaskExecutorFinished);
|
||||
|
||||
//onCalculateMotorSpeed();//测试用
|
||||
}
|
||||
|
||||
GonggaShanRecordCtl::~GonggaShanRecordCtl()
|
||||
@ -90,6 +60,133 @@ GonggaShanRecordCtl::~GonggaShanRecordCtl()
|
||||
m_logFile.close();
|
||||
}
|
||||
tcpServer6005->deleteLater();
|
||||
|
||||
if (m_gpsReader) {
|
||||
m_gpsReader->closePort();
|
||||
m_gpsReader->deleteLater();
|
||||
}
|
||||
}
|
||||
|
||||
void GonggaShanRecordCtl::onGpsAcquisition()
|
||||
{
|
||||
logStatus(QStringLiteral("开始GPS定位采集..."), true, false);
|
||||
|
||||
// 如果已有GPS读取器，先关闭
|
||||
if (m_gpsReader) {
|
||||
m_gpsReader->closePort();
|
||||
m_gpsReader->deleteLater();
|
||||
m_gpsReader = nullptr;
|
||||
}
|
||||
|
||||
// 创建GPS读取器并打开COM17端口
|
||||
m_gpsReader = new GsbGpsReader(this);
|
||||
|
||||
// 连接GPS数据就绪信号
|
||||
connect(m_gpsReader, &GsbGpsReader::gpsDataReady, this, [this](const GsbGpsParse::GpsData& data) {
|
||||
if (data.isValid) {
|
||||
// 格式化时间
|
||||
QString timeStr;
|
||||
if (data.utcTime.length() >= 6) {
|
||||
timeStr = QString("%1:%2:%3")
|
||||
.arg(data.utcTime.mid(0, 2))
|
||||
.arg(data.utcTime.mid(2, 2))
|
||||
.arg(data.utcTime.mid(4, 2));
|
||||
}
|
||||
|
||||
// 格式化日期
|
||||
QString dateStr;
|
||||
if (data.utcDate.length() == 6) {
|
||||
dateStr = QString("20%1-%2-%3")
|
||||
.arg(data.utcDate.mid(4, 2))
|
||||
.arg(data.utcDate.mid(2, 2))
|
||||
.arg(data.utcDate.mid(0, 2));
|
||||
}
|
||||
|
||||
QString info = QStringLiteral("GPS数据已获取:")
|
||||
+ QStringLiteral("\n 时间: %1")
|
||||
+ QStringLiteral("\n 日期: %2")
|
||||
+ QStringLiteral("\n 纬度: %3°")
|
||||
+ QStringLiteral("\n 经度: %4°")
|
||||
+ QStringLiteral("\n 高程: %5m")
|
||||
+ QStringLiteral("\n 卫星数: %6")
|
||||
+ QStringLiteral("\n 状态: %7");
|
||||
|
||||
logStatus(info
|
||||
.arg(timeStr)
|
||||
.arg(dateStr)
|
||||
.arg(data.latitude, 0, 'f', 6)
|
||||
.arg(data.longitude, 0, 'f', 6)
|
||||
.arg(data.altitude, 0, 'f', 2)
|
||||
.arg(data.satelliteCount)
|
||||
.arg(data.status == GsbGpsParse::GpsStatus::Valid ? QStringLiteral("已定位") :
|
||||
data.status == GsbGpsParse::GpsStatus::Differential ? QStringLiteral("差分定位") : QStringLiteral("未定位")),
|
||||
false, true);
|
||||
|
||||
// 发送GPS数据给任务执行器
|
||||
emit gpsAcquisitionDoneSignal_gonggashan(data.latitude, data.longitude, data.altitude);
|
||||
}
|
||||
});
|
||||
|
||||
// 连接端口错误信号
|
||||
connect(m_gpsReader, &GsbGpsReader::portError, this, [this](const QString& error) {
|
||||
logStatus(QStringLiteral("GPS端口错误: %1").arg(error), true, true);
|
||||
});
|
||||
|
||||
// 尝试打开串口 (G6301 USB GPS, 默认波特率4800)
|
||||
QString portName = "COM19";
|
||||
if (m_gpsReader->openPort(portName, 4800)) {
|
||||
logStatus(QStringLiteral("已打开GPS端口: %1 (波特率: 4800)").arg(portName));
|
||||
|
||||
// 等待有效GPS数据 (最多等待30秒)
|
||||
if (m_gpsReader->waitForValidData(30000)) {
|
||||
logStatus(QStringLiteral("GPS定位成功!"), false, true);
|
||||
} else {
|
||||
logStatus(QStringLiteral("GPS定位超时(30s)，未能获取有效数据"), false, true);
|
||||
logStatus(QStringLiteral("提示: 请检查GPS天线是否放置在开阔区域，卫星信号可能需要1-2分钟稳定"), false, true);
|
||||
}
|
||||
} else {
|
||||
logStatus(QStringLiteral("无法打开GPS端口: %1").arg(portName), true, true);
|
||||
}
|
||||
}
|
||||
|
||||
void GonggaShanRecordCtl::onCalculateMotorSpeed(double hyperFrameRate)
|
||||
{
|
||||
FramerateRotateSpeedCal tmp;
|
||||
double rotationSpeed = tmp.calculate(300, 17.6, 900, hyperFrameRate);
|
||||
double rotationSpeed2 = tmp.calculate(300, 17.6, 900, 60);
|
||||
|
||||
int a;
|
||||
}
|
||||
|
||||
void GonggaShanRecordCtl::logStatus(const QString& message, bool isHearderBlankLine, bool isTailBlankLine)
|
||||
{
|
||||
QString timestamp = QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss");
|
||||
|
||||
if (isHearderBlankLine)
|
||||
{
|
||||
ui.status_textEdit->append("\n");
|
||||
}
|
||||
ui.status_textEdit->append(QString("[%1] %2").arg(timestamp, message));
|
||||
if (isTailBlankLine)
|
||||
{
|
||||
ui.status_textEdit->append("\n");
|
||||
}
|
||||
|
||||
if (m_logStream.device())
|
||||
{
|
||||
if (isHearderBlankLine)
|
||||
{
|
||||
m_logStream << "\n";
|
||||
}
|
||||
m_logStream << "[" << timestamp << "] " << message << "\n";
|
||||
if (isTailBlankLine)
|
||||
{
|
||||
m_logStream << "\n";
|
||||
}
|
||||
|
||||
m_logStream.flush();
|
||||
m_logFile.flush();
|
||||
}
|
||||
}
|
||||
|
||||
void GonggaShanRecordCtl::startListen()
|
||||
@ -130,7 +227,6 @@ void GonggaShanRecordCtl::startRecord(int position)
|
||||
QString pos = QString::number(position);
|
||||
logStatus("Reach pos: " + pos, true);
|
||||
|
||||
m_taskScheduler->setTaskRunning(true);
|
||||
m_taskExecutor->start("pos_" + pos);
|
||||
}
|
||||
|
||||
@ -148,8 +244,6 @@ void GonggaShanRecordCtl::onFiberImagerExposureCompleteSignal(int exposureTime)
|
||||
|
||||
void GonggaShanRecordCtl::onRcordFinished()
|
||||
{
|
||||
m_taskScheduler->setTaskRunning(false);
|
||||
|
||||
logStatus("Record Finished.");
|
||||
}
|
||||
|
||||
@ -161,7 +255,6 @@ void GonggaShanRecordCtl::onTaskExecutorFinished(bool success)
|
||||
} else {
|
||||
logStatus("Task failed or stopped.");
|
||||
}
|
||||
m_taskScheduler->setTaskRunning(false);
|
||||
}
|
||||
|
||||
// ==================== GonggashanTaskExecutor 实现 ====================
|
||||
@ -183,24 +276,37 @@ GonggashanTaskExecutor::~GonggashanTaskExecutor()
|
||||
void GonggashanTaskExecutor::buildStateMachine()
|
||||
{
|
||||
// ---- 创建所有状态 ----
|
||||
m_gpsAcquisitionState = new QState(QState::ExclusiveStates);//1、获取当前gps位置高度和时间；2、是否同步电脑系统时间
|
||||
m_gpsAcquisitionState->setObjectName("GpsAcquisition");
|
||||
|
||||
m_hyperExposureState = new QState(QState::ExclusiveStates);
|
||||
m_hyperExposureState->setObjectName("HyperExposure");
|
||||
|
||||
m_fiberExposureState = new QState(QState::ExclusiveStates);
|
||||
m_fiberExposureState->setObjectName("FiberExposure");
|
||||
|
||||
m_gpsAcquisitionState = new QState(QState::ExclusiveStates);
|
||||
m_gpsAcquisitionState->setObjectName("GpsAcquisition");
|
||||
|
||||
m_motorCalcState = new QState(QState::ExclusiveStates);
|
||||
m_motorCalcState = new QState(QState::ExclusiveStates);//根据高度计算电机转速，和高光谱帧率
|
||||
m_motorCalcState->setObjectName("MotorCalc");
|
||||
|
||||
m_dataCollectionState = new QState(QState::ExclusiveStates);
|
||||
m_dataCollectionState = new QState(QState::ExclusiveStates);//高光谱数据采集完成后，将gps信息写入hdr文件
|
||||
m_dataCollectionState->setObjectName("DataCollection");
|
||||
|
||||
m_finalState = new QFinalState();
|
||||
m_finalState->setObjectName("Completed");
|
||||
|
||||
// ---- GpsAcquisition 状态：等待外部硬件回调 ----
|
||||
connect(m_gpsAcquisitionState, &QState::entered, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Enter m_gpsAcquisitionState";
|
||||
emit gpsAcquisitionSignal();
|
||||
});
|
||||
connect(m_gpsAcquisitionState, &QState::exited, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Exit m_gpsAcquisitionState";
|
||||
leavePhase(m_gpsAcquisitionState);
|
||||
});
|
||||
QSignalTransition* gpsDone = new QSignalTransition(this, &GonggashanTaskExecutor::gpsAcquisitionDone);
|
||||
gpsDone->setTargetState(m_hyperExposureState);
|
||||
m_gpsAcquisitionState->addTransition(gpsDone);
|
||||
|
||||
// ---- HyperExposure 状态：等待外部硬件回调 ----
|
||||
connect(m_hyperExposureState, &QState::entered, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Enter m_hyperExposureState";
|
||||
@ -219,37 +325,25 @@ void GonggashanTaskExecutor::buildStateMachine()
|
||||
// ---- FiberExposure 状态：等待外部硬件回调 ----
|
||||
connect(m_fiberExposureState, &QState::entered, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Enter m_fiberExposureState";
|
||||
emit fiberExposureSignal();
|
||||
emit fiberExposureRecordSignal(m_posInfo);
|
||||
});
|
||||
connect(m_fiberExposureState, &QState::exited, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Exit m_fiberExposureState";
|
||||
leavePhase(m_fiberExposureState);
|
||||
});
|
||||
QSignalTransition* fiberDone = new QSignalTransition(this, &GonggashanTaskExecutor::fiberExposureDone);
|
||||
fiberDone->setTargetState(m_gpsAcquisitionState);
|
||||
fiberDone->setTargetState(m_motorCalcState);
|
||||
m_fiberExposureState->addTransition(fiberDone);
|
||||
|
||||
// ---- GpsAcquisition 状态：等待外部硬件回调 ----
|
||||
connect(m_gpsAcquisitionState, &QState::entered, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Enter m_gpsAcquisitionState";
|
||||
emit gpsAcquisitionSignal();
|
||||
});
|
||||
connect(m_gpsAcquisitionState, &QState::exited, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Exit m_gpsAcquisitionState";
|
||||
leavePhase(m_gpsAcquisitionState);
|
||||
});
|
||||
QSignalTransition* gpsDone = new QSignalTransition(this, &GonggashanTaskExecutor::gpsAcquisitionDone);
|
||||
gpsDone->setTargetState(m_motorCalcState);
|
||||
m_gpsAcquisitionState->addTransition(gpsDone);
|
||||
|
||||
// ---- MotorCalc 状态：等待外部硬件回调 ----
|
||||
connect(m_motorCalcState, &QState::entered, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Enter m_motorCalcState";
|
||||
});
|
||||
emit calculateMotorSpeedSignal(m_hyperFrameRate);
|
||||
});
|
||||
connect(m_motorCalcState, &QState::exited, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Exit m_motorCalcState";
|
||||
leavePhase(m_motorCalcState);
|
||||
});
|
||||
});
|
||||
QSignalTransition* motorDone = new QSignalTransition(this, &GonggashanTaskExecutor::motorSpeedDone);
|
||||
motorDone->setTargetState(m_dataCollectionState);
|
||||
m_motorCalcState->addTransition(motorDone);
|
||||
@ -257,9 +351,7 @@ void GonggashanTaskExecutor::buildStateMachine()
|
||||
// ---- DataCollection 状态：同步阶段 ----
|
||||
connect(m_dataCollectionState, &QState::entered, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Enter m_dataCollectionState";
|
||||
if (dataCollectionImpl()) {
|
||||
QTimer::singleShot(0, this, &GonggashanTaskExecutor::dataCollectionDone);
|
||||
}
|
||||
emit startCollectionSignal(m_posInfo, m_gpsData);
|
||||
});
|
||||
connect(m_dataCollectionState, &QState::exited, this, [this]() {
|
||||
qDebug() << "GonggashanTaskExecutor: Exit m_dataCollectionState";
|
||||
@ -284,15 +376,14 @@ void GonggashanTaskExecutor::buildStateMachine()
|
||||
}
|
||||
|
||||
// ---- 设置状态机 ----
|
||||
m_machine->addState(m_hyperExposureState);
|
||||
m_machine->addState(m_fiberExposureState);
|
||||
m_machine->addState(m_gpsAcquisitionState);
|
||||
m_machine->addState(m_motorCalcState);
|
||||
m_machine->addState(m_hyperExposureState);
|
||||
m_machine->addState(m_fiberExposureState);
|
||||
m_machine->addState(m_dataCollectionState);
|
||||
m_machine->addState(m_finalState);
|
||||
|
||||
m_machine->setInitialState(m_hyperExposureState);
|
||||
|
||||
m_machine->setInitialState(m_gpsAcquisitionState);
|
||||
}
|
||||
|
||||
QState* GonggashanTaskExecutor::currentState() const
|
||||
@ -341,15 +432,16 @@ void GonggashanTaskExecutor::stop()
|
||||
emit stopRequested();
|
||||
}
|
||||
|
||||
void GonggashanTaskExecutor::onHyperExposureComplete(int exposureTime)
|
||||
void GonggashanTaskExecutor::onHyperExposureComplete(double exposureTime, double frameRate)
|
||||
{
|
||||
if (currentState() != m_hyperExposureState) return;
|
||||
m_hyperExposureTime = exposureTime;
|
||||
m_hyperFrameRate = frameRate;
|
||||
hyperExposureImpl(exposureTime);
|
||||
emit hyperExposureDone();
|
||||
}
|
||||
|
||||
void GonggashanTaskExecutor::onFiberExposureComplete(int exposureTime)
|
||||
void GonggashanTaskExecutor::onFiberExposureComplete(double exposureTime)
|
||||
{
|
||||
if (currentState() != m_fiberExposureState) return;
|
||||
m_fiberExposureTime = exposureTime;
|
||||
@ -375,6 +467,12 @@ void GonggashanTaskExecutor::onMotorSpeedCalculated(double speed)
|
||||
}
|
||||
}
|
||||
|
||||
void GonggashanTaskExecutor::onRcordComplete()
|
||||
{
|
||||
if (currentState() != m_dataCollectionState) return;
|
||||
emit dataCollectionDone();
|
||||
}
|
||||
|
||||
bool GonggashanTaskExecutor::preparationImpl()
|
||||
{
|
||||
qDebug() << "GonggashanTaskExecutor: Enter m_preparationState\n";
|
||||
@ -405,9 +503,514 @@ bool GonggashanTaskExecutor::motorCalcImpl()
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GonggashanTaskExecutor::dataCollectionImpl()
|
||||
// ==================== GsbGpsParse 实现 ====================
|
||||
|
||||
GsbGpsParse::GsbGpsParse(QObject* parent)
|
||||
: QObject(parent)
|
||||
{
|
||||
qDebug() << "GonggashanTaskExecutor: Start data collection";
|
||||
emit startCollectionSignal(m_posInfo, m_gpsData);
|
||||
m_gpsData.hasGga = false;
|
||||
m_gpsData.hasRmc = false;
|
||||
}
|
||||
|
||||
bool GsbGpsParse::parseNmeaSentence(const QString& sentence)
|
||||
{
|
||||
QString trimmed = sentence.trimmed();
|
||||
if (trimmed.isEmpty() || !trimmed.startsWith('$')) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// NMEA校验
|
||||
if (!validateNmeaChecksum(trimmed)) {
|
||||
qDebug() << "GsbGpsParse: Invalid NMEA checksum:" << trimmed;
|
||||
return false;
|
||||
}
|
||||
|
||||
QStringList fields = trimmed.split(',');
|
||||
if (fields.size() < 3) {
|
||||
return false;
|
||||
}
|
||||
|
||||
QString talkerId = fields[0].mid(1, 2); // GP, GL, GA, etc.
|
||||
QString sentenceType = fields[0].mid(3);
|
||||
|
||||
if (sentenceType == "GGA") {
|
||||
return parseGpgga(fields);
|
||||
} else if (sentenceType == "RMC") {
|
||||
return parseGprmc(fields);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool GsbGpsParse::parseGpgga(const QStringList& fields)
|
||||
{
|
||||
// $GPGGA 格式:
|
||||
// 0: $GPGGA
|
||||
// 1: UTC时间 HHMMSS.SSS
|
||||
// 2: 纬度 DDMM.MMMM
|
||||
// 3: N/S
|
||||
// 4: 经度 DDDMM.MMMM
|
||||
// 5: E/W
|
||||
// 6: GPS质量指示 (0=无效, 1=GPS定位, 2=差分GPS定位)
|
||||
// 7: 卫星数目
|
||||
// 8: HDOP
|
||||
// 9: 海拔高度 (米)
|
||||
// 10: 海拔高度单位 (M)
|
||||
// 11: 大地水准面差
|
||||
// 12: 大地水准面差单位 (M)
|
||||
// 13: 差分GPS数据龄期
|
||||
// 14: 差分参考站ID
|
||||
|
||||
if (fields.size() < 12) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ok = false;
|
||||
m_gpsData.rawNmea = fields.join(',');
|
||||
|
||||
// 保存时间
|
||||
m_pendingTime = fields[1];
|
||||
|
||||
// 解析纬度
|
||||
double lat = convertLatitude(fields[2], fields[3]);
|
||||
if (lat != 0.0) {
|
||||
m_gpsData.latitude = lat;
|
||||
}
|
||||
|
||||
// 解析经度
|
||||
double lon = convertLongitude(fields[4], fields[5]);
|
||||
if (lon != 0.0) {
|
||||
m_gpsData.longitude = lon;
|
||||
}
|
||||
|
||||
// 解析GPS定位质量指示
|
||||
int quality = fields[6].toInt(&ok);
|
||||
if (ok) {
|
||||
switch (quality) {
|
||||
case 0:
|
||||
m_gpsData.status = GpsStatus::Invalid;
|
||||
break;
|
||||
case 1:
|
||||
m_gpsData.status = GpsStatus::Valid;
|
||||
break;
|
||||
case 2:
|
||||
m_gpsData.status = GpsStatus::Differential;
|
||||
break;
|
||||
default:
|
||||
m_gpsData.status = GpsStatus::Invalid;
|
||||
}
|
||||
}
|
||||
|
||||
// 解析卫星数目
|
||||
int satCount = fields[7].toInt(&ok);
|
||||
if (ok) {
|
||||
m_gpsData.satelliteCount = satCount;
|
||||
}
|
||||
|
||||
// 解析高程
|
||||
double altitude = fields[9].toDouble(&ok);
|
||||
if (ok) {
|
||||
m_gpsData.altitude = altitude;
|
||||
}
|
||||
|
||||
// 应用暂存的时间（从RMC获取的日期）
|
||||
if (!m_pendingTime.isEmpty() && m_pendingTime.length() >= 6) {
|
||||
m_gpsData.utcTime = m_pendingTime;
|
||||
}
|
||||
if (!m_pendingDate.isEmpty()) {
|
||||
m_gpsData.utcDate = m_pendingDate;
|
||||
}
|
||||
|
||||
// 标记已收到GPGGA
|
||||
m_gpsData.hasGga = true;
|
||||
|
||||
// 只有当GPGGA和GPRMC都收到后才发送信号
|
||||
if (m_gpsData.hasRmc) {
|
||||
bool wasValid = m_gpsData.isValid;
|
||||
validateData();
|
||||
emit gpsDataUpdated(m_gpsData);
|
||||
if (wasValid != m_gpsData.isValid) {
|
||||
emit gpsStatusChanged(m_gpsData.isValid);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GsbGpsParse::parseGprmc(const QStringList& fields)
|
||||
{
|
||||
// $GPRMC 格式:
|
||||
// 0: $GPRMC
|
||||
// 1: UTC时间 HHMMSS.SSS
|
||||
// 2: 状态 (A=有效, V=无效)
|
||||
// 3: 纬度 DDMM.MMMMM
|
||||
// 4: N/S
|
||||
// 5: 经度 DDDMM.MMMMM
|
||||
// 6: E/W
|
||||
// 7: 速度 (节)
|
||||
// 8: 航向 (度)
|
||||
// 9: UTC日期 DDMMYY
|
||||
// 10: 磁偏角
|
||||
// 11: 磁偏角方向 E/W
|
||||
|
||||
if (fields.size() < 10) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 检查状态 - V表示无效，A表示有效
|
||||
QString status = fields[2];
|
||||
if (status != "A") {
|
||||
// 数据无效，清除之前的状态
|
||||
m_gpsData.status = GpsStatus::Invalid;
|
||||
m_gpsData.isValid = false;
|
||||
m_gpsData.hasRmc = false; // 清除RMC标记，等待下次有效数据
|
||||
return false;
|
||||
}
|
||||
|
||||
// 解析纬度 (DDMM.MMMMM -> DD.DDDDD)
|
||||
// 公式: DD + MM/60 + MMMMM/600000
|
||||
double lat = convertLatitude(fields[3], fields[4]);
|
||||
if (lat != 0.0) {
|
||||
m_gpsData.latitude = lat;
|
||||
}
|
||||
|
||||
// 解析经度 (DDDMM.MMMMM -> DDD.DDDDD)
|
||||
// 公式: DDD + MM/60 + MMMMM/600000
|
||||
double lon = convertLongitude(fields[5], fields[6]);
|
||||
if (lon != 0.0) {
|
||||
m_gpsData.longitude = lon;
|
||||
}
|
||||
|
||||
// 保存日期
|
||||
m_pendingDate = fields[9];
|
||||
m_gpsData.utcDate = fields[9];
|
||||
|
||||
// 保存时间
|
||||
QString time = fields[1];
|
||||
if (time.length() >= 6) {
|
||||
m_pendingTime = time;
|
||||
m_gpsData.utcTime = time;
|
||||
}
|
||||
|
||||
// 标记已收到RMC
|
||||
m_gpsData.hasRmc = true;
|
||||
|
||||
// 只有当GPGGA和GPRMC都收到后才发送信号
|
||||
if (m_gpsData.hasGga) {
|
||||
bool wasValid = m_gpsData.isValid;
|
||||
validateData();
|
||||
emit gpsDataUpdated(m_gpsData);
|
||||
if (wasValid != m_gpsData.isValid) {
|
||||
emit gpsStatusChanged(m_gpsData.isValid);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GsbGpsParse::validateNmeaChecksum(const QString& sentence)
|
||||
{
|
||||
// NMEA语句格式: $...*HH
|
||||
// HH是$和*之间所有字符的XOR校验和
|
||||
|
||||
int starIndex = sentence.indexOf('*');
|
||||
if (starIndex < 0 || starIndex + 2 > sentence.length()) {
|
||||
return true; // 没有校验和字段，假设有效
|
||||
}
|
||||
|
||||
QString checksumStr = sentence.mid(starIndex + 1, 2);
|
||||
bool ok;
|
||||
int expectedChecksum = checksumStr.toInt(&ok, 16);
|
||||
if (!ok) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 计算实际校验和
|
||||
int actualChecksum = 0;
|
||||
for (int i = 1; i < starIndex; ++i) {
|
||||
actualChecksum ^= sentence[i].toLatin1();
|
||||
}
|
||||
|
||||
return actualChecksum == expectedChecksum;
|
||||
}
|
||||
|
||||
double GsbGpsParse::convertLatitude(const QString& degMin, const QString& direction)
|
||||
{
|
||||
if (degMin.isEmpty()) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// 格式: DDMM.MMMMM (度分格式)
|
||||
// 公式: DD + MM/60 + MMMMM/3600000
|
||||
// 例如: 3150.34850 -> 31 + 50/60 + 34850/3600000 = 31.839141
|
||||
|
||||
bool ok;
|
||||
double value = degMin.toDouble(&ok);
|
||||
if (!ok) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
int degrees = static_cast<int>(value / 100);
|
||||
int minutesInt = static_cast<int>(value) % 100;
|
||||
double fractionalMinutes = value - static_cast<int>(value);
|
||||
|
||||
// 按用户指定公式: DD + MM/60 + MMMMM/3600000
|
||||
double decimal = degrees + (minutesInt / 60.0) + (fractionalMinutes * 100.0) / 600000.0;
|
||||
|
||||
if (direction == "S") {
|
||||
decimal = -decimal;
|
||||
}
|
||||
|
||||
return decimal;
|
||||
}
|
||||
|
||||
double GsbGpsParse::convertLongitude(const QString& degMin, const QString& direction)
|
||||
{
|
||||
if (degMin.isEmpty()) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// 格式: DDDMM.MMMMM (度分格式)
|
||||
// 公式: DDD + MM/60 + MMMMM/3600000
|
||||
// 例如: 11707.94287 -> 117 + 07/60 + 94287/3600000 = 117.132381
|
||||
|
||||
bool ok;
|
||||
double value = degMin.toDouble(&ok);
|
||||
if (!ok) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
int degrees = static_cast<int>(value / 100);
|
||||
int minutesInt = static_cast<int>(value) % 100;
|
||||
double fractionalMinutes = value - static_cast<int>(value);
|
||||
|
||||
// 按用户指定公式: DDD + MM/60 + MMMMM/3600000
|
||||
double decimal = degrees + (minutesInt / 60.0) + (fractionalMinutes * 100.0) / 600000.0;
|
||||
|
||||
if (direction == "W") {
|
||||
decimal = -decimal;
|
||||
}
|
||||
|
||||
return decimal;
|
||||
}
|
||||
|
||||
void GsbGpsParse::validateData()
|
||||
{
|
||||
// GPS数据有效性判定条件:
|
||||
// 1. GPS状态为Valid或Differential (status != Invalid)
|
||||
// 2. 卫星数目 >= 最小要求
|
||||
// 3. 经纬度不为0
|
||||
|
||||
bool isValid = (m_gpsData.status != GpsStatus::Invalid) &&
|
||||
(m_gpsData.satelliteCount >= m_minSatelliteCount) &&
|
||||
(m_gpsData.latitude != 0.0 || m_gpsData.longitude != 0.0);
|
||||
|
||||
m_gpsData.isValid = isValid;
|
||||
}
|
||||
|
||||
bool GsbGpsParse::isGpsDataValid() const
|
||||
{
|
||||
return m_gpsData.isValid;
|
||||
}
|
||||
|
||||
QString GsbGpsParse::toDisplayString() const
|
||||
{
|
||||
if (!m_gpsData.isValid) {
|
||||
return QString("GPS数据无效 - %1 (卫星: %2)")
|
||||
.arg(statusDescription())
|
||||
.arg(m_gpsData.satelliteCount);
|
||||
}
|
||||
|
||||
QString timeStr;
|
||||
if (m_gpsData.utcTime.length() >= 6) {
|
||||
timeStr = QString("%1:%2:%3")
|
||||
.arg(m_gpsData.utcTime.mid(0, 2))
|
||||
.arg(m_gpsData.utcTime.mid(2, 2))
|
||||
.arg(m_gpsData.utcTime.mid(4, 2));
|
||||
}
|
||||
|
||||
QString dateStr;
|
||||
if (m_gpsData.utcDate.length() == 6) {
|
||||
dateStr = QString("20%1-%2-%3")
|
||||
.arg(m_gpsData.utcDate.mid(4, 2))
|
||||
.arg(m_gpsData.utcDate.mid(2, 2))
|
||||
.arg(m_gpsData.utcDate.mid(0, 2));
|
||||
}
|
||||
|
||||
return QString("GPS: 时间=%1 日期=%2 纬度=%3 经度=%4 高程=%5m 卫星=%6 %7")
|
||||
.arg(timeStr)
|
||||
.arg(dateStr)
|
||||
.arg(m_gpsData.latitude, 0, 'f', 6)
|
||||
.arg(m_gpsData.longitude, 0, 'f', 6)
|
||||
.arg(m_gpsData.altitude, 0, 'f', 2)
|
||||
.arg(m_gpsData.satelliteCount)
|
||||
.arg(statusDescription());
|
||||
}
|
||||
|
||||
QString GsbGpsParse::statusDescription() const
|
||||
{
|
||||
switch (m_gpsData.status) {
|
||||
case GpsStatus::Invalid:
|
||||
return QStringLiteral("未定位");
|
||||
case GpsStatus::Valid:
|
||||
return QStringLiteral("已定位");
|
||||
case GpsStatus::Differential:
|
||||
return QStringLiteral("差分定位");
|
||||
default:
|
||||
return QStringLiteral("未知");
|
||||
}
|
||||
}
|
||||
|
||||
// ==================== GsbGpsReader 实现 ====================
|
||||
|
||||
GsbGpsReader::GsbGpsReader(QObject* parent)
|
||||
: QObject(parent)
|
||||
, m_parser(this)
|
||||
, m_serialPort(new QSerialPort(this))
|
||||
{
|
||||
connect(m_serialPort, &QSerialPort::readyRead, this, &GsbGpsReader::onReadyRead);
|
||||
connect(m_serialPort, &QSerialPort::errorOccurred, this, [this](QSerialPort::SerialPortError error) {
|
||||
if (error != QSerialPort::NoError) {
|
||||
emit portError(m_serialPort->errorString());
|
||||
}
|
||||
});
|
||||
// 连接parser的gpsDataUpdated信号，当GPGGA和GPRMC都收到后发送gpsDataReady
|
||||
connect(&m_parser, &GsbGpsParse::gpsDataUpdated, this, &GsbGpsReader::gpsDataReady);
|
||||
}
|
||||
|
||||
GsbGpsReader::~GsbGpsReader()
|
||||
{
|
||||
closePort();
|
||||
}
|
||||
|
||||
bool GsbGpsReader::openPort(const QString& portName, qint32 baudRate)
|
||||
{
|
||||
if (m_serialPort->isOpen()) {
|
||||
m_serialPort->close();
|
||||
}
|
||||
|
||||
m_serialPort->setPortName(portName);
|
||||
m_serialPort->setBaudRate(baudRate);
|
||||
m_serialPort->setDataBits(QSerialPort::Data8);
|
||||
m_serialPort->setParity(QSerialPort::NoParity);
|
||||
m_serialPort->setStopBits(QSerialPort::OneStop);
|
||||
m_serialPort->setFlowControl(QSerialPort::NoFlowControl);
|
||||
|
||||
if (m_serialPort->open(QIODevice::ReadWrite)) {
|
||||
emit portOpened();
|
||||
return true;
|
||||
}
|
||||
|
||||
emit portError(m_serialPort->errorString());
|
||||
return false;
|
||||
}
|
||||
|
||||
void GsbGpsReader::closePort()
|
||||
{
|
||||
if (m_serialPort->isOpen()) {
|
||||
m_serialPort->close();
|
||||
emit portClosed();
|
||||
}
|
||||
}
|
||||
|
||||
bool GsbGpsReader::isPortOpen() const
|
||||
{
|
||||
return m_serialPort->isOpen();
|
||||
}
|
||||
|
||||
GsbGpsParse::GpsData GsbGpsReader::currentGpsData() const
|
||||
{
|
||||
return m_parser.gpsData();
|
||||
}
|
||||
|
||||
bool GsbGpsReader::waitForValidData(int timeoutMs)
|
||||
{
|
||||
QElapsedTimer timer;
|
||||
timer.start();
|
||||
|
||||
while (!m_parser.isGpsDataValid() && timer.elapsed() < timeoutMs) {
|
||||
QThread::msleep(100);
|
||||
QCoreApplication::processEvents();
|
||||
}
|
||||
|
||||
return m_parser.isGpsDataValid();
|
||||
}
|
||||
|
||||
void GsbGpsReader::onReadyRead()
|
||||
{
|
||||
QByteArray data = m_serialPort->readAll();
|
||||
QString text = QString::fromLatin1(data);
|
||||
|
||||
// 按行分割处理NMEA语句
|
||||
static QString buffer;
|
||||
buffer += text;
|
||||
|
||||
QStringList lines = buffer.split('\n', QString::SkipEmptyParts);
|
||||
buffer = lines.takeLast();
|
||||
|
||||
for (const QString& line : lines) {
|
||||
QString trimmed = line.trimmed();
|
||||
if (trimmed.startsWith('$')) {
|
||||
// 只调用解析，GPGGA和GPRMC都收到后parse内部会发送gpsDataUpdated信号
|
||||
m_parser.parseNmeaSentence(trimmed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============ FramerateRotateSpeedCal 实现 ============
|
||||
FramerateRotateSpeedCal::FramerateRotateSpeedCal(QObject* parent)
|
||||
: QObject(parent)
|
||||
{
|
||||
}
|
||||
|
||||
double FramerateRotateSpeedCal::calculateVerticalGroundResolution()
|
||||
{
|
||||
if (m_verticalPixels <= 0) {
|
||||
return 0.0;
|
||||
}
|
||||
// 将角度转换为弧度
|
||||
double fovRadians = m_verticalFov * m_pi / 180.0;
|
||||
// 计算地面覆盖宽度，然后除以像素数得到分辨率
|
||||
double groundWidth = 2.0 * m_altitude * std::tan(fovRadians / 2.0);
|
||||
m_groundResolution = groundWidth / m_verticalPixels;
|
||||
return m_groundResolution;
|
||||
}
|
||||
|
||||
double FramerateRotateSpeedCal::calculateSpeed()
|
||||
{
|
||||
if (m_framerate <= 0.0 || m_groundResolution <= 0.0) {
|
||||
return 0.0;
|
||||
}
|
||||
m_speed = m_framerate * m_groundResolution;
|
||||
return m_speed;
|
||||
}
|
||||
|
||||
double FramerateRotateSpeedCal::calculateRotationSpeed()
|
||||
{
|
||||
if (m_altitude <= 0.0 || m_speed <= 0.0) {
|
||||
return 0.0;
|
||||
}
|
||||
// 旋转一周的周长 = 2 * PI * altitude
|
||||
// rotation_speed = speed / circumference
|
||||
double circumference = 2.0 * m_pi * m_altitude;
|
||||
double m_rotationSpeed = m_speed / circumference;// 单位: 转/秒
|
||||
|
||||
m_rotationSpeedDegPerSec = m_speed / m_altitude * 180.0 / m_pi;
|
||||
return m_rotationSpeedDegPerSec;
|
||||
}
|
||||
|
||||
double FramerateRotateSpeedCal::calculate(double altitude, double verticalFov, int verticalPixels, double framerate)
|
||||
{
|
||||
m_altitude = altitude;
|
||||
m_verticalFov = verticalFov;
|
||||
m_verticalPixels = verticalPixels;
|
||||
m_framerate = framerate;
|
||||
|
||||
calculateVerticalGroundResolution();
|
||||
calculateSpeed();
|
||||
calculateRotationSpeed();
|
||||
|
||||
emit calculationCompleted(m_groundResolution, m_speed, m_rotationSpeedDegPerSec);
|
||||
|
||||
return m_rotationSpeedDegPerSec;
|
||||
}
|
||||
|
||||
@ -14,12 +14,17 @@
|
||||
#include <QSignalTransition>
|
||||
#include <QFinalState>
|
||||
#include <QTimer>
|
||||
#include <QSerialPort>
|
||||
#include <QElapsedTimer>
|
||||
|
||||
#include "ui_gonggashanCtl.h"
|
||||
|
||||
#include "CommunicationViaTCP.h"
|
||||
#include "AppSettings.h"
|
||||
|
||||
class GsbGpsParse;
|
||||
class GsbGpsReader;
|
||||
|
||||
// ============ 执行阶段枚举 ============
|
||||
enum class GonggaShanExecPhase {
|
||||
Idle, // 空闲
|
||||
@ -32,31 +37,6 @@ enum class GonggaShanExecPhase {
|
||||
Completed // 完成
|
||||
};
|
||||
|
||||
// ============ 任务调度器 ============
|
||||
class GonggashanTaskScheduler : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
enum class TaskState
|
||||
{
|
||||
Idle,
|
||||
Running
|
||||
};
|
||||
|
||||
explicit GonggashanTaskScheduler(QObject* parent = nullptr);
|
||||
~GonggashanTaskScheduler();
|
||||
|
||||
bool isTaskRunning() const;
|
||||
void setTaskRunning(bool running);
|
||||
|
||||
Q_SIGNALS:
|
||||
void taskStateChanged(TaskState state);
|
||||
|
||||
private:
|
||||
TaskState m_taskState;
|
||||
};
|
||||
|
||||
// ============ 任务执行器 ============
|
||||
class GonggashanTaskExecutor : public QObject
|
||||
{
|
||||
@ -72,18 +52,21 @@ public slots:
|
||||
void start(const QString& posInfo);
|
||||
void stop();
|
||||
|
||||
void onHyperExposureComplete(int exposureTime);
|
||||
void onFiberExposureComplete(int exposureTime);
|
||||
void onHyperExposureComplete(double exposureTime, double frameRate);
|
||||
void onFiberExposureComplete(double exposureTime);
|
||||
void onGpsAcquired(double latitude, double longitude, double altitude);
|
||||
void onMotorSpeedCalculated(double speed);
|
||||
void onRcordComplete();
|
||||
|
||||
signals:
|
||||
void hyperAutoExposureSignal_gonggashan();
|
||||
void fiberExposureSignal();
|
||||
void fiberExposureRecordSignal(QString posInfo);
|
||||
void gpsAcquisitionSignal();
|
||||
void motorSpeedSignal(double speed);
|
||||
void startCollectionSignal(const QString& posInfo, const QString& gpsData);
|
||||
|
||||
void calculateMotorSpeedSignal(double hyperFrameRate);
|
||||
|
||||
void finished(bool success);
|
||||
void errorOccurred(const QString& error);
|
||||
|
||||
@ -103,7 +86,6 @@ protected:
|
||||
virtual bool fiberExposureImpl(int exposureTime);
|
||||
virtual bool gpsAcquisitionImpl(double lat, double lon, double alt);
|
||||
virtual bool motorCalcImpl();
|
||||
virtual bool dataCollectionImpl();
|
||||
|
||||
private slots:
|
||||
void onFinalStateEntered();
|
||||
@ -116,8 +98,9 @@ private:
|
||||
QString m_posInfo;
|
||||
QString m_gpsData;
|
||||
double m_motorSpeed = 0.0;
|
||||
int m_hyperExposureTime = 0;
|
||||
int m_fiberExposureTime = 0;
|
||||
double m_hyperExposureTime = 0;
|
||||
double m_hyperFrameRate = 0;
|
||||
double m_fiberExposureTime = 0;
|
||||
bool m_stopRequested = false;
|
||||
|
||||
// Qt State Machine
|
||||
@ -145,14 +128,18 @@ public Q_SLOTS:
|
||||
void onFiberImagerExposureCompleteSignal(int exposureTime);
|
||||
|
||||
Q_SIGNALS:
|
||||
void hyperAutoExposureSignal_gonggashan();
|
||||
void hyperAutoExposureDoneSignal_gonggashan(double exposureTime);
|
||||
void gpsAcquisitionDoneSignal_gonggashan(double lat, double lon, double alt);
|
||||
|
||||
void fiberExposureSignal_gonggashan();
|
||||
void calculateMotorSpeedDoneSignal_gonggashan(double speed);
|
||||
|
||||
void hyperAutoExposureSignal_gonggashan();
|
||||
void hyperAutoExposureDoneSignal_gonggashan(double exposureTime, double frameRate);
|
||||
|
||||
void fiberExposureRecordSignal_gonggashan(QString posInfo);
|
||||
void fiberExposureDoneSignal_gonggashan(double exposureTime);
|
||||
|
||||
// Emitted when user changes any of the R/G/B wavelength values
|
||||
void startRcordSignal(QString posInfo);
|
||||
void startRcordSignal_gonggashan(QString posInfo);
|
||||
void recordFinishedSignal_gonggashan();
|
||||
|
||||
private Q_SLOTS:
|
||||
void startListen();
|
||||
@ -163,14 +150,199 @@ private Q_SLOTS:
|
||||
// GonggashanTaskExecutor 反馈槽
|
||||
void onTaskExecutorFinished(bool success);
|
||||
|
||||
void onGpsAcquisition();
|
||||
void onCalculateMotorSpeed(double hyperFrameRate);
|
||||
|
||||
private:
|
||||
void logStatus(const QString& message, bool isHearderBlankLine = false, bool isTailBlankLine = false);
|
||||
|
||||
Ui::gongga_control ui;
|
||||
QPointer<MotorParams::CommunicationViaTCP> tcpServer6005;
|
||||
GonggashanTaskScheduler* m_taskScheduler;
|
||||
GonggashanTaskExecutor* m_taskExecutor; // 新增
|
||||
QString m_logFilePath;
|
||||
QFile m_logFile;
|
||||
QTextStream m_logStream;
|
||||
|
||||
// GPS 串口读取器
|
||||
GsbGpsReader* m_gpsReader = nullptr;
|
||||
};
|
||||
|
||||
// ============ USB GPS NMEA-0183 解析器 (G6301, COM17) ============
|
||||
class GsbGpsParse : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
// GPS数据有效性状态
|
||||
enum class GpsStatus {
|
||||
Invalid, // 无效或未定位
|
||||
Valid, // 有效定位
|
||||
Differential // 差分定位
|
||||
};
|
||||
|
||||
// GPS解析数据结构
|
||||
struct GpsData {
|
||||
QString rawNmea; // 原始NMEA语句
|
||||
QString utcTime; // UTC时间 (HHMMSS.SSS)
|
||||
QString utcDate; // UTC日期 (DDMMYY)
|
||||
double latitude = 0.0; // 纬度 (十进制度)
|
||||
double longitude = 0.0; // 经度 (十进制度)
|
||||
double altitude = 0.0; // 高程 (米)
|
||||
int satelliteCount = 0; // 卫星数目
|
||||
GpsStatus status = GpsStatus::Invalid; // 定位状态
|
||||
bool isValid = false; // 综合有效性
|
||||
bool hasGga = false; // 是否已收到GPGGA语句
|
||||
bool hasRmc = false; // 是否已收到GPRMC语句
|
||||
};
|
||||
|
||||
explicit GsbGpsParse(QObject* parent = nullptr);
|
||||
~GsbGpsParse() = default;
|
||||
|
||||
// 设置最小卫星数目要求 (默认4颗)
|
||||
void setMinSatelliteCount(int count) { m_minSatelliteCount = count; }
|
||||
int minSatelliteCount() const { return m_minSatelliteCount; }
|
||||
|
||||
// 解析单条NMEA语句
|
||||
bool parseNmeaSentence(const QString& sentence);
|
||||
|
||||
// 获取当前解析的GPS数据
|
||||
const GpsData& gpsData() const { return m_gpsData; }
|
||||
|
||||
// 检查GPS数据是否有效
|
||||
bool isGpsDataValid() const;
|
||||
|
||||
// 格式化输出
|
||||
QString toDisplayString() const;
|
||||
|
||||
// 获取状态描述
|
||||
QString statusDescription() const;
|
||||
|
||||
signals:
|
||||
void gpsDataUpdated(const GpsData& data);
|
||||
void gpsStatusChanged(bool isValid);
|
||||
|
||||
private:
|
||||
// 解析 $GPGGA - GPS定位数据
|
||||
bool parseGpgga(const QStringList& fields);
|
||||
|
||||
// 解析 $GPRMC - 推荐最小定位数据
|
||||
bool parseGprmc(const QStringList& fields);
|
||||
|
||||
// NMEA校验
|
||||
bool validateNmeaChecksum(const QString& sentence);
|
||||
|
||||
// 转换纬度格式 (DDMM.MMMM -> DD.DDDDD)
|
||||
double convertLatitude(const QString& degMin, const QString& direction);
|
||||
|
||||
// 转换经度格式 (DDDMM.MMMM -> DDD.DDDDD)
|
||||
double convertLongitude(const QString& degMin, const QString& direction);
|
||||
|
||||
// 检查数据有效性
|
||||
void validateData();
|
||||
|
||||
GpsData m_gpsData;
|
||||
int m_minSatelliteCount = 4; // 默认要求至少4颗卫星
|
||||
QString m_pendingTime;
|
||||
QString m_pendingDate;
|
||||
};
|
||||
|
||||
// ============ USB GPS 串口读取器 (G6301, COM17) ============
|
||||
class GsbGpsReader : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
explicit GsbGpsReader(QObject* parent = nullptr);
|
||||
~GsbGpsReader();
|
||||
|
||||
// 打开指定串口
|
||||
bool openPort(const QString& portName, qint32 baudRate = 4800);
|
||||
|
||||
// 关闭串口
|
||||
void closePort();
|
||||
|
||||
// 串口是否打开
|
||||
bool isPortOpen() const;
|
||||
|
||||
// 获取GPS解析器（用于设置参数如最小卫星数）
|
||||
GsbGpsParse* gpsParser() { return &m_parser; }
|
||||
const GsbGpsParse* gpsParser() const { return &m_parser; }
|
||||
|
||||
// 获取GPS数据
|
||||
GsbGpsParse::GpsData currentGpsData() const;
|
||||
|
||||
// 等待有效GPS数据 (带超时)
|
||||
bool waitForValidData(int timeoutMs = 30000);
|
||||
|
||||
signals:
|
||||
void gpsDataReady(const GsbGpsParse::GpsData& data);
|
||||
void portOpened();
|
||||
void portClosed();
|
||||
void portError(const QString& error);
|
||||
|
||||
private slots:
|
||||
void onReadyRead();
|
||||
|
||||
private:
|
||||
GsbGpsParse m_parser;
|
||||
QSerialPort* m_serialPort;
|
||||
};
|
||||
|
||||
// ============ 帧率与旋转速度计算器 ============
|
||||
class FramerateRotateSpeedCal : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
explicit FramerateRotateSpeedCal(QObject* parent = nullptr);
|
||||
~FramerateRotateSpeedCal() = default;
|
||||
|
||||
// 设置参数
|
||||
void setAltitude(double altitude) { m_altitude = altitude; }
|
||||
void setVerticalFov(double fovDegrees) { m_verticalFov = fovDegrees; }
|
||||
void setVerticalPixels(int pixels) { m_verticalPixels = pixels; }
|
||||
void setFramerate(double framerate) { m_framerate = framerate; }
|
||||
|
||||
// 获取参数
|
||||
double altitude() const { return m_altitude; }
|
||||
double verticalFov() const { return m_verticalFov; }
|
||||
int verticalPixels() const { return m_verticalPixels; }
|
||||
double framerate() const { return m_framerate; }
|
||||
|
||||
// 计算垂直航向分辨率（地面分辨率）
|
||||
// 基于高度、视场角和像素个数
|
||||
// 公式: resolution = 2 * altitude * tan(FOV/2) / pixels
|
||||
double calculateVerticalGroundResolution();
|
||||
|
||||
// 计算速度
|
||||
// 基于帧率和垂直航向分辨率
|
||||
// 公式: speed = framerate * ground_resolution
|
||||
double calculateSpeed();
|
||||
|
||||
// 计算旋转速度
|
||||
// 基于高度和计算出的速度
|
||||
// 公式: rotation_speed = speed / (2 * PI * altitude)
|
||||
double calculateRotationSpeed();
|
||||
|
||||
// 一站式计算：设置参数后一次性计算所有结果
|
||||
double calculate(double altitude, double verticalFov, int verticalPixels, double framerate);
|
||||
|
||||
// 获取计算结果
|
||||
double groundResolution() const { return m_groundResolution; }
|
||||
double speed() const { return m_speed; }
|
||||
double rotationSpeed() const { return m_rotationSpeedDegPerSec; }
|
||||
|
||||
signals:
|
||||
void calculationCompleted(double groundResolution, double speed, double rotationSpeed);
|
||||
|
||||
private:
|
||||
double m_pi = 3.14159265358979323846; // 圆周率
|
||||
double m_altitude = 0.0; // 高度（米）
|
||||
double m_verticalFov = 0.0; // 垂直视场角（度）
|
||||
int m_verticalPixels = 0; // 垂直方向像素个数
|
||||
double m_framerate = 0.0; // 帧率（Hz）
|
||||
|
||||
double m_groundResolution = 0.0; // 垂直航向分辨率（米/像素）
|
||||
double m_speed = 0.0; // 速度（米/秒）
|
||||
double m_rotationSpeedDegPerSec = 0.0; // 旋转速度（度/秒）
|
||||
};
|
||||
|
||||
@ -1115,16 +1115,14 @@ void HPPA::setupGonggashanAutoRecordConnection()
|
||||
connect(m_gonggaShanRecordCtl, &GonggaShanRecordCtl::hyperAutoExposureSignal_gonggashan, this, &HPPA::onGonggashanHyperAutoExposure);
|
||||
connect(m_omc, &OneMotorControl::hyperAutoExposureDoneSignal_gonggashan, m_gonggaShanRecordCtl, &GonggaShanRecordCtl::hyperAutoExposureDoneSignal_gonggashan);
|
||||
|
||||
connect(m_gonggaShanRecordCtl, &GonggaShanRecordCtl::fiberExposureSignal_gonggashan, this, &HPPA::onGonggashanFiberAutoExposure);
|
||||
connect(m_gonggaShanRecordCtl, &GonggaShanRecordCtl::fiberExposureRecordSignal_gonggashan, this, &HPPA::onGonggashanFiberAutoExposureRecord);
|
||||
connect(m_fodisWindow, &FodisWindow::startExposureSignal, m_gonggaShanRecordCtl, &GonggaShanRecordCtl::onFiberImagerStartExposureSignal);
|
||||
connect(m_fodisWindow, &FodisWindow::exposureCompleteSignal, m_gonggaShanRecordCtl, &GonggaShanRecordCtl::onFiberImagerExposureCompleteSignal);
|
||||
connect(m_fodisWindow, &FodisWindow::exposureCompleteSignal, m_gonggaShanRecordCtl, &GonggaShanRecordCtl::fiberExposureDoneSignal_gonggashan);
|
||||
|
||||
connect(m_fodisWindow, &FodisWindow::startExposureSignal, m_gonggaShanRecordCtl, &GonggaShanRecordCtl::onFiberImagerStartExposureSignal);
|
||||
connect(m_fodisWindow, &FodisWindow::exposureCompleteSignal, this, &HPPA::onStartRecordStep1);
|
||||
connect(m_fodisWindow, &FodisWindow::exposureCompleteSignal, m_gonggaShanRecordCtl, &GonggaShanRecordCtl::onFiberImagerExposureCompleteSignal);
|
||||
|
||||
connect(m_gonggaShanRecordCtl, &GonggaShanRecordCtl::startRcordSignal, this, &HPPA::onGonggashanRecord);
|
||||
connect(m_gonggaShanRecordCtl, &GonggaShanRecordCtl::startRcordSignal_gonggashan, this, &HPPA::onGonggashanRecord);
|
||||
connect(m_omc, &OneMotorControl::sequenceComplete, m_fodisWindow, &FodisWindow::closeFiberImager);
|
||||
connect(m_omc, &OneMotorControl::sequenceComplete, m_gonggaShanRecordCtl, &GonggaShanRecordCtl::onRcordFinished);
|
||||
connect(m_omc, &OneMotorControl::sequenceComplete, m_gonggaShanRecordCtl, &GonggaShanRecordCtl::recordFinishedSignal_gonggashan);
|
||||
}
|
||||
|
||||
void HPPA::onGonggashanHyperAutoExposure()
|
||||
@ -1141,9 +1139,9 @@ void HPPA::onGonggashanHyperAutoExposure()
|
||||
m_omc->multiPosHyperAutoExposure();
|
||||
}
|
||||
|
||||
void HPPA::onGonggashanFiberAutoExposure()
|
||||
void HPPA::onGonggashanFiberAutoExposureRecord(QString posInfo)
|
||||
{
|
||||
//m_fodisWindow->openFiberImager_expose_record(posInfo);
|
||||
m_fodisWindow->openFiberImager_expose_record(posInfo);
|
||||
}
|
||||
|
||||
void HPPA::onGonggashanRecord(QString posInfo)
|
||||
@ -1166,8 +1164,8 @@ void HPPA::onGonggashanRecord(QString posInfo)
|
||||
{
|
||||
onconnect();
|
||||
}
|
||||
|
||||
m_fodisWindow->openFiberImager_expose_record(posInfo);
|
||||
|
||||
onStartRecordStep1();
|
||||
}
|
||||
|
||||
void HPPA::recordFromRobotArm(int fileCounter)
|
||||
|
||||
@ -459,7 +459,7 @@ public Q_SLOTS:
|
||||
|
||||
void onGonggashanRecord(QString posInfo);
|
||||
void onGonggashanHyperAutoExposure();
|
||||
void onGonggashanFiberAutoExposure();
|
||||
void onGonggashanFiberAutoExposureRecord(QString posInfo);
|
||||
|
||||
protected:
|
||||
void closeEvent(QCloseEvent* event) override;
|
||||
|
||||
@ -68,7 +68,7 @@ signals:
|
||||
|
||||
void broadcastLocationSignal(std::vector<double>);
|
||||
|
||||
void hyperAutoExposureDoneSignal_gonggashan(double exposureTime);
|
||||
void hyperAutoExposureDoneSignal_gonggashan(double exposureTime, double frameRate);
|
||||
|
||||
private:
|
||||
Ui::OneMotorControl_UI ui;
|
||||
|
||||
Reference in New Issue
Block a user