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@76:7fd3ac1afe3e, 2021-08-07 (annotated)
- Committer:
- NaotoMorita
- Date:
- Sat Aug 07 06:54:58 2021 +0000
- Revision:
- 76:7fd3ac1afe3e
- Parent:
- 73:84ffa0166e6c
- Child:
- 77:2bf856e3eca4
async update
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); |
NaotoMorita | 70:99f974d8960e | 7 | rollPID.setSetPoint(0.0); |
NaotoMorita | 70:99f974d8960e | 8 | rollratePID.setSetPoint(0.0); |
cocorlow | 56:888379912f81 | 9 | pitchPID.setBias(0.0); |
NaotoMorita | 70:99f974d8960e | 10 | pitchratePID.setBias(0.0); |
NaotoMorita | 70:99f974d8960e | 11 | rollPID.setBias(0.0); |
NaotoMorita | 70:99f974d8960e | 12 | rollratePID.setBias(0.0); |
cocorlow | 56:888379912f81 | 13 | pitchPID.setOutputLimits(-1.0,1.0); |
cocorlow | 56:888379912f81 | 14 | pitchratePID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 70:99f974d8960e | 15 | rollPID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 70:99f974d8960e | 16 | rollratePID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 70:99f974d8960e | 17 | pitchPID.setInputLimits(-M_PI,M_PI); |
NaotoMorita | 70:99f974d8960e | 18 | pitchratePID.setInputLimits(-M_PI,M_PI); |
NaotoMorita | 70:99f974d8960e | 19 | rollPID.setInputLimits(-M_PI,M_PI); |
NaotoMorita | 70:99f974d8960e | 20 | rollratePID.setInputLimits(-M_PI,M_PI); |
cocorlow | 56:888379912f81 | 21 | |
NaotoMorita | 70:99f974d8960e | 22 | servoRight.period_us(15000.0); |
NaotoMorita | 70:99f974d8960e | 23 | servoLeft.period_us(15000.0); |
NaotoMorita | 70:99f974d8960e | 24 | servoThrust.period_us(15000.0); |
NaotoMorita | 70:99f974d8960e | 25 | servoRight.pulsewidth_us(1500.0); |
NaotoMorita | 70:99f974d8960e | 26 | servoLeft.pulsewidth_us(1500.0); |
NaotoMorita | 70:99f974d8960e | 27 | servoThrust.pulsewidth_us(1100.0); |
cocorlow | 56:888379912f81 | 28 | |
NaotoMorita | 76:7fd3ac1afe3e | 29 | sd.baud(57600); |
NaotoMorita | 76:7fd3ac1afe3e | 30 | sd.printf("\r\nFlight Start\r\n"); |
cocorlow | 56:888379912f81 | 31 | accelgyro.initialize(); |
cocorlow | 56:888379912f81 | 32 | //加速度計のフルスケールレンジを設定 |
cocorlow | 56:888379912f81 | 33 | accelgyro.setFullScaleAccelRange(ACCEL_FSR); |
cocorlow | 56:888379912f81 | 34 | //角速度計のフルスケールレンジを設定 |
cocorlow | 56:888379912f81 | 35 | accelgyro.setFullScaleGyroRange(GYRO_FSR); |
cocorlow | 56:888379912f81 | 36 | //MPU6050のLPFを設定 |
cocorlow | 56:888379912f81 | 37 | accelgyro.setDLPFMode(MPU6050_LPF); |
NaotoMorita | 73:84ffa0166e6c | 38 | //MPU6050のレートを設定 |
NaotoMorita | 73:84ffa0166e6c | 39 | accelgyro.setRate(MPU6050_SAMPLERATE); |
cocorlow | 56:888379912f81 | 40 | //地磁気 |
cocorlow | 56:888379912f81 | 41 | mag_sensor.enable(); |
cocorlow | 56:888379912f81 | 42 | } |
cocorlow | 56:888379912f81 | 43 | |
cocorlow | 56:888379912f81 | 44 | void calibrate() |
cocorlow | 56:888379912f81 | 45 | { |
NaotoMorita | 65:ea184054e659 | 46 | pc.serial.printf("\r\nEnter to Calibration Mode\r\n"); |
cocorlow | 56:888379912f81 | 47 | wait(5); |
NaotoMorita | 65:ea184054e659 | 48 | pc.serial.printf("Acc and Gyro Calibration Start\r\n"); |
cocorlow | 56:888379912f81 | 49 | |
cocorlow | 56:888379912f81 | 50 | int iter_n = 10000; |
cocorlow | 56:888379912f81 | 51 | |
NaotoMorita | 68:b9f6938fab9d | 52 | long axs = 0; |
NaotoMorita | 68:b9f6938fab9d | 53 | long ays = 0; |
NaotoMorita | 68:b9f6938fab9d | 54 | long azs = 0; |
NaotoMorita | 68:b9f6938fab9d | 55 | double axs2 = 0.0f; |
NaotoMorita | 68:b9f6938fab9d | 56 | double ays2 = 0.0f; |
NaotoMorita | 68:b9f6938fab9d | 57 | double azs2 = 0.0f; |
NaotoMorita | 68:b9f6938fab9d | 58 | long gxs = 0; |
NaotoMorita | 68:b9f6938fab9d | 59 | long gys = 0; |
NaotoMorita | 68:b9f6938fab9d | 60 | long gzs = 0; |
NaotoMorita | 68:b9f6938fab9d | 61 | double gxs2 = 0.0f; |
NaotoMorita | 68:b9f6938fab9d | 62 | double gys2 = 0.0f; |
NaotoMorita | 68:b9f6938fab9d | 63 | double gzs2 = 0.0f; |
cocorlow | 56:888379912f81 | 64 | for(int i = 0;i<iter_n ;i++) |
cocorlow | 56:888379912f81 | 65 | { |
NaotoMorita | 68:b9f6938fab9d | 66 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
NaotoMorita | 68:b9f6938fab9d | 67 | axs += ax; |
NaotoMorita | 68:b9f6938fab9d | 68 | ays += ay; |
NaotoMorita | 68:b9f6938fab9d | 69 | azs += az; |
NaotoMorita | 68:b9f6938fab9d | 70 | axs2 += double(ax*ax)/iter_n; |
NaotoMorita | 68:b9f6938fab9d | 71 | ays2 += double(ay*ay)/iter_n; |
NaotoMorita | 68:b9f6938fab9d | 72 | azs2 += double(az*az)/iter_n; |
NaotoMorita | 68:b9f6938fab9d | 73 | |
cocorlow | 56:888379912f81 | 74 | gxs += gx; |
cocorlow | 56:888379912f81 | 75 | gys += gy; |
NaotoMorita | 68:b9f6938fab9d | 76 | gzs += gz; |
NaotoMorita | 68:b9f6938fab9d | 77 | gxs2 += double(gx*gx)/iter_n; |
NaotoMorita | 68:b9f6938fab9d | 78 | gys2 += double(gy*gy)/iter_n; |
NaotoMorita | 68:b9f6938fab9d | 79 | gzs2 += double(gz*gz)/iter_n; |
cocorlow | 56:888379912f81 | 80 | //wait(0.01); |
cocorlow | 56:888379912f81 | 81 | } |
NaotoMorita | 68:b9f6938fab9d | 82 | axs = axs /iter_n; |
NaotoMorita | 68:b9f6938fab9d | 83 | ays = ays /iter_n; |
NaotoMorita | 68:b9f6938fab9d | 84 | azs = azs /iter_n; |
cocorlow | 56:888379912f81 | 85 | gxs = gxs /iter_n; |
cocorlow | 56:888379912f81 | 86 | gys = gys /iter_n; |
NaotoMorita | 68:b9f6938fab9d | 87 | gzs = gzs /iter_n; |
NaotoMorita | 68:b9f6938fab9d | 88 | double var_accx = (axs2 - double(axs*axs))/ ACCEL_SSF / ACCEL_SSF; |
NaotoMorita | 68:b9f6938fab9d | 89 | double var_accy = (ays2 - double(ays*ays))/ ACCEL_SSF / ACCEL_SSF; |
NaotoMorita | 68:b9f6938fab9d | 90 | double var_accz = (azs2 - double(azs*azs))/ ACCEL_SSF / ACCEL_SSF; |
NaotoMorita | 68:b9f6938fab9d | 91 | double var_gyrox = (gxs2 - double(gxs*gxs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f; |
NaotoMorita | 68:b9f6938fab9d | 92 | double var_gyroy = (gys2 - double(gys*gys))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f; |
NaotoMorita | 68:b9f6938fab9d | 93 | double var_gyroz = (gzs2 - double(gzs*gzs))/ GYRO_SSF * 0.0174533f / GYRO_SSF * 0.0174533f; |
NaotoMorita | 68:b9f6938fab9d | 94 | pc.serial.printf("AccCovariance : %f, %f, %f \r\n",var_accx,var_accy,var_accz); |
NaotoMorita | 68:b9f6938fab9d | 95 | pc.serial.printf("GyroCovariance : %f, %f, %f \r\n",var_gyrox,var_gyroy,var_gyroz); |
NaotoMorita | 68:b9f6938fab9d | 96 | |
NaotoMorita | 65:ea184054e659 | 97 | pc.serial.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs); |
NaotoMorita | 65:ea184054e659 | 98 | |
NaotoMorita | 76:7fd3ac1afe3e | 99 | /* |
NaotoMorita | 65:ea184054e659 | 100 | pc.serial.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); |
NaotoMorita | 65:ea184054e659 | 101 | pc.serial.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); |
NaotoMorita | 76:7fd3ac1afe3e | 102 | */ |
NaotoMorita | 76:7fd3ac1afe3e | 103 | pc.serial.printf("Initial Magbias : %f, %f, %f, %f \r\n", magbias[0], magbias[1], magbias[2], magbias[3]); |
NaotoMorita | 70:99f974d8960e | 104 | pc.serial.printf("Acc Scale and Mag Calibration Start\r\n"); |
NaotoMorita | 70:99f974d8960e | 105 | for(int i = 0;i<3;i++){ |
NaotoMorita | 70:99f974d8960e | 106 | accMin[i] = -1.0f; |
NaotoMorita | 70:99f974d8960e | 107 | accMax[i] = 1.0f; |
NaotoMorita | 70:99f974d8960e | 108 | } |
cocorlow | 56:888379912f81 | 109 | |
NaotoMorita | 70:99f974d8960e | 110 | accMax[2] = accScaleCalibrate(5); |
NaotoMorita | 70:99f974d8960e | 111 | accMin[0] = accScaleCalibrate(1); |
NaotoMorita | 76:7fd3ac1afe3e | 112 | accMax[0] = accScaleCalibrate(2); |
NaotoMorita | 76:7fd3ac1afe3e | 113 | accMin[1] = accScaleCalibrate(3); |
NaotoMorita | 76:7fd3ac1afe3e | 114 | accMax[1] = accScaleCalibrate(4); |
NaotoMorita | 76:7fd3ac1afe3e | 115 | accMin[2] = accScaleCalibrate(6); |
NaotoMorita | 70:99f974d8960e | 116 | |
NaotoMorita | 76:7fd3ac1afe3e | 117 | /* |
NaotoMorita | 61:c05353850017 | 118 | magCalibrator.getExtremes(magbiasMin,magbiasMax); |
NaotoMorita | 65:ea184054e659 | 119 | pc.serial.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); |
NaotoMorita | 65:ea184054e659 | 120 | pc.serial.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); |
NaotoMorita | 68:b9f6938fab9d | 121 | magCalibrator.setExtremes(magbiasMin,magbiasMax); |
NaotoMorita | 76:7fd3ac1afe3e | 122 | */ |
NaotoMorita | 76:7fd3ac1afe3e | 123 | pc.serial.printf("Magbias : %f, %f, %f, %f \r\n", magbias[0], magbias[1], magbias[2], magbias[3]); |
NaotoMorita | 70:99f974d8960e | 124 | pc.serial.printf("accMin : %f, %f, %f\r\n", accMin[0], accMin[1], accMin[2]); |
NaotoMorita | 70:99f974d8960e | 125 | pc.serial.printf("accMax : %f, %f, %f\r\n", accMax[0], accMax[1], accMax[2]); |
NaotoMorita | 70:99f974d8960e | 126 | |
NaotoMorita | 70:99f974d8960e | 127 | pc.serial.printf("Keep Level \r\n"); |
NaotoMorita | 70:99f974d8960e | 128 | wait(5); |
NaotoMorita | 70:99f974d8960e | 129 | |
NaotoMorita | 65:ea184054e659 | 130 | pc.serial.printf("Calculating pitch/roll Offset \r\n"); |
cocorlow | 56:888379912f81 | 131 | //姿勢オフセットを計算 |
cocorlow | 56:888379912f81 | 132 | rpy_align.y = 0.0f*M_PI/180.0f; |
cocorlow | 56:888379912f81 | 133 | rpy_align.x = 0.0f*M_PI/180.0f; |
cocorlow | 56:888379912f81 | 134 | float ave_pitch = 0.0f; |
cocorlow | 56:888379912f81 | 135 | float ave_roll = 0.0f; |
NaotoMorita | 76:7fd3ac1afe3e | 136 | ekf.setQgbias(0.0f); |
NaotoMorita | 76:7fd3ac1afe3e | 137 | ekf.setQab(0.0f); |
NaotoMorita | 68:b9f6938fab9d | 138 | getIMUval(); |
NaotoMorita | 76:7fd3ac1afe3e | 139 | //ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm()); |
cocorlow | 56:888379912f81 | 140 | Timer _t; |
cocorlow | 56:888379912f81 | 141 | _t.start(); |
cocorlow | 56:888379912f81 | 142 | for (int i = 0 ; i < 2200; i++) |
cocorlow | 56:888379912f81 | 143 | { |
cocorlow | 56:888379912f81 | 144 | float tstart = _t.read(); |
cocorlow | 56:888379912f81 | 145 | //姿勢角を更新 |
cocorlow | 56:888379912f81 | 146 | getIMUval(); |
NaotoMorita | 68:b9f6938fab9d | 147 | ekf.updateQhat(gyro, att_dt); |
NaotoMorita | 68:b9f6938fab9d | 148 | ekf.updateErrState(gyro, att_dt); |
NaotoMorita | 68:b9f6938fab9d | 149 | ekf.updateStaticAccMeasures(acc,accref); |
NaotoMorita | 68:b9f6938fab9d | 150 | ekf.fuseErr2Qhat(); |
NaotoMorita | 68:b9f6938fab9d | 151 | ekf.updateMagMeasures(mag); |
NaotoMorita | 68:b9f6938fab9d | 152 | ekf.computeAngles(rpy, rpy_align); |
cocorlow | 56:888379912f81 | 153 | if(i>199) |
cocorlow | 56:888379912f81 | 154 | { |
NaotoMorita | 70:99f974d8960e | 155 | ave_pitch += rpy.x; |
NaotoMorita | 70:99f974d8960e | 156 | ave_roll += rpy.y; |
cocorlow | 56:888379912f81 | 157 | } |
cocorlow | 56:888379912f81 | 158 | wait(0.001); |
cocorlow | 56:888379912f81 | 159 | float tend = _t.read(); |
cocorlow | 56:888379912f81 | 160 | att_dt = (tend-tstart); |
cocorlow | 56:888379912f81 | 161 | } |
cocorlow | 56:888379912f81 | 162 | |
NaotoMorita | 70:99f974d8960e | 163 | pc.serial.printf("aliginment data(rpy.x, rpy.y, rpy.z) : %ff*M_PI/180.0f, %ff*M_PI/180.0f, 0.0f*M_PI/180.0f\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI); |
NaotoMorita | 70:99f974d8960e | 164 | |
NaotoMorita | 70:99f974d8960e | 165 | |
NaotoMorita | 70:99f974d8960e | 166 | pc.serial.printf("Calibration Complete\r\n"); |
cocorlow | 56:888379912f81 | 167 | |
cocorlow | 56:888379912f81 | 168 | while(1) |
cocorlow | 56:888379912f81 | 169 | { |
cocorlow | 56:888379912f81 | 170 | wait(1000); |
cocorlow | 56:888379912f81 | 171 | } |
NaotoMorita | 70:99f974d8960e | 172 | } |
NaotoMorita | 70:99f974d8960e | 173 | |
NaotoMorita | 70:99f974d8960e | 174 | float accScaleCalibrate(int attNo) |
NaotoMorita | 70:99f974d8960e | 175 | { |
NaotoMorita | 70:99f974d8960e | 176 | //attNo 1:Right down (acc.x Negative) |
NaotoMorita | 70:99f974d8960e | 177 | //attNo 2:Left down (acc.x Positive) |
NaotoMorita | 70:99f974d8960e | 178 | //attNo 3:Nose down (acc.y Negative) |
NaotoMorita | 70:99f974d8960e | 179 | //attNo 4:Tail down (acc.y Positive) |
NaotoMorita | 70:99f974d8960e | 180 | //attNo 5:Level (acc.z Positive) |
NaotoMorita | 70:99f974d8960e | 181 | //attNo 6:upside down (acc.z Negative) |
NaotoMorita | 70:99f974d8960e | 182 | //acc scale calibration |
NaotoMorita | 70:99f974d8960e | 183 | switch(attNo){ |
NaotoMorita | 70:99f974d8960e | 184 | case 1: |
NaotoMorita | 70:99f974d8960e | 185 | pc.serial.printf("Right down (acc.x Negative)\r\n"); |
NaotoMorita | 70:99f974d8960e | 186 | break; |
NaotoMorita | 70:99f974d8960e | 187 | case 2: |
NaotoMorita | 70:99f974d8960e | 188 | pc.serial.printf("Left down (acc.x Positive)\r\n"); |
NaotoMorita | 70:99f974d8960e | 189 | break; |
NaotoMorita | 70:99f974d8960e | 190 | case 3: |
NaotoMorita | 70:99f974d8960e | 191 | pc.serial.printf("Nose down (acc.y Negative)\r\n"); |
NaotoMorita | 70:99f974d8960e | 192 | break; |
NaotoMorita | 70:99f974d8960e | 193 | case 4: |
NaotoMorita | 70:99f974d8960e | 194 | pc.serial.printf("Tail down (acc.y Positive)\r\n"); |
NaotoMorita | 70:99f974d8960e | 195 | break; |
NaotoMorita | 70:99f974d8960e | 196 | case 5: |
NaotoMorita | 70:99f974d8960e | 197 | pc.serial.printf("Level (acc.z Positive)\r\n"); |
NaotoMorita | 70:99f974d8960e | 198 | break; |
NaotoMorita | 70:99f974d8960e | 199 | case 6: |
NaotoMorita | 70:99f974d8960e | 200 | pc.serial.printf("Upside down (acc.z Negative)\r\n"); |
NaotoMorita | 70:99f974d8960e | 201 | break; |
NaotoMorita | 70:99f974d8960e | 202 | default : |
NaotoMorita | 70:99f974d8960e | 203 | pc.serial.printf("error"); |
NaotoMorita | 70:99f974d8960e | 204 | break; |
NaotoMorita | 70:99f974d8960e | 205 | } |
NaotoMorita | 70:99f974d8960e | 206 | |
NaotoMorita | 70:99f974d8960e | 207 | while(1){ |
NaotoMorita | 70:99f974d8960e | 208 | double accx = 0.0; |
NaotoMorita | 70:99f974d8960e | 209 | double accy = 0.0; |
NaotoMorita | 70:99f974d8960e | 210 | double accz = 0.0; |
NaotoMorita | 70:99f974d8960e | 211 | for(int i = 0;i<100 ;i++) |
NaotoMorita | 70:99f974d8960e | 212 | { |
NaotoMorita | 70:99f974d8960e | 213 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
NaotoMorita | 70:99f974d8960e | 214 | accx += ax/ ACCEL_SSF; |
NaotoMorita | 70:99f974d8960e | 215 | accy += ay/ ACCEL_SSF; |
NaotoMorita | 70:99f974d8960e | 216 | accz += az/ ACCEL_SSF; |
NaotoMorita | 70:99f974d8960e | 217 | wait(0.01); |
NaotoMorita | 70:99f974d8960e | 218 | } |
NaotoMorita | 70:99f974d8960e | 219 | bool breakFlag = false; |
NaotoMorita | 70:99f974d8960e | 220 | switch(attNo){ |
NaotoMorita | 70:99f974d8960e | 221 | case 1: |
NaotoMorita | 70:99f974d8960e | 222 | if(abs(accx/100.0+1.0)<0.1){ |
NaotoMorita | 70:99f974d8960e | 223 | breakFlag = true; |
NaotoMorita | 70:99f974d8960e | 224 | }; |
NaotoMorita | 70:99f974d8960e | 225 | break; |
NaotoMorita | 70:99f974d8960e | 226 | case 2: |
NaotoMorita | 70:99f974d8960e | 227 | if(abs(accx/100.0-1.0)<0.1){ |
NaotoMorita | 70:99f974d8960e | 228 | breakFlag = true; |
NaotoMorita | 70:99f974d8960e | 229 | }; |
NaotoMorita | 70:99f974d8960e | 230 | break; |
NaotoMorita | 70:99f974d8960e | 231 | case 3: |
NaotoMorita | 70:99f974d8960e | 232 | if(abs(accy/100.0+1.0)<0.1){ |
NaotoMorita | 70:99f974d8960e | 233 | breakFlag = true; |
NaotoMorita | 70:99f974d8960e | 234 | }; |
NaotoMorita | 70:99f974d8960e | 235 | break; |
NaotoMorita | 70:99f974d8960e | 236 | case 4: |
NaotoMorita | 70:99f974d8960e | 237 | if(abs(accy/100.0-1.0)<0.1){ |
NaotoMorita | 70:99f974d8960e | 238 | breakFlag = true; |
NaotoMorita | 70:99f974d8960e | 239 | }; |
NaotoMorita | 70:99f974d8960e | 240 | break; |
NaotoMorita | 70:99f974d8960e | 241 | case 5: |
NaotoMorita | 70:99f974d8960e | 242 | if(abs(accz/100.0-1.0)<0.1){ |
NaotoMorita | 70:99f974d8960e | 243 | breakFlag = true; |
NaotoMorita | 70:99f974d8960e | 244 | }; |
NaotoMorita | 70:99f974d8960e | 245 | break; |
NaotoMorita | 70:99f974d8960e | 246 | case 6: |
NaotoMorita | 70:99f974d8960e | 247 | if(abs(accz/100.0+1.0)<0.1){ |
NaotoMorita | 70:99f974d8960e | 248 | breakFlag = true; |
NaotoMorita | 70:99f974d8960e | 249 | }; |
NaotoMorita | 70:99f974d8960e | 250 | break; |
NaotoMorita | 70:99f974d8960e | 251 | } |
NaotoMorita | 70:99f974d8960e | 252 | if(breakFlag){break;}; |
NaotoMorita | 70:99f974d8960e | 253 | pc.serial.printf("acc %f %f %f\r\n", accx/100.0,accy/100.0,accz/100.0); |
NaotoMorita | 70:99f974d8960e | 254 | } |
NaotoMorita | 70:99f974d8960e | 255 | pc.serial.printf("Keep it hold\r\n"); |
NaotoMorita | 70:99f974d8960e | 256 | int iter_n = 1000; |
NaotoMorita | 70:99f974d8960e | 257 | double accx = 0.0; |
NaotoMorita | 70:99f974d8960e | 258 | double accy = 0.0; |
NaotoMorita | 70:99f974d8960e | 259 | double accz = 0.0; |
NaotoMorita | 76:7fd3ac1afe3e | 260 | float magval[3] = {0,0,0}; |
NaotoMorita | 70:99f974d8960e | 261 | for(int i = 0;i<iter_n ;i++) |
NaotoMorita | 70:99f974d8960e | 262 | { |
NaotoMorita | 70:99f974d8960e | 263 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
NaotoMorita | 70:99f974d8960e | 264 | accx += ax/ ACCEL_SSF; |
NaotoMorita | 70:99f974d8960e | 265 | accy += ay/ ACCEL_SSF; |
NaotoMorita | 70:99f974d8960e | 266 | accz += az/ ACCEL_SSF; |
NaotoMorita | 70:99f974d8960e | 267 | |
NaotoMorita | 70:99f974d8960e | 268 | mag_sensor.getAxis(mdata); // flush the magnetmeter |
NaotoMorita | 76:7fd3ac1afe3e | 269 | magval[0] = (mdata.x - magbias[0]); |
NaotoMorita | 76:7fd3ac1afe3e | 270 | magval[1] = (mdata.y - magbias[1]); |
NaotoMorita | 76:7fd3ac1afe3e | 271 | magval[2] = (mdata.z - magbias[2]); |
NaotoMorita | 76:7fd3ac1afe3e | 272 | float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2]; |
NaotoMorita | 76:7fd3ac1afe3e | 273 | float lr = 0.00001f; |
NaotoMorita | 76:7fd3ac1afe3e | 274 | float f = mag_r - magbias[3]*magbias[3]; |
NaotoMorita | 76:7fd3ac1afe3e | 275 | magbias[0] = magbias[0] + 4 * lr * f * magval[0]; |
NaotoMorita | 76:7fd3ac1afe3e | 276 | magbias[1] = magbias[1] + 4 * lr * f * magval[1]; |
NaotoMorita | 76:7fd3ac1afe3e | 277 | magbias[2] = magbias[2] + 4 * lr * f * magval[2]; |
NaotoMorita | 76:7fd3ac1afe3e | 278 | magbias[3] = magbias[3] + 4 * lr * f * magbias[3]; |
NaotoMorita | 70:99f974d8960e | 279 | wait(0.001); |
NaotoMorita | 70:99f974d8960e | 280 | } |
NaotoMorita | 70:99f974d8960e | 281 | float returnval = 0.0f; |
NaotoMorita | 70:99f974d8960e | 282 | switch(attNo){ |
NaotoMorita | 70:99f974d8960e | 283 | case 1: |
NaotoMorita | 70:99f974d8960e | 284 | returnval = accx/1000.0f; |
NaotoMorita | 70:99f974d8960e | 285 | break; |
NaotoMorita | 70:99f974d8960e | 286 | case 2: |
NaotoMorita | 70:99f974d8960e | 287 | returnval = accx/1000.0f; |
NaotoMorita | 70:99f974d8960e | 288 | break; |
NaotoMorita | 70:99f974d8960e | 289 | case 3: |
NaotoMorita | 70:99f974d8960e | 290 | returnval = accy/1000.0f; |
NaotoMorita | 70:99f974d8960e | 291 | break; |
NaotoMorita | 70:99f974d8960e | 292 | case 4: |
NaotoMorita | 70:99f974d8960e | 293 | returnval = accy/1000.0f; |
NaotoMorita | 70:99f974d8960e | 294 | break; |
NaotoMorita | 70:99f974d8960e | 295 | case 5: |
NaotoMorita | 70:99f974d8960e | 296 | returnval = accz/1000.0f; |
NaotoMorita | 70:99f974d8960e | 297 | break; |
NaotoMorita | 70:99f974d8960e | 298 | case 6: |
NaotoMorita | 70:99f974d8960e | 299 | returnval = accz/1000.0f; |
NaotoMorita | 70:99f974d8960e | 300 | break; |
NaotoMorita | 70:99f974d8960e | 301 | } |
NaotoMorita | 70:99f974d8960e | 302 | |
NaotoMorita | 70:99f974d8960e | 303 | return returnval; |
cocorlow | 56:888379912f81 | 304 | } |