Diff: Qcal.cpp

Revision:

0:5ed58f4ef4fb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Qcal.cpp	Thu Dec 07 08:53:42 2017 +0000
@@ -0,0 +1,66 @@
+#include "mbed.h"
+#include <math.h>
+#include "Qcal.h"/*
+#define float lat1 ;//緯度一度あたりの距離(m)
+#define float lon1 ;//経度一度あたりの距離(m)
+  
+#define float X ;//現在地−ゴール間の経度方向距離(m)
+#define float Y ;//現在地−ゴール間の緯度方向距離(m)
+  
+#define float X2 ;//現在地−ゴール間の経度方向距離(m)
+#define float Y2 ;//現在地−ゴール間の緯度方向距離(m)
+
+#define float pan ;
+#define float dan ;
+        
+#define float an ;//理想回転角
+*/
+
+
+QCal::QCal(float gn,float ge,float lat1,float lon1){
+
+        float X;//現在地−ゴール間の経度方向距離(m)
+        float Y;//現在地−ゴール間の緯度方向距離(m)
+  
+        float X2;//現在地−ゴール間の経度方向距離(m)
+        float Y2;//現在地−ゴール間の緯度方向距離(m)
+
+        float pan;
+        float dan;
+        
+        float an;//理想回転角
+}
+
+float QCal::QCalculate(float X,float Y,float mx,float my){
+    float PI = 3.1416;
+    pan = (atan2(X,Y)*180)/PI;//位置角(°)panを定義
+    dan = (atan2(mx,my)*180)/PI;//方位角(°)danを定義
+
+    if(fabs(180-(pan-dan))-180>0){
+    an = (180-fabs(pan-dan));
+    }
+    
+    else if(fabs(180-(pan-dan))-180<0){
+    an = fabs(pan-dan)-180;
+    }
+
+    return an;
+}
+
+float QCal::QCalculateRed(float X,float Y,float X2,float Y2){
+    float PI = 3.1416;
+    pan = (atan2(X2,Y2)*180)/PI;//位置角(°)panを定義
+    dan = (atan2(X2-X,Y2-Y)*180)/PI;//方位角(°)danを定義
+    
+    if(fabs(180-(pan-dan))-180>0){
+    an = (180-fabs(pan-dan));
+    }
+    
+    else if(fabs(180-(pan-dan))-180<0){
+    an = fabs(pan-dan)-180;
+    }    
+    
+    return an;
+}
+    
+