Qcal.cpp

00001 #include "mbed.h"
00002 #include <math.h>
00003 #include "Qcal.h"/*
00004 #define float lat1 ;//緯度一度あたりの距離(m)
00005 #define float lon1 ;//経度一度あたりの距離(m)
00006   
00007 #define float X ;//現在地−ゴール間の経度方向距離(m)
00008 #define float Y ;//現在地−ゴール間の緯度方向距離(m)
00009   
00010 #define float X2 ;//現在地−ゴール間の経度方向距離(m)
00011 #define float Y2 ;//現在地−ゴール間の緯度方向距離(m)
00012 
00013 #define float pan ;
00014 #define float dan ;
00015         
00016 #define float an ;//理想回転角
00017 */
00018 
00019 
00020 QCal::QCal(float gn,float ge,float lat1,float lon1){
00021 
00022         float X;//現在地−ゴール間の経度方向距離(m)
00023         float Y;//現在地−ゴール間の緯度方向距離(m)
00024   
00025         float X2;//現在地−ゴール間の経度方向距離(m)
00026         float Y2;//現在地−ゴール間の緯度方向距離(m)
00027 
00028         float pan;
00029         float dan;
00030         
00031         float an;//理想回転角
00032 }
00033 
00034 float QCal::QCalculate(float X,float Y,float mx,float my){
00035     float PI = 3.1416;
00036     pan = (atan2(X,Y)*180)/PI;//位置角(°)panを定義
00037     dan = (atan2(mx,my)*180)/PI;//方位角(°)danを定義
00038 
00039     if(fabs(180-(pan-dan))-180>0){
00040     an = (180-fabs(pan-dan));
00041     }
00042     
00043     else if(fabs(180-(pan-dan))-180<0){
00044     an = fabs(pan-dan)-180;
00045     }
00046 
00047     return an;
00048 }
00049 
00050 float QCal::QCalculateRed(float X,float Y,float X2,float Y2){
00051     float PI = 3.1416;
00052     pan = (atan2(X2,Y2)*180)/PI;//位置角(°)panを定義
00053     dan = (atan2(X2-X,Y2-Y)*180)/PI;//方位角(°)danを定義
00054     
00055     if(fabs(180-(pan-dan))-180>0){
00056     an = (180-fabs(pan-dan));
00057     }
00058     
00059     else if(fabs(180-(pan-dan))-180<0){
00060     an = fabs(pan-dan)-180;
00061     }    
00062     
00063     return an;
00064 }
00065     
00066