Diff: run.cpp

Revision:

66:e5afad70fdd8

Parent:

61:c05353850017

Child:

68:b9f6938fab9d

--- a/run.cpp	Tue Jun 22 02:08:31 2021 +0000
+++ b/run.cpp	Tue Jun 22 02:19:14 2021 +0000
@@ -2,70 +2,18 @@
 
 void run()
 {
-    pc.printf("reading calibration value\r\n");
-    //キャリブレーション値を取得
-    U read_calib;
-    readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4);
-    wait(3);
-    pos_tail = (int)read_calib.i[0];
-    agoffset[3] = float(read_calib.i[7]);
-    agoffset[4] = float(read_calib.i[8]);
-    agoffset[5] = float(read_calib.i[9]);
-    magbiasMin[0] = float(read_calib.i[1])/1000.0f;
-    magbiasMin[1] = float(read_calib.i[2])/1000.0f;
-    magbiasMin[2] = float(read_calib.i[3])/1000.0f;
-    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;
-    magCalibrator.setExtremes(magbiasMin,magbiasMax);
-//    tail_address[pos_tail] = (int)read_calib.i[10];
-    
-    switch(pos_tail){
-    case 0:
-        pc.printf("This MBED is Located at Left \r\n");
-        break;
-    case 1:
-        pc.printf("This MBED is Located at Center \r\n");
-        break;
-    case 2:
-        pc.printf("This MBED is Located at Right \r\n");
-        break;
-    default:   // error situation
-        pc.printf("error\r\n");
-        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());
-    float val2_thmg = 0;
-    float val2_accnorm = 0;
-    float sumLPaccnorm = 0;
+    float sum2accnorm = 0;
+    float sumaccnorm = 0;
     for(int i = 0; i < 1000; i++){
         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();
+        sumaccnorm += acc.Norm();
+        sum2accnorm += acc.Norm()*acc.Norm();
     }
-    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);
-    for (int i = 0; i < 3; i++)
-    {
-        if (i == pos_tail)
-        {
-            break;
-        }
-        else
-        {
-            tail.Subscribe(tail_address[i], &(posValues[i]));
-        }
-    }
+    accref.z = -sumaccnorm / 1000.0f;
+    float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z);
+    pc.Subscribe(0000, &(posValues));
     
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
@@ -78,8 +26,8 @@
         float tstart = _t.read();
         //姿勢角を更新
         getIMUval();
-        ekf.updateBetweenMeasures(gyro, att_dt);
-        ekf.computeAngles(rpy, rpy_g, rpy_align);
+        //ekf.updateBetweenMeasures(gyro, att_dt);
+        //ekf.computeAngles(rpy, rpy_g, rpy_align);
         PIDtick.loop(); 
         
         float tend = _t.read();