Diff: run.cpp

Revision:

68:b9f6938fab9d

Parent:

66:e5afad70fdd8

Child:

70:99f974d8960e

--- a/run.cpp	Tue Jun 22 02:45:43 2021 +0000
+++ b/run.cpp	Mon Jun 28 01:40:26 2021 +0000
@@ -2,32 +2,57 @@
 
 void run()
 {
+    pc.serial.printf("\r\nrun Mode\r\n");
+    wait(0.5);
+    Timer _t;
+    _t.start();
+    magCalibrator.setExtremes(magbiasMin,magbiasMax);
     getIMUval();
-    ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
+    //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
+    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.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 = -sumaccnorm / 1000.0f;
+    accref.z =  sumaccnorm / 1000.0f;
     float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z);
+    pc.serial.printf("%f \r\n",sigma_accnorm);
     pc.Subscribe(0000, &(posValues));
     
     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(); 
         
         float tend = _t.read();