Diff: main.cpp

Revision:

1:ee2713435312

Parent:

0:eee5e3d906ce

Child:

2:d97700414618

--- a/main.cpp	Tue May 12 01:03:31 2015 +0000
+++ b/main.cpp	Tue May 12 11:36:34 2015 +0000
@@ -102,9 +102,21 @@
     }
     //send normal data
     //Create GPS Infomation Packet
-    agz.createReceiveStatusCommand(MyID,SenderIDc,agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+    agz.createReceiveStatusCommand(MyID,SenderIDc,
+                                    agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
                                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
                                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
+    
+    //debug***************************************************
+    printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
+            agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+            agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+            agz.get_agzCov_lati(),agz.get_agzCov_longi()
+            );
+    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
+        printf("\n");
+    //debug end***************************************************
+    
     //Select Destination
     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
     //Send -> Base
@@ -124,7 +136,7 @@
         agz.nowStatus = GPS_AVAIL;
         
         if(flag){//初期値設定
-            if(myGPS->longitudeH>=138 && myGPS->longitudeH<=141 && myGPS->latitudeH>=36 && myGPS->latitudeH<=38){
+            if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
                 flag = false;
                 x[0][0]=(double)myGPS->latitudeL;                        
                 x[0][1]=(double)myGPS->longitudeL;   
@@ -137,8 +149,8 @@
         //Kalman Filter
         Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
                 
-        agz.reNewRobotPoint(myGPS->longitudeH,myGPS->longitudeL,myGPS->latitudeH,myGPS->latitudeL);
-        agz.reNewRobotPointKalman(myGPS->longitudeKH,myGPS->longitudeKL,myGPS->latitudeKH,myGPS->latitudeKL);
+        agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
+        agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
     }
     else agz.nowStatus = GPS_UNAVAIL;
     
@@ -267,7 +279,7 @@
                 switch(Command_type){
                     //Get Request command
                     case STATUS_REQUEST:{
-                        Send_Status(SenderIDc);                            
+                        Send_Status(SenderIDc);                     
                         break;          
                     }
                     default:{
@@ -290,13 +302,7 @@
         if (refresh_Timer.read_ms() >= refresh_Time) {
             refresh_Timer.reset();
             Get_GPS(&myGPS);
-            
-            printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
-                    agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                    agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                    agz.get_agzCov_lati(),agz.get_agzCov_longi()
-                    );
-            
+
         }
     }
 }
\ No newline at end of file