Diff: hil.cpp

Revision:

82:c183c29d2427

Parent:

77:2bf856e3eca4

Child:

83:e69ab831031c

--- a/hil.cpp	Tue Sep 07 08:22:39 2021 +0000
+++ b/hil.cpp	Thu Sep 16 09:30:21 2021 +0000
@@ -11,40 +11,34 @@
         }
     rpy_align.x = 0.0f;
     rpy_align.y = 0.0f;
-    //accref.z =  1.0f;
-    
-    ax = accelgyro.getAccelerationX() - agoffset[0];
-    ay = accelgyro.getAccelerationY() - agoffset[1];
-    az = accelgyro.getAccelerationZ() - agoffset[2];
-    gx = accelgyro.getRotationX() - agoffset[3];
-    gy = accelgyro.getRotationY() - agoffset[4];
-    gz = accelgyro.getRotationZ() - agoffset[5];
-    
-    float sigma_acc =  sqrt(0.000020f);
-    float sigma_gyro = sqrt(0.000005f);
-    float sigma_mag =  sqrt(0.00001f);
+    accref.z =  1.0f;
+    float sigma_acc =  0.0045;
+    float sigma_gyro = 0.0022;
+    float sigma_mag =  0.0032;
+    float sigma_vi =  0.32;
     // gx gy gz ax ay az
     // 加速度値を分解能で割って加速度(G)に変換する
-    acc.x = float(vp.accData[0]) / ACCEL_SSF + mapfloat(float(ax) / ACCEL_SSF,accMin[0],accMax[0],-1.0f,1.0f);  //FS_SEL_0 16,384 LSB / g
-    acc.y = float(vp.accData[1]) / ACCEL_SSF + mapfloat(float(ay) / ACCEL_SSF,accMin[1],accMax[1],-1.0f,1.0f);
-    acc.z = float(vp.accData[2]) / ACCEL_SSF + mapfloat(float(az) / ACCEL_SSF,accMin[2],accMax[2],-1.0f,1.0f)-accref.z;
+    acc.x = float(vp.accData[0]) / ACCEL_SSF + sigma_acc*randn();  //FS_SEL_0 16,384 LSB / g
+    acc.y = float(vp.accData[1]) / ACCEL_SSF + sigma_acc*randn();
+    acc.z = float(vp.accData[2]) / ACCEL_SSF + sigma_acc*randn();
     acc = accMedian.Process(acc);
     // 角速度値を分解能で割って角速度(rad per sec)に変換する
-    gyro.x = float(vp.gyroData[0]) / GYRO_SSF * 0.0174533f + float(gx) / GYRO_SSF * 0.0174533f;  // (rad/s)
-    gyro.y = float(vp.gyroData[1]) / GYRO_SSF * 0.0174533f + float(gy) / GYRO_SSF * 0.0174533f;
-    gyro.z = float(vp.gyroData[2]) / GYRO_SSF * 0.0174533f + float(gz) / GYRO_SSF * 0.0174533f;
+    gyro.x = float(vp.gyroData[0]) / GYRO_SSF * 0.0174533f + sigma_gyro*randn();  // (rad/s)
+    gyro.y = float(vp.gyroData[1]) / GYRO_SSF * 0.0174533f + sigma_gyro*randn();
+    gyro.z = float(vp.gyroData[2]) / GYRO_SSF * 0.0174533f + sigma_gyro*randn();
     gyro = gyroMedian.Process(gyro);
     mag.x = -float(vp.magData[1])/1000.0f + sigma_mag*randn();
     mag.y = -float(vp.magData[0])/1000.0f + sigma_mag*randn();
     mag.z =  float(vp.magData[2])/1000.0f + sigma_mag*randn();
     mag = magMedian.Process(mag);
+    vi.x =  float(vp.viData[0])/1000.0f + sigma_vi*randn();
+    vi.y =  float(vp.viData[1])/1000.0f + sigma_vi*randn();
+    vi.z =  float(vp.viData[2])/1000.0f + sigma_vi*randn();
+    
     if(acc.Norm()<0.01f || mag.Norm() < 0.001f){
         acc.x = 0.0f;
         acc.y = 0.0f;
         acc.z = 1.0f;
-        gyro.x = 0.0f;
-        gyro.y = 0.0f;
-        gyro.z = 0.0f;
         mag.x = 0.113657f;
         mag.y = -0.278425f;
         mag.z = 0.980554f;