Revision 1:12c5a8179ad2, committed 2021-03-18

Comitter:

Jumpei

Date:

Thu Mar 18 08:39:47 2021 +0000

Parent:

0:65f9be2858c9

Commit message:

for Tazawa

Changed in this revision

--- a/main.cpp	Wed Feb 22 08:59:47 2017 +0000
+++ b/main.cpp	Thu Mar 18 08:39:47 2021 +0000
@@ -1,3 +1,4 @@
+
 char buf1[0x4000]; // 通常エリア
 char buf2[0x4000] __attribute__((section("AHBSRAM0"))); // Ethernet用エリア (0x2007c000)
 
@@ -12,6 +13,10 @@
 #define B 6356752.314245
 #define e2 0.006694379
 #define m 6335439.327292
+#define p 0.3
+#define q 0.4
+#define hh 10
+#define h 250
 
 #define aa 11
 #define bb 21
@@ -19,6 +24,7 @@
 #define aa_ 12
 #define bb_ 22
 #define cc_ 32
+#define offset_head 0.0
 
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
@@ -31,28 +37,51 @@
 SPI spi(p5, p6, p7);
 mpu9250_spi imu(spi,p8);
 Serial sbus(p13, p14);
-Timer t, anglesT;
+Timer t, t2, anglesT;
 
 double gyro[3],angles_before[3],angles[3],houi;
-float m_min[] = {-35.921,   -33.910,   -41.263};
-float m_max[] = {41.938,    44.207,    40.922};
+
+/*float m_min[] = {-53.592,   -23.689,   -39.986};                                //2020/10.16
+float m_max[] = {39.651,    65.966,    55.875};*/
+/*float m_min[] = {-48.523,   -24.965,   -38.065};                                //2020/10.16
+float m_max[] = {38.384,    67.424,    56.224};//2020/10/21*/
+/*float m_min[] = {-53.049,   -24.783,   -37.366};                                //2020/10.16
+float m_max[] = {36.573,    73.437,    58.494};*///2020/10/30
+float m_min[] = {-54.316,   -11.550,   -16.275};                                
+float m_max[] = {40.375,    27.825,    26.325};//2020/11/11  //2/8 change
+/*float m_min[] = {-52.687,   -25,876   -34.398};                                
+float m_max[] = {34.581,    68.517,    59.367}*/;//2020/11/11
+/*float m_min[] = {-48.885,   -25.694,   -31.954};                                
+float m_max[] = {39.832,    68.517,    61.637};*///2020/11/27
+/*float m_min[] = {-51.238,   -26.605,   -42.430};                                
+float m_max[] = {32.047,    68.882,    69.495};*///2020/11/30
+/*float m_min[] = {-53.773,   -22.961,   -31.954};                                
+float m_max[] = {40.918,    30.775,    30.082};*///2020/11/11  //2/8 change
+
 float offsetM[3];
 int i = 0, j = 0, k = 0;
 double myLat_rad, myLong_rad, Latitude_rad, Longitude_rad;
 
-double sekkin = 0.0, sekkin_hani = 10.0;
+double sekkin = 20.0, sekkin_hani = 40.0;
 
 double Time, Lat, Long, heading;
 double Timef, Latf, Longf, Heading;
-double Lat_before, Latf_before, Long_before, Longf_before, Heading_before, distance_before;
+double Lat_before, Latf_before, Long_before, Longf_before, Heading_before, distance_before, heading_before;
 double distance, direction;
+double to_Long, to_Lat;
 
-double b_head, b_head_front;
+double b_head, b_head_front, c_head_front;
+double wp_distance, wp_direction, wp_head;
 double range;
+double heading0,heading1, heading2, heading3, heading4, heading5;
+double Time_4,Time_3,Time_2,Time_1;
+
 
 int rud = 1024, ele = 1024, ail = 1024;
 double ele_dis = 0.0, ail_dis = 0.0;
 int ele_range, ail_range;
+int ele_before = 1024,ail_before = 1024;                                        //change
+int count = 0;
 
 //int marker, marker_me;
 
@@ -328,15 +357,30 @@
     );
     wait(0.2);
     
