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
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?
User | Revision | Line number | New 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 | } |