HAPSRG / Mbed 2 deprecated HAPStail

Dependencies:   mbed MatrixMath LPS25HB_I2C LSM9DS1 Matrix2 PIDcontroller LoopTicker SBUS_without_mainfile UsaPack solaESKF_wind Vector3 CalibrateMagneto FastPWM

Revision:
88:0fc5df2fddcb
Parent:
87:981895e1d4f2
Child:
107:78e6f7bee68e
--- a/servo.cpp	Fri Jan 07 09:49:19 2022 +0000
+++ b/servo.cpp	Wed Jan 12 07:00:08 2022 +0000
@@ -19,6 +19,16 @@
     rpy.y = euler(2,1);
     rpy.z = euler(3,1);
     
+    if (updateValues.calibrationFlag == 1111)
+    {
+        alignment();
+    }
+    else
+    {
+        rpy.x -= roll_offset;
+        rpy.y -= pitch_offset;
+    }
+    
     // 自身の位置に応じてエレベータ舵角を決定する
     float derc =  rc[1];
     float darc =  rc[0]-rc[1];