Update : IMU Serial logging 및 monitoring을 위한 소스

+9513
\ No newline at end of file

 import CoDrone
 import CoDrone
+import math
+
+ac_x = 0
+ac_y = 0
+ac_z = 0
+normal_x = 0 
+normal_y = 0 
+normal_z = 0
+deltha_x = [0,0,0]
+deltha_y = [0,0,0]
+deltha_z = [0,0,0]
+deltha = 0
+
+def map(x,input_min,input_max,output_min,output_max):
+    return (x-input_min)*(output_max-output_min)/(input_max-input_min)+output_min
+
+
 
 
 def main():
 def main():
     drone = CoDrone.CoDrone()
     drone = CoDrone.CoDrone()
@@ -6,8 +23,67 @@ def main():
 
 
     # print the acceleration of drone
     # print the acceleration of drone
     while 1:
     while 1:
+        normal_x = 0 
+        normal_y = 0 
+        normal_z = 0
+  
+        for i in range(3):
+            deltha_x[i] = 0
+            deltha_y[i] = 0 
+            deltha_z[i] = 0 
+            angle = 0
+            angle_value = 0
+       
+        
+
+        for i in range(4):
+            acceleration = drone.get_accelerometer()
+            normal_x = 0 
+            normal_y = 0 
+            normal_z = 0
+            ac_x = acceleration.X
+            ac_y = acceleration.Y
+            ac_z = acceleration.Z
+
+            normal_x = map(ac_x, -32768,32767,-5000,5000)
+            normal_y = map(ac_y, -32768,32767,-5000,5000)
+            normal_z = map(ac_z, -32768,32767,-5000,5000)
+            deltha_x[1] = deltha_x[1]+(normal_x)
+            deltha_y[1] = deltha_y[1]+(normal_y) 
+            deltha_z[1] = deltha_z[1]+(normal_z)
+        
+            deltha_x[1] = int(deltha_x[1]/4)
+            deltha_y[1] = int(deltha_y[1]/4) 
+            deltha_z[1] = int(deltha_z[1]/4)
+
+
+
+        for i in range(4):
             acceleration = drone.get_accelerometer()
             acceleration = drone.get_accelerometer()
-        print(acceleration.X, acceleration.Y, acceleration.Z)
+            normal_x = 0 
+            normal_y = 0 
+            normal_z = 0
+            ac_x = acceleration.X
+            ac_y = acceleration.Y
+            ac_z = acceleration.Z
+
+            normal_x = map(ac_x, -32768,32767,-5000,5000)
+            normal_y = map(ac_y, -32768,32767,-5000,5000)
+            normal_z = map(ac_z, -32768,32767,-5000,5000)
+            deltha_x[2] = deltha_x[2]+(normal_x)
+            deltha_y[2] = deltha_y[2]+(normal_y) 
+            deltha_z[2] = deltha_z[2]+(normal_z)
+
+        deltha_x[2] = int(deltha_x[2]/4)
+        deltha_y[2] = int(deltha_y[2]/4) 
+        deltha_z[2] = int(deltha_z[2]/4)
+
+        deltha_x[0] = abs(deltha_x[1]-deltha_x[2]) 
+        deltha_y[0] = abs(deltha_y[1]-deltha_y[2]) 
+        deltha_z[0] = abs(deltha_z[1]-deltha_z[2])
+        deltha = deltha_x[0] + deltha_y[0] + deltha_z[0]
+        print("AcX : ", deltha_x[0], "AcY : ", deltha_y[0], "AcZ : ", deltha_z[0] - 230, "Total : ", deltha)
+
         
         
 if __name__ == '__main__':
 if __name__ == '__main__':
     main()
     main()