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