File content as of revision 65:ea184054e659:

#include "global.hpp"

void setup()
{
    pitchPID.setSetPoint(0.0);
    pitchratePID.setSetPoint(0.0); 
    pitchPID.setBias(0.0);
    pitchratePID.setBias(0.0);  
    pitchPID.setOutputLimits(-1.0,1.0);
    pitchratePID.setOutputLimits(-1.0,1.0);
    pitchPID.setInputLimits(-M_PI, M_PI);
    pitchratePID.setInputLimits(-M_PI, M_PI);
    
    elevServo.period_us(15000.0);
    elevServo.pulsewidth_us(1500.0);
    rudServo.period_us(15000.0);
    rudServo.pulsewidth_us(1500.0);
    
    accelgyro.initialize();
    //加速度計のフルスケールレンジを設定
    accelgyro.setFullScaleAccelRange(ACCEL_FSR);
    //角速度計のフルスケールレンジを設定
    accelgyro.setFullScaleGyroRange(GYRO_FSR);
    //MPU6050のLPFを設定
    accelgyro.setDLPFMode(MPU6050_LPF);
    //地磁気
    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 gxs = 0;
    int gys = 0;
    int gzs = 0;
    int iter_n = 10000;
    
    for(int i = 0;i<iter_n ;i++)
    {
        accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
        gxs += gx;
        gys += gy;
        gzs -= gz;
        //wait(0.01);
    }
    gxs = gxs /iter_n; 
    gys = gys /iter_n; 
    gzs = gzs /iter_n; 
    agoffset[3] = gxs;
    agoffset[4] = gys;
    agoffset[5] = gzs;
    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("Mag Calibration Start\r\n");
    
    float inputMag[3];
    float outputMag[3];
    while(1)
    {
        mag_sensor.getAxis(mdata); // flush the magnetmeter
        inputMag[0] = mdata.x;
        inputMag[1] = mdata.y;
        inputMag[2] = mdata.z;
        magCalibrator.run(inputMag,outputMag);
        if(userButton.read() == 1)
        {
            break;
        }
        wait(0.001);
    }
    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.getExtremes(magbiasMin,magbiasMax);
    
    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;
    Timer _t;
    _t.start();
    for (int i = 0 ; i < 2200; i++)
    {
        float tstart = _t.read();
        //姿勢角を更新
        getIMUval();
        //ekf.updateBetweenMeasures(gyro, att_dt);
        //ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
        //ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
        //ekf.computeAngles(rpy, rpy_g, rpy_align);
        if(i>199)
        {
            ave_pitch += rpy.y;
            ave_roll += rpy.x;
        }
        wait(0.001);
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
    
    pc.serial.printf("aliginment data : %f(pitch deg) %f(roll deg)\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI);
    
    while(1)
    {
        wait(1000);
    }
}