Diff: run.cpp

Revision:

139:b378528c05f2

Parent:

135:49f8916588da

Child:

141:725321fe2949

--- a/run.cpp	Wed Dec 01 19:17:36 2021 +0000
+++ b/run.cpp	Mon Dec 06 08:26:16 2021 +0000
@@ -2,198 +2,198 @@
 
 void run()
 {
-    wait(0.5);
-
-    //センサの初期化・ジャイロバイアス・加速度スケールの取得
-    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);
-    }
-    magref.x /= float(n_init);
-    magref.y /= float(n_init);
-    magref.z /= float(n_init);
-    palt0 /= float(n_init);
-    twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
-    
-    //ESKFの共分散設定
-    eskf.setGravity(0.0f,0.0f,9.8f);
-    eskf.setPhatPosition(0.1f);
-    eskf.setPhatVelocity(0.1f);
-    eskf.setPhatAngleError(0.5f);
-    eskf.setPhatAccBias(0.001f);
-    eskf.setPhatGyroBias(0.001f);
-    eskf.setPhatGravity(0.0000001f);
-    
-    eskf.setQVelocity(0.001f);
-    eskf.setQAngleError(0.0000025f);
-    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;
-    Rgps(5,5) = 0.1f;
-    
-    float dynAccCriteria = 0.4f;
-    Matrix Racc(3,3);
-    setDiag(Racc,100.0f);
-    Matrix RaccDyn(3,3);
-    setDiag(RaccDyn,5000.0f);
-
-    Matrix Rheading(1,1);
-    Rheading(1,1) = 0.01f;
-    
-    _t.start();
-    //センサ正常性チェック
-    if(hilFlag == false){
-        while(1){
-            float tstart = _t.read();
-            getIMUval();
-            getGPSval();
-            eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
-            eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
-            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(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){
-                preflightCheck = false;
-                twelite.serial.printf("PreFlight Check : no gps lock\r\n");
-            }
-            
-            if(preflightCheck == true){
-                break;
-            }
-        }
-    }
-    twelite.serial.printf("PreFlight Check Completed\r\n");
-    //usaPack通信開始
-    pc.Subscribe(0000, &(vp));
-
-    //制御ループのアタッチ
-    LoopTicker PIDtick;
-    PIDtick.attach(calcServoOut,PID_dt);
-    
-    float tgps = _t.read();
-    float theading = _t.read();
-    while(1)
-    {
-        float tstart = _t.read();
-        //センサの値を取得
-        if(hilFlag == true){
-            getHilIMUval();
-        }else{
-            getIMUval();
-        }
-        //ekfの更新
-        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
-        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
-
-        if(hilFlag == true){
-            if(tstart-tgps>0.2f){
-                getHilGPSval();
-                tgps = _t.read();
-                gpsUpdateFlag = true;
-            }else{
-                gpsUpdateFlag = false;
-            }
-        }else{
-            if(userButton.read()==1)
-            {
-                gpsLlh0Fixed = false;
-            };  
-            getGPSval();
-        }
-        
-        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){
-            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);
-        }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);
-            }else{
-                eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
-            }
-        }
-        PIDtick.loop(); 
-        
-        //制御時間を計測
-        float tend = _t.read();
-        att_dt = (tend-tstart);
-    }
+//    wait(0.5);
+//
+//    //センサの初期化・ジャイロバイアス・加速度スケールの取得
+//    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);
+//    }
+//    magref.x /= float(n_init);
+//    magref.y /= float(n_init);
+//    magref.z /= float(n_init);
+//    palt0 /= float(n_init);
+//    twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
+//    
+//    //ESKFの共分散設定
+//    eskf.setGravity(0.0f,0.0f,9.8f);
+//    eskf.setPhatPosition(0.1f);
+//    eskf.setPhatVelocity(0.1f);
+//    eskf.setPhatAngleError(0.5f);
+//    eskf.setPhatAccBias(0.001f);
+//    eskf.setPhatGyroBias(0.001f);
+//    eskf.setPhatGravity(0.0000001f);
+//    
+//    eskf.setQVelocity(0.001f);
+//    eskf.setQAngleError(0.0000025f);
+//    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;
+//    Rgps(5,5) = 0.1f;
+//    
+//    float dynAccCriteria = 0.4f;
+//    Matrix Racc(3,3);
+//    setDiag(Racc,100.0f);
+//    Matrix RaccDyn(3,3);
+//    setDiag(RaccDyn,5000.0f);
+//
+//    Matrix Rheading(1,1);
+//    Rheading(1,1) = 0.01f;
+//    
+//    _t.start();
+//    //センサ正常性チェック
+//    if(hilFlag == false){
+//        while(1){
+//            float tstart = _t.read();
+//            getIMUval();
+//            getGPSval();
+//            eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+//            eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+//            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(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){
+//                preflightCheck = false;
+//                twelite.serial.printf("PreFlight Check : no gps lock\r\n");
+//            }
+//            
+//            if(preflightCheck == true){
+//                break;
+//            }
+//        }
+//    }
+//    twelite.serial.printf("PreFlight Check Completed\r\n");
+//    //usaPack通信開始
+//    pc.Subscribe(0000, &(vp));
+//
+//    //制御ループのアタッチ
+//    LoopTicker PIDtick;
+//    PIDtick.attach(calcServoOut,PID_dt);
+//    
+//    float tgps = _t.read();
+//    float theading = _t.read();
+//    while(1)
+//    {
+//        float tstart = _t.read();
+//        //センサの値を取得
+//        if(hilFlag == true){
+//            getHilIMUval();
+//        }else{
+//            getIMUval();
+//        }
+//        //ekfの更新
+//        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+//        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+//
+//        if(hilFlag == true){
+//            if(tstart-tgps>0.2f){
+//                getHilGPSval();
+//                tgps = _t.read();
+//                gpsUpdateFlag = true;
+//            }else{
+//                gpsUpdateFlag = false;
+//            }
+//        }else{
+//            if(userButton.read()==1)
+//            {
+//                gpsLlh0Fixed = false;
+//            };  
+//            getGPSval();
+//        }
+//        
+//        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){
+//            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);
+//        }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);
+//            }else{
+//                eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+//            }
+//        }
+//        PIDtick.loop(); 
+//        
+//        //制御時間を計測
+//        float tend = _t.read();
+//        att_dt = (tend-tstart);
+//    }
 }
\ No newline at end of file