20230531

src/tool/adcndtmultimapping/mainwindow.cpp

@@ -240,6 +240,9 @@ void MainWindow::on_pushButton_Load_clicked()
 
     mnPac = 0;
 
+    mbCalcCalib = false;
+    mbCalcCalib_Roll = false;
+
     int ncalindex = mpCB_Mode->currentIndex();
     if(ncalindex == 0)
     {
@@ -428,6 +431,17 @@ void MainWindow::onTimer()
             CalcCalib();
             mbCalcCalib = true;
         }
+        if((gsplit_cur>100.0) &&(mbCalcCalib_Roll == false))
+        {
+            if(mvectortrace.size()>0)
+            {
+                if(fabs(mvectortrace[mvectortrace.size()-1].y) > 50.0)
+                {
+                    CalcCalibRoll();
+                    mbCalcCalib_Roll = true;
+                }
+            }
+        }
         update();
     }
     //记录执行了多少次
@@ -1383,8 +1397,51 @@ void MainWindow::CalcCalib()
             while(calib_yaw < 0)calib_yaw = calib_yaw + 2.0*M_PI;
 
             mpLE_CalcCalib_Yaw->setText(QString::number(calib_yaw*180.0/M_PI));
+
+
+            double calib_pitch = (-1.0) * asin((gps_z - ndt_z)/gps_dis);
+            while(calib_pitch>2.0*M_PI) calib_pitch = calib_pitch - 2.0*M_PI;
+            while(calib_pitch < 0)calib_pitch = calib_pitch + 2.0*M_PI;
+
+            mpLE_CalcCalib_Pitch->setText(QString::number(calib_pitch * 180.0/M_PI));
+
+
+        }
+
+    }
+
+}
+
+void MainWindow::CalcCalibRoll()
+{
+    if((mvectorgps.size()>0) &&(mvectortrace.size()>0))
+    {
+        double gps_x = mvectorgps[mvectorgps.size()-1].x;
+        double gps_y = mvectorgps[mvectorgps.size()-1].y;
+        double gps_z = mvectorgps[mvectorgps.size()-1].z;
+
+        double ndt_x = mvectortrace[mvectortrace.size()-1].x;
+        double ndt_y = mvectortrace[mvectortrace.size()-1].y;
+        double ndt_z = mvectortrace[mvectortrace.size()-1].z;
+
+        double gps_dis = sqrt(pow(gps_x,2)+pow(gps_y,2));
+        double ndt_dis = sqrt(pow(ndt_x,2)+pow(ndt_y,2));
+
+        if((gps_dis>0)&&(ndt_dis>0) &&(fabs(gps_y)>10))
+        {
+
+
+
+            double calib_roll = (1.0) * asin((gps_z - ndt_z)/gps_y);
+            while(calib_roll>2.0*M_PI) calib_roll = calib_roll - 2.0*M_PI;
+            while(calib_roll < 0)calib_roll = calib_roll + 2.0*M_PI;
+
+            mpLE_CalcCalib_Roll->setText(QString::number(calib_roll * 180.0/M_PI));
+
+
         }
 
     }
+
 }
 

src/tool/adcndtmultimapping/mainwindow.h

@@ -135,6 +135,8 @@ private:
 
     bool mbCalcCalib = false;
 
+    bool mbCalcCalib_Roll = false;
+
     inline QDateTime GetDateTimeFromRH(iv::RecordHead rh);
     inline bool ReadARecord(int & nRecSize,char ** pstrName, int * pnDataSize, char ** pstrData);
 
@@ -147,6 +149,7 @@ private:
     void UpdateMapParam();
 
     void CalcCalib();
+    void CalcCalibRoll();
 
 public:
      void resizeEvent(QResizeEvent *event);