HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

setup.cpp@68:b9f6938fab9d, 2021-06-28 (annotated)

Committer:
NaotoMorita
Date:
Mon Jun 28 01:40:26 2021 +0000
Revision:
68:b9f6938fab9d
Parent:
65:ea184054e659
Child:
70:99f974d8960e
commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 56:888379912f81 1 #include "global.hpp"
cocorlow 56:888379912f81 2
cocorlow 56:888379912f81 3 void setup()
cocorlow 56:888379912f81 4 {
cocorlow 56:888379912f81 5 pitchPID.setSetPoint(0.0);
cocorlow 56:888379912f81 6 pitchratePID.setSetPoint(0.0);
cocorlow 56:888379912f81 7 pitchPID.setBias(0.0);
cocorlow 56:888379912f81 8 pitchratePID.setBias(0.0);
cocorlow 56:888379912f81 9 pitchPID.setOutputLimits(-1.0,1.0);
cocorlow 56:888379912f81 10 pitchratePID.setOutputLimits(-1.0,1.0);
cocorlow 56:888379912f81 11 pitchPID.setInputLimits(-M_PI, M_PI);
cocorlow 56:888379912f81 12 pitchratePID.setInputLimits(-M_PI, M_PI);
cocorlow 56:888379912f81 13
NaotoMorita 65:ea184054e659 14 elevServo.period_us(15000.0);
NaotoMorita 65:ea184054e659 15 elevServo.pulsewidth_us(1500.0);
NaotoMorita 65:ea184054e659 16 rudServo.period_us(15000.0);
NaotoMorita 65:ea184054e659 17 rudServo.pulsewidth_us(1500.0);
cocorlow 56:888379912f81 18
cocorlow 56:888379912f81 19 accelgyro.initialize();
cocorlow 56:888379912f81 20 //加速度計のフルスケールレンジを設定
cocorlow 56:888379912f81 21 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
cocorlow 56:888379912f81 22 //角速度計のフルスケールレンジを設定
cocorlow 56:888379912f81 23 accelgyro.setFullScaleGyroRange(GYRO_FSR);
cocorlow 56:888379912f81 24 //MPU6050のLPFを設定
cocorlow 56:888379912f81 25 accelgyro.setDLPFMode(MPU6050_LPF);
cocorlow 56:888379912f81 26 //地磁気
cocorlow 56:888379912f81 27 mag_sensor.enable();
cocorlow 56:888379912f81 28 }
cocorlow 56:888379912f81 29
cocorlow 56:888379912f81 30 void calibrate()
cocorlow 56:888379912f81 31 {
NaotoMorita 65:ea184054e659 32 pc.serial.printf("\r\nEnter to Calibration Mode\r\n");
cocorlow 56:888379912f81 33 wait(5);
NaotoMorita 65:ea184054e659 34 pc.serial.printf("Acc and Gyro Calibration Start\r\n");
cocorlow 56:888379912f81 35
cocorlow 56:888379912f81 36 int iter_n = 10000;
cocorlow 56:888379912f81 37
NaotoMorita 68:b9f6938fab9d 38 long axs = 0;
NaotoMorita 68:b9f6938fab9d 39 long ays = 0;
NaotoMorita 68:b9f6938fab9d 40 long azs = 0;
NaotoMorita 68:b9f6938fab9d 41 double axs2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 42 double ays2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 43 double azs2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 44 long gxs = 0;
NaotoMorita 68:b9f6938fab9d 45 long gys = 0;
NaotoMorita 68:b9f6938fab9d 46 long gzs = 0;
NaotoMorita 68:b9f6938fab9d 47 double gxs2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 48 double gys2 = 0.0f;
NaotoMorita 68:b9f6938fab9d 49 double gzs2 = 0.0f;
cocorlow 56:888379912f81 50 for(int i = 0;i<iter_n ;i++)
cocorlow 56:888379912f81 51 {
NaotoMorita 68:b9f6938fab9d 52 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 68:b9f6938fab9d 53 axs += ax;
NaotoMorita 68:b9f6938fab9d 54 ays += ay;
NaotoMorita 68:b9f6938fab9d 55 azs += az;
NaotoMorita 68:b9f6938fab9d 56 axs2 += double(ax*ax)/iter_n;
NaotoMorita 68:b9f6938fab9d 57 ays2 += double(ay*ay)/iter_n;
NaotoMorita 68:b9f6938fab9d 58 azs2 += double(az*az)/iter_n;
NaotoMorita 68:b9f6938fab9d 59
cocorlow 56:888379912f81 60 gxs += gx;
cocorlow 56:888379912f81 61 gys += gy;
NaotoMorita 68:b9f6938fab9d 62 gzs += gz;
NaotoMorita 68:b9f6938fab9d 63 gxs2 += double(gx*gx)/iter_n;
NaotoMorita 68:b9f6938fab9d 64 gys2 += double(gy*gy)/iter_n;
NaotoMorita 68:b9f6938fab9d 65 gzs2 += double(gz*gz)/iter_n;
cocorlow 56:888379912f81 66 //wait(0.01);
cocorlow 56:888379912f81 67 }
NaotoMorita 68:b9f6938fab9d 68 axs = axs /iter_n;
NaotoMorita 68:b9f6938fab9d 69 ays = ays /iter_n;
NaotoMorita 68:b9f6938fab9d 70 azs = azs /iter_n;
cocorlow 56:888379912f81 71 gxs = gxs /iter_n;
cocorlow 56:888379912f81 72 gys = gys /iter_n;
NaotoMorita 68:b9f6938fab9d 73 gzs = gzs /iter_n;
NaotoMorita 68:b9f6938fab9d 74 double var_accx = (axs2 - double(axs*axs))/ ACCEL_SSF / ACCEL_SSF;
NaotoMorita 68:b9f6938fab9d 75 double var_accy = (ays2 - double(ays*ays))/ ACCEL_SSF / ACCEL_SSF;
NaotoMorita 68:b9f6938fab9d 76 double var_accz = (azs2 - double(azs*azs))/ ACCEL_SSF / ACCEL_SSF;
NaotoMorita 68:b9f6938fab9d 77 double var_gyrox = (gxs2 - double(gxs*gxs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f;
NaotoMorita 68:b9f6938fab9d 78 double var_gyroy = (gys2 - double(gys*gys))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f;
NaotoMorita 68:b9f6938fab9d 79 double var_gyroz = (gzs2 - double(gzs*gzs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f;
NaotoMorita 68:b9f6938fab9d 80 pc.serial.printf("AccCovariance : %f, %f, %f \r\n",var_accx,var_accy,var_accz);
NaotoMorita 68:b9f6938fab9d 81 pc.serial.printf("GyroCovariance : %f, %f, %f \r\n",var_gyrox,var_gyroy,var_gyroz);
NaotoMorita 68:b9f6938fab9d 82
NaotoMorita 65:ea184054e659 83 pc.serial.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
NaotoMorita 65:ea184054e659 84
NaotoMorita 61:c05353850017 85
NaotoMorita 65:ea184054e659 86 pc.serial.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
NaotoMorita 65:ea184054e659 87 pc.serial.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
NaotoMorita 65:ea184054e659 88
NaotoMorita 65:ea184054e659 89 pc.serial.printf("Mag Calibration Start\r\n");
cocorlow 56:888379912f81 90
NaotoMorita 61:c05353850017 91 float inputMag[3];
NaotoMorita 61:c05353850017 92 float outputMag[3];
cocorlow 56:888379912f81 93 while(1)
cocorlow 56:888379912f81 94 {
cocorlow 56:888379912f81 95 mag_sensor.getAxis(mdata); // flush the magnetmeter
NaotoMorita 61:c05353850017 96 inputMag[0] = mdata.x;
NaotoMorita 61:c05353850017 97 inputMag[1] = mdata.y;
NaotoMorita 61:c05353850017 98 inputMag[2] = mdata.z;
NaotoMorita 61:c05353850017 99 magCalibrator.run(inputMag,outputMag);
cocorlow 56:888379912f81 100 if(userButton.read() == 1)
cocorlow 56:888379912f81 101 {
cocorlow 56:888379912f81 102 break;
cocorlow 56:888379912f81 103 }
cocorlow 56:888379912f81 104 wait(0.001);
cocorlow 56:888379912f81 105 }
NaotoMorita 61:c05353850017 106 magCalibrator.getExtremes(magbiasMin,magbiasMax);
NaotoMorita 65:ea184054e659 107 pc.serial.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
NaotoMorita 65:ea184054e659 108 pc.serial.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
NaotoMorita 68:b9f6938fab9d 109 magCalibrator.setExtremes(magbiasMin,magbiasMax);
cocorlow 56:888379912f81 110
NaotoMorita 65:ea184054e659 111 pc.serial.printf("Calculating pitch/roll Offset \r\n");
cocorlow 56:888379912f81 112 //姿勢オフセットを計算
cocorlow 56:888379912f81 113 rpy_align.y = 0.0f*M_PI/180.0f;
cocorlow 56:888379912f81 114 rpy_align.x = 0.0f*M_PI/180.0f;
cocorlow 56:888379912f81 115 float ave_pitch = 0.0f;
cocorlow 56:888379912f81 116 float ave_roll = 0.0f;
NaotoMorita 68:b9f6938fab9d 117 ekf.Q(4,4) = 0.00001f;
NaotoMorita 68:b9f6938fab9d 118 ekf.Q(5,5) = 0.00001f;
NaotoMorita 68:b9f6938fab9d 119 ekf.Q(6,6) = 0.00001f;
NaotoMorita 68:b9f6938fab9d 120 ekf.Qab(1,1) = 0.0f;
NaotoMorita 68:b9f6938fab9d 121 ekf.Qab(2,2) = 0.0f;
NaotoMorita 68:b9f6938fab9d 122 ekf.Qab(3,3) = 0.0f;
NaotoMorita 68:b9f6938fab9d 123 getIMUval();
NaotoMorita 68:b9f6938fab9d 124 ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
cocorlow 56:888379912f81 125 Timer _t;
cocorlow 56:888379912f81 126 _t.start();
cocorlow 56:888379912f81 127 for (int i = 0 ; i < 2200; i++)
cocorlow 56:888379912f81 128 {
cocorlow 56:888379912f81 129 float tstart = _t.read();
cocorlow 56:888379912f81 130 //姿勢角を更新
cocorlow 56:888379912f81 131 getIMUval();
NaotoMorita 68:b9f6938fab9d 132 ekf.updateQhat(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 133 ekf.updateErrState(gyro, att_dt);
NaotoMorita 68:b9f6938fab9d 134 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 68:b9f6938fab9d 135 ekf.fuseErr2Qhat();
NaotoMorita 68:b9f6938fab9d 136 ekf.updateMagMeasures(mag);
NaotoMorita 68:b9f6938fab9d 137 ekf.computeAngles(rpy, rpy_align);
cocorlow 56:888379912f81 138 if(i>199)
cocorlow 56:888379912f81 139 {
cocorlow 56:888379912f81 140 ave_pitch += rpy.y;
cocorlow 56:888379912f81 141 ave_roll += rpy.x;
cocorlow 56:888379912f81 142 }
cocorlow 56:888379912f81 143 wait(0.001);
cocorlow 56:888379912f81 144 float tend = _t.read();
cocorlow 56:888379912f81 145 att_dt = (tend-tstart);
cocorlow 56:888379912f81 146 }
cocorlow 56:888379912f81 147
NaotoMorita 65:ea184054e659 148 pc.serial.printf("aliginment data : %f(pitch deg) %f(roll deg)\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI);
cocorlow 56:888379912f81 149
cocorlow 56:888379912f81 150 while(1)
cocorlow 56:888379912f81 151 {
cocorlow 56:888379912f81 152 wait(1000);
cocorlow 56:888379912f81 153 }
cocorlow 56:888379912f81 154 }