solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: hil.cpp
- Revision:
- 93:b827f78a717a
- Parent:
- 92:00460f6df439
- Child:
- 94:579e875a4244
--- a/hil.cpp Thu Oct 28 09:44:47 2021 +0000 +++ b/hil.cpp Fri Oct 29 13:30:24 2021 +0000 @@ -12,19 +12,30 @@ rpy_align.x = 0.0f; rpy_align.y = 0.0f; accref.z = 1.0f; - float sigma_acc = sqrt(0.00020f); - float sigma_gyro = sqrt(0.000005f); - float sigma_mag = sqrt(0.002f); + + float sigma_mag = 0.1f; + + lsm.readAccel(); + lsm.readMag(); + lsm.readGyro(); + + float accmx = lsm.ax * 9.8f - agoffset[0]; + float accmy = lsm.ay * 9.8f - agoffset[1]; + float accmz = lsm.az * 9.8f - agoffset[2]; + float gyromx = (lsm.gx * M_PI / 180.0f) - agoffset[3]; + float gyromy = (lsm.gy * M_PI / 180.0f) - agoffset[4]; + float gyromz = (lsm.gz * M_PI / 180.0f) - agoffset[5]; + // gx gy gz ax ay az // 加速度値を分解能で割って加速度(m/s^2)に変換する - acc.x = 9.8f*float(vp.accData[0]) / 4096.0f + sigma_acc*randn(); //FS_SEL_0 16,384 LSB / g - acc.y = 9.8f*float(vp.accData[1]) / 4096.0f + sigma_acc*randn(); - acc.z = 9.8f*float(vp.accData[2]) / 4096.0f + sigma_acc*randn(); + acc.x = 9.8f*float(vp.accData[0]) / 4096.0f + accmx; //FS_SEL_0 16,384 LSB / g + acc.y = 9.8f*float(vp.accData[1]) / 4096.0f + accmy; + acc.z = 9.8f*float(vp.accData[2]) / 4096.0f + accmy; acc = accMedian.Process(acc); // 角速度値を分解能で割って角速度(rad per sec)に変換する - gyro.x = float(vp.gyroData[0]) / 131.0f * 0.0174533f + sigma_gyro*randn(); // (rad/s) - gyro.y = float(vp.gyroData[1]) / 131.0f * 0.0174533f + sigma_gyro*randn(); - gyro.z = float(vp.gyroData[2]) / 131.0f * 0.0174533f + sigma_gyro*randn(); + gyro.x = float(vp.gyroData[0]) / 131.0f * 0.0174533f + gyromx; // (rad/s) + gyro.y = float(vp.gyroData[1]) / 131.0f * 0.0174533f + gyromy; + gyro.z = float(vp.gyroData[2]) / 131.0f * 0.0174533f + gyromz; gyro = gyroMedian.Process(gyro); mag.x = float(vp.magData[0])/1000.0f + sigma_mag*randn(); @@ -47,8 +58,8 @@ void getHilGPSval() { - float sigma_vi = 2.0f/1.1774f; - float sigma_pi = 2.0f/1.1774f; + float sigma_vi = 0.1f; + float sigma_pi = 1.0f; //GPSの速度情報m/s vi.x = float(vp.viData[0])/1000.0f + sigma_vi*randn();