Diff: AigamozuControlPackets.cpp

Revision:

19:13b24b50800e

Parent:

18:01882120e6cf

Child:

20:fec2d6dec897

--- a/AigamozuControlPackets.cpp	Tue May 12 11:36:12 2015 +0000
+++ b/AigamozuControlPackets.cpp	Wed May 13 04:08:12 2015 +0000
@@ -55,6 +55,19 @@
     return agzCov.y;
 }
 
+double AigamozuControlPackets::get_basePointKalman_lati(int i){
+    return basePointKalman[i].x;
+    }
+double AigamozuControlPackets::get_basePointKalman_longi(int i){
+    return basePointKalman[i].y;
+    }
+double AigamozuControlPackets::get_basePoint_lati(int i){
+    return basePoint[i].x;
+    }
+double AigamozuControlPackets::get_basePoint_longi(int i){
+    return basePoint[i].y;
+    }
+    
 //////////////////////////////
 // set関数
 //////////////////////////////
@@ -250,9 +263,16 @@
     }
     
 //Updata Base Point   
-void AigamozuControlPackets::reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
-    basePoint[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
-    basePoint[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
+void AigamozuControlPackets::reNewBasePoint(int id, uint8_t *latitude,uint8_t *longitude){
+    UNION_double_char lat,longi;
+    for(int i = 0;i < 8;i++){
+        lat.char_value[i]=latitude[i];
+    }
+    for(int i = 0;i < 8;i++){
+        longi.char_value[i]=longitude[i];
+    }
+    basePoint[id].x = lat.double_value;
+    basePoint[id].y = longi.double_value;
 }   
 
 //Update Robot Point    
@@ -263,9 +283,17 @@
     }
     
 //Updata Base Point   
-void AigamozuControlPackets::reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
-    basePointKalman[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
-    basePointKalman[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
+void AigamozuControlPackets::reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude){
+    UNION_double_char lat,longi;
+    for(int i = 0;i < 8;i++){
+        lat.char_value[i]=latitude[i];
+    }
+    for(int i = 0;i < 8;i++){
+        longi.char_value[i]=longitude[i];
+    }
+    
+    basePointKalman[id].x = lat.double_value;
+    basePointKalman[id].y = longi.double_value;
 }