HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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

Diff: servo.cpp

Revision:
76:7fd3ac1afe3e
Parent:
73:84ffa0166e6c
Child:
77:2bf856e3eca4
--- a/servo.cpp	Tue Jul 20 11:57:05 2021 +0000
+++ b/servo.cpp	Sat Aug 07 06:54:58 2021 +0000
@@ -3,10 +3,15 @@
 // 割り込まれた時点での出力(computeの結果）を返す関数
 void calcServoOut()
 {
-    // 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
+    if(serialControlSource){
+        for(int i = 0;i<4;i++){
+            rc[i] = float(vp.actData[i])/1000.0f;
+        }
+    }else{
+        // 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
+        }
     }
     
     //rc[1] = joystick.getX();