-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathconvert-alm.py
166 lines (140 loc) · 5.78 KB
/
convert-alm.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# Copyright 2023 Jussi Kivilinna
# License: MIT (see LICENSE file)
import sys
import math
import struct
#from pyubx2 import UBXReader, SET
def ubx_checksum(packet):
ck_a = 0
ck_b = 0
for byte in packet:
ck_a = (ck_a + byte) & 0xff
ck_b = (ck_b + ck_a) & 0xff
return bytes((ck_a, ck_b))
if __name__ == "__main__":
if ubx_checksum(b"\x06\x01\x02\x00\xf0\x05") != b"\xfe\x16":
sys.exit(1)
almanac = {}
sat_id = 0
with open(sys.argv[1], "rb") as inputfile:
for line in inputfile:
### Input needs to be in YUMA format
### GPS satellites with IDs 1-32
### QZSS satellites with IDs 192-202
data = [ s.strip() for s in line.decode("utf-8").split(":") ]
if data[0] == 'ID':
sat_id = int(data[1])
almanac[sat_id] = {}
almanac[sat_id]['skip'] = True
almanac[sat_id]['id'] = sat_id
if sat_id >= 1 and sat_id <= 32:
almanac[sat_id]['skip'] = False
almanac[sat_id]['sat_type'] = 'GPS'
almanac[sat_id]['msgId'] = 0x00
almanac[sat_id]['svId'] = sat_id
almanac[sat_id]['e_ref'] = 0.0
almanac[sat_id]['I_ref'] = 0.30 # from IS-GPS-200
elif sat_id >= 193 and sat_id <= 202:
almanac[sat_id]['skip'] = False
almanac[sat_id]['sat_type'] = 'QZSS'
almanac[sat_id]['msgId'] = 0x05
almanac[sat_id]['svId'] = sat_id - 193 + 1
if almanac[sat_id]['svId'] <= 5:
# From IS-QZSS-PNT-004
almanac[sat_id]['e_ref'] = 0.06 # semicircles
almanac[sat_id]['I_ref'] = 0.25
else:
# From IS-QZSS-PNT-004
almanac[sat_id]['e_ref'] = 0.0 # semicircles
almanac[sat_id]['I_ref'] = 0.0
# Geosynchronous QZSS not supported in UBX message spec
almanac[sat_id]['skip'] = True
elif data[0] == 'Health':
almanac[sat_id]['svHealth'] = int(data[1])
if (almanac[sat_id]['svHealth'] != 0):
almanac[sat_id]['skip'] = True # Skip unhealthy satellites
elif data[0] == 'Eccentricity':
# UBX scaling: 2^-21
eccentricity_abs = float(data[1])
eccentricity_relative = eccentricity_abs - almanac[sat_id]['e_ref']
almanac[sat_id]['e'] = round(eccentricity_relative / 2**-21)
elif data[0] == 'week':
week = round(float(data[1]))
almanac[sat_id]['gpsWeek'] = week
almanac[sat_id]['almWNa'] = week % 256
elif data[0] == 'Time of Applicability(s)':
# UBX scaling: 2^12
# UBX unit: s
almanac[sat_id]['toa'] = round(float(data[1]) / 2**12)
elif data[0] == 'Orbital Inclination(rad)':
# UBX scaling: 2^-19
# UBX unit: semi-circles
# Input: YUMA format, direct measurement of inclination angle
# Output: SEM format, relative to 0.30 semicircles
inclination_abs = float(data[1]) / math.pi
inclination_relative = inclination_abs - almanac[sat_id]['I_ref']
almanac[sat_id]['deltaI'] = round(inclination_relative / 2**-19)
elif data[0] == 'Rate of Right Ascen(r/s)':
# UBX scaling: 2^-38
# UBX unit: semi-circles/s
almanac[sat_id]['omegaDot'] = round(float(data[1]) / (2**-38 * math.pi))
elif data[0] == 'SQRT(A) (m 1/2)':
# UBX scaling: 2^-11
# UBX unit: m^0.5
almanac[sat_id]['sqrtA'] = round(float(data[1]) / 2**-11)
elif data[0] == 'Right Ascen at Week(rad)':
# UBX scaling: 2^-23
# UBX unit: semi-circles
almanac[sat_id]['omega0'] = round(float(data[1]) / (2**-23 * math.pi))
elif data[0] == 'Argument of Perigee(rad)':
# UBX scaling: 2^-23
# UBX unit: semi-circles
almanac[sat_id]['omega'] = round(float(data[1]) / (2**-23 * math.pi))
elif data[0] == 'Mean Anom(rad)':
# UBX scaling: 2^-23
# UBX unit: semi-circles
almanac[sat_id]['m0'] = round(float(data[1]) / (2**-23 * math.pi))
elif data[0] == 'Af0(s)':
# UBX scaling: 2^-20
# UBX unit: s
almanac[sat_id]['af0'] = round(float(data[1]) / 2**-20)
elif data[0] == 'Af1(s/s)':
# UBX scaling: 2^-38
# UBX unit: s/s
almanac[sat_id]['af1'] = round(float(data[1]) / 2**-38)
out = b''
for sat_id in almanac:
if not almanac[sat_id]['skip']:
# Generate UBX-MGA-GPS-ALM / UBX-MGA-QZSS-ALM messages
ubx_gps_alm_length = 36
header = struct.pack('<BBBBH', 0xb5, 0x62, 0x13, almanac[sat_id]['msgId'],
ubx_gps_alm_length)
payload = struct.pack('<BBBBHBBhhIiiihhI',
0x02,
0x00,
almanac[sat_id]['svId'],
almanac[sat_id]['svHealth'],
almanac[sat_id]['e'],
almanac[sat_id]['almWNa'],
almanac[sat_id]['toa'],
almanac[sat_id]['deltaI'],
almanac[sat_id]['omegaDot'],
almanac[sat_id]['sqrtA'],
almanac[sat_id]['omega0'],
almanac[sat_id]['omega'],
almanac[sat_id]['m0'],
almanac[sat_id]['af0'],
almanac[sat_id]['af1'],
0)
print("-------")
print("Converted UBX-MGA almanac for %s satellite ID %d." %
( almanac[sat_id]['sat_type'], almanac[sat_id]['id']))
frame = header + payload + ubx_checksum(header[2 : ] + payload)
# Validate frame
# msg = UBXReader.parse(frame, validate=True, msgmode=SET)
# print(msg)
out = out + frame
with open(sys.argv[2], "wb") as outputfile:
outputfile.write(out)