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@65:ea184054e659, 2021-06-22 (annotated)

Committer:
NaotoMorita
Date:
Tue Jun 22 02:08:31 2021 +0000
Revision:
65:ea184054e659
Parent:
61:c05353850017
Child:
68:b9f6938fab9d
setup

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 gxs = 0;
cocorlow 56:888379912f81 37 int gys = 0;
cocorlow 56:888379912f81 38 int gzs = 0;
cocorlow 56:888379912f81 39 int iter_n = 10000;
cocorlow 56:888379912f81 40
cocorlow 56:888379912f81 41 for(int i = 0;i<iter_n ;i++)
cocorlow 56:888379912f81 42 {
cocorlow 56:888379912f81 43 accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
cocorlow 56:888379912f81 44 gxs += gx;
cocorlow 56:888379912f81 45 gys += gy;
cocorlow 56:888379912f81 46 gzs -= gz;
cocorlow 56:888379912f81 47 //wait(0.01);
cocorlow 56:888379912f81 48 }
cocorlow 56:888379912f81 49 gxs = gxs /iter_n;
cocorlow 56:888379912f81 50 gys = gys /iter_n;
cocorlow 56:888379912f81 51 gzs = gzs /iter_n;
cocorlow 56:888379912f81 52 agoffset[3] = gxs;
cocorlow 56:888379912f81 53 agoffset[4] = gys;
cocorlow 56:888379912f81 54 agoffset[5] = gzs;
NaotoMorita 65:ea184054e659 55 pc.serial.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
NaotoMorita 65:ea184054e659 56
NaotoMorita 61:c05353850017 57
NaotoMorita 65:ea184054e659 58 pc.serial.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
NaotoMorita 65:ea184054e659 59 pc.serial.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
NaotoMorita 65:ea184054e659 60
NaotoMorita 65:ea184054e659 61 pc.serial.printf("Mag Calibration Start\r\n");
cocorlow 56:888379912f81 62
NaotoMorita 61:c05353850017 63 float inputMag[3];
NaotoMorita 61:c05353850017 64 float outputMag[3];
cocorlow 56:888379912f81 65 while(1)
cocorlow 56:888379912f81 66 {
cocorlow 56:888379912f81 67 mag_sensor.getAxis(mdata); // flush the magnetmeter
NaotoMorita 61:c05353850017 68 inputMag[0] = mdata.x;
NaotoMorita 61:c05353850017 69 inputMag[1] = mdata.y;
NaotoMorita 61:c05353850017 70 inputMag[2] = mdata.z;
NaotoMorita 61:c05353850017 71 magCalibrator.run(inputMag,outputMag);
cocorlow 56:888379912f81 72 if(userButton.read() == 1)
cocorlow 56:888379912f81 73 {
cocorlow 56:888379912f81 74 break;
cocorlow 56:888379912f81 75 }
cocorlow 56:888379912f81 76 wait(0.001);
cocorlow 56:888379912f81 77 }
NaotoMorita 61:c05353850017 78 magCalibrator.getExtremes(magbiasMin,magbiasMax);
NaotoMorita 65:ea184054e659 79 pc.serial.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
NaotoMorita 65:ea184054e659 80 pc.serial.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
NaotoMorita 61:c05353850017 81 magCalibrator.getExtremes(magbiasMin,magbiasMax);
cocorlow 56:888379912f81 82
NaotoMorita 65:ea184054e659 83 pc.serial.printf("Calculating pitch/roll Offset \r\n");
cocorlow 56:888379912f81 84 //姿勢オフセットを計算
cocorlow 56:888379912f81 85 rpy_align.y = 0.0f*M_PI/180.0f;
cocorlow 56:888379912f81 86 rpy_align.x = 0.0f*M_PI/180.0f;
cocorlow 56:888379912f81 87 float ave_pitch = 0.0f;
cocorlow 56:888379912f81 88 float ave_roll = 0.0f;
cocorlow 56:888379912f81 89 Timer _t;
cocorlow 56:888379912f81 90 _t.start();
cocorlow 56:888379912f81 91 for (int i = 0 ; i < 2200; i++)
cocorlow 56:888379912f81 92 {
cocorlow 56:888379912f81 93 float tstart = _t.read();
cocorlow 56:888379912f81 94 //姿勢角を更新
cocorlow 56:888379912f81 95 getIMUval();
NaotoMorita 65:ea184054e659 96 //ekf.updateBetweenMeasures(gyro, att_dt);
NaotoMorita 65:ea184054e659 97 //ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
NaotoMorita 65:ea184054e659 98 //ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
NaotoMorita 65:ea184054e659 99 //ekf.computeAngles(rpy, rpy_g, rpy_align);
cocorlow 56:888379912f81 100 if(i>199)
cocorlow 56:888379912f81 101 {
cocorlow 56:888379912f81 102 ave_pitch += rpy.y;
cocorlow 56:888379912f81 103 ave_roll += rpy.x;
cocorlow 56:888379912f81 104 }
cocorlow 56:888379912f81 105 wait(0.001);
cocorlow 56:888379912f81 106 float tend = _t.read();
cocorlow 56:888379912f81 107 att_dt = (tend-tstart);
cocorlow 56:888379912f81 108 }
cocorlow 56:888379912f81 109
NaotoMorita 65:ea184054e659 110 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 111
cocorlow 56:888379912f81 112 while(1)
cocorlow 56:888379912f81 113 {
cocorlow 56:888379912f81 114 wait(1000);
cocorlow 56:888379912f81 115 }
cocorlow 56:888379912f81 116 }