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:
- osaka
- Date:
- 2021-10-20
- Revision:
- 87:89bbbcdb667b
- Parent:
- 86:456f00d52974
- Child:
- 88:be349faa1976
File content as of revision 87:89bbbcdb667b:
#include "global.hpp" void run() { wait(0.5); Timer _t; _t.start(); //センサの初期化・ジャイロバイアス・加速度スケールの取得 float sumaccnorm = 0; int n_init = 1000; if(hilFlag == false){ ekf.defineQhat(rpy_align); for(int i = 0; i < n_init; i++){ sumaccnorm += acc.Norm(); } accref.z = sumaccnorm / float(n_init); } //センサ正常性チェック //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の更新 ekf.updateNominal(gyro,acc,accref,att_dt); ekf.updateErrState(gyro,acc, att_dt); if(obsCount == 50){ //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref); //ekf.updateSinkRate(vi.z,acc,accref); obsCount = 0; }else{ //ekf.updateGPSVelocity(vi.x,vi.y,vi.z,acc,accref); //ekf.updateAccMeasures(acc,accref); if(ekf.determinDynStatus(acc,accref)){ ekf.updateAccMeasures(acc,accref); }else{ ekf.updateStaticAccMeasures(acc,accref); } obsCount += 1; } //ekf.updateSinkRate(vi.z,acc,accref); //ekf.updateGyroBiasConstraints(gyro); ekf.fuseErr2Nominal(); ekf.resetBias(); ekf.computeAngles(rpy, rpy_align); PIDtick.loop(); //制御時間を計測 float tend = _t.read(); att_dt = (tend-tstart); } }