Diff: run.cpp

Revision:

69:0caaad87cf1d

Parent:

66:a210d7130a44

Child:

70:9e7be21475f8

diff -r 11ffadc7adb1 -r 0caaad87cf1d run.cpp
--- a/run.cpp	Tue Jun 22 12:00:40 2021 +0000
+++ b/run.cpp	Mon Jun 28 03:30:54 2021 +0000
@@ -39,22 +39,30 @@
     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());
-    float val2_thmg = 0;
-    float val2_accnorm = 0;
-    float sumLPaccnorm = 0;
+    //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
+    ekf.defineQhat(rpy_align);
+    float sum2accnorm = 0;
+    float sumaccnorm = 0;
+    
+    Timer _t;
+    _t.start();
+    
     for(int i = 0; i < 1000; i++){
+        float tstart = _t.read();
         getIMUval();
-        val_thmg += acos((mag % acc)/mag.Norm()/acc.Norm());
-        val2_thmg += (acos((mag % acc)/mag.Norm()/acc.Norm()))*(acos((mag % acc)/mag.Norm()/acc.Norm()));
-        sumLPaccnorm += LPacc.Norm();
-        val2_accnorm += LPacc.Norm()*LPacc.Norm();
+        ekf.updateQhat(gyro, att_dt);
+        ekf.updateErrState(gyro, att_dt);
+        ekf.updateStaticAccMeasures(acc,accref);
+        ekf.fuseErr2Qhat();
+        //ekf.resetGyroBias(gyroBias);
+        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 = -sumLPaccnorm / 1000;
-    val_thmg /= 1000;
-    sigma_thmg = sqrt(val2_thmg/1000-val_thmg*val_thmg);
-    sigma_accnorm = sqrt(val2_accnorm/1000-accref.z*accref.z);
-    pc.printf("sigma: %f %f \r\n",sigma_thmg,sigma_accnorm);
+    accref.z =  sumaccnorm / 1000.0f;
     for (int i = 0; i < 3; i++)
     {
         if (i == pos_tail)
@@ -71,23 +79,24 @@
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
-    Timer _t;
-    _t.start();
     
     while(1)
     {
         float tstart = _t.read();
         //姿勢角を更新
         getIMUval();
-        ekf.updateBetweenMeasures(gyro, att_dt);
-        ekf.computeAngles(rpy, rpy_g, rpy_align);
+        ekf.updateQhat(gyro, att_dt);
+        ekf.updateErrState(gyro, att_dt);
+        if(ekf.determinDynStatus(acc,accref)){
+            ekf.updateAccMeasures(acc,accref);
+        }else{
+            ekf.updateStaticAccMeasures(acc,accref);
+        }
+        ekf.fuseErr2Qhat();
+        ekf.resetBias();
+        ekf.updateMagMeasures(mag);
+        ekf.computeAngles(rpy, rpy_align);
         PIDtick.loop(); 
-//        if (broadcast_time.ms != system_time.ms)
-//        {
-//            system_time = broadcast_time;
-//            system_dt.reset();
-//            system_dt.start();
-//        }
         
         float tend = _t.read();
         att_dt = (tend-tstart);