大 田澤
/
Group_Control_No2_3actions
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 1:12c5a8179ad2
- Parent:
- 0:65f9be2858c9
--- 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