Diff: run.cpp

Revision:

76:7fd3ac1afe3e

Parent:

75:a505b9896da1

Child:

77:2bf856e3eca4

--- a/run.cpp	Tue Jul 20 11:57:05 2021 +0000
+++ b/run.cpp	Sat Aug 07 06:54:58 2021 +0000
@@ -22,7 +22,7 @@
         ekf.updateStaticAccMeasures(acc,accref);
         //ekf.fuseErr2Qhat();
         //ekf.resetBias();
-        ekf.updateMagMeasures(mag);
+        //ekf.updateMagMeasures(mag);
         ekf.computeAngles(rpy, rpy_align);
         sumaccnorm += acc.Norm();
         sum2accnorm += acc.Norm()*acc.Norm();
@@ -31,8 +31,52 @@
     }
     accref.z =  sumaccnorm / 1000.0f;
     float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z);
+    float Rscdyn = 263.980f;
+    float Qabdyn = 124.810f;
     pc.serial.printf("%f \r\n",sigma_accnorm);
     pc.Subscribe(0000, &(vp));
+    if(serialParamSource){
+        while(1){
+            pc.serial.attach(NULL, Serial::RxIrq);
+            pc.serial.printf("%d %d %d %d %d \r\n",checkParamSerial[0],checkParamSerial[1],checkParamSerial[2],checkParamSerial[3],checkParamSerial[4]);
+            pc.serial.attach(&pc, &UsaPack::Receive, Serial::RxIrq);
+            switch(vp.commandIndex){
+            case 1:
+                NVIC_SystemReset();
+                break;
+            case 10:
+                ekf.setQqerr(float(vp.commandVal));
+                checkParamSerial[0] = 1;
+                break;
+            case 11:
+                ekf.setQgbias(float(vp.commandVal));
+                checkParamSerial[1] = 1;
+                break;
+            case 12:
+                Qabdyn=float(vp.commandVal);
+                checkParamSerial[2] = 1;
+                break;
+            case 13:
+                Rscdyn=float(vp.commandVal);
+                checkParamSerial[3] = 1;
+                break;
+            case 14:
+                
+                checkParamSerial[4] = 1;
+                break;
+            default :
+                break;
+            }
+            int cpsSum = 0;
+            for(int i = 0;i<5;i++){
+                cpsSum +=  checkParamSerial[i];
+            }
+            if(cpsSum == 5){
+                break;
+            }
+            wait(0.01);
+        }   
+    }
     
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
@@ -51,16 +95,30 @@
 
         ekf.updateQhat(gyro, att_dt);
         ekf.updateErrState(gyro, att_dt);
-        //ekf.updateAllMeasures(acc,gyro,mag,accref,tstart,att_dt);
-        if(ekf.determinDynStatus(acc,accref)){
+        ekf.updateAccMeasures(acc,accref);
+        ekf.updateGyroBiasConstraints(gyro);
+        ekf.updateVelocityConstraints();
+        /*
+        if(obsCount == 450){
+            if(ekf.determinDynStatus(acc,accref)==false){
+                ekf.setRsoftconst(1.0f,1.0f);
+                ekf.setQab(0.0f);
+            }else{
+                ekf.setRsoftconst(263.0f,500.0f);
+                ekf.setQab(Qabdyn);
+            }
             ekf.updateAccMeasures(acc,accref);
+            obsCount = 0;
         }else{
-            ekf.updateStaticAccMeasures(acc,accref);
+            obsCount += 1;
+            if(ekf.determinDynStatus(acc,accref)==false){
+                ekf.updateStaticAccMeasures(acc,accref);
+            }
         }
-        //ekf.updateGyroBiasMeasures(gyro,mag,tstart,att_dt);
+        */
+        //ekf.updateMagMeasures(mag);
         ekf.fuseErr2Qhat();
-        ekf.resetBias();
-        //ekf.updateMagMeasures(mag);
+        //ekf.resetBias();
         ekf.computeAngles(rpy, rpy_align);
         PIDtick.loop();