@@ -30,11 +30,13 @@ class GPSGeodetic:
3030 magnetic_variation = np .int16 (0 )
3131 climb_rate = np .int16 (0 )
3232 heading_rate = np .int16 (0 )
33+ accuracy = np .uint16 (0 )
34+ altitude_accuracy = np .int16 (0 )
3335 original_data = bytes ()
3436 valid = False
3537
3638 def from_bytes (self , data , offset = 0 ):
37- if len (data ) != 91 :
39+ if len (data ) != 91 and len ( data ) != 54 :
3840 print ('Error GPSGeo len:' , len (data ))
3941 self .valid = False
4042 return False
@@ -63,8 +65,9 @@ def from_bytes(self, data, offset=0):
6365 [self .magnetic_variation ] = struct .unpack_from ('>h' , data , offset = 44 )
6466 [self .climb_rate ] = struct .unpack_from ('>h' , data , offset = 46 )
6567 [self .heading_rate ] = struct .unpack_from ('>h' , data , offset = 48 )
66-
67- # TODO continue other fields
68+ if len (data ) == 54 :
69+ [self .accuracy ] = struct .unpack_from ('>H' , data , offset = 50 )
70+ [self .altitude_accuracy ] = struct .unpack_from ('>H' , data , offset = 52 )
6871
6972 # print('latitude', self.latitude / 1e7, 'longitude', self.longitude / 1e7)
7073
@@ -84,10 +87,32 @@ def tobytes(self):
8487 if len (self .original_data ) > 0 :
8588 return self .original_data
8689 else :
87- # TODO Write all fields
88- data = np .zeros (91 , dtype = np .uint8 )
89- struct .pack_into ('>i' , data , 23 , int (self .latitude ))
90- struct .pack_into ('>i' , data , 27 , int (self .longitude ))
90+ data = np .zeros (54 , dtype = np .uint8 )
91+ struct .pack_into ('>B' , data , 0 , self .message_id )
92+ struct .pack_into ('>H' , data , 1 , self .nav_valid )
93+ struct .pack_into ('>H' , data , 3 , self .nav_type )
94+ struct .pack_into ('>H' , data , 5 , self .extended_week_number )
95+ struct .pack_into ('>I' , data , 7 , self .tow )
96+ struct .pack_into ('>H' , data , 11 , self .year )
97+ struct .pack_into ('>B' , data , 13 , self .month )
98+ struct .pack_into ('>B' , data , 14 , self .day )
99+ struct .pack_into ('>B' , data , 15 , self .hour )
100+ struct .pack_into ('>B' , data , 16 , self .minute )
101+ struct .pack_into ('>H' , data , 17 , self .second )
102+ struct .pack_into ('>I' , data , 19 , self .satellite_id_list )
103+ struct .pack_into ('>i' , data , 23 , self .latitude )
104+ struct .pack_into ('>i' , data , 27 , self .longitude )
105+ struct .pack_into ('>i' , data , 31 , self .altitude_ellipsoid )
106+ struct .pack_into ('>i' , data , 35 , self .altitude_mls )
107+ struct .pack_into ('>b' , data , 39 , self .map_datum )
108+ struct .pack_into ('>H' , data , 40 , self .speed_over_ground )
109+ struct .pack_into ('>H' , data , 42 , self .course_over_ground )
110+ struct .pack_into ('>h' , data , 44 , self .magnetic_variation )
111+ struct .pack_into ('>h' , data , 46 , self .climb_rate )
112+ struct .pack_into ('>h' , data , 48 , self .heading_rate )
113+ struct .pack_into ('>H' , data , 50 , self .accuracy )
114+ struct .pack_into ('>H' , data , 52 , self .altitude_accuracy )
115+
91116 return data
92117
93118
0 commit comments