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:
- cocorlow
- Date:
- 2021-12-06
- Revision:
- 139:b378528c05f2
- Parent:
- 135:49f8916588da
- Child:
- 141:725321fe2949
File content as of revision 139:b378528c05f2:
#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); // } }