Diff: AigamozuControlPackets.h

Revision:

17:a6fa8cc96d94

Parent:

13:a5bc425540a7

--- a/AigamozuControlPackets.h	Sat Apr 25 01:05:51 2015 +0000
+++ b/AigamozuControlPackets.h	Tue May 12 01:02:47 2015 +0000
@@ -7,7 +7,7 @@
 #define MANUAL_COMMAND_LENGTH 17
 #define REQUEST_COMMNAD_LENGTH 11
 #define CHANGE_MODE_COMMAND_LENGTH 12
-#define RECEIVE_STATUS_COMMNAD_LENGTH 32
+#define RECEIVE_STATUS_COMMNAD_LENGTH 64
 
 #define vertex2D vector2D
 
@@ -20,6 +20,11 @@
     uint8_t b[4];
     };
 
+union UNION_double_char{
+    double double_value;
+    uint8_t char_value[8];
+    };
+
 struct vector2D{
     double x;
     double y;
@@ -40,8 +45,8 @@
     void createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode);
     
     //Create Packet: Robot -> Controller/Base 
-    void createReceiveStatusCommand(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL);
-    void createReceiveStatusCommandwithKalman(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL);
+    void createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,
+                                    double covarLati,double covarLongi);
     //uint8_t* createAckPacket(uint8_t fromID,uint8_t toID);
     
     //using create packet
@@ -66,7 +71,15 @@
     void reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL);
     void reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
     void reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL);
+    
+    double get_agzPoint_lati();
+    double get_agzPoint_longi();
+    double get_agzPointKalman_lati();
+    double get_agzPointKalman_longi();
+    double get_agzCov_lati();
+    double get_agzCov_longi();
         
+    void set_agzCov(double cov_lati,double cov_longi);
     //--------PRIVATE-----------//
     private:
     //
@@ -82,12 +95,13 @@
     //Auto Type 2 -> GPS 
     void gpsAuto();
     
-    vector2D agzPoint;
+    vector2D agzPoint;//自分自身の位置の情報（double型）
     vector2D agzPointKalman;
+    vector2D agzCov;//カルマンフィルタの共分散
     vector2D sub_vector( const vector2D& a, const vector2D& b );
     bool checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P);
     
-    vector2D basePoint[4];
+    vector2D basePoint[4];//ベースの位置の情報（double型）
     vector2D basePointKalman[4];
     
     };