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
Diff: run.cpp
- Revision:
- 87:89bbbcdb667b
- Parent:
- 86:456f00d52974
- Child:
- 88:be349faa1976
diff -r 456f00d52974 -r 89bbbcdb667b run.cpp --- a/run.cpp Mon Oct 18 12:18:15 2021 +0000 +++ b/run.cpp Wed Oct 20 01:50:52 2021 +0000 @@ -2,50 +2,41 @@ void run() { - //pc.serial.printf("\r\nrun Mode\r\n"); wait(0.5); Timer _t; _t.start(); - magCalibrator.setExtremes(magbiasMin,magbiasMax); - float sum2accnorm = 0; + //センサの初期化・ジャイロバイアス・加速度スケールの取得 float sumaccnorm = 0; int n_init = 1000; if(hilFlag == false){ ekf.defineQhat(rpy_align); for(int i = 0; i < n_init; i++){ - float tstart = _t.read(); - getIMUval(); - ekf.updateStaticAccMeasures(acc,accref); - //ekf.updateGyroBiasConstraints(gyro); - ekf.fuseErr2Nominal(); - 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 / 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(); + //getIMUval(); } + //ekfの更新 ekf.updateNominal(gyro,acc,accref,att_dt); ekf.updateErrState(gyro,acc, att_dt); if(obsCount == 50){ @@ -70,55 +61,11 @@ ekf.fuseErr2Nominal(); ekf.resetBias(); ekf.computeAngles(rpy, rpy_align); - ekf.computeVb(vb); + PIDtick.loop(); + //制御時間を計測 float tend = _t.read(); att_dt = (tend-tstart); } -} - - /* - 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); - } - } - */ \ No newline at end of file +} \ No newline at end of file