Diff: run.cpp

Revision:

84:028bd650e8bc

Parent:

79:aa2631950f46

Child:

85:0b14a2a600fc

diff -r 0a644de28415 -r 028bd650e8bc run.cpp
--- a/run.cpp	Mon Dec 06 11:16:56 2021 +0000
+++ b/run.cpp	Mon Dec 06 12:58:20 2021 +0000
@@ -2,6 +2,7 @@
 
 void run()
 {
+    /*
     pc.printf("reading calibration value\r\n");
     //キャリブレーション値を取得
     U read_calib;
@@ -21,6 +22,32 @@
     rpy_align.y = float(read_calib.i[11])/200000.0f;
     magCalibrator.setExtremes(magbiasMin,magbiasMax);
 //    tail_address[pos_tail] = (int)read_calib.i[10];
+    */
+    
+    //センサの初期化・ジャイロバイアス・加速度スケールの取得
+    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);
     
     switch(pos_tail){
     case 0:
@@ -39,15 +66,53 @@
     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);
     
+    //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;
+    
     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 sum2accnorm = 0;
     float sumaccnorm = 0;
     for(int i = 0; i < 1000; i++){
         float tstart = _t.read();
@@ -64,36 +129,114 @@
         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;
+        }
+    }
 
     tail.Subscribe(tail_address[pos_tail], &(updateValues));
     
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
+    float tgps = _t.read();
+    float theading = _t.read();
     
     while(1)
     {
         float tstart = _t.read();
-        //姿勢角を更新
+        //センサの値を取得
         getIMUval();
-        ekf.updateNominal(gyro,acc,accref,att_dt);
-        ekf.updateErrState(gyro,acc, att_dt);
-        if(ekf.determinDynStatus(acc,accref)){
-            ekf.updateAccMeasures(acc,accref);
-        }else{   
-            ekf.updateStaticAccMeasures(acc,accref);
-        }
-        if(itowVEL_log == updateValues.gps_itowVEL){
-            ekf.updateGPSVelocity(updateValues.vgps[0],updateValues.vgps[1],updateValues.vgps[2],acc,accref);
+        
+        //ekfの更新
+        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){
+                headingUpdateFlag = true;
+                theading = _t.read();
+            }
         }
         
-        ekf.updateGyroBiasConstraints(gyro);
-        ekf.fuseErr2Nominal();
-        ekf.resetBias();
-        ekf.computeAngles(rpy, rpy_align);
-        ekf.computeVb(vb);
+        if(updateValues.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);
+            //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);
     }