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:
- 87:89bbbcdb667b
- Parent:
- 77:2bf856e3eca4
- Child:
- 88:be349faa1976
--- a/setup.cpp Mon Oct 18 12:18:15 2021 +0000 +++ b/setup.cpp Wed Oct 20 01:50:52 2021 +0000 @@ -26,279 +26,16 @@ servoLeft.pulsewidth_us(1500.0); servoThrust.pulsewidth_us(1100.0); - sd.baud(57600); + sd.baud(115200); sd.printf("\r\nFlight Start\r\n"); - accelgyro.initialize(); - //加速度計のフルスケールレンジを設定 - accelgyro.setFullScaleAccelRange(ACCEL_FSR); - //角速度計のフルスケールレンジを設定 - accelgyro.setFullScaleGyroRange(GYRO_FSR); - //MPU6050のLPFを設定 - accelgyro.setDLPFMode(MPU6050_LPF); - //MPU6050のレートを設定 - accelgyro.setRate(MPU6050_SAMPLERATE); - //地磁気 - mag_sensor.enable(); + twelite.baud(38400); + twelite.printf("\r\nTelemetory Start\r\n"); } void calibrate() { - pc.serial.printf("\r\nEnter to Calibration Mode\r\n"); - wait(5); - pc.serial.printf("Acc and Gyro Calibration Start\r\n"); - - int iter_n = 10000; - - long axs = 0; - long ays = 0; - long azs = 0; - double axs2 = 0.0f; - double ays2 = 0.0f; - double azs2 = 0.0f; - long gxs = 0; - long gys = 0; - long gzs = 0; - double gxs2 = 0.0f; - double gys2 = 0.0f; - double gzs2 = 0.0f; - for(int i = 0;i<iter_n ;i++) - { - accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); - axs += ax; - ays += ay; - azs += az; - axs2 += double(ax*ax)/iter_n; - ays2 += double(ay*ay)/iter_n; - azs2 += double(az*az)/iter_n; - - gxs += gx; - gys += gy; - gzs += gz; - gxs2 += double(gx*gx)/iter_n; - gys2 += double(gy*gy)/iter_n; - gzs2 += double(gz*gz)/iter_n; - //wait(0.01); - } - axs = axs /iter_n; - ays = ays /iter_n; - azs = azs /iter_n; - gxs = gxs /iter_n; - gys = gys /iter_n; - gzs = gzs /iter_n; - double var_accx = (axs2 - double(axs*axs))/ ACCEL_SSF / ACCEL_SSF; - double var_accy = (ays2 - double(ays*ays))/ ACCEL_SSF / ACCEL_SSF; - double var_accz = (azs2 - double(azs*azs))/ ACCEL_SSF / ACCEL_SSF; - double var_gyrox = (gxs2 - double(gxs*gxs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f; - double var_gyroy = (gys2 - double(gys*gys))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f; - double var_gyroz = (gzs2 - double(gzs*gzs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f; - pc.serial.printf("AccCovariance : %f, %f, %f \r\n",var_accx,var_accy,var_accz); - pc.serial.printf("GyroCovariance : %f, %f, %f \r\n",var_gyrox,var_gyroy,var_gyroz); - - 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; - accMax[i] = 1.0f; - } - - accMax[2] = accScaleCalibrate(5); - accMin[0] = accScaleCalibrate(1); - 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]); - - pc.serial.printf("Keep Level \r\n"); - wait(5); - - pc.serial.printf("Calculating pitch/roll Offset \r\n"); - //姿勢オフセットを計算 - rpy_align.y = 0.0f*M_PI/180.0f; - rpy_align.x = 0.0f*M_PI/180.0f; - float ave_pitch = 0.0f; - float ave_roll = 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()); - Timer _t; - _t.start(); - for (int i = 0 ; i < 2200; i++) - { - float tstart = _t.read(); - //姿勢角を更新 - getIMUval(); - ekf.updateNominal(gyro,acc,accref,att_dt); - ekf.updateErrState(gyro,acc, att_dt); - ekf.updateStaticAccMeasures(acc,accref); - ekf.fuseErr2Nominal(); - //ekf.updateMagMeasures(mag); - ekf.computeAngles(rpy, rpy_align); - if(i>199) - { - ave_pitch += rpy.x; - ave_roll += rpy.y; - } - wait(0.001); - float tend = _t.read(); - att_dt = (tend-tstart); - } - - pc.serial.printf("aliginment data(rpy.x, rpy.y, rpy.z) : %ff*M_PI/180.0f, %ff*M_PI/180.0f, 0.0f*M_PI/180.0f\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI); - - - pc.serial.printf("Calibration Complete\r\n"); - while(1) { wait(1000); } -} - -float accScaleCalibrate(int attNo) -{ - //attNo 1:Right down (acc.x Negative) - //attNo 2:Left down (acc.x Positive) - //attNo 3:Nose down (acc.y Negative) - //attNo 4:Tail down (acc.y Positive) - //attNo 5:Level (acc.z Positive) - //attNo 6:upside down (acc.z Negative) - //acc scale calibration - switch(attNo){ - case 1: - pc.serial.printf("Right down (acc.x Negative)\r\n"); - break; - case 2: - pc.serial.printf("Left down (acc.x Positive)\r\n"); - break; - case 3: - pc.serial.printf("Nose down (acc.y Negative)\r\n"); - break; - case 4: - pc.serial.printf("Tail down (acc.y Positive)\r\n"); - break; - case 5: - pc.serial.printf("Level (acc.z Positive)\r\n"); - break; - case 6: - pc.serial.printf("Upside down (acc.z Negative)\r\n"); - break; - default : - pc.serial.printf("error"); - break; - } - - while(1){ - double accx = 0.0; - double accy = 0.0; - double accz = 0.0; - for(int i = 0;i<100 ;i++) - { - accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); - accx += ax/ ACCEL_SSF; - accy += ay/ ACCEL_SSF; - accz += az/ ACCEL_SSF; - wait(0.01); - } - bool breakFlag = false; - switch(attNo){ - case 1: - if(abs(accx/100.0+1.0)<0.1){ - breakFlag = true; - }; - break; - case 2: - if(abs(accx/100.0-1.0)<0.1){ - breakFlag = true; - }; - break; - case 3: - if(abs(accy/100.0+1.0)<0.1){ - breakFlag = true; - }; - break; - case 4: - if(abs(accy/100.0-1.0)<0.1){ - breakFlag = true; - }; - break; - case 5: - if(abs(accz/100.0-1.0)<0.1){ - breakFlag = true; - }; - break; - case 6: - if(abs(accz/100.0+1.0)<0.1){ - breakFlag = true; - }; - break; - } - if(breakFlag){break;}; - pc.serial.printf("acc %f %f %f\r\n", accx/100.0,accy/100.0,accz/100.0); - } - pc.serial.printf("Keep it hold\r\n"); - int iter_n = 1000; - 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); - accx += ax/ ACCEL_SSF; - accy += ay/ ACCEL_SSF; - accz += az/ ACCEL_SSF; - - mag_sensor.getAxis(mdata); // flush the magnetmeter - 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; - switch(attNo){ - case 1: - returnval = accx/1000.0f; - break; - case 2: - returnval = accx/1000.0f; - break; - case 3: - returnval = accy/1000.0f; - break; - case 4: - returnval = accy/1000.0f; - break; - case 5: - returnval = accz/1000.0f; - break; - case 6: - returnval = accz/1000.0f; - break; - } - - return returnval; } \ No newline at end of file