solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
servo.cpp@56:888379912f81, 2021-05-31 (annotated)
- Committer:
- cocorlow
- Date:
- Mon May 31 18:59:36 2021 +0000
- Revision:
- 56:888379912f81
- Child:
- 57:923e3df16159
file divided
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 | // 割り込まれた時点での出力(computeの結果)を返す関数 |
cocorlow | 56:888379912f81 | 4 | void calcServoOut() |
cocorlow | 56:888379912f81 | 5 | { |
cocorlow | 56:888379912f81 | 6 | if(sbus.failSafe == false) |
cocorlow | 56:888379912f81 | 7 | { |
cocorlow | 56:888379912f81 | 8 | // sbusデータの読み込み |
cocorlow | 56:888379912f81 | 9 | for (int i = 0; i < 16;i ++) |
cocorlow | 56:888379912f81 | 10 | { |
cocorlow | 56:888379912f81 | 11 | rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input |
cocorlow | 56:888379912f81 | 12 | } |
cocorlow | 56:888379912f81 | 13 | } |
cocorlow | 56:888379912f81 | 14 | pitchPID.setGain(6.36, 10.6,0.0); |
cocorlow | 56:888379912f81 | 15 | pitchratePID.setGain(0.9540,0.0,0.0); |
cocorlow | 56:888379912f81 | 16 | pitchPID.setProcessValue(rpy.y); |
cocorlow | 56:888379912f81 | 17 | pitchratePID.setProcessValue(gyro.y); |
cocorlow | 56:888379912f81 | 18 | float de = pitchPID.compute()+pitchratePID.compute()-rc[1]; |
cocorlow | 56:888379912f81 | 19 | |
cocorlow | 56:888379912f81 | 20 | scaledServoOut[0]=de; |
cocorlow | 56:888379912f81 | 21 | |
cocorlow | 56:888379912f81 | 22 | float LP_servo = 0.2; |
cocorlow | 56:888379912f81 | 23 | for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++) |
cocorlow | 56:888379912f81 | 24 | { |
cocorlow | 56:888379912f81 | 25 | servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i]; |
cocorlow | 56:888379912f81 | 26 | if(servoOut[i]<servoPwmMin) |
cocorlow | 56:888379912f81 | 27 | { |
cocorlow | 56:888379912f81 | 28 | servoOut[i] = servoPwmMin; |
cocorlow | 56:888379912f81 | 29 | } |
cocorlow | 56:888379912f81 | 30 | if(servoOut[i]>servoPwmMax) |
cocorlow | 56:888379912f81 | 31 | { |
cocorlow | 56:888379912f81 | 32 | servoOut[i] = servoPwmMax; |
cocorlow | 56:888379912f81 | 33 | } |
cocorlow | 56:888379912f81 | 34 | } |
cocorlow | 56:888379912f81 | 35 | |
cocorlow | 56:888379912f81 | 36 | servo.pulsewidth_us(servoOut[0]); |
cocorlow | 56:888379912f81 | 37 | |
cocorlow | 56:888379912f81 | 38 | //観測アップデート |
cocorlow | 56:888379912f81 | 39 | dynacc = ekf.calcDynAcc(LPacc, accref); |
cocorlow | 56:888379912f81 | 40 | th_mg = abs(acos(LPmag/LPmag.Norm() % LPacc/LPacc.Norm())-val_thmg); |
cocorlow | 56:888379912f81 | 41 | |
cocorlow | 56:888379912f81 | 42 | accnormerr = abs(LPacc.Norm()-accref.Norm()); |
cocorlow | 56:888379912f81 | 43 | //静止時100個の平均 0.01877522 0.00514146 0.00477393 |
cocorlow | 56:888379912f81 | 44 | |
cocorlow | 56:888379912f81 | 45 | //int ang_th = th_mg < 0.01877522; |
cocorlow | 56:888379912f81 | 46 | //int dyn_th = dynaccnorm < 0.00514146; |
cocorlow | 56:888379912f81 | 47 | //int norm_th = accnormerr< 0.00477393; |
cocorlow | 56:888379912f81 | 48 | int ang_th = th_mg < 0.01877522/5.0; |
cocorlow | 56:888379912f81 | 49 | int dyn_th = dynacc.Norm() < 0.00514146/5.0; |
cocorlow | 56:888379912f81 | 50 | int norm_th = accnormerr< 0.00477393/5.0; |
cocorlow | 56:888379912f81 | 51 | if(dyn_th+ang_th+norm_th>0) |
cocorlow | 56:888379912f81 | 52 | { |
cocorlow | 56:888379912f81 | 53 | //if(dyn_th+ang_th+norm_th>-1){ |
cocorlow | 56:888379912f81 | 54 | obs_count += 1; |
cocorlow | 56:888379912f81 | 55 | ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag); |
cocorlow | 56:888379912f81 | 56 | ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc); |
cocorlow | 56:888379912f81 | 57 | } |
cocorlow | 56:888379912f81 | 58 | |
cocorlow | 56:888379912f81 | 59 | if(loop_count >= 10) |
cocorlow | 56:888379912f81 | 60 | { |
cocorlow | 56:888379912f81 | 61 | writeSdcard(); |
cocorlow | 56:888379912f81 | 62 | loop_count = 1; |
cocorlow | 56:888379912f81 | 63 | obs_count = 0; |
cocorlow | 56:888379912f81 | 64 | } |
cocorlow | 56:888379912f81 | 65 | else |
cocorlow | 56:888379912f81 | 66 | { |
cocorlow | 56:888379912f81 | 67 | loop_count +=1; |
cocorlow | 56:888379912f81 | 68 | } |
cocorlow | 56:888379912f81 | 69 | } |