File content as of revision 0:fbc5f6dc768b:

#define target_lng 135
#define target_lat 35

float theta;
float alpha = 30; //閾値　定数のほうがいいの？

float targetX;
float targetY;


int step = 0;


/*--- 制御部分 ---*/

while(1) {
    switch(step){
    
    // 投下検出
    case '0':
        if(/*投下検出*/){

            /*滑空安定まで数秒待機*/

            step = 1;
            break;
        }


    // 制御ルーチン
    case '1':
        /* いずれも地球を完全球体と仮定 */

        /* 北から東回りで目標方向の角度を出力 */
        targetY = cos( target_lat ) * sin( target_lng - gms.longitude );
        targetX = cos( gms.latitude ) * sin( target_lat ) - sin( gms.latitude ) * cos( target_lat ) * cos( target_lng - gms.longitude );
        theta = atan2f( targetY, targetX ); 

        /* 南から東回りーπ〜＋πのθを算出できるように基底変換
        targetX = - ( target_lat - gms.latitude ) * DEG_TO_RAD;
        targetY =  ( target_lng - gms.longitude ) * DEG_TO_RAD;
        
        theta = atan2f( targetY, targetX ) + 3.14159265 // 北から西回り0〜2πの角度を算出
        */

        if(yaw < theta - alpha) { //目標は左方向

            /*左ひっぱる*/

        } else if (yaw > theta - alpha) { //目標は右方向

            /*右ひっぱる*/

        }


    }


}