Diff: servo.cpp

Revision:

56:888379912f81

Child:

57:923e3df16159

diff -r b11c9fef094a -r 888379912f81 servo.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo.cpp	Mon May 31 18:59:36 2021 +0000
@@ -0,0 +1,69 @@
+#include "global.hpp"
+
+// 割り込まれた時点での出力(computeの結果）を返す関数
+void calcServoOut()
+{
+    if(sbus.failSafe == false)
+    {
+        // sbusデータの読み込み
+        for (int i = 0; i < 16;i ++)
+        {
+            rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
+        }
+    }
+    pitchPID.setGain(6.36, 10.6,0.0);
+    pitchratePID.setGain(0.9540,0.0,0.0);
+    pitchPID.setProcessValue(rpy.y);
+    pitchratePID.setProcessValue(gyro.y);
+    float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
+    
+    scaledServoOut[0]=de;
+    
+    float LP_servo = 0.2;
+    for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
+    {
+        servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
+        if(servoOut[i]<servoPwmMin)
+        {
+            servoOut[i] = servoPwmMin;
+        }
+        if(servoOut[i]>servoPwmMax)
+        {
+            servoOut[i] = servoPwmMax;
+        }
+    }
+    
+    servo.pulsewidth_us(servoOut[0]);
+     
+    //観測アップデート
+    dynacc = ekf.calcDynAcc(LPacc, accref);
+    th_mg = abs(acos(LPmag/LPmag.Norm() % LPacc/LPacc.Norm())-val_thmg);
+    
+    accnormerr = abs(LPacc.Norm()-accref.Norm());
+    //静止時100個の平均　0.01877522 0.00514146  0.00477393
+
+    //int ang_th  = th_mg < 0.01877522;
+    //int dyn_th = dynaccnorm < 0.00514146;
+    //int norm_th =  accnormerr< 0.00477393;
+    int ang_th  = th_mg < 0.01877522/5.0;
+    int dyn_th = dynacc.Norm() < 0.00514146/5.0;
+    int norm_th =  accnormerr< 0.00477393/5.0;
+    if(dyn_th+ang_th+norm_th>0)
+    {
+    //if(dyn_th+ang_th+norm_th>-1){
+            obs_count += 1;
+            ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
+            ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
+    } 
+     
+    if(loop_count >= 10)
+    {
+        writeSdcard();
+        loop_count = 1;
+        obs_count = 0;
+    }
+    else
+    {
+        loop_count +=1;
+    }
+}
\ No newline at end of file