File content as of revision 0:5ed58f4ef4fb:

#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;
}