File content as of revision 77:2bf856e3eca4:

#include "global.hpp"

void setup()
{
    pitchPID.setSetPoint(0.0);
    pitchratePID.setSetPoint(0.0); 
    rollPID.setSetPoint(0.0); 
    rollratePID.setSetPoint(0.0); 
    pitchPID.setBias(0.0);
    pitchratePID.setBias(0.0); 
    rollPID.setBias(0.0); 
    rollratePID.setBias(0.0); 
    pitchPID.setOutputLimits(-1.0,1.0);
    pitchratePID.setOutputLimits(-1.0,1.0);
    rollPID.setOutputLimits(-1.0,1.0); 
    rollratePID.setOutputLimits(-1.0,1.0);
    pitchPID.setInputLimits(-M_PI,M_PI);
    pitchratePID.setInputLimits(-M_PI,M_PI);
    rollPID.setInputLimits(-M_PI,M_PI); 
    rollratePID.setInputLimits(-M_PI,M_PI);
    
    servoRight.period_us(15000.0);
    servoLeft.period_us(15000.0);
    servoThrust.period_us(15000.0);
    servoRight.pulsewidth_us(1500.0);
    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);
    //角速度計のフルスケールレンジを設定
    accelgyro.setFullScaleGyroRange(GYRO_FSR);
    //MPU6050のLPFを設定
    accelgyro.setDLPFMode(MPU6050_LPF);
    //MPU6050のレートを設定
    accelgyro.setRate(MPU6050_SAMPLERATE);
    //地磁気
    mag_sensor.enable();
}

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;
}