Diff: AigamozuControlPackets.cpp

Revision:

18:01882120e6cf

Parent:

17:a6fa8cc96d94

Child:

19:13b24b50800e

--- a/AigamozuControlPackets.cpp	Tue May 12 01:02:47 2015 +0000
+++ b/AigamozuControlPackets.cpp	Tue May 12 11:36:12 2015 +0000
@@ -98,11 +98,11 @@
     
     latitude_data.double_value=latitude;
     longitude_data.double_value=longitude;
-    longitude_data.double_value=latitudeKalman;
+    latitudeKalman_data.double_value=latitudeKalman;
     longitudeKalman_data.double_value=longitudeKalman;
     covarLati_data.double_value=covarLati;
     covarLongi_data.double_value=covarLongi;
-      
+    
     uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S',nowMode,71,80,83,
                     latitude_data.char_value[0],latitude_data.char_value[1],latitude_data.char_value[2],latitude_data.char_value[3],latitude_data.char_value[4],latitude_data.char_value[5],latitude_data.char_value[6],latitude_data.char_value[7],
                     longitude_data.char_value[0],longitude_data.char_value[1],longitude_data.char_value[2],longitude_data.char_value[3],longitude_data.char_value[4],longitude_data.char_value[5],longitude_data.char_value[6],longitude_data.char_value[7],
@@ -244,8 +244,8 @@
     
 //Update Robot Point    
 void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
-    agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
-    agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
+    agzPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
+    agzPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
     
     }
     
@@ -257,8 +257,8 @@
 
 //Update Robot Point    
 void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
-    agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
-    agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
+    agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
+    agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
     
     }