-    //byte dev1[]={0x00,0x13,0xA2,0x00,0x40,0xAF,0x4F,0x4A};//1
-    byte dev2[]={0x00,0x13,0xA2,0x00,0x40,0xF9,0x87,0xC9};//2
-    byte dev3[]={0x00,0x13,0xA2,0x00,0x40,0xF9,0x88,0x91};//3*/
+    //byte dev1[]={0x00,0x13,0xA2,0x00,0x40,0xAF,0x4F,0x4A};//1                   //高木さん設定
+    //byte dev2[]={0x00,0x13,0xA2,0x00,0x40,0xF9,0x87,0xC9};//2
+    //byte dev3[]={0x00,0x13,0xA2,0x00,0x40,0xF9,0x88,0x91};//3
+    
+    byte dev1[]={0x00,0x13,0xA2,0x00,0x40,0xCA,0x9A,0xE1};//1                   //change
+    byte dev3[]={0x00,0x13,0xA2,0x00,0x40,0xCA,0x9A,0xC7};//2                 //菊池設定
+    //byte dev4[]={0x00,0x13,0xA2,0x00,0x41,0xB8,0xE5,0x9D};//2                 //菊池設定
     
     t.start();
     anglesT.start();
+    t2.start();
     //sbus.attach(SBUS, Serial::RxIrq);
     imu.Timer_start();
-       
+    
+    imu.AK8936_read_Orientation();
+    heading0 = imu.getdata[0];
+    myled2 = !myled2;
+    heading_before = heading0;
+    heading1 = heading0;
+    heading2 = heading0;
+    heading3 = heading0;
+    heading4 = heading0;
+    heading5 = heading0;
+    
     while(1){
         if(n == 500){
             myled1 = !myled1;
@@ -352,28 +396,55 @@
         byte discard, re;
         char a[5][13], b[5][13], c[13];
         
-        /*if(t.read_ms() > 200){
+        /*imu.AK8936_read_Orientation();
+        heading = imu.getdata[0];
+        //heading0 = imu.getdata[0];
+        myled2 = !myled2;*/
+            
+            /*heading = (heading0 + heading1 + heading2 + heading3 + heading4 + heading5)/6;  //移動平均
+            heading5 = heading4;
+            heading4 = heading3;
+            heading3 = heading2;
+            heading2 = heading1;
+            heading1 = heading0;*/
+                    
+            /*heading = (1 - s)*heading_before +s*heading;              //ローパスフィルタ
+            heading_before = heading;*/    
+        
+        if(t.read_ms() > 100){
             
             imu.AK8936_read_Orientation();
             heading = imu.getdata[0];
-            myled2 = !myled2;   
+            //heading0 = imu.getdata[0];
+            myled2 = !myled2;
+            
+            /*heading = heading + offset_head;                                  //赤いドローンに制御装置付けていた時に方位センサの向きが90°異なっていたため
+            if( heading > 360 ){
+                heading = heading - 360.0;
+            }*/
             
-            sprintf(s,"%02d%f,\r",aa_,heading);//1
+            /*heading = (heading0 + heading1 + heading2 + heading3 + heading4 + heading5)/6;  //移動平均
+            heading5 = heading4;
+            heading4 = heading3;
+            heading3 = heading2;
+            heading2 = heading1;
+            heading1 = heading0;*/
+                    
+            /*heading = (1 - p)*heading_before +p*heading;              //ローパスフィルタ
+            heading_before = heading;*/ 
+            
+            //sprintf(s,"%02d%f,\r",aa_,heading);//1
             //sprintf(s,"%02d%f,\r",bb_,heading);//2
             //sprintf(s,"%02d%f,\r",cc_,heading);//3
-                
-            //pc.printf("\n");
-            //pc.printf("MY ");
-            //pc.printf(s);
-                
-            //xbee_uart(dev1,s);
-            xbee_uart(dev2,s);
+            
+            //xbee_uart(dev1,s);  //by kikuchi
+            //xbee_uart(dev2,s);
             //xbee_uart(dev3,s);
             
-            myled3 = !myled3;
+            //myled3 = !myled3;
             
             t.reset();
-        }*/
+        }
         
         while(sGps.readable() > 0){
             if(sGps.getc() == '$'){
@@ -392,19 +463,6 @@
                         }
                     }
                                     
-                    /*pc.printf("\n");
-                    pc.printf(b[0]);
-                    pc.printf(":");
-                    pc.printf(b[1]);
-                    pc.printf(":");
-                    pc.printf(b[2]);
-                    pc.printf(":");
-                    pc.printf(b[3]);
-                    pc.printf(":");
-                    pc.printf(b[4]);
-                    pc.printf(":");
-                    pc.printf("%f",heading);*/
-                                        
                     Time = atof(b[0]);
                     Lat = atof(b[2]);
                     Long = atof(b[4]);
@@ -412,19 +470,28 @@
                     
                     imu.AK8936_read_Orientation();
                     heading = imu.getdata[0];
+                    //heading0 = imu.getdata[0];
                     myled2 = !myled2;
+                    
+                    /*heading = (heading0 + heading1 + heading2 + heading3 + heading4 + heading5)/6;  //移動平均
+                    heading5 = heading4;
+                    heading4 = heading3;
+                    heading3 = heading2;
+                    heading2 = heading1;
+                    heading1 = heading0;*/
+                    
+                    /*heading = (1 - p)*heading_before +p*heading0;              //ローパスフィルタ
+                    heading_before = heading;*/     
                                         
-                    sprintf(s,"%02d%9.2f,%10.5f,%11.5f,%9.5f,\r",aa,Time,Lat,Long,heading);//1
+                    sprintf(s,"%02d%9.2f,%10.5f,%11.5f,%9.5f,\r\n",bb,Time,Lat,Long,heading);//1
                     //sprintf(s,"%02d%9.2f,%10.5f,%11.5f,%9.5f,%04d,\r",aa,Time,Lat,Long,heading,marker_me);//1
                                         
-                    pc.printf("\n");
+                    /*pc.printf("\n");                                            //モニター用
                     pc.printf("MY ");
-                    pc.printf(s);
-                                        
-                    xbee_uart(dev2,s);
+                    pc.printf(s);*/
+                    
+                    xbee_uart(dev1,s);
                     //xbee_uart(dev3,s);
-                    //xbee_send_frame(dev2, s, 46);
-                    //xbee_send_frame(dev3, s, sizeof(s));
                     
                     t.reset();
                 }
