File content as of revision 88:0fc5df2fddcb:

#include "global.hpp"

void run()
{
    
    pc.printf("reading calibration value\r\n");
    //キャリブレーション値を取得
    U read_calib;
    readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4);
    wait(3);
    pos_tail = (int)read_calib.i[0];
    roll_offset = (float)read_calib.i[10] / 200000.0f;
    pitch_offset = (float)read_calib.i[11] / 200000.0f;
    /*
    agoffset[3] = float(read_calib.i[7]);
    agoffset[4] = float(read_calib.i[8]);
    agoffset[5] = float(read_calib.i[9]);
    magbiasMin[0] = float(read_calib.i[1])/1000.0f;
    magbiasMin[1] = float(read_calib.i[2])/1000.0f;
    magbiasMin[2] = float(read_calib.i[3])/1000.0f;
    magbiasMax[0] = float(read_calib.i[4])/1000.0f;
    magbiasMax[1] = float(read_calib.i[5])/1000.0f;
    magbiasMax[2] = float(read_calib.i[6])/1000.0f;
    rpy_align.x = float(read_calib.i[10])/200000.0f;
    rpy_align.y = float(read_calib.i[11])/200000.0f;
    magCalibrator.setExtremes(magbiasMin,magbiasMax);
//    tail_address[pos_tail] = (int)read_calib.i[10];
    */
    /*
    //センサの初期化・ジャイロバイアス・加速度スケールの取得
    int n_init = 1000;
    for(int i = 0;i<n_init;i++){
        lsm.readAccel();
        lsm.readMag();
        lsm.readGyro();
        agoffset[0] += lsm.ax * 9.8f;
        agoffset[1] += lsm.ay * 9.8f;
        agoffset[2] += lsm.az * 9.8f-9.8f;
        agoffset[3] +=(lsm.gx * M_PI / 180.0f);
        agoffset[4] +=(lsm.gy * M_PI / 180.0f); 
        agoffset[5] +=(lsm.gz * M_PI / 180.0f); 
        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
        magref.x += lsm.mx;
        magref.y += lsm.my;
        magref.z += lsm.mz;
    }
    for(int i = 0;i<6;i++){
        agoffset[i] /= float(n_init);
    }
    magref.x /= float(n_init);
    magref.y /= float(n_init);
    magref.z /= float(n_init);
    palt0 /= float(n_init);
    */
    
    switch(pos_tail){
    case 0:
        pc.printf("This MBED is Located at Left \r\n");
        break;
    case 1:
        pc.printf("This MBED is Located at Center \r\n");
        break;
    case 2:
        pc.printf("This MBED is Located at Right \r\n");
        break;
    default:   // error situation
        pc.printf("error\r\n");
        break;
    }
    pc.printf("tail_address : %d\r\n", tail_address[pos_tail]);
    pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",rpy_align.x*180.0f/M_PI,rpy_align.y*180.0f/M_PI);
    
    //ESKFの共分散設定
    eskf.setGravity(0.0f,0.0f,9.8f);
    eskf.setPhatPosition(0.1f);
    eskf.setPhatVelocity(0.1f);
    eskf.setPhatAngleError(0.5f);
    eskf.setPhatAccBias(0.001f);
    eskf.setPhatGyroBias(0.001f);
    eskf.setPhatGravity(0.0000001f);
    
    eskf.setQVelocity(0.001f);
    eskf.setQAngleError(0.0000025f);
    eskf.setQAccBias(0.000001f);
    eskf.setQGyroBias(0.000001f);
    
    Matrix Rgpspos(3,3);
    setDiag(Rgpspos,1.0f);
    
    Matrix Rgpsvel(3,3);
    Rgpsvel(1,1) = 0.01f;
    Rgpsvel(2,2) = 0.01f;
    Rgpsvel(3,3) = 0.1f;
    
    Matrix Rgps(5,5);
    setDiag(Rgps,0.05f);
    Rgps(4,4) = 0.1f;
    Rgps(5,5) = 0.1f;
    
    float dynAccCriteria = 0.4f;
    Matrix Racc(3,3);
    setDiag(Racc,100.0f);
    Matrix RaccDyn(3,3);
    setDiag(RaccDyn,5000.0f);

    Matrix Rheading(1,1);
    Rheading(1,1) = 0.01f;
    
    wait(0.5);
    Timer _t;
    _t.start();
    
    /*
    magCalibrator.setExtremes(magbiasMin,magbiasMax);
    for(int i = 0; i < 1000; i++){
        getIMUval();
    }
    ekf.defineQhat(rpy_align);
    float sum2accnorm = 0;
    float sumaccnorm = 0;
    for(int i = 0; i < 1000; i++){
        float tstart = _t.read();
        getIMUval();
        ekf.updateStaticAccMeasures(acc,accref);
        ekf.updateGyroBiasConstraints(gyro);
        ekf.fuseErr2Nominal();
        ekf.resetBias();
        //ekf.updateMagMeasures(mag);
        ekf.computeAngles(rpy, rpy_align);
        sumaccnorm += acc.Norm();
        sum2accnorm += acc.Norm()*acc.Norm();
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
    accref.z =  sumaccnorm / 1000.0f;
    */
    
    while(1)
    {
        float tstart = _t.read();
        getIMUval();
        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        Vector3 pi(updateValues.pi[0], updateValues.pi[1], updateValues.pi[2]);
        Vector3 vi(updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
        eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
        float heading = atan2f(-mag.y,mag.x);
        eskf.updateHeading(heading,Rheading);
        Matrix Raccpreflight(3,3);
        setDiag(Raccpreflight,5.0f);
        eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight);
        
        bool preflightCheck = true;
        Matrix gyroBias = eskf.getGyroBias();
        if(fabsf(gyro.x-gyroBias(1,1))>2.0f || fabsf(gyro.y-gyroBias(2,1))>2.0f || fabsf(gyro.z-gyroBias(3,1))>2.0f){
            preflightCheck = false;
            //twelite.serial.printf("PreFlight Check : high gyro value\r\n");
        }
        Matrix accBias = eskf.getAccBias();
        if(fabsf(acc.x-accBias(1,1))>2.0f || fabsf(acc.y-gyroBias(2,1))>2.0f){
            preflightCheck = false;
            //twelite.serial.printf("PreFlight Check : high acc value\r\n");
        }
        Matrix vihat = eskf.getVihat();
        if(fabsf(vihat(1,1))>2.0f || fabsf(vihat(2,1))>2.0f||fabsf(vihat(3,1))>2.0f){
            preflightCheck = false;
            //twelite.serial.printf("PreFlight Check : high velocity value\r\n");
        }
        Matrix pihat = eskf.getPihat();
        if(fabsf(pihat(1,1))>2.0f || fabsf(pihat(2,1))>2.0f||fabsf(pihat(3,1))>2.0f){
            preflightCheck = false;
            //twelite.serial.printf("PreFlight Check : not home position\r\n");
        }
        if(sbus.failSafe){
            preflightCheck = false;
            //twelite.serial.printf("PreFlight Check : no RC\r\n");
        }
        // sbusデータの読み込み
        for (int i =0 ; i < 16;i ++){
            rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
        }
        if (rc[4]>-0.3f && rc[6] < -0.3f){
            preflightCheck = false;
            //twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n");
        }
        if(!(updateValues.gps_fix == 0x02 || updateValues.gps_fix == 0x03)){
            preflightCheck = false;
            //twelite.serial.printf("PreFlight Check : no gps lock\r\n");
        }
        if(preflightCheck == true){
            break;
        }
        break;  //pre-flight checkなし
    }

    tail.Subscribe(tail_address[pos_tail], &(updateValues));
    
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    float tgps = _t.read();
    float theading = _t.read();
    
    while(1)
    {
        float tstart = _t.read();
        //センサの値を取得
        getIMUval();
        
        //ekfの更新
        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        
        headingUpdateFlag = false;
        if(tstart-theading>0.05f){
            Matrix euler = eskf.computeAngles();
            if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
                headingUpdateFlag = true;
                theading = _t.read();
            }
        }
        //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
        pc.printf("%f %f %f %f %f %f\n", updateValues.pi[0], updateValues.pi[1], updateValues.pi[2], updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
        
        if((gpsitow != updateValues.gps_itow) && (updateValues.gps_fix == 0x03)){
            gpsitow = updateValues.gps_itow;
            Vector3 pi(updateValues.pi[0], updateValues.pi[1], updateValues.pi[2]);
            Vector3 vi(updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
        }else if(headingUpdateFlag == true){
                float heading = atan2f(-mag.y,mag.x);
                eskf.updateHeading(heading,Rheading);
        }else{
            Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
            dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
            //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2));
            if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
                eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
            }else{
                eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
            }
        }
        
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}