|
| 1 | +""" |
| 2 | +QZSS MADOCA-PPP correction data decoder |
| 3 | +
|
| 4 | +[1] Quasi-Zenith Satellite System Interface Specification Multi-GNSS |
| 5 | + Advanced Orbit and Clock Augmentation - Precise Point Positioning |
| 6 | + (IS-QZSS-MDC-004), 2025 |
| 7 | +
|
| 8 | +""" |
| 9 | + |
| 10 | +import numpy as np |
| 11 | +import bitstruct as bs |
| 12 | +from cssrlib.cssrlib import cssr, sCSSRTYPE, sCSSR, local_corr, sCType |
| 13 | +from cssrlib.gnss import gpst2time, uGNSS, prn2sat, rCST |
| 14 | + |
| 15 | + |
| 16 | +class areaInfo(): |
| 17 | + def __init__(self, sid, latr, lonr, p1=0, p2=0): |
| 18 | + self.sid = sid |
| 19 | + self.latr = latr |
| 20 | + self.lonr = lonr |
| 21 | + if sid == 0: |
| 22 | + self.lats = p1 |
| 23 | + self.lons = p2 |
| 24 | + elif sid == 1: |
| 25 | + self.rng = p1 |
| 26 | + |
| 27 | + |
| 28 | +class ionoCorr(): |
| 29 | + def __init__(self): |
| 30 | + self.t0 = None |
| 31 | + self.qi = [] |
| 32 | + self.iodssr = 0 |
| 33 | + self.ct = 0 |
| 34 | + self.sat = [] |
| 35 | + self.c = np.zeros((6)) |
| 36 | + |
| 37 | + |
| 38 | +class cssr_mdc(cssr): |
| 39 | + def __init__(self, foutname=None): |
| 40 | + super().__init__(foutname) |
| 41 | + |
| 42 | + self.cssrmode = sCSSRTYPE.QZS_MADOCA |
| 43 | + self.buff = bytearray(250*10) |
| 44 | + |
| 45 | + self.pnt = {} |
| 46 | + self.ci = {} |
| 47 | + |
| 48 | + self.area = -1 |
| 49 | + self.narea_t = {1: 8, 2: 16, 3: 5, 4: 1, 5: 8} |
| 50 | + self.MAXNET = np.sum(list(self.narea_t.values())) |
| 51 | + |
| 52 | + for inet in range(self.MAXNET+1): |
| 53 | + self.lc.append(local_corr()) |
| 54 | + self.lc[inet].inet = inet |
| 55 | + self.lc[inet].flg_trop = 0 |
| 56 | + self.lc[inet].flg_stec = 0 |
| 57 | + self.lc[inet].nsat_n = 0 |
| 58 | + self.lc[inet].t0 = {} |
| 59 | + |
| 60 | + def find_grid_index(self, pos): |
| 61 | + """ find region/area """ |
| 62 | + |
| 63 | + latd, lond = np.rad2deg(pos[0:2]) |
| 64 | + self.reg = self.area = -1 |
| 65 | + |
| 66 | + for reg in self.pnt.key(): |
| 67 | + for area in self.pnt[reg].key(): |
| 68 | + p = self.pnt[reg][area] |
| 69 | + if p.sid == 0: # rectangle shape: |
| 70 | + if p.latr-p.lats <= latd and latd <= p.latr+p.lats and \ |
| 71 | + p.lonr-p.lons <= lond and lond <= p.lonr+p.lons: |
| 72 | + self.reg = reg |
| 73 | + self.area = area |
| 74 | + break |
| 75 | + elif p.sid == 1: # circle |
| 76 | + dn = (latd - p.latr)*rCST.D2R*rCST.RE_WGS84 |
| 77 | + de = (lond - p.lonr)*rCST.D2R * \ |
| 78 | + rCST.RE_WGS84*np.cos(p.lonr*rCST.D2R) |
| 79 | + r = np.sqrt(dn**2+de**2) |
| 80 | + if r <= p.rng: |
| 81 | + self.reg = reg |
| 82 | + self.area = area |
| 83 | + break |
| 84 | + |
| 85 | + inet = self.get_inet(self.reg, self.area) |
| 86 | + return inet |
| 87 | + |
| 88 | + def get_dpos(self, pos): |
| 89 | + """ calculate position offset from reference """ |
| 90 | + if self.reg < 0 or self.area < 0: |
| 91 | + print("get_dpos: region, area not defined.") |
| 92 | + return 0, 0 |
| 93 | + |
| 94 | + latd, lond = np.rad2deg(pos[0:2]) |
| 95 | + p = self.pnt[self.reg][self.area] |
| 96 | + dlat = latd - p.latr |
| 97 | + dlon = lond - p.lonr |
| 98 | + |
| 99 | + return dlat, dlon |
| 100 | + |
| 101 | + def get_stec(self, dlat=0.0, dlon=0.0): |
| 102 | + """ calculate STEC correction by interporation """ |
| 103 | + if self.inet < 0: |
| 104 | + print("get_stec: region, area not defined.") |
| 105 | + return 0.0 |
| 106 | + |
| 107 | + p = self.lc[self.inet] |
| 108 | + # if p.inet_ref != self.iodssr: |
| 109 | + # return 0.0 |
| 110 | + stec = np.zeros(self.nsat_n) |
| 111 | + |
| 112 | + for i, sat in enumerate(self.sat_n): |
| 113 | + stec[i] = [1, dlat, dlon, dlat*dlon, dlat**2, dlon**2]@p.ci[sat] |
| 114 | + |
| 115 | + return stec |
| 116 | + |
| 117 | + def decode_mdc_stec_area(self, buff, i=0): |
| 118 | + """ decoder for MT1 - STEC Coverage Message """ |
| 119 | + tow, uid, mi, iodssr = bs.unpack_from('u20u4u1u4', buff, i) |
| 120 | + i += 29 |
| 121 | + self.tow0 = tow//3600*3600 |
| 122 | + reg, alrt, len_, narea = bs.unpack_from('u8u1u16u5', buff, i) |
| 123 | + i += 30 |
| 124 | + if reg not in self.pnt: |
| 125 | + self.pnt[reg] = {} |
| 126 | + |
| 127 | + for k in range(narea): |
| 128 | + area, sid = bs.unpack_from('u5u1', buff, i) |
| 129 | + i += 6 |
| 130 | + |
| 131 | + if sid == 0: # rectangle shape |
| 132 | + latr, lonr, lats, lons = bs.unpack_from('s11u12u8u8', buff, i) |
| 133 | + if self.monlevel > 2: |
| 134 | + print(f"{reg} {area:2d} {sid} {latr*0.1:5.1f} " |
| 135 | + f"{lonr*0.1:5.1f} {lats*0.1:3.1f} {lons*0.1:3.1f}") |
| 136 | + self.pnt[reg][area] = areaInfo( |
| 137 | + sid, latr*0.1, lonr*0.1, lats*0.1, lons*0.1) |
| 138 | + else: # circle range |
| 139 | + latr, lonr, rng = bs.unpack_from('s15u16u8', buff, i) |
| 140 | + if self.monlevel > 2: |
| 141 | + print(f"{reg} {area:2d} {sid} {latr*0.01:6.2f} " |
| 142 | + f"{lonr*0.01:6.2f} {rng*10}") |
| 143 | + self.pnt[reg][area] = areaInfo( |
| 144 | + sid, latr*0.01, lonr*0.01, rng*10.0) |
| 145 | + i += 39 |
| 146 | + return i |
| 147 | + |
| 148 | + def get_inet(self, reg, area): |
| 149 | + """ region, area to inet conversion """ |
| 150 | + |
| 151 | + inet = 0 |
| 152 | + for r in self.narea_t.keys(): |
| 153 | + if r >= reg: |
| 154 | + break |
| 155 | + inet += self.narea_t[r] |
| 156 | + |
| 157 | + inet += area |
| 158 | + |
| 159 | + return inet |
| 160 | + |
| 161 | + def decode_mdc_stec_corr(self, buff, i=0): |
| 162 | + """ decoder for MT2 - STEC Correction Message """ |
| 163 | + dtow, uid, mi, iodssr = bs.unpack_from('u12u4u1u4', buff, i) |
| 164 | + i += 21 |
| 165 | + reg, area, stype_ = bs.unpack_from('u8u5u2', buff, i) |
| 166 | + i += 15 |
| 167 | + |
| 168 | + nsat = bs.unpack_from('u5u5u5u5u5', buff, i) |
| 169 | + i += 25 |
| 170 | + # gps, glo, gal, bds, qzss |
| 171 | + sys_t = [uGNSS.GPS, uGNSS.GLO, uGNSS.GAL, uGNSS.BDS, uGNSS.QZS] |
| 172 | + |
| 173 | + inet = self.get_inet(reg, area) |
| 174 | + |
| 175 | + self.lc[inet].nsat_n = np.sum(nsat) |
| 176 | + ci = {} |
| 177 | + qi = {} |
| 178 | + stype = {} |
| 179 | + sat_ = [] |
| 180 | + j = 0 |
| 181 | + for gnss in range(5): |
| 182 | + sys = sys_t[gnss] |
| 183 | + for k in range(nsat[gnss]): |
| 184 | + c01 = c10 = c11 = c02 = c20 = 0.0 |
| 185 | + |
| 186 | + prn, qi_, c00 = bs.unpack_from('u6u6s14', buff, i) |
| 187 | + i += 26 |
| 188 | + |
| 189 | + if sys == uGNSS.QZS: |
| 190 | + prn += 192 |
| 191 | + sat = prn2sat(sys, prn) |
| 192 | + qi[sat] = qi_ |
| 193 | + stype[sat] = stype_ |
| 194 | + |
| 195 | + if stype_ > 0: |
| 196 | + c01, c10 = bs.unpack_from('s12s12', buff, i) |
| 197 | + i += 24 |
| 198 | + if stype_ > 1: |
| 199 | + c11 = bs.unpack_from('s10', buff, i)[0] |
| 200 | + i += 10 |
| 201 | + if stype_ > 2: |
| 202 | + c02, c20 = bs.unpack_from('s8s8', buff, i) |
| 203 | + i += 16 |
| 204 | + |
| 205 | + sat_ += [sat] |
| 206 | + ci[sat] = np.array( |
| 207 | + [c00*0.05, c01*0.02, c10*0.02, c11*0.02, |
| 208 | + c02*5e-3, c20*5e-3]) |
| 209 | + j += 1 |
| 210 | + |
| 211 | + self.lc[inet].stype = stype |
| 212 | + self.lc[inet].sat_n = sat_ |
| 213 | + self.lc[inet].ci = ci |
| 214 | + self.lc[inet].stec_quality = qi |
| 215 | + |
| 216 | + t0 = gpst2time(self.week, self.tow0+dtow) |
| 217 | + self.set_t0(inet, 0, sCType.STEC, t0) |
| 218 | + self.lc[inet].cstat |= (1 << sCType.STEC) |
| 219 | + |
| 220 | + self.lc[inet].inet_ref = iodssr |
| 221 | + |
| 222 | + return i |
| 223 | + |
| 224 | + def decode_cssr(self, msg, i=0): |
| 225 | + """decode Compact SSR message with MADOCA-PPP extension """ |
| 226 | + df = {'msgtype': 4073} |
| 227 | + while df['msgtype'] in [1, 2, 4073]: |
| 228 | + df = bs.unpack_from_dict('u12u4', ['msgtype', 'subtype'], msg, i) |
| 229 | + i += 16 |
| 230 | + if df['msgtype'] not in [1, 2, 4073]: |
| 231 | + return -1 |
| 232 | + |
| 233 | + self.subtype = df['subtype'] |
| 234 | + if df['msgtype'] == 4073: # Compact SSR |
| 235 | + if self.subtype == sCSSR.MASK: |
| 236 | + i = self.decode_cssr_mask(msg, i) |
| 237 | + elif self.subtype == sCSSR.ORBIT: # orbit |
| 238 | + i = self.decode_cssr_orb(msg, i) |
| 239 | + elif self.subtype == sCSSR.CLOCK: # clock |
| 240 | + i = self.decode_cssr_clk(msg, i) |
| 241 | + elif self.subtype == sCSSR.CBIAS: # cbias |
| 242 | + i = self.decode_cssr_cbias(msg, i) |
| 243 | + elif self.subtype == sCSSR.PBIAS: # pbias |
| 244 | + i = self.decode_cssr_pbias(msg, i) |
| 245 | + elif self.subtype == sCSSR.BIAS: # bias |
| 246 | + i = self.decode_cssr_bias(msg, i) |
| 247 | + elif self.subtype == sCSSR.URA: # ura |
| 248 | + i = self.decode_cssr_ura(msg, i) |
| 249 | + elif df['msgtype'] in [1, 2] and df['subtype'] == 0: |
| 250 | + if df['msgtype'] == 1: # MADOCA MT1 |
| 251 | + i = self.decode_mdc_stec_area(msg, i) |
| 252 | + elif df['msgtype'] == 2: # MADOCA MT2 |
| 253 | + i = self.decode_mdc_stec_corr(msg, i) |
| 254 | + |
| 255 | + if i <= 0: |
| 256 | + return 0 |
| 257 | + if self.monlevel >= 2: |
| 258 | + print(f"tow={int(self.tow):6d} msgtype={df['msgtype']:4d} " |
| 259 | + f"subtype={self.subtype:2d} inet={self.inet:2d}") |
| 260 | + |
| 261 | + if self.monlevel > 0 and self.fh is not None: |
| 262 | + self.out_log() |
0 commit comments