@@ -432,41 +499,35 @@
         }
         
         while( xbee_readable() > 0 ){
-        //while( xbee.readable() > 0 ){
             if(sci_read(1) == 0x7E){
-            //if(xbee.getc() == 0x7E){
                 wait_us(200);
-                for(i = 0; i < 4; i++){
+                for(i = 0; i < 13; i++){
                     discard = sci_read(1);
-                    //discard = xbee.getc();
                     wait_us(200);
                 }
                 for(;;){
                     discard = sci_read(1);
-                    //discard = xbee.getc();
                     wait_us(200);
-                    //if(discard == 49) break;
-                    if(discard == 50) break;
+                    if(discard == 49) break;
+                    //if(discard == 50) break;
                     //if(discard == 51) break;
                 }
                 re = sci_read(1);
-                //re = xbee.getc();
                 wait_us(200);
-                if(re == 49){
-                    char No[] = {discard, re};
-                    int Noi = atoi(No);
-                    
-                    for(j = 0; j < 4; j++){
-                        for(k = 0; k < 13; k++){
-                            a[j][k] = sci_read(1);
-                            //a[j][k] = xbee.getc();
-                            wait_us(200);
-                            if(a[j][k] == ','){
-                                a[j][k] = '\0';
-                                break;
-                            }
+                 if(re == 49){
+                char No[] = {discard, re};
+                int Noi = atoi(No);
+                
+                for(j = 0; j < 4; j++){
+                    for(k = 0; k < 13; k++){
+                        a[j][k] = sci_read(1);
+                        wait_us(200);
+                        if(a[j][k] == ','){
+                            a[j][k] = '\0';
+                            break;
                         }
                     }
+                }
                     
                     /*for(j = 0; j < 5; j++){
                         for(k = 0; k < 13; k++){
@@ -486,7 +547,7 @@
                     Heading = atof(a[3]);
                     //marker = atoi(a[4]);
                     
-                    pc.printf("\n");
+                    /*pc.printf("\n");
                     if(Noi == 11){
                         pc.printf("A  ");
                     }else if(Noi == 21){
@@ -495,10 +556,10 @@
                         pc.printf("C  ");
                     }else{
                         pc.printf("%d ",Noi);
-                    }
+                    }*/
                 
-                    //pc.printf("Time = ");
-                    pc.printf("%f",Timef);
+                    //pc.printf("Time = ");                                     //モニター用
+                    /*pc.printf("%f",Timef);
                     //pc.printf(Time);
                     pc.printf(" : ");
                     //pc.printf("Latitude = ");
@@ -508,12 +569,18 @@
                     //pc.printf("Longitude = ");
                     pc.printf("%f",Longf);
                     //pc.printf(Long);
-                    pc.printf(" : ");
+                    pc.printf(" : ");*/
                     //pc.printf("Heading = ");
-                    pc.printf("%f",Heading);
+                    //pc.printf("%f",Heading);
                     //pc.printf(" : ");
                     //pc.printf("Marker = ");
                     //pc.printf("%d",marker);
+                    
+                    
+                    /*char dis[40];                                             //モニター用
+                    sprintf(dis,"dis=%5.2f,b_head=%9.5f\r\n",distance,b_head);
+                    printf(dis);*/
+                    
                 }
                 if(re == 50){
                     char No[] = {discard, re};
@@ -558,12 +625,8 @@
             pc.printf("%d\n",channels_to[5]);
         }else;
         
-        /*if(channels_to[5] == 1541 && marker != 2222){
-            marker = 2222;
-        }*/
-        
-        if((Lat_before != Lat) && (Latf_before != Latf)){
-            if((Lat != 0.0) && (Latf != 0.0) && (Long != 0.0) && (Longf != 0.0)){
+        //if((Lat_before != Lat) && (Latf_before != Latf)){
+              if((Lat != 0.0) && (Latf != 0.0) && (Long != 0.0) && (Longf != 0.0)){
                 Lat_before = Lat;
                 Latf_before = Latf;
                 Long_before = Long;
@@ -571,8 +634,11 @@
                 to_rad(Lat, Long, Latf, Longf);
                 distance = hubeni_distance(myLat_rad, myLong_rad, Latitude_rad, Longitude_rad);
                 direction = directions(myLat_rad, myLong_rad, Latitude_rad, Longitude_rad);
+                
+                /*char dis[40];
+                sprintf(dis,"dis=%5.2f,b_head=%9.5f\r\n",distance,b_head);
+                printf(dis);*/
             
-                //pc.printf("\nDistance = %10.3f  Direction = %10.3f\n",distance,direction);
             }else{
                 if(distance != -2.0){
                     //distance = -1.0;
@@ -591,28 +657,35 @@
                     //pc.printf("\n");
                     //pc.printf(s_to);
             
-                    char sta[40];
-                    sprintf(sta,"Time=%.2f, ail=%d, ele=%d\r",Time,ail,ele);
+                    //char sta[40];
+                    //sprintf(sta,"F:Time=%.2f, ail=%d, ele=%d\r",Time,ail,ele);
                     //pc.printf(sta);
-                    xbee_uart(dev3,sta);
+                    //xbee_uart(dev3,sta);
                     //xbee_send_frame(dev3, sta, sizeof(sta));
             
                     distance = -2.0;
                 }
             }
-        }
+        //}
         
-        //if((distance > sekkin) && (Heading_before != Heading) && (sbus.channel(6) != 1541) && (marker == 1111)){
-        //if((distance > sekkin) && (Heading_before != Heading) && (marker == 1111)){
-        if((distance > sekkin) && (Heading_before != Heading)){
-            
             Heading_before = Heading;
             
             imu.AK8936_read_Orientation();
             heading = imu.getdata[0];
+            //heading0 = imu.getdata[0];
             myled2 = !myled2;
             
-            b_head_front = Heading + (180.0 - heading); //先導機と方位を揃える
+            /*heading = (heading0 + heading1 + heading2 + heading3 + heading4 + heading5)/6;  //移動平均
+            heading5 = heading4;
+            heading4 = heading3;
+            heading3 = heading2;
+            heading2 = heading1;
+            heading1 = heading0;*/
+                    
+            /*heading = (1 - p)*heading_before +p*heading0;              //ローパスフィルタ
+            heading_before = heading;*/     
+            
+            /*b_head_front = Heading + (180.0 - heading); //先導機と方位を揃える
             if(b_head_front > 360.0){
                 b_head_front = b_head_front - 360.0;
             }else if(b_head_front < 0.0){
@@ -623,22 +696,81 @@
                 rud = map(b_head_front,180.0,360.0,1084,1384);
             }else{
                 rud = map(b_head_front,180.0,0.0,963,664);
+            }*/
+            
+            b_head_front = Heading + (180.0 - heading);                         //interaction mode:dir control           
+            b_head_front = b_head_front / 2;
+                                     
+            if(b_head_front > 180.0){
+                b_head_front = b_head_front - 180.0;
+            }else if(b_head_front < 0.0){
+                b_head_front = b_head_front + 180.0;
+            }else;
+            
+            if(b_head_front >= 90.0){
+                rud = map(b_head_front,90.0,180.0,1084,1200);                   //change
+            }else{
+                rud = map(b_head_front,90.0,0.0,963,848);
             }
             
-            
+            /*if(b_head_front >= 90.0){
+                rud = map(b_head_front,90.0,180.0,1084,1284);
+            }else{
+                rud = map(b_head_front,90.0,0.0,963,764);
+            }*/  
+                        
             b_head = direction + (180.0 - heading); //先導機に追従
             if(b_head > 360.0){
                 b_head = b_head - 360.0;
             }else if(b_head < 0.0){
                 b_head = b_head + 360.0;  
-            }else ;
+            }else 
+            
+            if(0 <= distance && distance <= 10.0 ){//---------------------------------------------反発制御
+                                                                                                                
+            //range = map(distance,0.0,10.0,400.0,100.0);                           //距離値 → 制御値 範囲変換 ( 距離1～距離2 → 制御値1～制御値2 )
+            range = map(distance,0.0,10.0,450.0,250.0); 
             
-            if(distance > sekkin + sekkin_hani){
-                distance = sekkin + sekkin_hani;
-            }
-            range = map(distance,sekkin,sekkin + sekkin_hani,150.0,660.0);
+            if(b_head >= 0.0 && b_head < 90.0){                                 //親機の方向ごとの反発制御値
+                ele = 1024 + (range * cos(map(b_head,0.0,90.0,0.0,(PI/2.0))));   //elevetor = ニュートラル値 + ( 制御量 × （0~90°のcos値) )
+                ail = 1024 + ((range*1.2) * sin(map(b_head,0.0,90.0,0.0,(PI/2.0))));  // aileron = ニュートラル値 + ( 制御量 × （0~90°のsin値) )
+            }else if(b_head >= 90.0 && b_head < 180.0){
+                ele = 1024 - (range * sin(map(b_head,90.0,180.0,0.0,(PI/2.0))));
+                ail = 1024 + ((range*1.2) * cos(map(b_head,90.0,180.0,0.0,(PI/2.0))));
+            }else if(b_head >= 180.0 && b_head < 270.0){
+                ele = 1024 - (range * cos(map(b_head,180.0,270.0,0.0,(PI/2.0))));
+                ail = 1024 - (range * sin(map(b_head,180.0,270.0,0.0,(PI/2.0))));      
+            }else{
+                ele = 1024 + (range * sin(map(b_head,270.0,360.0,0.0,(PI/2.0))));
+                ail = 1024 - (range * cos(map(b_head,270.0,360.0,0.0,(PI/2.0))));
+            } 
+                
+                /*ele = (1 - q)*ele_before + q*ele;              //ローパスフィルタ change
+                ele_before = ele; 
+                ail = (1 - q)*ail_before + q*ail;              //ローパスフィルタ
+                ail_before = ail;*/
             
+                ele = 1024;
+                ail = 1024;
+                //rud = 1024;
             
+            char sta[60];    
+            //sprintf(sta,"Blu=%-d,D=%-4.1f,h=%-5.1f,a=%4d,e=%4d\r\n",t2.read_ms(),distance,b_head,ail,ele);
+            //sprintf(sta,"Blu=%-7.1f,D=%-4.1f,h=%-5.1f,a=%4d,e=%4d\r\n",Time,distance,b_head,ail,ele);
+            sprintf(sta,"Blu=%-d,h=%-5.1f,r=%4d\r\n",t2.read_ms(),heading,rud);
+            //sprintf(sta,"Blu=%-7.1f,Dis=%-4.1f,head=%-5.1f,ail=%4d,ele=%4d\r\n",Time,distance,heading,ail,ele);
+            //xbee_uart(dev3,sta);
+            //pc.printf(sta);
+            
+            ++count;                                                            //counter
+            if(count%h == 0) {
+                xbee_uart(dev3,sta);
+                }
+            
+            }else if (distance > 50.0){//---------------------------------------接近制御 
+                
+            range = map(distance,20.0,70.0,200.0,700.0);    
+                        
             if(b_head >= 0.0 && b_head < 90.0){
                 ele = 1024 - (range * cos(map(b_head,0.0,90.0,0.0,(PI/2.0))));
                 ail = 1024 - (range * sin(map(b_head,0.0,90.0,0.0,(PI/2.0))));
@@ -647,103 +779,117 @@
                 ail = 1024 - (range * cos(map(b_head,90.0,180.0,0.0,(PI/2.0))));
             }else if(b_head >= 180.0 && b_head < 270.0){
                 ele = 1024 + (range * cos(map(b_head,180.0,270.0,0.0,(PI/2.0))));
-                ail = 1024 + (range * sin(map(b_head,180.0,270.0,0.0,(PI/2.0))));
+                ail = 1024 + ((range*1.1) * sin(map(b_head,180.0,270.0,0.0,(PI/2.0))));
             }else{
                 ele = 1024 - (range * sin(map(b_head,270.0,360.0,0.0,(PI/2.0))));
-                ail = 1024 + (range * cos(map(b_head,270.0,360.0,0.0,(PI/2.0))));
-            }
-            
-            /*if(b_head >= 0.0 && b_head < 90.0){
-                ele_dis = (distance * cos(map(b_head,0.0,90.0,0.0,(PI/2.0))));
-                ail_dis = (distance * sin(map(b_head,0.0,90.0,0.0,(PI/2.0))));
-                ele_range = -1;
-                ail_range = -1;
-            }else if(b_head >= 90.0 && b_head < 180.0){
-                ele_dis = (distance * sin(map(b_head,90.0,180.0,0.0,(PI/2.0))));
-                ail_dis = (distance * cos(map(b_head,90.0,180.0,0.0,(PI/2.0))));
-                ele_range = 1;
-                ail_range = -1;
-            }else if(b_head >= 180.0 && b_head < 270.0){
-                ele_dis = (distance * cos(map(b_head,180.0,270.0,0.0,(PI/2.0))));
-                ail_dis = (distance * sin(map(b_head,180.0,270.0,0.0,(PI/2.0))));
-                ele_range = 1;
-                ail_range = 1;
-            }else{
-                ele_dis = (distance * sin(map(b_head,270.0,360.0,0.0,(PI/2.0))));
-                ail_dis = (distance * cos(map(b_head,270.0,360.0,0.0,(PI/2.0))));
-                ele_range = -1;
-                ail_range = 1;
+                ail = 1024 + ((range*1.1) * cos(map(b_head,270.0,360.0,0.0,(PI/2.0))));
             }
             
-            if(ele_dis > sekkin + sekkin_hani){
-                ele_dis = sekkin + sekkin_hani;
-            }
-            ele = 1024 + ele_range * map(ele_dis,sekkin,sekkin + sekkin_hani,150.0,660.0);
+                /*ele = (1 - p)*ele_before + p*ele;              //ローパスフィルタ change
+                ele_before = ele; 
+                ail = (1 - p)*ail_before + p*ail;              //ローパスフィルタ
+                ail_before = ail;*/
+                                
+                ele = 1024;                                                     
+                ail = 1024;
+                //rud = 1024;
+                
+            char sta[60];       
+            //sprintf(sta,"Blu=%-d,D=%-4.1f,h=%-5.1f,a=%4d,e=%4d\r\n",t2.read_ms(),distance,b_head,ail,ele);
+            sprintf(sta,"Blu=%-d,h=%-5.1f,r=%4d\r\n",t2.read_ms(),heading,rud);
+            //sprintf(sta,"Blu=%-7.1f,D=%-4.1f,h=%-5.1f,a=%4d,e=%4d\r\n",Time,distance,b_head,ail,ele);
+            //xbee_uart(dev3,sta);
+            //pc.printf(sta);
             
-            if(ail_dis > sekkin + sekkin_hani){
-                ail_dis = sekkin + sekkin_hani;
-            }
-            ail = 1024 + ail_range * map(ail_dis,sekkin,sekkin + sekkin_hani,150.0,660.0);*/
-                           
-            if(ele < 364){
-                ele = 364;
-            }else if(ele > 1684){
-                ele = 1684;
-            }else;
-            
-            if(ail < 364){
-                ail = 364;
-            }else if(ail > 1684){
-                ail = 1684;
-            }else;
-            
+            ++count;                                                        //counter
+            if(count%h == 0) {
+                xbee_uart(dev3,sta);
+                }                                                     
             
-            //request = true;
-            /*while(1){
-                if(through_before != through){
-                    through_before = through;
-                    break;
-                }
-            }*/
-            
-            /*while(1){
-                if(sbus.readable() > 0){
-                    break;
+            }else if(10.0 < distance && distance <= 50.0){   
+                    
+                    /*to_Lat   = 3521.70265;                                      //way point takama 2020/10/21
+                    to_Long  = 13916.74652;*/
+                    /*to_Lat   = 3521.70420;                                      //way point takama 2020/11/18
+                    to_Long  = 13916.74612;*/
+                    /*to_Lat   = 3521.70364;                                      //way point takama 2020/11/27
+                    to_Long  = 13916.74271;*/
+                    to_Lat   = 3521.70246;                                      //way point takama 2020/11/30
+                    to_Long  = 13916.74538;
+                    /*to_Lat   = 3521.87999;                                      //way point shibahu 2020/11/16
+                    to_Long  = 13916.47731;*/                                         
+                    /*to_Lat   = 3521.70546;                                      //way point takama 2021/2/10
+                    to_Long  = 13916.74720;*/
+                    
+                    to_rad(Lat, Long, to_Lat, to_Long);
+                    wp_distance = hubeni_distance(myLat_rad, myLong_rad, Latitude_rad, Longitude_rad);
+                    wp_direction = directions(myLat_rad, myLong_rad, Latitude_rad, Longitude_rad);
+                    
+                    wp_head = wp_direction + (180.0 - heading);                     //wp_head : 自機とW.Pの方位角θ₂
+                    if(wp_head > 360.0){
+                        wp_head = wp_head - 360.0;
+                    }else if(wp_head < 0.0){
+                        wp_head = wp_head + 360.0;  
+                    }else ;
+                                        
+                    
+                    range = map(wp_distance,0.0,100.0,0.0,1200.0);
+                        
+                if(wp_head >= 0.0 && wp_head < 90.0){
+                    ele = 1024 - (range * cos(map(wp_head,0.0,90.0,0.0,(PI/2.0))));
+                    ail = 1024 - (range * sin(map(wp_head,0.0,90.0,0.0,(PI/2.0))));
+                }else if(wp_head >= 90.0 && wp_head < 180.0){
+                    ele = 1024 + (range * sin(map(wp_head,90.0,180.0,0.0,(PI/2.0))));
+                    ail = 1024 - (range * cos(map(wp_head,90.0,180.0,0.0,(PI/2.0))));
+                }else if(wp_head >= 180.0 && wp_head < 270.0){
+                    ele = 1024 + (range * cos(map(wp_head,180.0,270.0,0.0,(PI/2.0))));
+                    ail = 1024 + ((range*1.1) * sin(map(wp_head,180.0,270.0,0.0,(PI/2.0))));
+                }else{
+                    ele = 1024 - (range * sin(map(wp_head,270.0,360.0,0.0,(PI/2.0))));
+                    ail = 1024 + ((range*1.1) * cos(map(wp_head,270.0,360.0,0.0,(PI/2.0))));
                 }
-            }*/
-            /*while(sbus.readable() > 0){
-                if(sbus.getc() == '~'){
-                    for(i = 0; i < 4; i++){
-                        for(k = 0; k < 6; k++){
-                            sbus_from[i][k] = sbus.getc();
-                            if(sbus_from[i][k] == ','){
-                                sbus_from[i][k] = '\0';
-                                channels_from[i] = atoi(sbus_from[i]);
-                                break;
-                            }
-                        }
+                
+                /*ele = (1 - p)*ele_before + p*ele;              //ローパスフィルタ change
+                ele_before = ele; 
+                ail = (1 - p)*ail_before + p*ail;              //ローパスフィルタ
+                ail_before = ail;*/
+                
+                ele = 1024;                                                         //direction only
+                ail = 1024;
+                //rud = 1024; 
+                char sta[60];    
+                //sprintf(sta,"BluW=%-d,D=%-4.1f,h=%-5.1f,a=%4d,e=%4d\r\n",t2.read_ms(),distance,b_head,ail,ele);
+                //sprintf(sta,"Blu=%-7.1f,D=%-4.1f,h=%-5.1f,a=%4d,e=%4d\r\n",Time,distance,b_head,ail,ele);
+                sprintf(sta,"Blu=%-d,h=%-5.1f,r=%4d\r\n",t2.read_ms(),heading,rud);
+                //sprintf(sta,"BluW=%-7.1f,Dis=%-4.1f,head=%-5.1f,ail=%4d,ele=%4d\r\n",Time,distance,heading,ail,ele);
+                //sprintf(sta,"Blu=%-7.1f,Dis=%-4.1f,wp_dis=%-4.1f,ail=%4d,ele=%4d\r\n",Time,distance,wp_distance,ail,ele);
+                //sprintf(sta,"Blu=%-7.1f,Dis=%-4.1f,wp_dis=%-4.1f,wp_head=%-5.1f,ail=%4d,ele=%4d\r\n",Time,distance,wp_distance,wp_head,ail,ele);
+                //sprintf(sta,"Blu=%-7.1f,Dis=%-4.1f,wp_dis=%-4.1f,head=%-5.1f,ail=%4d,ele=%4d\r\n",Time,distance,wp_distance,heading,ail,ele);
+                //xbee_uart(dev3,sta);
+                //pc.printf(sta);
+                
+                ++count;                                                        //counter
+                if(count%h == 0) {
+                    xbee_uart(dev3,sta);
                     }
                 
-                    channels_to[0] = channels_from[0];
-                    channels_to[1] = channels_from[1];
-                    channels_to[2] = channels_from[2];
-                    channels_to[3] = channels_from[3];
-                    //channels_to[4] = channels_from[4];
-                    //channels_to[5] = channels_from[5];
+            }else{
+                
+                ele = 1024;                                                         //direction only
+                ail = 1024;
+                rud = 1024;
                 
-                    pc.printf("\n");
-                    pc.printf("%6d%6d%6d%6d",
-                            channels_from[0],
-                            channels_from[1],
-                            channels_from[2],
-                            channels_from[3]
-                    );
-                }
-                //not_use = sbus.getc();
-                sbus.getc();
-            }*/
-            //request = false;
-            
+                char sta[60];
+                //sprintf(sta,"Blu=%-d,D=%-4.1f,h=%-5.1f,a=%4d,e=%4d\r\n",t2.read_ms(),distance,b_head,ail,ele);
+                sprintf(sta,"Blu=%-d,h=%-5.1f,r=%4d\r\n",t2.read_ms(),heading,rud);
+                
+                ++count;                                                        //counter
+                if(count%h == 0) {
+                    xbee_uart(dev3,sta);
+                    }
+                
+            } 
+                     
             sprintf(s_to,"~%04d,%04d,%04d,%04d,%04d,%04d,",
                         ail,
                         ele,
@@ -752,36 +898,13 @@
                         channels_to[4],
                         channels_to[5]
             );
-            sbus.printf(s_to);
-            //pc.printf("\n");
-            //pc.printf(s_to);
             
-            char sta[60];
-            /*if(ail > 1024 && ele > 1024){
-                sprintf(sta,"ail = Right,  ele = Forward\n");
-            }else if(ail > 1024 && ele < 1024){
-                sprintf(sta,"ail = Right,  ele = Back\n");
-            }else if(ail > 1024 && ele == 1024){
-                sprintf(sta,"ail = Right,  ele = Center\n");
-            }else if(ail < 1024 && ele > 1024){
-                sprintf(sta,"ail = Left,  ele = Forward\n");
-            }else if(ail < 1024 && ele < 1024){
-                sprintf(sta,"ail = Left,  ele = Back\n");
-            }else if(ail < 1024 && ele == 1024){
-                sprintf(sta,"ail = Left,  ele = Center\n");
-            }else if(ail == 1024 && ele > 1024){
-                sprintf(sta,"ail = Center,  ele = Forward\n");
-            }else if(ail == 1024 && ele < 1024){
-                sprintf(sta,"ail = Center,  ele = Back\n");
-            }else{
-                sprintf(sta,"ail = Center,  ele = Center\n");
-            }*/
-            sprintf(sta,"Time=%9.2f,Dis=%5.2f,Dir=%6.2f,ail=%4d,ele=%4d\r",Time,distance,direction,ail,ele);
-            //pc.printf(sta);
-            xbee_uart(dev3,sta);
-            //xbee_send_frame(dev3, sta, sizeof(sta));
+            sbus.printf(s_to);
+            ++count;                                                        //counter
+            if(count%hh == 0) {
+                xbee_uart(dev1,s);
+            }
             
-        }
-        
+        }               
     }
-}
\ No newline at end of file
+//}
\ No newline at end of file

--- a/xbee/xbee.cpp	Wed Feb 22 08:59:47 2017 +0000
+++ b/xbee/xbee.cpp	Thu Mar 18 08:39:47 2021 +0000
@@ -1168,6 +1168,7 @@
 	#else
 		#ifdef ARM_MBED
 			_xbee_serial.baud(57600);
+			//_xbee_serial.baud(38400);
 			_xbee_serial.attach(&_xbee_serial_callback);
 			#ifdef DEBUG
 				_xbee_debug.baud(38400);