File content as of revision 144:b3a713b4f1c4:

#include "global.hpp"

void preflightCalibration()
{
    wait(0.5f);
    //センサの初期化・ジャイロバイアス・加速度スケールの取得
    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_F / 180.0f);
        agoffset[4] +=(lsm.gy * M_PI_F / 180.0f); 
        agoffset[5] +=(lsm.gz * M_PI_F / 180.0f); 
        palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
        magref(0) += lsm.mx;
        magref(1) += lsm.my;
        magref(2) += lsm.mz;
    }
    for(int i = 0;i<6;i++){
        agoffset[i] /= float(n_init);
    }
    magref /= float(n_init);
    palt0 /= float(n_init);
    twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
    wait(1.0f);
}


void preflightCheck()
{
    //センサ正常性チェック
    if(hilFlag == false){
        while(1){
            bool preflightCheck = true;
            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.0f,1680.0f,-1.0f,1.0f) + (1.0f - 0.65f) * rc[i]; // mapped input
            }
            if (rc[7] > 0.3f){
                preflightCheck = false;
                motor1.pulsewidth_us(motorPwmMin);
                motor2.pulsewidth_us(motorPwmMin);
                motor3.pulsewidth_us(motorPwmMin);
                motor4.pulsewidth_us(motorPwmMin);
                motor5.pulsewidth_us(motorPwmMin);
                motor6.pulsewidth_us(motorPwmMin);
                twelite.serial.printf("PreFlight Check : switch arming position\r\n");
                pc.serial.printf("PreFlight Check : switch arming position\r\n");
            }
            
            
            if(preflightCheck == true){
                break;
            }
            pc.serial.printf("PreFlight Check : failed\r\n");
        }
    }
    twelite.serial.printf("PreFlight Check Completed\r\n");
}

void setEskfCov(){
    
    //ESKFの共分散設定
    eskf.setGravity(0.0f,0.0f,9.8f);
    eskf.setPhatPosition(2.0f,2.0f);
    eskf.setPhatVelocity(1.01f,1.01f);
    eskf.setPhatAngleError(0.1f);
    eskf.setPhatAccBias(0.0001f);
    eskf.setPhatGyroBias(0.0001f);
    eskf.setPhatGravity(0.0000001f);
    
    eskf.setQVelocity(0.1f,0.2f);
    eskf.setQAngleError(0.0000025f);
    eskf.setQAccBias(0.0000001f);
    eskf.setQGyroBias(0.0000001f);
    }