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:
- 2022-06-24
- Revision:
- 143:53808e4e684c
- Parent:
- 141:725321fe2949
File content as of revision 143:53808e4e684c:
#include "global.hpp" void run() { preflightCalibration(); setEskfCov(); MatrixXf Rgps(5,5); setDiag(Rgps,1.5f); Rgps(2,2) = 0.1f; Rgps(3,3) = 0.1f; Rgps(4,4) = 0.1f; MatrixXf Rwhole = MatrixXf::Zero(9,9); Rwhole(0,0) = 1.5f; Rwhole(1,1) = 1.5f; Rwhole(2,2) = 0.1f; Rwhole(3,3) = 0.1f; Rwhole(4,4) = 0.1f; Rwhole(5,5) = 5000.0f; Rwhole(6,6) = 5000.0f; Rwhole(7,7) = 5000.0f; Rwhole(8,8) = 0.007f; _t.start(); preflightCheck(); wait(1.0f); //usaPack通信開始 制御ループのアタッチ pc.Subscribe(0000, &(vp)); LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); float tgps = _t.read(); float theading = _t.read(); float tstart = _t.read(); while(1) { float tstart = _t.read(); //センサの値を取得 if(hilFlag == true){ getHilIMUval(); }else{ getIMUval(); } //ekfの更新 eskf.updateNominal(acc, gyro, att_dt); eskf.updateErrState(acc, gyro, att_dt); if(hilFlag == true){ if(tstart-tgps>0.2f){ getHilGPSval(); tgps = _t.read(); gpsUpdateFlag = true; }else{ gpsUpdateFlag = false; } }else{ if(userButton.read()==1) { gpsLlh0Fixed = false; }; getGPSval(); } if(gpsUpdateFlag == true){ float heading = std::atan2(-mag(1),mag(0)); Vector3f dynacc = eskf.calcDynAcc(acc); dynaccnorm2 = dynacc.squaredNorm(); if(dynaccnorm2 > 0.16f){ Rwhole(5,5) = 100.0f; Rwhole(6,6) = 100.0f; Rwhole(7,7) = 100.0f; }else{ Rwhole(5,5) = 5000.0f; Rwhole(6,6) = 5000.0f; Rwhole(7,7) = 5000.0f; } eskf.updateWhole(pi, palt, vi,acc,heading, Rwhole); } PIDtick.loop(); //制御時間を計測 float tend = _t.read(); att_dt = (tend-tstart); } }