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:
- 141:725321fe2949
- Parent:
- 139:b378528c05f2
- Child:
- 143:53808e4e684c
diff -r 53dbdb207542 -r 725321fe2949 run.cpp --- a/run.cpp Mon Dec 06 11:37:55 2021 +0000 +++ b/run.cpp Fri Dec 10 10:43:50 2021 +0000 @@ -2,198 +2,196 @@ 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); -// } + 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_F / 180.0f); + agoffset[4] +=(lsm.gy * M_PI_F / 180.0f); + agoffset[5] +=(lsm.gz * M_PI_F / 180.0f); + palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars()); + magref(0) += lsm.mx; + magref(1) += lsm.my; + magref(2) += lsm.mz; + } + for(int i = 0;i<6;i++){ + agoffset[i] /= float(n_init); + } + magref /= 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); + + Matrix3f Rgpspos; + setDiag(Rgpspos,1.0f); + + Matrix3f Rgpsvel; + Rgpsvel(0,0) = 0.01f; + Rgpsvel(1,1) = 0.01f; + Rgpsvel(2,2) = 0.1f; + + MatrixXf Rgps(5,5); + setDiag(Rgps,0.05f); + Rgps(3,3) = 0.1f; + Rgps(4,4) = 0.1f; + + float dynAccCriteria = 0.4f; + Matrix3f Racc; + setDiag(Racc,100.0f); + Matrix3f RaccDyn; + setDiag(RaccDyn,5000.0f); + + Matrix<float, 1, 1> Rheading; + Rheading(0, 0) = 0.01f; + + _t.start(); + //センサ正常性チェック + if(hilFlag == false){ + while(1){ + float tstart = _t.read(); + getIMUval(); + getGPSval(); + eskf.updateNominal(acc, gyro, att_dt); + eskf.updateErrState(acc, gyro, att_dt); + eskf.updateGPS(pi, palt, vi, Rgps); + float heading = std::atan2(-mag(2), mag(0)); + eskf.updateHeading(heading, Rheading); + Matrix3f Raccpreflight; + setDiag(Raccpreflight, 5.0f); + eskf.updateAcc(acc, Raccpreflight); + + bool preflightCheck = true; + Vector3f gyroBias = eskf.getGyroBias(); + if(fabsf(gyro(0)-gyroBias(0))>2.0f || fabsf(gyro(1)-gyroBias(1))>2.0f || fabsf(gyro(2)-gyroBias(2))>2.0f){ + preflightCheck = false; + twelite.serial.printf("PreFlight Check : high gyro value\r\n"); + } + Vector3f accBias = eskf.getAccBias(); + if(fabsf(acc(0)-accBias(0))>2.0f || fabsf(acc(1)-gyroBias(1))>2.0f){ + preflightCheck = false; + twelite.serial.printf("PreFlight Check : high acc value\r\n"); + } + Vector3f vihat = eskf.getVihat(); + if(fabsf(vihat(0))>2.0f || fabsf(vihat(1))>2.0f||fabsf(vihat(2))>2.0f){ + preflightCheck = false; + twelite.serial.printf("PreFlight Check : high velocity value\r\n"); + } + Vector3f pihat = eskf.getPihat(); + if(fabsf(pihat(0))>2.0f || fabsf(pihat(1))>2.0f||fabsf(pihat(2))>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(acc, gyro, att_dt); + eskf.updateErrState(acc, 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){ + Vector3f euler = eskf.computeAngles(); + if(fabsf(euler(1))<30.0f*M_PI_F/180.0f){ + headingUpdateFlag = true; + theading = _t.read(); + } + } + + if(gpsUpdateFlag == true){ + eskf.updateGPS(pi, palt, vi, Rgps); + //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos); + //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); + }else if(headingUpdateFlag == true){ + float heading = std::atan2(-mag(1),mag(0)); + eskf.updateHeading(heading,Rheading); + }else{ + Vector3f dynacc = eskf.calcDynAcc(acc); + dynaccnorm2 = dynacc.squaredNorm(); + //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2)); + if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){ + eskf.updateAcc(acc, RaccDyn); + }else{ + eskf.updateAcc(acc, Racc); + } + } + PIDtick.loop(); + + //制御時間を計測 + float tend = _t.read(); + att_dt = (tend-tstart); + } } \ No newline at end of file