Init
This commit is contained in:
lumos
commit
002d78faa0
2259
.gitignore
vendored
Normal file
2259
.gitignore
vendored
Normal file
File diff suppressed because it is too large
Load Diff
199
CordConvert20251005.m
Normal file
199
CordConvert20251005.m
Normal file
@ -0,0 +1,199 @@
|
||||
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 该程序用于获取A机位姿信息和B机位置信息，并计算A机对B机指向
|
||||
% By ZXJ，2025.10.02
|
||||
|
||||
clear all;
|
||||
close all;
|
||||
clc;
|
||||
|
||||
Re=6378.14; %% 地球半径，km
|
||||
|
||||
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 双机位置（地心球面坐标系）
|
||||
% A机位置
|
||||
Lon_A=dms2degrees([86 4 51.16]); %% A机经度，deg
|
||||
Lat_A=dms2degrees([41 45 20.24]); %% A机纬度，deg
|
||||
Hei_A=4.5; %% A机高度，km
|
||||
% Lon_A=120.2994; %% A机经度，deg
|
||||
% Lat_A=30.250212; %% A机纬度，deg
|
||||
% Hei_A=375.29/1000; %% A机高度，km
|
||||
|
||||
% B机位置
|
||||
Lon_B=dms2degrees([86 4 51.161]); %% B机经度，deg
|
||||
Lat_B=dms2degrees([41 45 20.24]); %% B机纬度，deg
|
||||
Hei_B=4.5; %% B机高度，km
|
||||
% Lon_B=dms2degrees([76 12 51.16]); %% B机经度，deg
|
||||
% Lat_B=dms2degrees([41 45 20.24]); %% B机纬度，deg
|
||||
% Hei_B=5.1; %% B机高度，km
|
||||
% Lon_B=120.29950478; %% B机经度，deg
|
||||
% Lat_B=30.250028015; %% B机纬度，deg
|
||||
% Hei_B=375.29/1000; %% B机高度，km
|
||||
|
||||
% 将角度由度转化为弧度
|
||||
Lon_A_Rad=deg2rad(Lon_A); %% A机经度，rad
|
||||
Lat_A_Rad=deg2rad(Lat_A); %% A机纬度，rad
|
||||
|
||||
Lon_B_Rad=deg2rad(Lon_B); %% B机经度，rad
|
||||
Lat_B_Rad=deg2rad(Lat_B); %% B机纬度，rad
|
||||
|
||||
% 双机位置（地心直角坐标系）及双机距离，km
|
||||
X_A=(Re+Hei_A)*cos(Lat_A_Rad)*cos(Lon_A_Rad);
|
||||
Y_A=(Re+Hei_A)*cos(Lat_A_Rad)*sin(Lon_A_Rad);
|
||||
Z_A=(Re+Hei_A)*sin(Lat_A_Rad);
|
||||
|
||||
X_B=(Re+Hei_B)*cos(Lat_B_Rad)*cos(Lon_B_Rad);
|
||||
Y_B=(Re+Hei_B)*cos(Lat_B_Rad)*sin(Lon_B_Rad);
|
||||
Z_B=(Re+Hei_B)*sin(Lat_B_Rad);
|
||||
|
||||
% B机相对于A机的直角坐标，km
|
||||
XYZ_BA=[X_B-X_A; Y_B-Y_A; Z_B-Z_A];
|
||||
% 双机距离，km
|
||||
D_BA=sqrt(sum(XYZ_BA.^2));
|
||||
% D_BA=sqrt((X_B-X_A)^2+(Y_B-Y_A)^2+(Z_B-Z_A)^2);
|
||||
|
||||
% % 根据坐标转换矩阵计算B机相对于A机的方位角、俯仰角
|
||||
% % 坐标转换矩阵
|
||||
% Matrix_UEN2XYZ=[cos(Lat_A_Rad)*cos(Lon_A_Rad) cos(Lat_A_Rad)*sin(Lon_A_Rad) sin(Lat_A_Rad); ...
|
||||
% -sin(Lon_A_Rad) cos(Lon_A_Rad) 0;...
|
||||
% -sin(Lat_A_Rad)*cos(Lon_A_Rad) -sin(Lat_A_Rad)*sin(Lon_A_Rad) cos(Lat_A_Rad)];
|
||||
%
|
||||
% % B机相对于A机的直角坐标转换为天东北坐标
|
||||
% UEN_BA = Matrix_UEN2XYZ * XYZ_BA;
|
||||
% U_BA=UEN_BA(1);
|
||||
% E_BA=UEN_BA(2);
|
||||
% N_BA=UEN_BA(3);
|
||||
%
|
||||
% % B机相对于A机的方位角和俯仰角
|
||||
% Az_BA=mod(rad2deg(atan2(E_BA,N_BA)),360);
|
||||
% Alt_BA=rad2deg(atan2(U_BA,sqrt(E_BA^2+N_BA^2)));
|
||||
|
||||
% 根据三轴坐标旋转矩阵计算B机相对于A机的方位角、俯仰角
|
||||
Lon_A_Rad1=Lon_A_Rad;
|
||||
Lat_A_Rad1=-Lat_A_Rad;
|
||||
MR_X=[1 0 0;...
|
||||
0 1 0;...
|
||||
0 0 1];
|
||||
MR_Y=[cos(Lat_A_Rad1) 0 -sin(Lat_A_Rad1);...
|
||||
0 1 0;...
|
||||
sin(Lat_A_Rad1) 0 cos(Lat_A_Rad1)];
|
||||
MR_Z=[cos(Lon_A_Rad1) sin(Lon_A_Rad1) 0;...
|
||||
-sin(Lon_A_Rad1) cos(Lon_A_Rad1) 0;...
|
||||
0 0 1];
|
||||
|
||||
% 天东北坐标
|
||||
UEN_BA0=MR_X*MR_Y*MR_Z*XYZ_BA;
|
||||
U_BA0=UEN_BA0(1);
|
||||
E_BA0=UEN_BA0(2);
|
||||
N_BA0=UEN_BA0(3);
|
||||
|
||||
% B机相对于A机在大地坐标系中的方位角和俯仰角
|
||||
Az_BA0=mod(rad2deg(atan2(E_BA0,N_BA0)),360);
|
||||
Alt_BA0=rad2deg(atan2(U_BA0,sqrt(E_BA0^2+N_BA0^2)));
|
||||
|
||||
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 根据POS设备实时姿态角解算B机在A机载体坐标系中的位置
|
||||
% POS设备测量到的实时姿态角
|
||||
% HeadingReal=63.802;
|
||||
% PitchReal=-0.109;
|
||||
% RollReal=-0.135;
|
||||
HeadingReal=65.516;
|
||||
PitchReal=-0.112;
|
||||
RollReal=1.648;
|
||||
|
||||
% 角度换算为弧度
|
||||
HeadingReal_Rad=deg2rad(HeadingReal);
|
||||
PitchReal_Rad=deg2rad(PitchReal);
|
||||
RollReal_Rad=deg2rad(RollReal);
|
||||
|
||||
% 由实时姿态计算位置的坐标旋转矩阵
|
||||
MR_E_Real=[1 0 0;...
|
||||
0 cos(PitchReal_Rad) -sin(PitchReal_Rad);...
|
||||
0 sin(PitchReal_Rad) cos(PitchReal_Rad)];
|
||||
MR_N_Real=[cos(RollReal_Rad) 0 -sin(RollReal_Rad);...
|
||||
0 1 0;...
|
||||
sin(RollReal_Rad) 0 cos(RollReal_Rad)];
|
||||
MR_U_Real=[cos(HeadingReal_Rad) -sin(HeadingReal_Rad) 0;...
|
||||
sin(HeadingReal_Rad) cos(HeadingReal_Rad) 0;...
|
||||
0 0 1];
|
||||
|
||||
% B机在A机载体坐标系中的位置
|
||||
ENU_BA_Real=MR_N_Real*MR_E_Real*MR_U_Real*[E_BA0;N_BA0;U_BA0];
|
||||
E_BA_Real=ENU_BA_Real(1);
|
||||
N_BA_Real=ENU_BA_Real(2);
|
||||
U_BA_Real=ENU_BA_Real(3);
|
||||
|
||||
% B机在A机载体坐标系中的方位角和俯仰角
|
||||
Az_BA_Real=mod(rad2deg(atan2(E_BA_Real,N_BA_Real)),360);
|
||||
Alt_BA_Real=rad2deg(atan2(U_BA_Real,sqrt(E_BA_Real^2+N_BA_Real^2)));
|
||||
|
||||
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 静止状态下POS设备安装面的姿态角计算零点指向
|
||||
% 静止状态下，POS设备在自身安装面上测到的姿态角
|
||||
Heading_IMUBase=63.888;
|
||||
Pitch_IMUBase=-0.121;
|
||||
Roll_IMUBase=-0.082;
|
||||
|
||||
% POS设备在自身安装面上姿态角换算为弧度
|
||||
Heading_IMUBase_Rad=-deg2rad(Heading_IMUBase);
|
||||
Pitch_IMUBase_Rad=-deg2rad(Pitch_IMUBase);
|
||||
Roll_IMUBase_Rad=-deg2rad(Roll_IMUBase);
|
||||
|
||||
% 由静止姿态计算零点位置的坐标旋转矩阵
|
||||
MR_E_IMUBase=[1 0 0;...
|
||||
0 cos(-Pitch_IMUBase_Rad) -sin(-Pitch_IMUBase_Rad);...
|
||||
0 sin(-Pitch_IMUBase_Rad) cos(-Pitch_IMUBase_Rad)];
|
||||
MR_N_IMUBase=[cos(-Roll_IMUBase_Rad) 0 -sin(-Roll_IMUBase_Rad);...
|
||||
0 1 0;...
|
||||
sin(-Roll_IMUBase_Rad) 0 cos(-Roll_IMUBase_Rad)];
|
||||
MR_U_IMUBase=[cos(Heading_IMUBase_Rad) -sin(Heading_IMUBase_Rad) 0;...
|
||||
sin(Heading_IMUBase_Rad) cos(Heading_IMUBase_Rad) 0;...
|
||||
0 0 1];
|
||||
|
||||
% B机在A机载体坐标系中的位置
|
||||
ENU_BA_IMUBase=MR_N_IMUBase*MR_E_IMUBase*MR_U_IMUBase*ENU_BA_Real;
|
||||
E_BA_IMUBase=ENU_BA_IMUBase(1);
|
||||
N_BA_IMUBase=ENU_BA_IMUBase(2);
|
||||
U_BA_IMUBase=ENU_BA_IMUBase(3);
|
||||
|
||||
% B机在A机载体坐标系中的方位角和俯仰角
|
||||
Az_BA_IMUBase=mod(rad2deg(atan2(E_BA_IMUBase,N_BA_IMUBase)),360);
|
||||
Alt_BA_IMUBase=rad2deg(atan2(U_BA_IMUBase,sqrt(E_BA_IMUBase^2+N_BA_IMUBase^2)));
|
||||
|
||||
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 静止状态下样机安装面的姿态角计算零点指向
|
||||
% POS设备在样机安装面上测到的姿态角
|
||||
Heading_OPABase=64.062;
|
||||
Pitch_OPABase=-0.332;
|
||||
Roll_OPABase=89.812;
|
||||
|
||||
% 静止状态下，样机安装面法线方向对应的姿态角
|
||||
Heading_OPABaseNormal=Heading_OPABase+90; %% 样机安装在前进方向右侧加90°，安装在左侧则减90°
|
||||
Pitch_OPABaseNormal=Pitch_OPABase-90;
|
||||
Roll_OPABaseNormal=Roll_OPABase;
|
||||
|
||||
% 法线方向对应的姿态角换算为弧度
|
||||
Heading_OPABaseNormal_Rad=deg2rad(Heading_OPABaseNormal);
|
||||
PitchRad_OPABaseNormal_Rad=deg2rad(Pitch_OPABaseNormal);
|
||||
RollRad_OPABaseNormal_Rad=deg2rad(Roll_OPABaseNormal);
|
||||
|
||||
% 由静止姿态计算零点位置的坐标旋转矩阵
|
||||
MR_E_OPABaseNormal=[1 0 0;...
|
||||
0 cos(PitchRad_OPABaseNormal_Rad) -sin(PitchRad_OPABaseNormal_Rad);...
|
||||
0 sin(PitchRad_OPABaseNormal_Rad) cos(PitchRad_OPABaseNormal_Rad)];
|
||||
MR_N_OPABaseNormal=[cos(RollRad_OPABaseNormal_Rad) 0 -sin(RollRad_OPABaseNormal_Rad);...
|
||||
0 1 0;...
|
||||
sin(RollRad_OPABaseNormal_Rad) 0 cos(RollRad_OPABaseNormal_Rad)];
|
||||
MR_U_OPABaseNormal=[cos(Heading_OPABaseNormal_Rad) -sin(Heading_OPABaseNormal_Rad) 0;...
|
||||
sin(Heading_OPABaseNormal_Rad) cos(Heading_OPABaseNormal_Rad) 0;...
|
||||
0 0 1];
|
||||
|
||||
% B机在A机载体坐标系中的位置（由东北天转换）
|
||||
ENU_BA_OPABaseNormal=MR_N_OPABaseNormal*MR_E_OPABaseNormal*MR_U_OPABaseNormal*ENU_BA_IMUBase;
|
||||
E_BA_OPABaseNormal=ENU_BA_OPABaseNormal(1);
|
||||
N_BA_OPABaseNormal=ENU_BA_OPABaseNormal(2);
|
||||
U_BA_OPABaseNormal=ENU_BA_OPABaseNormal(3);
|
||||
|
||||
% B机在A机载体坐标系中的方位角和俯仰角
|
||||
Az_BA_OPABaseNormal=mod(rad2deg(atan2(E_BA_OPABaseNormal,N_BA_OPABaseNormal)),360);
|
||||
Alt_BA_OPABaseNormal=rad2deg(atan2(U_BA_OPABaseNormal,sqrt(E_BA_OPABaseNormal^2+N_BA_OPABaseNormal^2)));
|
||||
|
||||
60
PosCalibrate.pro
Normal file
60
PosCalibrate.pro
Normal file
@ -0,0 +1,60 @@
|
||||
QT += core gui serialport
|
||||
|
||||
greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
|
||||
|
||||
CONFIG += c++17
|
||||
|
||||
# You can make your code fail to compile if it uses deprecated APIs.
|
||||
# In order to do so, uncomment the following line.
|
||||
#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0
|
||||
|
||||
SOURCES += \
|
||||
camera.cpp \
|
||||
imageview.cpp \
|
||||
main.cpp \
|
||||
mainwindow.cpp \
|
||||
posdevice.cpp \
|
||||
rotationstage.cpp
|
||||
|
||||
HEADERS += \
|
||||
LTDMC.h \
|
||||
calPoint.h \
|
||||
camera.h \
|
||||
LTDMC.h \
|
||||
imageview.h \
|
||||
mainwindow.h \
|
||||
posdevice.h \
|
||||
rotationstage.h
|
||||
|
||||
FORMS += \
|
||||
mainwindow.ui
|
||||
|
||||
|
||||
INCLUDEPATH+=\
|
||||
"C:\opencv\build\include"\
|
||||
"C:\Program Files\Basler\pylon 6\Development\include"\
|
||||
|
||||
|
||||
LIBS+= -L"C:\opencv\build\x64\vc16\lib"
|
||||
LIBS+= -L"C:\Program Files\Basler\pylon 6\Development\lib\x64"
|
||||
LIBS+= -L$$PWD
|
||||
|
||||
|
||||
|
||||
CONFIG(debug,debug|release):LIBS+= \
|
||||
-lopencv_world4120d \
|
||||
-lPylonC \
|
||||
-lLTDMC \
|
||||
|
||||
|
||||
|
||||
CONFIG(release,debug|release):LIBS+= \
|
||||
-lopencv_world4120 \
|
||||
-lPylonC \
|
||||
-lLTDMC \
|
||||
|
||||
|
||||
# Default rules for deployment.
|
||||
qnx: target.path = /tmp/$${TARGET}/bin
|
||||
else: unix:!android: target.path = /opt/$${TARGET}/bin
|
||||
!isEmpty(target.path): INSTALLS += target
|
||||
168
calPoint.h
Normal file
168
calPoint.h
Normal file
@ -0,0 +1,168 @@
|
||||
#ifndef CALPOINT_H
|
||||
#define CALPOINT_H
|
||||
#include "qglobal.h"
|
||||
#include <cmath>
|
||||
#include "QDebug"
|
||||
// 定义常量
|
||||
const double Re = 6378.14; // 地球半径，km
|
||||
const double PI = 3.14159265358979323846;
|
||||
const double Heading_IMUBase =63.888;
|
||||
const double Pitch_IMUBase = -0.121;
|
||||
const double Roll_IMUBase = -0.082;
|
||||
|
||||
const double Heading_OPABase=64.062;
|
||||
const double Pitch_OPABase = -0.332;
|
||||
const double Roll_OPABase = 89.812;
|
||||
|
||||
const double OpticalAxisAngle_N0=90;
|
||||
const double OpticalAxisAngle_E0= 90;
|
||||
const double OpticalAxisAngle_D0=0;
|
||||
|
||||
// 角度转弧度函数
|
||||
double deg2rad(double deg) {
|
||||
return deg * PI / 180.0;
|
||||
}
|
||||
|
||||
// 弧度转角度函数
|
||||
double rad2deg(double rad) {
|
||||
return rad * 180.0 / PI;
|
||||
}
|
||||
|
||||
// 模运算函数
|
||||
double mod(double x, double y) {
|
||||
return x - y * std::floor(x / y);
|
||||
}
|
||||
|
||||
// 矩阵乘法函数
|
||||
void matrixMultiply(double A[3][3], double B[3][3], double result[3][3]) {
|
||||
for(int i=0; i<3; i++) {
|
||||
for(int j=0; j<3; j++) {
|
||||
result[i][j] = 0;
|
||||
for(int k=0; k<3; k++) {
|
||||
result[i][j] += A[i][k] * B[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void matrixMultiplyVector(double A[3][3], double B[3], double result[3]) {
|
||||
for(int i=0; i<3; i++) {
|
||||
result[i] = 0;
|
||||
for(int j=0; j<3; j++) {
|
||||
result[i] += A[i][j] * B[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
QVector<double> calRes(double Lon_A,double Lat_A,double Hei_A,double Lon_B,double Lat_B, double Hei_B,double HeadingReal,double PitchReal,double RollReal){
|
||||
// 将角度由度转化为弧度
|
||||
double Lon_A_Rad = deg2rad(Lon_A); // A机经度，rad
|
||||
double Lat_A_Rad = deg2rad(Lat_A); // A机纬度，rad
|
||||
double Lon_B_Rad = deg2rad(Lon_B); // B机经度，rad
|
||||
double Lat_B_Rad = deg2rad(Lat_B); // B机纬度，rad
|
||||
|
||||
// 双机位置（地心直角坐标系）及双机距离，km
|
||||
double X_A = (Re + Hei_A) * std::cos(Lat_A_Rad) * std::cos(Lon_A_Rad);
|
||||
double Y_A = (Re + Hei_A) * std::cos(Lat_A_Rad) * std::sin(Lon_A_Rad);
|
||||
double Z_A = (Re + Hei_A) * std::sin(Lat_A_Rad);
|
||||
|
||||
double X_B = (Re + Hei_B) * std::cos(Lat_B_Rad) * std::cos(Lon_B_Rad);
|
||||
double Y_B = (Re + Hei_B) * std::cos(Lat_B_Rad) * std::sin(Lon_B_Rad);
|
||||
double Z_B = (Re + Hei_B) * std::sin(Lat_B_Rad);
|
||||
|
||||
// B机相对于A机的直角坐标，km
|
||||
double XYZ_BA[3] = {X_B - X_A, Y_B - Y_A, Z_B - Z_A};
|
||||
|
||||
// 双机距离，km
|
||||
double D_BA = std::sqrt(XYZ_BA[0]*XYZ_BA[0] + XYZ_BA[1]*XYZ_BA[1] + XYZ_BA[2]*XYZ_BA[2]);
|
||||
qDebug()<< "双机距离: "<<D_BA<<"km";
|
||||
|
||||
// 根据三轴坐标旋转矩阵计算B机相对于A机的方位角、俯仰角
|
||||
double Lon_A_Rad1 = Lon_A_Rad;
|
||||
double Lat_A_Rad1 = -Lat_A_Rad;
|
||||
|
||||
double MR_X[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
|
||||
|
||||
double MR_Y[3][3] = {
|
||||
{std::cos(Lat_A_Rad1), 0, -std::sin(Lat_A_Rad1)},
|
||||
{0, 1, 0},
|
||||
{std::sin(Lat_A_Rad1), 0, std::cos(Lat_A_Rad1)}
|
||||
};
|
||||
|
||||
double MR_Z[3][3] = {
|
||||
{std::cos(Lon_A_Rad1), std::sin(Lon_A_Rad1), 0},
|
||||
{-std::sin(Lon_A_Rad1), std::cos(Lon_A_Rad1), 0},
|
||||
{0, 0, 1}
|
||||
};
|
||||
|
||||
// 矩阵乘法：MR_X * MR_Y * MR_Z
|
||||
double temp1[3][3], MR_XYZ[3][3];
|
||||
matrixMultiply(MR_X, MR_Y, temp1);
|
||||
matrixMultiply(temp1, MR_Z, MR_XYZ);
|
||||
|
||||
// 矩阵乘法：MR_XYZ * XYZ_BA 天东北坐标
|
||||
double UEN_BA0[3];
|
||||
matrixMultiplyVector(MR_XYZ, XYZ_BA, UEN_BA0);
|
||||
|
||||
double NED_BA0[3];
|
||||
NED_BA0[0] = UEN_BA0[2];
|
||||
NED_BA0[1] = UEN_BA0[1];
|
||||
NED_BA0[2]= -UEN_BA0[0];
|
||||
|
||||
// POS姿态角度换算为弧度
|
||||
double HeadingReal_Rad = deg2rad(Heading_OPABase-Heading_IMUBase-HeadingReal);
|
||||
double PitchReal_Rad = deg2rad(Pitch_OPABase-Pitch_IMUBase-PitchReal);
|
||||
double RollReal_Rad = deg2rad(Roll_OPABase-Roll_IMUBase-RollReal);
|
||||
|
||||
// 由实时姿态计算位置的坐标旋转矩阵
|
||||
double NED_MR_D[3][3] = {
|
||||
{std::cos(HeadingReal_Rad), -std::sin(HeadingReal_Rad), 0},
|
||||
{std::sin(HeadingReal_Rad), std::cos(HeadingReal_Rad), 0},
|
||||
{0, 0, 1}
|
||||
};
|
||||
|
||||
double NED_MR_E[3][3] = {
|
||||
{std::cos(PitchReal_Rad), 0, std::sin(PitchReal_Rad)},
|
||||
{0, 1, 0},
|
||||
{-std::sin(PitchReal_Rad), 0, std::cos(PitchReal_Rad)}
|
||||
};
|
||||
|
||||
double NED_MR_N[3][3] = {
|
||||
{1, 0, 0},
|
||||
{0, std::cos(RollReal_Rad), -std::sin(RollReal_Rad)},
|
||||
{0, std::sin(RollReal_Rad), std::cos(RollReal_Rad)}
|
||||
};
|
||||
|
||||
|
||||
|
||||
double temp2[3][3], NED_XYZ[3][3];
|
||||
matrixMultiply(NED_MR_N, NED_MR_E, temp2);
|
||||
matrixMultiply(temp2, NED_MR_D, NED_XYZ);
|
||||
|
||||
|
||||
|
||||
// 矩阵乘法：MR_XYZ * XYZ_BA 天东北坐标
|
||||
double NED_BA_Real[3];
|
||||
matrixMultiplyVector(NED_XYZ, NED_BA0, NED_BA_Real);
|
||||
|
||||
double NED_N_BA_Real=NED_BA_Real[0];
|
||||
double NED_E_BA_Real=NED_BA_Real[1];
|
||||
double NED_D_BA_Real=NED_BA_Real[2];
|
||||
|
||||
double BARotate_N = rad2deg(atan2(NED_D_BA_Real,NED_E_BA_Real))-OpticalAxisAngle_N0;
|
||||
double BARotate_E = rad2deg(atan2(NED_D_BA_Real,NED_N_BA_Real))-OpticalAxisAngle_E0;
|
||||
|
||||
double OpticalAxisAngle_D0_Rad = deg2rad(OpticalAxisAngle_D0);
|
||||
double OpticalAxisRotate_N = std::cos(OpticalAxisAngle_D0_Rad)*BARotate_N-std::sin(OpticalAxisAngle_D0_Rad)*BARotate_E;
|
||||
double OpticalAxisRotate_E = std::sin(OpticalAxisAngle_D0_Rad)*BARotate_N+std::cos(OpticalAxisAngle_D0_Rad)*BARotate_E;
|
||||
|
||||
qDebug()<<"OpticalAxisRotate_N is "<< OpticalAxisRotate_N;
|
||||
qDebug()<<"OpticalAxisRotate_E is "<< OpticalAxisRotate_E;
|
||||
// 输出结果
|
||||
QVector<double> res;
|
||||
res.append(OpticalAxisRotate_E);
|
||||
res.append(OpticalAxisRotate_N);
|
||||
return res;
|
||||
}
|
||||
|
||||
#endif // CALPOINT_H
|
||||
130
camera.cpp
Normal file
130
camera.cpp
Normal file
@ -0,0 +1,130 @@
|
||||
#include "camera.h"
|
||||
|
||||
Camera::Camera(QObject *parent)
|
||||
: QObject{parent}
|
||||
{
|
||||
state = false;
|
||||
PylonInitialize();
|
||||
cam = new CBaslerUniversalInstantCamera ( CTlFactory::GetInstance().CreateFirstDevice());
|
||||
qDebug() << "Using device " << cam->GetDeviceInfo().GetModelName() ;
|
||||
|
||||
|
||||
}
|
||||
|
||||
Camera::~Camera()
|
||||
{
|
||||
state = false;
|
||||
task->join();
|
||||
PylonTerminate();
|
||||
}
|
||||
|
||||
void Camera::open()
|
||||
{
|
||||
state = true;
|
||||
try {
|
||||
// 确保相机未在采集
|
||||
if(cam->IsGrabbing()){
|
||||
cam->StopGrabbing();
|
||||
}
|
||||
|
||||
// 重新打开相机确保干净状态
|
||||
if(cam->IsOpen()) {
|
||||
cam->Close();
|
||||
}
|
||||
cam->Open();
|
||||
|
||||
// 等待相机初始化
|
||||
Sleep(50);
|
||||
|
||||
INodeMap& nodemap = cam->GetNodeMap();
|
||||
|
||||
// 使用安全的方式设置参数
|
||||
auto setIntegerParameter = [&nodemap](const char* name, int64_t value) {
|
||||
try {
|
||||
CIntegerParameter param(nodemap, name);
|
||||
if (param.IsWritable()) {
|
||||
// 检查值范围
|
||||
int64_t min = param.GetMin();
|
||||
int64_t max = param.GetMax();
|
||||
int64_t inc = param.GetInc();
|
||||
|
||||
// 调整值到有效范围内
|
||||
int64_t adjustedValue = max(min, min(value, max));
|
||||
// 对齐到步长
|
||||
if (inc > 1) {
|
||||
adjustedValue = (adjustedValue / inc) * inc;
|
||||
}
|
||||
|
||||
param.SetValue(adjustedValue);
|
||||
qDebug() << name << "set to:" << adjustedValue;
|
||||
return true;
|
||||
} else {
|
||||
qDebug() << name << "is not writable";
|
||||
return false;
|
||||
}
|
||||
} catch (const GenericException& e) {
|
||||
qDebug() << "Failed to set" << name << ":" << e.GetDescription();
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
// 设置参数
|
||||
setIntegerParameter("OffsetX", 1736);
|
||||
setIntegerParameter("OffsetY", 1264);
|
||||
setIntegerParameter("Width", 640);
|
||||
setIntegerParameter("Height", 480);
|
||||
|
||||
// 设置自动曝光
|
||||
if (cam->ExposureAuto.IsWritable()) {
|
||||
cam->ExposureAuto.SetValue(Basler_UniversalCameraParams::ExposureAuto_Continuous);
|
||||
}
|
||||
} catch (const GenericException &e) {
|
||||
qDebug() << "Exception in setCameraParameters:" << e.GetDescription();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
task = new std::thread(&Camera::getSinglePic,this);
|
||||
}
|
||||
|
||||
void Camera::close(){
|
||||
state = false;
|
||||
task->join();
|
||||
PylonTerminate();
|
||||
}
|
||||
void Camera::getSinglePic()
|
||||
{
|
||||
float sec = 1/FRAME;
|
||||
int timeout = GRABTIMEOUT;
|
||||
while(state){
|
||||
CGrabResultPtr ptrGrabResult;
|
||||
|
||||
// auto now = std::chrono::system_clock::now();
|
||||
|
||||
try {
|
||||
cam->GrabOne(timeout,ptrGrabResult,TimeoutHandling_ThrowException);
|
||||
} catch (const GenericException &e) {
|
||||
qDebug() << "Exception in setCameraParameters:" << e.GetDescription();
|
||||
}
|
||||
|
||||
// auto now_ = std::chrono::system_clock::now();
|
||||
// auto duration = now_ - now;
|
||||
// auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
|
||||
|
||||
// qDebug()<<ms ;
|
||||
// qDebug()<<ptrGrabResult->GetWidth();
|
||||
// qDebug()<<ptrGrabResult->GetHeight();
|
||||
if(ptrGrabResult->GrabSucceeded()){
|
||||
uint8_t *pImageBuffer = (uint8_t *) ptrGrabResult->GetBuffer();
|
||||
QImage img(pImageBuffer,ptrGrabResult->GetWidth(),ptrGrabResult->GetHeight(),ptrGrabResult->GetWidth(),QImage::Format_Grayscale8);
|
||||
emit sendPicData(img.copy());
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds((int)(sec*1000)));
|
||||
}
|
||||
|
||||
// qDebug()<<ptrGrabResult->GetW
|
||||
|
||||
}
|
||||
|
||||
48
camera.h
Normal file
48
camera.h
Normal file
@ -0,0 +1,48 @@
|
||||
#ifndef CAMERA_H
|
||||
#define CAMERA_H
|
||||
|
||||
#include <QObject>
|
||||
#include "pylon/PylonIncludes.h"
|
||||
#include "QDebug"
|
||||
#include "thread"
|
||||
#include "chrono"
|
||||
#include "QImage"
|
||||
#include <pylon/BaslerUniversalInstantCamera.h>
|
||||
#include "algorithm"
|
||||
|
||||
#define FRAME 10;
|
||||
#define GRABTIMEOUT 5000;
|
||||
|
||||
|
||||
// Namespace for using pylon objects.
|
||||
using namespace Pylon;
|
||||
|
||||
// Namespace for using GenApi objects.
|
||||
using namespace GenApi;
|
||||
|
||||
// Namespace for using cout.
|
||||
using namespace std;
|
||||
|
||||
|
||||
|
||||
class Camera : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
explicit Camera(QObject *parent = nullptr);
|
||||
~Camera();
|
||||
void open();
|
||||
void close();
|
||||
void getSinglePic();
|
||||
|
||||
|
||||
signals:
|
||||
void sendPicData(QImage);
|
||||
|
||||
private:
|
||||
CBaslerUniversalInstantCamera *cam;
|
||||
std::thread *task;
|
||||
bool state;
|
||||
};
|
||||
|
||||
#endif // CAMERA_H
|
||||
80
imageview.cpp
Normal file
80
imageview.cpp
Normal file
@ -0,0 +1,80 @@
|
||||
#include "imageview.h"
|
||||
#include <QDebug>
|
||||
|
||||
ImageView::ImageView(QWidget *parent)
|
||||
: QGraphicsView(parent),
|
||||
rubberBand(nullptr)
|
||||
{
|
||||
scene = new QGraphicsScene(this);
|
||||
setScene(scene);
|
||||
pximg = new QGraphicsPixmapItem();
|
||||
scene->addItem(pximg);
|
||||
|
||||
|
||||
setDragMode(QGraphicsView::NoDrag);
|
||||
setRenderHint(QPainter::Antialiasing);
|
||||
|
||||
// 设置视图属性
|
||||
setHorizontalScrollBarPolicy(Qt::ScrollBarAsNeeded);
|
||||
setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded);
|
||||
setTransformationAnchor(QGraphicsView::AnchorUnderMouse);
|
||||
setResizeAnchor(QGraphicsView::AnchorUnderMouse);
|
||||
state = false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void ImageView::mousePressEvent(QMouseEvent *event)
|
||||
{
|
||||
if (event->button() == Qt::LeftButton && pximg)
|
||||
{
|
||||
origin = event->pos();
|
||||
if (!rubberBand)
|
||||
rubberBand = new QRubberBand(QRubberBand::Rectangle, this);
|
||||
rubberBand->setGeometry(QRect(origin, QSize()));
|
||||
rubberBand->show();
|
||||
}
|
||||
if(event->button()==Qt::RightButton){
|
||||
state = false;
|
||||
}
|
||||
QGraphicsView::mousePressEvent(event);
|
||||
}
|
||||
|
||||
void ImageView::mouseMoveEvent(QMouseEvent *event)
|
||||
{
|
||||
if (rubberBand && rubberBand->isVisible())
|
||||
{
|
||||
rubberBand->setGeometry(QRect(origin, event->pos()).normalized());
|
||||
}
|
||||
QGraphicsView::mouseMoveEvent(event);
|
||||
}
|
||||
|
||||
void ImageView::mouseReleaseEvent(QMouseEvent *event)
|
||||
{
|
||||
if (event->button() == Qt::LeftButton && rubberBand && pximg)
|
||||
{
|
||||
rubberBand->hide();
|
||||
|
||||
// 获取ROI区域（视图坐标）
|
||||
QRect roiRect = rubberBand->geometry();
|
||||
|
||||
// 转换为场景坐标
|
||||
QPointF topLeft = mapToScene(roiRect.topLeft());
|
||||
QPointF bottomRight = mapToScene(roiRect.bottomRight());
|
||||
QRectF sceneRect(topLeft, bottomRight);
|
||||
|
||||
// 确保在图片范围内
|
||||
QRectF imageRect = pximg->boundingRect();
|
||||
sceneRect = sceneRect.intersected(imageRect);
|
||||
|
||||
if (!sceneRect.isEmpty())
|
||||
{
|
||||
// 发出信号
|
||||
qDebug()<<"跟踪清除";
|
||||
emit roiSelected(sceneRect);
|
||||
// qDebug() << "ROI Selected - Position:" << sceneRect.topLeft()
|
||||
// << "Size:" << sceneRect.size();
|
||||
}
|
||||
}
|
||||
QGraphicsView::mouseReleaseEvent(event);
|
||||
}
|
||||
40
imageview.h
Normal file
40
imageview.h
Normal file
@ -0,0 +1,40 @@
|
||||
#ifndef IMAGEVIEW_H
|
||||
#define IMAGEVIEW_H
|
||||
|
||||
#include <QGraphicsView>
|
||||
#include <QGraphicsScene>
|
||||
#include <QGraphicsPixmapItem>
|
||||
#include <QRubberBand>
|
||||
#include <QMouseEvent>
|
||||
#include "QGraphicsScene"
|
||||
#include "QGraphicsView"
|
||||
#include "QGraphicsPixmapItem"
|
||||
|
||||
class ImageView : public QGraphicsView
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
explicit ImageView(QWidget *parent = nullptr);
|
||||
QGraphicsPixmapItem *pximg;
|
||||
QGraphicsScene *scene;
|
||||
bool state;
|
||||
|
||||
|
||||
protected:
|
||||
void mousePressEvent(QMouseEvent *event) override;
|
||||
void mouseMoveEvent(QMouseEvent *event) override;
|
||||
void mouseReleaseEvent(QMouseEvent *event) override;
|
||||
|
||||
|
||||
signals:
|
||||
void roiSelected(const QRectF &roi);
|
||||
|
||||
private:
|
||||
|
||||
QRubberBand *rubberBand;
|
||||
QPoint origin;
|
||||
|
||||
};
|
||||
|
||||
#endif // IMAGEVIEW_H
|
||||
11
main.cpp
Normal file
11
main.cpp
Normal file
@ -0,0 +1,11 @@
|
||||
#include "mainwindow.h"
|
||||
|
||||
#include <QApplication>
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
QApplication a(argc, argv);
|
||||
MainWindow w;
|
||||
w.show();
|
||||
return a.exec();
|
||||
}
|
||||
638
mainwindow.cpp
Normal file
638
mainwindow.cpp
Normal file
@ -0,0 +1,638 @@
|
||||
#include "mainwindow.h"
|
||||
#include "ui_mainwindow.h"
|
||||
#include "calPoint.h"
|
||||
|
||||
MainWindow::MainWindow(QWidget *parent)
|
||||
: QMainWindow(parent)
|
||||
, ui(new Ui::MainWindow)
|
||||
{
|
||||
ui->setupUi(this);
|
||||
pos = new PosDevice(this);
|
||||
cam = new Camera(this);
|
||||
|
||||
rs = new RotationStage(this);
|
||||
tz = new QTimer(this);
|
||||
tz_ = new QTimer(this);
|
||||
tu = new QTimer(this);
|
||||
tu_ = new QTimer(this);
|
||||
|
||||
topLeft=0;
|
||||
topRight=0;
|
||||
selectWidth=0;
|
||||
selectHeight=0;
|
||||
isFirstFrame=false;
|
||||
loopState = false;
|
||||
|
||||
|
||||
ui->lineEdit->setMaxLength(500);
|
||||
ui->lineEdit_2->setMaxLength(500);
|
||||
ui->lineEdit_3->setMaxLength(500);
|
||||
ui->lineEdit_9->setMaxLength(500);
|
||||
ui->lineEdit_10->setMaxLength(500);
|
||||
ui->lineEdit_11->setMaxLength(500);
|
||||
|
||||
|
||||
|
||||
|
||||
tz->setInterval(10);
|
||||
tz_->setInterval(10);
|
||||
tu->setInterval(10);
|
||||
tu_->setInterval(10);
|
||||
|
||||
|
||||
|
||||
|
||||
connect(pos,&PosDevice::sendPosData,this,&MainWindow::revPosData);
|
||||
connect(cam,&Camera::sendPicData,this,&MainWindow::revPicData);
|
||||
connect(rs,&RotationStage::sendRotateData,this,&MainWindow::revRotateData);
|
||||
|
||||
connect(tz,&QTimer::timeout,this,&MainWindow::zmove);
|
||||
connect(tz_,&QTimer::timeout,this,&MainWindow::zmove_);
|
||||
connect(tu,&QTimer::timeout,this,&MainWindow::umove);
|
||||
connect(tu_,&QTimer::timeout,this,&MainWindow::umove_);
|
||||
|
||||
connect(ui->graphicsView, &ImageView::roiSelected, this, &MainWindow::onRoiSelected);
|
||||
|
||||
}
|
||||
|
||||
MainWindow::~MainWindow()
|
||||
{
|
||||
delete ui;
|
||||
}
|
||||
|
||||
cv::Rect MainWindow::trackRoiWithCorrelation(const cv::Mat &frame)
|
||||
{
|
||||
if(roiTemplate.empty()) {
|
||||
qDebug() << "模板为空";
|
||||
return cv::Rect();
|
||||
}
|
||||
|
||||
// 定义搜索区域（在上一帧ROI周围扩大区域）
|
||||
int searchMargin = 30; // 搜索边界，可根据需要调整
|
||||
cv::Rect searchArea;
|
||||
searchArea.x = std::max(0, lastRoi.x - searchMargin);
|
||||
searchArea.y = std::max(0, lastRoi.y - searchMargin);
|
||||
searchArea.width = std::min(frame.cols - searchArea.x,
|
||||
lastRoi.width + 2 * searchMargin);
|
||||
searchArea.height = std::min(frame.rows - searchArea.y,
|
||||
lastRoi.height + 2 * searchMargin);
|
||||
|
||||
// 检查搜索区域有效性
|
||||
if(searchArea.width <= 0 || searchArea.height <= 0) {
|
||||
qDebug() << "搜索区域无效";
|
||||
return lastRoi;
|
||||
}
|
||||
|
||||
// 提取搜索区域
|
||||
cv::Mat searchRegion = frame(searchArea);
|
||||
|
||||
// 检查模板和搜索区域大小
|
||||
if(searchRegion.rows < roiTemplate.rows || searchRegion.cols < roiTemplate.cols) {
|
||||
// 搜索区域比模板小，调整模板大小
|
||||
double scaleX = static_cast<double>(searchRegion.cols) / roiTemplate.cols;
|
||||
double scaleY = static_cast<double>(searchRegion.rows) / roiTemplate.rows;
|
||||
double scale = std::min(scaleX, scaleY) * 0.9; // 稍微缩小一点确保安全
|
||||
|
||||
cv::Mat resizedTemplate;
|
||||
cv::resize(roiTemplate, resizedTemplate, cv::Size(), scale, scale, cv::INTER_AREA);
|
||||
|
||||
// 模板匹配
|
||||
cv::Mat result;
|
||||
cv::matchTemplate(searchRegion, resizedTemplate, result, cv::TM_CCOEFF_NORMED);
|
||||
|
||||
// 找到最佳匹配位置
|
||||
double maxVal;
|
||||
cv::Point maxLoc;
|
||||
cv::minMaxLoc(result, nullptr, &maxVal, nullptr, &maxLoc);
|
||||
|
||||
// 设置匹配阈值
|
||||
double correlationThreshold = 0.5;
|
||||
if(maxVal < correlationThreshold) {
|
||||
qDebug() << "匹配度低:" << maxVal;
|
||||
return lastRoi;
|
||||
}
|
||||
|
||||
// 计算新的ROI位置
|
||||
cv::Rect newRoi;
|
||||
newRoi.x = searchArea.x + maxLoc.x;
|
||||
newRoi.y = searchArea.y + maxLoc.y;
|
||||
newRoi.width = resizedTemplate.cols;
|
||||
newRoi.height = resizedTemplate.rows;
|
||||
|
||||
qDebug() << "模板匹配成功, 相似度:" << maxVal << "位置:" << newRoi.x << newRoi.y;
|
||||
lastRoi = newRoi;
|
||||
return newRoi;
|
||||
} else {
|
||||
// 正常情况下的模板匹配
|
||||
cv::Mat result;
|
||||
cv::matchTemplate(searchRegion, roiTemplate, result, cv::TM_CCOEFF_NORMED);
|
||||
|
||||
// 找到最佳匹配位置
|
||||
double maxVal;
|
||||
cv::Point maxLoc;
|
||||
cv::minMaxLoc(result, nullptr, &maxVal, nullptr, &maxLoc);
|
||||
|
||||
// 设置匹配阈值
|
||||
double correlationThreshold = 0.6;
|
||||
if(maxVal < correlationThreshold) {
|
||||
qDebug() << "匹配度低:" << maxVal;
|
||||
return lastRoi;
|
||||
}
|
||||
|
||||
// 计算新的ROI位置
|
||||
cv::Rect newRoi;
|
||||
newRoi.x = searchArea.x + maxLoc.x;
|
||||
newRoi.y = searchArea.y + maxLoc.y;
|
||||
newRoi.width = roiTemplate.cols;
|
||||
newRoi.height = roiTemplate.rows;
|
||||
|
||||
qDebug() << "模板匹配成功, 相似度:" << maxVal << "位置:" << newRoi.x << newRoi.y;
|
||||
lastRoi = newRoi;
|
||||
return newRoi;
|
||||
}
|
||||
}
|
||||
|
||||
void MainWindow::updateTemplate(const cv::Mat &frame, const cv::Rect &roi)
|
||||
{
|
||||
static int updateCounter = 0;
|
||||
updateCounter++;
|
||||
|
||||
// 每N帧更新一次模板
|
||||
if(updateCounter % 15 == 0) { // 每15帧更新一次
|
||||
if(roi.x >= 0 && roi.y >= 0 &&
|
||||
roi.x + roi.width <= frame.cols &&
|
||||
roi.y + roi.height <= frame.rows) {
|
||||
|
||||
cv::Mat newTemplate = frame(roi).clone();
|
||||
|
||||
// 使用加权平均更新模板
|
||||
double alpha = 0.1; // 学习率，较小的值使模板更新更慢
|
||||
|
||||
if(roiTemplate.size() == newTemplate.size()) {
|
||||
cv::addWeighted(roiTemplate, 1.0 - alpha, newTemplate, alpha, 0, roiTemplate);
|
||||
qDebug() << "模板已更新";
|
||||
}
|
||||
}
|
||||
updateCounter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
QImage MainWindow::cvMatToQImage(const cv::Mat &mat)
|
||||
{
|
||||
switch(mat.type()) {
|
||||
case CV_8UC1: { // 灰度图
|
||||
QImage img(mat.data, mat.cols, mat.rows,
|
||||
static_cast<int>(mat.step), QImage::Format_Grayscale8);
|
||||
return img.copy(); // 创建副本以避免内存问题
|
||||
}
|
||||
case CV_8UC3: { // 彩色图
|
||||
QImage img(mat.data, mat.cols, mat.rows,
|
||||
static_cast<int>(mat.step), QImage::Format_RGB888);
|
||||
return img.rgbSwapped().copy();
|
||||
}
|
||||
case CV_8UC4: { // 带透明度的彩色图
|
||||
QImage img(mat.data, mat.cols, mat.rows,
|
||||
static_cast<int>(mat.step), QImage::Format_ARGB32);
|
||||
return img.copy();
|
||||
}
|
||||
default:
|
||||
qWarning() << "不支持的图像格式:" << mat.type();
|
||||
return QImage();
|
||||
}
|
||||
}
|
||||
|
||||
cv::Point2f MainWindow::calculateCentroid(const cv::Mat &image, const cv::Rect &roi)
|
||||
{
|
||||
// 提取ROI区域
|
||||
cv::Mat roiRegion = image(roi);
|
||||
|
||||
// 使用灰度图像直接计算矩
|
||||
cv::Moments m = cv::moments(roiRegion, true);
|
||||
|
||||
if(m.m00 == 0) {
|
||||
qDebug() << "无法计算质心：区域可能为全黑";
|
||||
return cv::Point2f(-1, -1);
|
||||
}
|
||||
|
||||
// 计算质心坐标
|
||||
float centroidX = static_cast<float>(m.m10 / m.m00);
|
||||
float centroidY = static_cast<float>(m.m01 / m.m00);
|
||||
|
||||
// 转换为图像坐标系
|
||||
centroidX += roi.x;
|
||||
centroidY += roi.y;
|
||||
|
||||
return cv::Point2f(centroidX, centroidY);
|
||||
}
|
||||
|
||||
void MainWindow::closeloop()
|
||||
{
|
||||
|
||||
|
||||
// if(ui->lineEdit_4->text().isEmpty()||ui->lineEdit_5->text().isEmpty()||ui->lineEdit_6->text().isEmpty()){
|
||||
// qDebug()<<"Error --> B Longtitude/Lantitude/Altitude is empty.";
|
||||
// return ;
|
||||
// }
|
||||
|
||||
while(loopState){
|
||||
|
||||
double ALon =ui->lineEdit_9->text().toDouble();
|
||||
double ALan = ui->lineEdit_10->text().toDouble();
|
||||
double AAlt = ui->lineEdit_11->text().toDouble();
|
||||
|
||||
double AHeading = ui->lineEdit->text().toDouble();
|
||||
double APitch = ui->lineEdit_2->text().toDouble();
|
||||
double ARoll = ui->lineEdit_3->text().toDouble();
|
||||
|
||||
// ALon = 120.30071541667; // A机经度，deg
|
||||
// ALan = 30.2623007866115; // A机纬度，deg
|
||||
// AAlt = 60.0/1000; // A机高度，km
|
||||
|
||||
|
||||
|
||||
double BLon = ui->lineEdit_4->text().toDouble();
|
||||
double BLan = ui->lineEdit_5->text().toDouble();
|
||||
double BAlt= ui->lineEdit_6->text().toDouble();
|
||||
|
||||
|
||||
|
||||
|
||||
// qDebug()<<"A Longtitude is "<<ALon<<"°, Lantitude is "<<ALan<<"°,Altitude is "<<AAlt<<"m.";
|
||||
// qDebug()<<"B Longtitude is "<<BLon<<"°, Lantitude is "<<BLan<<"°,Altitude is "<<BAlt<<"m.";
|
||||
// qDebug()<<"A Heading is "<<AHeading<<"°, Pitch is "<<APitch<<"°,Roll is "<<ARoll<<"°.";
|
||||
|
||||
QVector<double> res = calRes(ALon,ALan,AAlt,BLon,BLan,BAlt,AHeading,APitch,ARoll); //返回指向的方位角和俯仰角度数 E-->Z, N-->U,DEGEE
|
||||
|
||||
rs->move(0,2,res.at(0)*67108864/360);
|
||||
rs->move(0,3,res.at(1)*67108864/360);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds((int)(50)));
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void MainWindow::on_pushButton_clicked()
|
||||
{
|
||||
pos->open();
|
||||
}
|
||||
|
||||
void MainWindow::revPosData(QVector<QString> posData)
|
||||
{
|
||||
ui->lineEdit->setText(posData.at(0)); //Heading
|
||||
ui->lineEdit_2->setText(posData.at(1)); //Pitch
|
||||
ui->lineEdit_3->setText(posData.at(2)); //Roll
|
||||
|
||||
ui->lineEdit_10->setText(posData.at(3)); //Lantitude
|
||||
ui->lineEdit_9->setText(posData.at(4)); //Longtitude
|
||||
ui->lineEdit_11->setText(QString::number(posData.at(5).toDouble()/1000.0)); //Altitude
|
||||
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_3_clicked()
|
||||
{
|
||||
cam->open();
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_5_clicked()
|
||||
{
|
||||
pos->close();
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_4_clicked()
|
||||
{
|
||||
cam->close();
|
||||
}
|
||||
|
||||
void MainWindow::revPicData(QImage img){
|
||||
QPixmap tmppximg = QPixmap::fromImage(img);
|
||||
if(!ui->graphicsView->state){
|
||||
ui->graphicsView->pximg->setPixmap(tmppximg);
|
||||
}else{
|
||||
cv::Mat cvImg; //origin pic
|
||||
cvImg = cv::Mat(img.height(), img.width(), CV_8UC1,
|
||||
(void*)img.constBits(), img.bytesPerLine());
|
||||
|
||||
// 定义ROI区域
|
||||
cv::Rect currentRoi;
|
||||
|
||||
if(isFirstFrame) {
|
||||
// 第一帧，使用初始ROI
|
||||
currentRoi = cv::Rect(topLeft, topRight,
|
||||
selectWidth, selectHeight);
|
||||
|
||||
// 保存模板
|
||||
if(currentRoi.x >= 0 && currentRoi.y >= 0 &&
|
||||
currentRoi.x + currentRoi.width <= cvImg.cols &&
|
||||
currentRoi.y + currentRoi.height <= cvImg.rows) {
|
||||
roiTemplate = cvImg(currentRoi).clone();
|
||||
lastRoi = currentRoi;
|
||||
isFirstFrame = false;
|
||||
|
||||
qDebug() << "初始ROI设置: " << currentRoi.x << currentRoi.y
|
||||
<< currentRoi.width << "x" << currentRoi.height;
|
||||
|
||||
centroidInit = calculateCentroid(cvImg, currentRoi);
|
||||
qDebug() << "初始ROI质心坐标: (" << centroidInit.x << "," << centroidInit.y << ")";
|
||||
}
|
||||
} else {
|
||||
// 后续帧使用模板匹配
|
||||
currentRoi = trackRoiWithCorrelation(cvImg);
|
||||
}
|
||||
|
||||
// 在图像上绘制ROI矩形
|
||||
if(currentRoi.width > 0 && currentRoi.height > 0) {
|
||||
// 为了在灰度图上显示彩色矩形，先转换为BGR
|
||||
cv::Mat colorImg;
|
||||
cv::cvtColor(cvImg, colorImg, cv::COLOR_GRAY2BGR);
|
||||
cv::rectangle(colorImg, currentRoi, cv::Scalar(255, 0, 0), 1);
|
||||
|
||||
centroid = calculateCentroid(cvImg, currentRoi);
|
||||
|
||||
if(centroid.x >= 0 && centroid.y >= 0) {
|
||||
// 在质心位置绘制红色圆点
|
||||
cv::circle(colorImg, centroid, 5, cv::Scalar(255, 0, 0), -1); // 红色实心圆
|
||||
|
||||
// 绘制十字线
|
||||
cv::line(colorImg,
|
||||
cv::Point(centroid.x - 10, centroid.y),
|
||||
cv::Point(centroid.x + 10, centroid.y),
|
||||
cv::Scalar(0, 0, 255), 2); // 水平线
|
||||
cv::line(colorImg,
|
||||
cv::Point(centroid.x, centroid.y - 10),
|
||||
cv::Point(centroid.x, centroid.y + 10),
|
||||
cv::Scalar(0, 0, 255), 2); // 垂直线
|
||||
|
||||
// 显示质心坐标
|
||||
std::string centroidText = "Centroid: (" +
|
||||
std::to_string(centroid.x).substr(0, 5) + ", " +
|
||||
std::to_string(centroid.y).substr(0, 5) + ")";
|
||||
|
||||
cv::putText(colorImg, centroidText,
|
||||
cv::Point(currentRoi.x, currentRoi.y - 20),
|
||||
cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1);
|
||||
|
||||
|
||||
|
||||
|
||||
std::string xTBL = "xTBL: " + std::to_string((centroid.x-centroidInit.x)*0.003953).substr(0, 5)+" degree";
|
||||
std::string yTBL = "yTBL: " + std::to_string((centroid.y-centroidInit.y)*0.003953).substr(0, 5)+" degree";
|
||||
|
||||
|
||||
cv::putText(colorImg, xTBL,
|
||||
cv::Point(20, 20),
|
||||
cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1);
|
||||
|
||||
cv::putText(colorImg, yTBL,
|
||||
cv::Point(20, 40),
|
||||
cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1);
|
||||
|
||||
// qDebug() << "ROI质心坐标: (" << centroid.x << "," << centroid.y << ")";
|
||||
qDebug()<<"X 脱靶量（像素）"<< centroid.x-centroidInit.x;
|
||||
qDebug()<<"Y 脱靶量（像素）"<< centroid.y-centroidInit.y;
|
||||
|
||||
|
||||
// 更新模板
|
||||
updateTemplate(cvImg, currentRoi);
|
||||
|
||||
// 转换回QImage显示
|
||||
QImage resultImg = cvMatToQImage(colorImg);
|
||||
ui->graphicsView->pximg->setPixmap(QPixmap::fromImage(resultImg));
|
||||
} else {
|
||||
// 如果没有有效ROI，直接显示原图
|
||||
ui->graphicsView->pximg->setPixmap(tmppximg);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::revRotateData(QVector<double> rData)
|
||||
{
|
||||
ui->lineEdit_7->setText(QString::number(rData[0],'f',3));
|
||||
ui->lineEdit_8->setText(QString::number(rData[1],'f',3));
|
||||
|
||||
}
|
||||
|
||||
void MainWindow::on_pushButton_10_clicked()
|
||||
{
|
||||
rs->open();
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_6_pressed()
|
||||
{
|
||||
tz->start();
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_6_released()
|
||||
{
|
||||
tz->stop();
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::zmove(){
|
||||
rs->move(0,2,1000);
|
||||
}
|
||||
|
||||
void MainWindow::zmove_()
|
||||
{
|
||||
rs->move(0,2,-1000);
|
||||
}
|
||||
|
||||
void MainWindow::umove()
|
||||
{
|
||||
rs->move(0,3,1000);
|
||||
}
|
||||
|
||||
void MainWindow::umove_()
|
||||
{
|
||||
rs->move(0,3,-1000);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_7_pressed()
|
||||
{
|
||||
|
||||
if(rs->state){
|
||||
tz_->start();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_7_released()
|
||||
{
|
||||
if(rs->state){
|
||||
tz_->stop();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_8_pressed()
|
||||
{
|
||||
|
||||
if(rs->state){
|
||||
tu->start();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_8_released()
|
||||
{
|
||||
if(rs->state){
|
||||
tu->stop();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_9_pressed()
|
||||
{
|
||||
|
||||
if(rs->state){
|
||||
tu_->start();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_9_released()
|
||||
{
|
||||
if(rs->state){
|
||||
tu_->stop();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_11_clicked()
|
||||
{
|
||||
rs->close();
|
||||
}
|
||||
|
||||
void MainWindow::onRoiSelected(const QRectF &roi)
|
||||
{
|
||||
|
||||
qDebug() << "ROI Selected - Position:" << roi.topLeft()
|
||||
<< "Size:" << roi.size();
|
||||
topLeft = roi.topLeft().x();
|
||||
topRight = roi.topRight().y();
|
||||
selectWidth=roi.width();
|
||||
selectHeight=roi.height();
|
||||
isFirstFrame=true;
|
||||
ui->graphicsView->state=true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_2_clicked()
|
||||
{
|
||||
double ALon =ui->lineEdit_9->text().toDouble();
|
||||
double ALan = ui->lineEdit_10->text().toDouble();
|
||||
double AAlt =ui->lineEdit_11->text().toDouble();
|
||||
|
||||
double AHeading = ui->lineEdit->text().toDouble();
|
||||
double APitch = ui->lineEdit_2->text().toDouble();
|
||||
double ARoll = ui->lineEdit_3->text().toDouble();
|
||||
|
||||
// if(ui->lineEdit_4->text().isEmpty()||ui->lineEdit_5->text().isEmpty()||ui->lineEdit_6->text().isEmpty()){
|
||||
// QMessageBox::critical(this,"Error","B Longtitude/Lantitude/Altitude is empty.");
|
||||
// return ;
|
||||
// }
|
||||
double BLon = ui->lineEdit_4->text().toDouble();
|
||||
double BLan = ui->lineEdit_5->text().toDouble();
|
||||
double BAlt= ui->lineEdit_6->text().toDouble();
|
||||
|
||||
|
||||
|
||||
// gcal->setPositionA(ALon,ALan,AAlt);
|
||||
// gcal->setPositionB(BLon,BLan,BAlt);
|
||||
// gcal->setRealTimeAttitude(AHeading,APitch,ARoll);
|
||||
|
||||
|
||||
|
||||
|
||||
// // // A机位置
|
||||
// ALon = 120.30071541667; // A机经度，deg
|
||||
// ALan = 30.2623007866115; // A机纬度，deg
|
||||
// AAlt = 60.0/1000; // A机高度，km
|
||||
|
||||
// // // B机位置
|
||||
// BLon = 120.300913900136; // B机经度，deg
|
||||
// BLan = 30.2622503984337; // B机纬度，deg
|
||||
// BAlt = 60.0/1000 ;
|
||||
|
||||
|
||||
// // // 根据POS设备实时姿态角解算B机在A机载体坐标系中的位置
|
||||
// AHeading = 65.516;
|
||||
// APitch = -0.112;
|
||||
// ARoll = 1.648;
|
||||
|
||||
qDebug()<<"A Longtitude is "<<ALon<<"°, Lantitude is "<<ALan<<"°,Altitude is "<<AAlt<<"km.";
|
||||
qDebug()<<"B Longtitude is "<<BLon<<"°, Lantitude is "<<BLan<<"°,Altitude is "<<BAlt<<"km.";
|
||||
qDebug()<<"A Heading is "<<AHeading<<"°, Pitch is "<<APitch<<"°,Roll is "<<ARoll<<"°.";
|
||||
|
||||
|
||||
QVector<double> res = calRes(ALon,ALan,AAlt,BLon,BLan,BAlt,AHeading,APitch,ARoll); //返回指向的方位角和俯仰角度数 E-->Z, N-->U,DEGEE
|
||||
|
||||
rs->move(0,2,res.at(0)*67108864/360);
|
||||
rs->move(0,3,res.at(1)*67108864/360);
|
||||
|
||||
qDebug() <<"Single Close Loop track done";
|
||||
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_12_clicked()
|
||||
{
|
||||
rs->stop();
|
||||
loopState= false;
|
||||
// task->join();
|
||||
qDebug() <<"Open Loop track";
|
||||
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_13_clicked()
|
||||
{
|
||||
double zDegree;
|
||||
if(ui->lineEdit_12->text().isEmpty()){
|
||||
zDegree = 0.0;
|
||||
}else{
|
||||
if(ui->lineEdit_12->text().toDouble()<0 ||ui->lineEdit_12->text().toDouble()>360 ){
|
||||
QMessageBox::critical(this,"Error","Z Degree < 0 or Z Degree > 360");
|
||||
return;
|
||||
}
|
||||
zDegree = ui->lineEdit_12->text().toDouble();
|
||||
}
|
||||
|
||||
double uDegree;
|
||||
if(ui->lineEdit_13->text().isEmpty()){
|
||||
uDegree = 0.0;
|
||||
}else{
|
||||
if(ui->lineEdit_13->text().toDouble()<0 ||ui->lineEdit_13->text().toDouble()>360 ){
|
||||
QMessageBox::critical(this,"Error","U Degree < 0 or U Degree > 360");
|
||||
return ;
|
||||
}
|
||||
uDegree = ui->lineEdit_13->text().toDouble();
|
||||
}
|
||||
|
||||
|
||||
|
||||
rs->move(1,2,zDegree*67108864/360);
|
||||
rs->move(1,3,uDegree*67108864/360);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void MainWindow::on_pushButton_14_clicked()
|
||||
{
|
||||
|
||||
loopState =true ;
|
||||
qDebug() <<"Start RT Close Loop track";
|
||||
// task = std::thread(&MainWindow::closeloop,this);
|
||||
task = new std::thread(&MainWindow::closeloop,this);
|
||||
}
|
||||
|
||||
107
mainwindow.h
Normal file
107
mainwindow.h
Normal file
@ -0,0 +1,107 @@
|
||||
#ifndef MAINWINDOW_H
|
||||
#define MAINWINDOW_H
|
||||
|
||||
#include <QMainWindow>
|
||||
#include "posdevice.h"
|
||||
#include "camera.h"
|
||||
#include "QImage"
|
||||
#include "QPixmap"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include "rotationstage.h"
|
||||
#include "QTimer"
|
||||
#include "imageview.h"
|
||||
|
||||
QT_BEGIN_NAMESPACE
|
||||
namespace Ui {
|
||||
class MainWindow;
|
||||
}
|
||||
QT_END_NAMESPACE
|
||||
|
||||
class MainWindow : public QMainWindow
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
MainWindow(QWidget *parent = nullptr);
|
||||
~MainWindow();
|
||||
cv::Rect trackRoiWithCorrelation(const cv::Mat& frame);
|
||||
void updateTemplate(const cv::Mat& frame, const cv::Rect& roi);
|
||||
QImage cvMatToQImage(const cv::Mat& mat);
|
||||
cv::Point2f calculateCentroid(const cv::Mat& image, const cv::Rect& roi);
|
||||
void closeloop();
|
||||
|
||||
|
||||
|
||||
private slots:
|
||||
void on_pushButton_clicked();
|
||||
void revPosData(QVector<QString>);
|
||||
void revPicData(QImage);
|
||||
void revRotateData(QVector<double>);
|
||||
|
||||
void on_pushButton_3_clicked();
|
||||
|
||||
void on_pushButton_5_clicked();
|
||||
|
||||
void on_pushButton_4_clicked();
|
||||
|
||||
void on_pushButton_10_clicked();
|
||||
|
||||
void on_pushButton_6_pressed();
|
||||
|
||||
void on_pushButton_6_released();
|
||||
void zmove();
|
||||
void zmove_();
|
||||
void umove();
|
||||
void umove_();
|
||||
|
||||
|
||||
|
||||
void on_pushButton_7_pressed();
|
||||
|
||||
void on_pushButton_7_released();
|
||||
|
||||
void on_pushButton_8_pressed();
|
||||
|
||||
void on_pushButton_8_released();
|
||||
|
||||
void on_pushButton_9_pressed();
|
||||
|
||||
void on_pushButton_9_released();
|
||||
|
||||
void on_pushButton_11_clicked();
|
||||
|
||||
void onRoiSelected(const QRectF &roi);
|
||||
|
||||
void on_pushButton_2_clicked();
|
||||
|
||||
void on_pushButton_12_clicked();
|
||||
|
||||
|
||||
void on_pushButton_13_clicked();
|
||||
|
||||
void on_pushButton_14_clicked();
|
||||
|
||||
private:
|
||||
Ui::MainWindow *ui;
|
||||
PosDevice *pos;
|
||||
Camera *cam;
|
||||
QGraphicsScene *scene;
|
||||
RotationStage *rs;
|
||||
QTimer *tz,*tz_,*tu,*tu_;
|
||||
QGraphicsPixmapItem *pximg;
|
||||
int topLeft,topRight,selectWidth,selectHeight;
|
||||
bool isFirstFrame;
|
||||
|
||||
cv::Mat roiTemplate; // ROI模板
|
||||
cv::Rect lastRoi; // 上一帧的ROI位置
|
||||
|
||||
cv::Point2f centroidInit; // 初始位置
|
||||
cv::Point2f centroid; //当前位置
|
||||
|
||||
bool loopState;
|
||||
std::thread *task;
|
||||
|
||||
|
||||
|
||||
};
|
||||
#endif // MAINWINDOW_H
|
||||
359
mainwindow.ui
Normal file
359
mainwindow.ui
Normal file
@ -0,0 +1,359 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>MainWindow</class>
|
||||
<widget class="QMainWindow" name="MainWindow">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>829</width>
|
||||
<height>937</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>MainWindow</string>
|
||||
</property>
|
||||
<widget class="QWidget" name="centralwidget">
|
||||
<layout class="QGridLayout" name="gridLayout">
|
||||
<item row="5" column="0" colspan="2">
|
||||
<widget class="QGroupBox" name="groupBox_3">
|
||||
<property name="title">
|
||||
<string>Camera</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_4">
|
||||
<item row="0" column="0">
|
||||
<widget class="QPushButton" name="pushButton_3">
|
||||
<property name="text">
|
||||
<string>Connect to Camera</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="ImageView" name="graphicsView"/>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QPushButton" name="pushButton_4">
|
||||
<property name="text">
|
||||
<string>Disconnect</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QGroupBox" name="groupBox_5">
|
||||
<property name="title">
|
||||
<string>Pos</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_6">
|
||||
<item row="1" column="2">
|
||||
<widget class="QGroupBox" name="groupBox_2">
|
||||
<property name="title">
|
||||
<string>Target Postion</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_3">
|
||||
<item row="7" column="1" colspan="3">
|
||||
<widget class="QPushButton" name="pushButton_12">
|
||||
<property name="text">
|
||||
<string>Oepn Loop Track</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="1">
|
||||
<widget class="QLabel" name="label_6">
|
||||
<property name="text">
|
||||
<string>Altitude:</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<widget class="QLineEdit" name="lineEdit_5"/>
|
||||
</item>
|
||||
<item row="6" column="1" colspan="3">
|
||||
<widget class="QPushButton" name="pushButton_14">
|
||||
<property name="text">
|
||||
<string>Real Time Close Loop Track</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="5" column="1" colspan="3">
|
||||
<widget class="QPushButton" name="pushButton_2">
|
||||
<property name="text">
|
||||
<string>Single Close Loop Track</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="2">
|
||||
<widget class="QLineEdit" name="lineEdit_4"/>
|
||||
</item>
|
||||
<item row="3" column="2">
|
||||
<widget class="QLineEdit" name="lineEdit_6"/>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="label_4">
|
||||
<property name="text">
|
||||
<string>Longtitude:</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="label_5">
|
||||
<property name="text">
|
||||
<string>Latitude:</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0" colspan="2">
|
||||
<widget class="QGroupBox" name="groupBox">
|
||||
<property name="title">
|
||||
<string>Pos Info</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_2">
|
||||
<item row="3" column="2">
|
||||
<widget class="QLineEdit" name="lineEdit_11"/>
|
||||
</item>
|
||||
<item row="6" column="2">
|
||||
<widget class="QLineEdit" name="lineEdit_3"/>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<widget class="QLineEdit" name="lineEdit_10"/>
|
||||
</item>
|
||||
<item row="2" column="2">
|
||||
<widget class="QLineEdit" name="lineEdit_9"/>
|
||||
</item>
|
||||
<item row="5" column="0">
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="text">
|
||||
<string>Pitch:</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="0">
|
||||
<widget class="QLabel" name="label">
|
||||
<property name="text">
|
||||
<string>Heading:</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="5" column="2">
|
||||
<widget class="QLineEdit" name="lineEdit_2"/>
|
||||
</item>
|
||||
<item row="4" column="2">
|
||||
<widget class="QLineEdit" name="lineEdit"/>
|
||||
</item>
|
||||
<item row="6" column="0">
|
||||
<widget class="QLabel" name="label_3">
|
||||
<property name="text">
|
||||
<string>Roll:</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="0">
|
||||
<widget class="QLabel" name="label_11">
|
||||
<property name="text">
|
||||
<string>Altitude:</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QLabel" name="label_9">
|
||||
<property name="text">
|
||||
<string>Longtitude:</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_10">
|
||||
<property name="text">
|
||||
<string>Latitude:</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="2">
|
||||
<widget class="QPushButton" name="pushButton_5">
|
||||
<property name="text">
|
||||
<string>Disconnect</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="0" colspan="2">
|
||||
<widget class="QPushButton" name="pushButton">
|
||||
<property name="text">
|
||||
<string>Connect to PosDevice</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QGroupBox" name="groupBox_4">
|
||||
<property name="title">
|
||||
<string>Rotation Stage</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_5">
|
||||
<item row="2" column="0">
|
||||
<widget class="QLabel" name="label_7">
|
||||
<property name="text">
|
||||
<string>Z (Current Degree) ：</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="0">
|
||||
<widget class="QLabel" name="label_8">
|
||||
<property name="text">
|
||||
<string>U (Current Degree) ：</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0" colspan="6">
|
||||
<widget class="QPushButton" name="pushButton_11">
|
||||
<property name="text">
|
||||
<string>Disconnect</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="0" colspan="6">
|
||||
<widget class="QPushButton" name="pushButton_10">
|
||||
<property name="text">
|
||||
<string>Connect to Rotation Stage</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="0" colspan="6">
|
||||
<widget class="QGroupBox" name="groupBox_6">
|
||||
<property name="title">
|
||||
<string>Set Absolute Location</string>
|
||||
</property>
|
||||
<layout class="QGridLayout" name="gridLayout_7">
|
||||
<item row="0" column="1" colspan="2">
|
||||
<widget class="QLineEdit" name="lineEdit_12"/>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<widget class="QPushButton" name="pushButton_7">
|
||||
<property name="text">
|
||||
<string>-</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="1">
|
||||
<widget class="QPushButton" name="pushButton_8">
|
||||
<property name="text">
|
||||
<string>+</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QLabel" name="label_13">
|
||||
<property name="text">
|
||||
<string>U(Degree):</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1" colspan="2">
|
||||
<widget class="QLineEdit" name="lineEdit_13"/>
|
||||
</item>
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_12">
|
||||
<property name="text">
|
||||
<string>Z(Degree):</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QPushButton" name="pushButton_6">
|
||||
<property name="text">
|
||||
<string>+</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="2">
|
||||
<widget class="QPushButton" name="pushButton_9">
|
||||
<property name="text">
|
||||
<string>-</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="3" rowspan="5">
|
||||
<widget class="QPushButton" name="pushButton_13">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Set AbsLocation</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1" colspan="5">
|
||||
<widget class="QLineEdit" name="lineEdit_7"/>
|
||||
</item>
|
||||
<item row="3" column="1" colspan="5">
|
||||
<widget class="QLineEdit" name="lineEdit_8"/>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
<widget class="QMenuBar" name="menubar">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>829</width>
|
||||
<height>21</height>
|
||||
</rect>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QStatusBar" name="statusbar"/>
|
||||
</widget>
|
||||
<customwidgets>
|
||||
<customwidget>
|
||||
<class>ImageView</class>
|
||||
<extends>QGraphicsView</extends>
|
||||
<header>imageview.h</header>
|
||||
</customwidget>
|
||||
</customwidgets>
|
||||
<resources/>
|
||||
<connections/>
|
||||
</ui>
|
||||
47
posdevice.cpp
Normal file
47
posdevice.cpp
Normal file
@ -0,0 +1,47 @@
|
||||
#include "posdevice.h"
|
||||
|
||||
PosDevice::PosDevice(QObject *parent)
|
||||
: QObject{parent}
|
||||
{
|
||||
serialHandler = new QSerialPort(this);
|
||||
serialHandler->setPortName(COMNAME);
|
||||
serialHandler->setBaudRate(460800);
|
||||
|
||||
|
||||
}
|
||||
|
||||
bool PosDevice::open(){
|
||||
bool bState = false;
|
||||
if(serialHandler->open(QIODevice::ReadWrite)){
|
||||
bState = true;
|
||||
}
|
||||
connect(serialHandler,&QSerialPort::readyRead,this,&PosDevice::readData);
|
||||
return bState;
|
||||
}
|
||||
|
||||
void PosDevice::close(){
|
||||
disconnect(serialHandler,&QSerialPort::readyRead,this,&PosDevice::readData);
|
||||
serialHandler->close();
|
||||
}
|
||||
|
||||
|
||||
void PosDevice::readData(){
|
||||
QByteArray datatmp = serialHandler->readAll();
|
||||
if(!datatmp.isEmpty()){
|
||||
QString data = QString::fromUtf8(datatmp);
|
||||
QVector<QString> posdata;
|
||||
// qDebug()<<data.split(',').at(3);
|
||||
// qDebug()<<data.split(',').at(4);
|
||||
// qDebug()<<data.split(',').at(5);
|
||||
|
||||
posdata.append(data.split(',').at(3)); // Heading
|
||||
posdata.append(data.split(',').at(4)); // Pitch
|
||||
posdata.append(data.split(',').at(5)); // Roll
|
||||
|
||||
posdata.append(data.split(',').at(6)); // Lantitude
|
||||
posdata.append(data.split(',').at(7)); // Longtitude
|
||||
posdata.append(data.split(',').at(8)); // Altitude
|
||||
emit sendPosData(posdata);
|
||||
}
|
||||
|
||||
}
|
||||
31
posdevice.h
Normal file
31
posdevice.h
Normal file
@ -0,0 +1,31 @@
|
||||
#ifndef POSDEVICE_H
|
||||
#define POSDEVICE_H
|
||||
|
||||
#include <QObject>
|
||||
#include "QSerialPort"
|
||||
#define COMNAME "com6"
|
||||
#include "QDebug"
|
||||
#include "QString"
|
||||
#include "QVector"
|
||||
|
||||
|
||||
|
||||
class PosDevice : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
explicit PosDevice(QObject *parent = nullptr);
|
||||
bool open();
|
||||
void close();
|
||||
void readData();
|
||||
|
||||
signals:
|
||||
void sendPosData(QVector<QString>);
|
||||
|
||||
private:
|
||||
QSerialPort *serialHandler;
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // POSDEVICE_H
|
||||
74
rotationstage.cpp
Normal file
74
rotationstage.cpp
Normal file
@ -0,0 +1,74 @@
|
||||
#include "rotationstage.h"
|
||||
|
||||
RotationStage::RotationStage(QObject *parent)
|
||||
: QObject{parent}
|
||||
{
|
||||
m_ConnectNum = 8;
|
||||
m_strIPAdress = "192.168.5.11";
|
||||
short num = dmc_board_init_eth(m_ConnectNum,m_strIPAdress);
|
||||
qDebug()<<"dmc_board_init_eth返回值="<<num;
|
||||
if(num != 0)
|
||||
{
|
||||
QMessageBox::question(NULL,"错误","初始化卡失败！",QMessageBox::Yes | QMessageBox::No,QMessageBox::Yes);
|
||||
}
|
||||
else
|
||||
{
|
||||
QMessageBox::question(NULL,"正确","初始化卡成功！",QMessageBox::Yes | QMessageBox::No,QMessageBox::Yes);
|
||||
}
|
||||
|
||||
ushort CardNum;
|
||||
DWORD CardTypeList[8];
|
||||
ushort CardIdList[8];
|
||||
rtn = dmc_get_CardInfList(&CardNum,CardTypeList,CardIdList);
|
||||
if(rtn != 0)
|
||||
{
|
||||
QMessageBox::question(NULL,"错误","获取卡信息失败！",QMessageBox::Yes | QMessageBox::No,QMessageBox::Yes);
|
||||
}
|
||||
else
|
||||
{
|
||||
QMessageBox::question(NULL,"正确","获取卡信息成功！",QMessageBox::Yes | QMessageBox::No,QMessageBox::Yes);
|
||||
CardNo = CardIdList[0];
|
||||
}
|
||||
state = false;
|
||||
}
|
||||
|
||||
void RotationStage::open()
|
||||
{
|
||||
state = true;
|
||||
|
||||
task = new std::thread(&RotationStage::aquireData,this);
|
||||
}
|
||||
|
||||
void RotationStage::aquireData()
|
||||
{
|
||||
double factor = 67108864/360;
|
||||
while(state){
|
||||
QVector<double> rData;
|
||||
double rDataAxisZ = 0;
|
||||
double rDataAxisU = 0;
|
||||
dmc_get_position_unit(CardNo,2,&rDataAxisZ);
|
||||
dmc_get_position_unit(CardNo,3,&rDataAxisU);
|
||||
rData.append(rDataAxisZ/factor);
|
||||
rData.append(rDataAxisU/factor);
|
||||
// qDebug()<<rDataAxisZ;
|
||||
// qDebug()<<rDataAxisU;
|
||||
emit sendRotateData(rData);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
}
|
||||
|
||||
void RotationStage::move(int mode, int axis, int dis) //mode:0, axis:2(z),3(u),dis
|
||||
{
|
||||
dmc_pmove_unit(CardNo,axis,dis,mode);//定长运动
|
||||
}
|
||||
|
||||
void RotationStage::stop()
|
||||
{
|
||||
dmc_emg_stop(CardNo);
|
||||
}
|
||||
|
||||
void RotationStage::close()
|
||||
{
|
||||
state=false;
|
||||
task->join();
|
||||
}
|
||||
39
rotationstage.h
Normal file
39
rotationstage.h
Normal file
@ -0,0 +1,39 @@
|
||||
#ifndef ROTATIONSTAGE_H
|
||||
#define ROTATIONSTAGE_H
|
||||
|
||||
#include <QObject>
|
||||
#include "LTDMC.h"
|
||||
#include "QDebug"
|
||||
#include "QMessageBox"
|
||||
#include "thread"
|
||||
|
||||
class RotationStage : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
explicit RotationStage(QObject *parent = nullptr);
|
||||
void open();
|
||||
void aquireData();
|
||||
void move(int mode, int axis,int dis);
|
||||
void stop();
|
||||
void close();
|
||||
|
||||
bool state;
|
||||
|
||||
|
||||
|
||||
|
||||
signals:
|
||||
void sendRotateData(QVector<double>);
|
||||
|
||||
private:
|
||||
short rtn;
|
||||
ushort CardNo;
|
||||
WORD m_ConnectNum;
|
||||
const char* m_strIPAdress;
|
||||
|
||||
std::thread *task;
|
||||
};
|
||||
|
||||
|
||||
#endif // ROTATIONSTAGE_H
|
||||
Loading…
Reference in New Issue
Block a user