forward
Dependencies: VNH5019
Fork of AigamozuControlPackets by
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; }