forward
Dependencies: VNH5019
Fork of AigamozuControlPackets by
Diff: AigamozuControlPackets.h
- Revision:
- 17:a6fa8cc96d94
- Parent:
- 13:a5bc425540a7
- Child:
- 19:13b24b50800e
diff -r e441331aa4a1 -r a6fa8cc96d94 AigamozuControlPackets.h --- 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]; };