solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
run.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-08-07
- Revision:
- 76:7fd3ac1afe3e
- Parent:
- 75:a505b9896da1
- Child:
- 77:2bf856e3eca4
File content as of revision 76:7fd3ac1afe3e:
#include "global.hpp" void run() { pc.serial.printf("\r\nrun Mode\r\n"); wait(0.5); Timer _t; _t.start(); magCalibrator.setExtremes(magbiasMin,magbiasMax); ekf.defineQhat(rpy_align); float sum2accnorm = 0; float sumaccnorm = 0; for(int i = 0; i < 1000; i++){ float tstart = _t.read(); if(hilFlag == true){ getHILval(); }else{ getIMUval(); } //ekf.updateQhat(gyro, att_dt); //ekf.updateErrState(gyro, att_dt); ekf.updateStaticAccMeasures(acc,accref); //ekf.fuseErr2Qhat(); //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; float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z); float Rscdyn = 263.980f; float Qabdyn = 124.810f; pc.serial.printf("%f \r\n",sigma_accnorm); pc.Subscribe(0000, &(vp)); if(serialParamSource){ while(1){ pc.serial.attach(NULL, Serial::RxIrq); pc.serial.printf("%d %d %d %d %d \r\n",checkParamSerial[0],checkParamSerial[1],checkParamSerial[2],checkParamSerial[3],checkParamSerial[4]); pc.serial.attach(&pc, &UsaPack::Receive, Serial::RxIrq); switch(vp.commandIndex){ case 1: NVIC_SystemReset(); break; case 10: ekf.setQqerr(float(vp.commandVal)); checkParamSerial[0] = 1; break; case 11: ekf.setQgbias(float(vp.commandVal)); checkParamSerial[1] = 1; break; case 12: Qabdyn=float(vp.commandVal); checkParamSerial[2] = 1; break; case 13: Rscdyn=float(vp.commandVal); checkParamSerial[3] = 1; break; case 14: checkParamSerial[4] = 1; break; default : break; } int cpsSum = 0; for(int i = 0;i<5;i++){ cpsSum += checkParamSerial[i]; } if(cpsSum == 5){ break; } wait(0.01); } } LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); while(1) { float tstart = _t.read(); //姿勢角を更新 if(hilFlag == true){ getHILval(); }else{ getIMUval(); } ekf.updateQhat(gyro, att_dt); ekf.updateErrState(gyro, att_dt); ekf.updateAccMeasures(acc,accref); ekf.updateGyroBiasConstraints(gyro); ekf.updateVelocityConstraints(); /* if(obsCount == 450){ if(ekf.determinDynStatus(acc,accref)==false){ ekf.setRsoftconst(1.0f,1.0f); ekf.setQab(0.0f); }else{ ekf.setRsoftconst(263.0f,500.0f); ekf.setQab(Qabdyn); } ekf.updateAccMeasures(acc,accref); obsCount = 0; }else{ obsCount += 1; if(ekf.determinDynStatus(acc,accref)==false){ ekf.updateStaticAccMeasures(acc,accref); } } */ //ekf.updateMagMeasures(mag); ekf.fuseErr2Qhat(); //ekf.resetBias(); ekf.computeAngles(rpy, rpy_align); PIDtick.loop(); float tend = _t.read(); att_dt = (tend-tstart); } }