Diff: run.cpp

Revision:

104:f81befbc5ab7

Parent:

103:34e911b94440

Child:

105:aaaed895ffaf

--- a/run.cpp	Mon Feb 28 12:21:31 2022 +0000
+++ b/run.cpp	Tue Mar 01 06:53:06 2022 +0000
@@ -3,66 +3,7 @@
 void run()
 {
     
-    pc.printf("reading calibration value\r\n");
-    //キャリブレーション値を取得
-    U read_calib;
-    readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4);
-    wait(3);
-    pos_tail = (int)read_calib.i[0];
-    magbiasMin[0] = float(read_calib.i[1])/1000.0f;
-    magbiasMin[1] = float(read_calib.i[2])/1000.0f;
-    magbiasMin[2] = float(read_calib.i[3])/1000.0f;
-    magbiasMax[0] = float(read_calib.i[4])/1000.0f;
-    magbiasMax[1] = float(read_calib.i[5])/1000.0f;
-    magbiasMax[2] = float(read_calib.i[6])/1000.0f;
-    roll_offset = (float)read_calib.i[10] / 200000.0f;
-    pitch_offset = (float)read_calib.i[11] / 200000.0f;
-    
-    //センサの初期化・ジャイロバイアス・加速度スケールの取得
-    int n_init = 1000;
-    for(int i = 0;i<n_init;i++){
-        lsm.readAccel();
-        lsm.readMag();
-        lsm.readGyro();
-        agoffset[0] += lsm.ax * 9.8f;
-        agoffset[1] += lsm.ay * 9.8f;
-        agoffset[2] += lsm.az * 9.8f-9.8f;
-        agoffset[3] +=(lsm.gx * M_PI / 180.0f);
-        agoffset[4] +=(lsm.gy * M_PI / 180.0f); 
-        agoffset[5] +=(lsm.gz * M_PI / 180.0f); 
-        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
-        magref.x += lsm.mx;
-        magref.y += lsm.my;
-        magref.z += lsm.mz;
-    }
-    for(int i = 0;i<6;i++){
-        agoffset[i] /= float(n_init);
-    }
-    //eskf.setAccBias(agoffset[0],agoffset[1],agoffset[2]);
-    //eskf.setGyroBias(agoffset[3],agoffset[4],agoffset[5]);
-    magref.x /= float(n_init);
-    magref.y /= float(n_init);
-    magref.z /= float(n_init);
-    palt0 /= float(n_init);
-    
-    switch(pos_tail){
-    case 0:
-        pc.printf("This MBED is Located at Left \r\n");
-        break;
-    case 1:
-        pc.printf("This MBED is Located at Center \r\n");
-        break;
-    case 2:
-        pc.printf("This MBED is Located at Right \r\n");
-        break;
-    default:   // error situation
-        pc.printf("error\r\n");
-        break;
-    }
-    pc.printf("tail_address : %d\r\n", tail_address[pos_tail]);
-    pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",roll_offset*180.0f/M_PI,pitch_offset*180.0f/M_PI);
-    
-    wait(1);
+    preflightCalibration();
     
     //ESKFの共分散設定
     eskf.setGravity(0.0f,0.0f,9.8f);
@@ -78,14 +19,6 @@
     eskf.setQAccBias(0.000001f);
     eskf.setQGyroBias(0.000001f);
     
-    Matrix Rgpspos(3,3);
-    setDiag(Rgpspos,1.0f);
-    
-    Matrix Rgpsvel(3,3);
-    Rgpsvel(1,1) = 0.01f;
-    Rgpsvel(2,2) = 0.01f;
-    Rgpsvel(3,3) = 0.1f;
-    
     Matrix Rgps(5,5);
     setDiag(Rgps,0.05f);
     Rgps(4,4) = 0.1f;
@@ -101,58 +34,12 @@
     Rheading(1,1) = 0.01f;
     
     wait(0.5);
-    Timer _t;
+    
     _t.start();
     tail.Subscribe(tail_address[pos_tail], &(updateValues));
     
-    //pre-flight check
-    int i = 1;
-    while(1){
-        //float tstart = _t.read();
-        //getIMUval();
-        //eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
-        //eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
-        //Matrix Reye(3,3);
-        //setDiag(Reye,1.0f);
-        //Matrix zeroMat(3,1);
-        //zeroMat(1,1) = 0.0f;
-        //zeroMat(2,1) = 0.0f;
-        //zeroMat(3,1) = 0.0f;
-        //if(i%3==0){
-        //    eskf.updateGPSVelocity(zeroMat,Reye);
-        //}else if(i%3==1){
-        //    float heading = atan2f(-mag.y,mag.x);
-        //    eskf.updateHeading(heading,Rheading);
-        //}else{
-        //    eskf.updateAcc(MatrixMath::Vector2mat(acc),Reye);
-        //}
-        
-        bool preflightCheck = true;
-        if(sbus.failSafe){
-            pc.printf("sbus\r\n");
-            preflightCheck = false;
-        }
-        if(!(updateValues.gps_fix == 0x03)){
-            pc.printf("gps\r\n");
-            preflightCheck = false;
-        }
-        //pc.printf("%d %d \r\n",updateValues.gps_itow,updateValues.gps_fix );
-        
-        if(preflightCheck == true && i>500){
-            prefligt_finished = true;
-            break;
-        }
-        wait(0.01f);
-        pc.printf("pre-flight check\r\n");
-        send2center();
-        //float tend = _t.read();
-        //att_dt = (tend-tstart);
-        i++;
-    }
+    preflightCheck();
     
-    wait(2.0f);
-    
-    eskf.setPihat(updateValues.pi[0], updateValues.pi[1], palt);
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
@@ -161,7 +48,7 @@
     
     while(1)
     {
-        float tstart = _t.read();
+        tstart = _t.read();
         //センサの値を取得
         getIMUval();
         calcOpticalVel();
@@ -172,27 +59,7 @@
         
         //キャリブレーション中
         if(updateValues.calibrationFlag == 1111){
-            gpsUpdateFlag = false;
-            if(tstart-tgps>0.2f){
-                gpsUpdateFlag = true;
-                tgps = _t.read();
-            }
-            
-            headingUpdateFlag = false;
-            if(tstart-theading>0.05f){
-                headingUpdateFlag = true;
-                theading = _t.read();
-            }
-
-            if(gpsUpdateFlag == true){
-                Vector3 pi(0.0f, 0.0f, 0.0f);
-                Vector3 vi(0.0f, 0.0f, 0.0f);
-                eskf.updateGPS(MatrixMath::Vector2mat(pi),0.0f,MatrixMath::Vector2mat(vi),Rgps);
-            }else if(headingUpdateFlag == true){
-                eskf.updateHeading(0.0f,Rheading);
-            }else{
-                eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc*0.01f);
-            }
+            mainloopCalibration();
         }else{
             gpsUpdateFlag = false;
             //pc.printf("%d %d \r\n",updateValues.gps_itow,updateValues.gps_fix );