Diff: run.cpp

Revision:

71:62eb45ecffe9

Parent:

70:9e7be21475f8

Child:

73:be7a8b8188de

--- a/run.cpp	Tue Jun 29 10:53:06 2021 +0000
+++ b/run.cpp	Fri Aug 20 07:48:41 2021 +0000
@@ -17,14 +17,8 @@
     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.y = float(read_calib.i[10])/200000.0f;
-    rpy_align.x = float(read_calib.i[11])/200000.0f;
-    accMin[0] = float(read_calib.i[12])/1000.0f;
-    accMin[1] = float(read_calib.i[13])/1000.0f;
-    accMin[2] = float(read_calib.i[14])/1000.0f;
-    accMax[0] = float(read_calib.i[15])/1000.0f;
-    accMax[1] = float(read_calib.i[16])/1000.0f;
-    accMax[2] = float(read_calib.i[17])/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];
     
@@ -43,24 +37,26 @@
         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.y*180.0f/M_PI,rpy_align.x*180.0f/M_PI);
-    getIMUval();
-    //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
-    ekf.defineQhat(rpy_align);
-    float sum2accnorm = 0;
-    float sumaccnorm = 0;
+    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);
     
+    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.updateQhat(gyro, att_dt);
-        ekf.updateErrState(gyro, att_dt);
         ekf.updateStaticAccMeasures(acc,accref);
-        ekf.fuseErr2Qhat();
-        ekf.updateMagMeasures(mag);
+        ekf.updateGyroBiasConstraints(gyro);
+        ekf.fuseErr2Nominal();
+        ekf.resetBias();
+        //ekf.updateMagMeasures(mag);
         ekf.computeAngles(rpy, rpy_align);
         sumaccnorm += acc.Norm();
         sum2accnorm += acc.Norm()*acc.Norm();
@@ -68,6 +64,7 @@
         att_dt = (tend-tstart);
     }
     accref.z =  sumaccnorm / 1000.0f;
+    
     for (int i = 0; i < 3; i++)
     {
         if (i == pos_tail)
@@ -90,17 +87,29 @@
         float tstart = _t.read();
         //姿勢角を更新
         getIMUval();
-        ekf.updateQhat(gyro, att_dt);
-        ekf.updateErrState(gyro, att_dt);
-        if(ekf.determinDynStatus(acc,accref)){
-            ekf.updateAccMeasures(acc,accref);
+        ekf.updateNominal(gyro,acc,accref,att_dt);
+        ekf.updateErrState(gyro,acc, att_dt);
+        if(obsCount == 50){
+            if(ekf.determinDynStatus(acc,accref)){
+                ekf.updateMeasures(gyro,acc,accref);
+            }else{
+                ekf.updateStaticMeasures(gyro,acc,accref);
+            }
+            obsCount = 0;
         }else{
-            ekf.updateStaticAccMeasures(acc,accref);
+            if(ekf.determinDynStatus(acc,accref)){
+                ekf.updateAccMeasures(acc,accref);
+            }else{   
+                ekf.updateStaticAccMeasures(acc,accref);
+            }
+            obsCount += 1;
         }
-        ekf.fuseErr2Qhat();
+        
+        ekf.updateGyroBiasConstraints(gyro);
+        ekf.fuseErr2Nominal();
         ekf.resetBias();
-        ekf.updateMagMeasures(mag);
         ekf.computeAngles(rpy, rpy_align);
+        ekf.computeVb(vb);
         PIDtick.loop(); 
         
         float tend = _t.read();