HAPSRG
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
--- 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