HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Eigen Revision

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

run.cpp

Committer:
NaotoMorita
Date:
2021-10-26
Revision:
90:96c2b0ed4b96
Parent:
89:c9f64bd655d9
Child:
92:00460f6df439

File content as of revision 90:96c2b0ed4b96:

#include "global.hpp"

void run()
{
    wait(0.5);
    Timer _t;
    _t.start();

    //センサの初期化・ジャイロバイアス・加速度スケールの取得
    int n_init = 1;
    if(hilFlag == false){
        eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z);
    }
    eskf.setGravity(0.0f,0.0f,-9.8f);
    
    //センサ正常性チェック
    //usaPack通信開始
    pc.Subscribe(0000, &(vp));
    
    //制御ループのアタッチ
    LoopTicker PIDtick;
    PIDtick.attach(calcServoOut,PID_dt);
    
    while(1)
    {
        float tstart = _t.read();
        //センサの値を取得
        if(hilFlag == true){
            getHILval();
        }else{
            //getIMUval();
        }
        //ekfの更新
        eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
        eskf.fuseErr2Nominal();
        
        PIDtick.loop(); 
        
        //制御時間を計測
        float tend = _t.read();
        att_dt = (tend-tstart);
    }
}