Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: setup.cpp
- Revision:
- 76:7fd3ac1afe3e
- Parent:
- 73:84ffa0166e6c
- Child:
- 77:2bf856e3eca4
--- a/setup.cpp Tue Jul 20 11:57:05 2021 +0000 +++ b/setup.cpp Sat Aug 07 06:54:58 2021 +0000 @@ -26,6 +26,8 @@ servoLeft.pulsewidth_us(1500.0); servoThrust.pulsewidth_us(1100.0); + sd.baud(57600); + sd.printf("\r\nFlight Start\r\n"); accelgyro.initialize(); //加速度計のフルスケールレンジを設定 accelgyro.setFullScaleAccelRange(ACCEL_FSR); @@ -94,10 +96,11 @@ pc.serial.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs); - + /* pc.serial.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); pc.serial.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); - + */ + pc.serial.printf("Initial Magbias : %f, %f, %f, %f \r\n", magbias[0], magbias[1], magbias[2], magbias[3]); pc.serial.printf("Acc Scale and Mag Calibration Start\r\n"); for(int i = 0;i<3;i++){ accMin[i] = -1.0f; @@ -106,16 +109,18 @@ accMax[2] = accScaleCalibrate(5); accMin[0] = accScaleCalibrate(1); - accMax[0] =accScaleCalibrate(2); - accMin[1] =accScaleCalibrate(3); - accMax[1] =accScaleCalibrate(4); - accMin[2] =accScaleCalibrate(6); + accMax[0] = accScaleCalibrate(2); + accMin[1] = accScaleCalibrate(3); + accMax[1] = accScaleCalibrate(4); + accMin[2] = accScaleCalibrate(6); + /* magCalibrator.getExtremes(magbiasMin,magbiasMax); pc.serial.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); pc.serial.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); magCalibrator.setExtremes(magbiasMin,magbiasMax); - + */ + pc.serial.printf("Magbias : %f, %f, %f, %f \r\n", magbias[0], magbias[1], magbias[2], magbias[3]); pc.serial.printf("accMin : %f, %f, %f\r\n", accMin[0], accMin[1], accMin[2]); pc.serial.printf("accMax : %f, %f, %f\r\n", accMax[0], accMax[1], accMax[2]); @@ -128,14 +133,10 @@ rpy_align.x = 0.0f*M_PI/180.0f; float ave_pitch = 0.0f; float ave_roll = 0.0f; - ekf.Q(4,4) = 0.00001f; - ekf.Q(5,5) = 0.00001f; - ekf.Q(6,6) = 0.00001f; - ekf.Qab(1,1) = 0.0f; - ekf.Qab(2,2) = 0.0f; - ekf.Qab(3,3) = 0.0f; + ekf.setQgbias(0.0f); + ekf.setQab(0.0f); 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()); Timer _t; _t.start(); for (int i = 0 ; i < 2200; i++) @@ -256,6 +257,7 @@ double accx = 0.0; double accy = 0.0; double accz = 0.0; + float magval[3] = {0,0,0}; for(int i = 0;i<iter_n ;i++) { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); @@ -263,14 +265,17 @@ accy += ay/ ACCEL_SSF; accz += az/ ACCEL_SSF; - float inputMag[3]; - float outputMag[3]; mag_sensor.getAxis(mdata); // flush the magnetmeter - inputMag[0] = mdata.x; - inputMag[1] = mdata.y; - inputMag[2] = mdata.z; - magCalibrator.run(inputMag,outputMag); - + magval[0] = (mdata.x - magbias[0]); + magval[1] = (mdata.y - magbias[1]); + magval[2] = (mdata.z - magbias[2]); + float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2]; + float lr = 0.00001f; + float f = mag_r - magbias[3]*magbias[3]; + magbias[0] = magbias[0] + 4 * lr * f * magval[0]; + magbias[1] = magbias[1] + 4 * lr * f * magval[1]; + magbias[2] = magbias[2] + 4 * lr * f * magval[2]; + magbias[3] = magbias[3] + 4 * lr * f * magbias[3]; wait(0.001); } float returnval = 0.0f;