Diff: run.cpp

Revision:

90:0b1f62f7a83a

Parent:

89:316c20d3184c

Child:

91:393b9ae62681

--- a/run.cpp	Wed Jan 12 09:54:55 2022 +0000
+++ b/run.cpp	Thu Jan 13 08:38:56 2022 +0000
@@ -9,24 +9,15 @@
     readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4);
     wait(3);
     pos_tail = (int)read_calib.i[0];
-    roll_offset = (float)read_calib.i[10] / 200000.0f;
-    pitch_offset = (float)read_calib.i[11] / 200000.0f;
-    /*
-    agoffset[3] = float(read_calib.i[7]);
-    agoffset[4] = float(read_calib.i[8]);
-    agoffset[5] = float(read_calib.i[9]);
     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;
-    rpy_align.x = float(read_calib.i[10])/200000.0f;
-    rpy_align.y = float(read_calib.i[11])/200000.0f;
-    magCalibrator.setExtremes(magbiasMin,magbiasMax);
-//    tail_address[pos_tail] = (int)read_calib.i[10];
-    */
-    /*
+    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++){
@@ -51,7 +42,6 @@
     magref.y /= float(n_init);
     magref.z /= float(n_init);
     palt0 /= float(n_init);
-    */
     
     switch(pos_tail){
     case 0:
@@ -68,7 +58,9 @@
         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",rpy_align.x*180.0f/M_PI,rpy_align.y*180.0f/M_PI);
+    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);
     
     //ESKFの共分散設定
     eskf.setGravity(0.0f,0.0f,9.8f);
@@ -109,90 +101,6 @@
     wait(0.5);
     Timer _t;
     _t.start();
-    
-    /*
-    magCalibrator.setExtremes(magbiasMin,magbiasMax);
-    for(int i = 0; i < 1000; i++){
-        getIMUval();
-    }
-    ekf.defineQhat(rpy_align);
-    float sum2accnorm = 0;
-    float sumaccnorm = 0;
-    for(int i = 0; i < 1000; i++){
-        float tstart = _t.read();
-        getIMUval();
-        ekf.updateStaticAccMeasures(acc,accref);
-        ekf.updateGyroBiasConstraints(gyro);
-        ekf.fuseErr2Nominal();
-        ekf.resetBias();
-        //ekf.updateMagMeasures(mag);
-        ekf.computeAngles(rpy, rpy_align);
-        sumaccnorm += acc.Norm();
-        sum2accnorm += acc.Norm()*acc.Norm();
-        float tend = _t.read();
-        att_dt = (tend-tstart);
-    }
-    accref.z =  sumaccnorm / 1000.0f;
-    */
-    
-    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);
-        Vector3 pi(updateValues.pi[0], updateValues.pi[1], updateValues.pi[2]);
-        Vector3 vi(updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
-        eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
-        float heading = atan2f(-mag.y,mag.x);
-        eskf.updateHeading(heading,Rheading);
-        Matrix Raccpreflight(3,3);
-        setDiag(Raccpreflight,5.0f);
-        eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight);
-        
-        bool preflightCheck = true;
-        Matrix gyroBias = eskf.getGyroBias();
-        if(fabsf(gyro.x-gyroBias(1,1))>2.0f || fabsf(gyro.y-gyroBias(2,1))>2.0f || fabsf(gyro.z-gyroBias(3,1))>2.0f){
-            preflightCheck = false;
-            //twelite.serial.printf("PreFlight Check : high gyro value\r\n");
-        }
-        Matrix accBias = eskf.getAccBias();
-        if(fabsf(acc.x-accBias(1,1))>2.0f || fabsf(acc.y-gyroBias(2,1))>2.0f){
-            preflightCheck = false;
-            //twelite.serial.printf("PreFlight Check : high acc value\r\n");
-        }
-        Matrix vihat = eskf.getVihat();
-        if(fabsf(vihat(1,1))>2.0f || fabsf(vihat(2,1))>2.0f||fabsf(vihat(3,1))>2.0f){
-            preflightCheck = false;
-            //twelite.serial.printf("PreFlight Check : high velocity value\r\n");
-        }
-        Matrix pihat = eskf.getPihat();
-        if(fabsf(pihat(1,1))>2.0f || fabsf(pihat(2,1))>2.0f||fabsf(pihat(3,1))>2.0f){
-            preflightCheck = false;
-            //twelite.serial.printf("PreFlight Check : not home position\r\n");
-        }
-        if(sbus.failSafe){
-            preflightCheck = false;
-            //twelite.serial.printf("PreFlight Check : no RC\r\n");
-        }
-        // sbusデータの読み込み
-        for (int i =0 ; i < 16;i ++){
-            rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
-        }
-        if (rc[4]>-0.3f && rc[6] < -0.3f){
-            preflightCheck = false;
-            //twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n");
-        }
-        if(!(updateValues.gps_fix == 0x02 || updateValues.gps_fix == 0x03)){
-            preflightCheck = false;
-            //twelite.serial.printf("PreFlight Check : no gps lock\r\n");
-        }
-        if(preflightCheck == true){
-            break;
-        }
-        break;  //pre-flight checkなし
-    }
-
     tail.Subscribe(tail_address[pos_tail], &(updateValues));
     
     LoopTicker PIDtick;
@@ -211,35 +119,61 @@
         eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
         eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
         
-        headingUpdateFlag = false;
-        if(tstart-theading>0.05f){
-            Matrix euler = eskf.computeAngles();
-            if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
+        //キャリブレーション中
+        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();
             }
-        }
-        //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
-        //pc.printf("%f %f %f %f %f %f\r\n", updateValues.pi[0], updateValues.pi[1], updateValues.pi[2], updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
-        
-        if((gpsitow != updateValues.gps_itow) && (updateValues.gps_fix == 0x03)){
-            gpsitow = updateValues.gps_itow;
-            Vector3 pi(updateValues.pi[0], updateValues.pi[1], updateValues.pi[2]);
-            Vector3 vi(updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
-            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
-            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
-            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
-        }else if(headingUpdateFlag == true){
-                float heading = atan2f(-mag.y,mag.x);
-                eskf.updateHeading(heading,Rheading);
+
+            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);
+            }
         }else{
-            Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
-            dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
-            //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2));
-            if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
-                eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
+            gpsUpdateFlag = false;
+            if((gpsitow != updateValues.gps_itow) && (updateValues.gps_fix == 0x03)){
+                gpsUpdateFlag = true;
+                gpsitow = updateValues.gps_itow;
+                tgps = _t.read();
+            }
+            
+            headingUpdateFlag = false;
+            if(tstart-theading>0.05f){
+                Matrix euler = eskf.computeAngles();
+                if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
+                    headingUpdateFlag = true;
+                    theading = _t.read();
+                }
+            }
+            
+            if(gpsUpdateFlag==true){
+                Vector3 pi(updateValues.pi[0], updateValues.pi[1], updateValues.pi[2]);
+                Vector3 vi(updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
+                eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
+            }else if(headingUpdateFlag == true){
+                    float heading = atan2f(-mag.y,mag.x);
+                    eskf.updateHeading(heading,Rheading);
             }else{
-                eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+                Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
+                dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
+                if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
+                    eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
+                }else{
+                    eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+                }
             }
         }