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-11-30
- Revision:
- 135:49f8916588da
- Parent:
- 134:d57c6b2a706b
- Child:
- 139:b378528c05f2
File content as of revision 135:49f8916588da:
#include "global.hpp" void run() { wait(0.5); //センサの初期化・ジャイロバイアス・加速度スケールの取得 int n_init = 1000; for(int i = 0;i<n_init;i++){ lsm.readAccel(); lsm.readMag(); lsm.readGyro(); agoffset[0] += lsm.ax * 9.8f; agoffset[1] += lsm.ay * 9.8f; agoffset[2] += lsm.az * 9.8f-9.8f; agoffset[3] +=(lsm.gx * M_PI / 180.0f); agoffset[4] +=(lsm.gy * M_PI / 180.0f); agoffset[5] +=(lsm.gz * M_PI / 180.0f); palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars()); magref.x += lsm.mx; magref.y += lsm.my; magref.z += lsm.mz; } for(int i = 0;i<6;i++){ agoffset[i] /= float(n_init); } magref.x /= float(n_init); magref.y /= float(n_init); magref.z /= float(n_init); palt0 /= float(n_init); twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]); //ESKFの共分散設定 eskf.setGravity(0.0f,0.0f,9.8f); eskf.setPhatPosition(0.1f); eskf.setPhatVelocity(0.1f); eskf.setPhatAngleError(0.5f); eskf.setPhatAccBias(0.001f); eskf.setPhatGyroBias(0.001f); eskf.setPhatGravity(0.0000001f); eskf.setQVelocity(0.001f); eskf.setQAngleError(0.0000025f); eskf.setQAccBias(0.000001f); eskf.setQGyroBias(0.000001f); Matrix Rgpspos(3,3); setDiag(Rgpspos,1.0f); Matrix Rgpsvel(3,3); Rgpsvel(1,1) = 0.01f; Rgpsvel(2,2) = 0.01f; Rgpsvel(3,3) = 0.1f; Matrix Rgps(5,5); setDiag(Rgps,0.05f); Rgps(4,4) = 0.1f; Rgps(5,5) = 0.1f; float dynAccCriteria = 0.4f; Matrix Racc(3,3); setDiag(Racc,100.0f); Matrix RaccDyn(3,3); setDiag(RaccDyn,5000.0f); Matrix Rheading(1,1); Rheading(1,1) = 0.01f; _t.start(); //センサ正常性チェック if(hilFlag == false){ while(1){ float tstart = _t.read(); getIMUval(); getGPSval(); eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps); float heading = atan2f(-mag.y,mag.x); eskf.updateHeading(heading,Rheading); Matrix Raccpreflight(3,3); setDiag(Raccpreflight,5.0f); eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight); bool preflightCheck = true; Matrix gyroBias = eskf.getGyroBias(); if(fabsf(gyro.x-gyroBias(1,1))>2.0f || fabsf(gyro.y-gyroBias(2,1))>2.0f || fabsf(gyro.z-gyroBias(3,1))>2.0f){ preflightCheck = false; twelite.serial.printf("PreFlight Check : high gyro value\r\n"); } Matrix accBias = eskf.getAccBias(); if(fabsf(acc.x-accBias(1,1))>2.0f || fabsf(acc.y-gyroBias(2,1))>2.0f){ preflightCheck = false; twelite.serial.printf("PreFlight Check : high acc value\r\n"); } Matrix vihat = eskf.getVihat(); if(fabsf(vihat(1,1))>2.0f || fabsf(vihat(2,1))>2.0f||fabsf(vihat(3,1))>2.0f){ preflightCheck = false; twelite.serial.printf("PreFlight Check : high velocity value\r\n"); } Matrix pihat = eskf.getPihat(); if(fabsf(pihat(1,1))>2.0f || fabsf(pihat(2,1))>2.0f||fabsf(pihat(3,1))>2.0f){ preflightCheck = false; twelite.serial.printf("PreFlight Check : not home position\r\n"); } if(sbus.failSafe){ preflightCheck = false; twelite.serial.printf("PreFlight Check : no RC\r\n"); } // sbusデータの読み込み for (int i =0 ; i < 16;i ++){ rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input } if (rc[4]>-0.3f && rc[6] < -0.3f){ preflightCheck = false; twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n"); } if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){ preflightCheck = false; twelite.serial.printf("PreFlight Check : no gps lock\r\n"); } if(preflightCheck == true){ break; } } } twelite.serial.printf("PreFlight Check Completed\r\n"); //usaPack通信開始 pc.Subscribe(0000, &(vp)); //制御ループのアタッチ LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); float tgps = _t.read(); float theading = _t.read(); while(1) { float tstart = _t.read(); //センサの値を取得 if(hilFlag == true){ getHilIMUval(); }else{ getIMUval(); } //ekfの更新 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(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(); } headingUpdateFlag = false; if(tstart-theading>0.05f){ Matrix euler = eskf.computeAngles(); if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){ headingUpdateFlag = true; theading = _t.read(); } } if(gpsUpdateFlag == true){ eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps); //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos); //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); }else if(headingUpdateFlag == true){ float heading = atan2f(-mag.y,mag.x); eskf.updateHeading(heading,Rheading); }else{ Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc)); dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1); //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2)); if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){ eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn); }else{ eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc); } } PIDtick.loop(); //制御時間を計測 float tend = _t.read(); att_dt = (tend-tstart); } }