Harald Welte | 47ab210 | 2012-07-18 22:04:09 +0200 | [diff] [blame^] | 1 | /*
|
| 2 | * rrlp.c
|
| 3 | *
|
| 4 | * RRLP implementation
|
| 5 | *
|
| 6 | *
|
| 7 | * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com>
|
| 8 | *
|
| 9 | * This program is free software: you can redistribute it and/or modify
|
| 10 | * it under the terms of the GNU General Public License as published by
|
| 11 | * the Free Software Foundation, either version 2 of the License, or
|
| 12 | * (at your option) any later version.
|
| 13 | *
|
| 14 | * This program is distributed in the hope that it will be useful,
|
| 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
| 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
| 17 | * GNU General Public License for more details.
|
| 18 | *
|
| 19 | * You should have received a copy of the GNU General Public License
|
| 20 | * along with this program. If not, see <http://www.gnu.org/licenses/>.
|
| 21 | */
|
| 22 |
|
| 23 |
|
| 24 | #include <errno.h>
|
| 25 | #include <math.h>
|
| 26 |
|
| 27 | #include "gps.h"
|
| 28 | #include "rrlp.h"
|
| 29 |
|
| 30 | #include <PDU.h>
|
| 31 | #include <GPS-AssistData.h>
|
| 32 | #include <NavigationModel.h>
|
| 33 | #include <IonosphericModel.h>
|
| 34 | #include <UTCModel.h>
|
| 35 | #include <Almanac.h>
|
| 36 | #include <RefLocation.h>
|
| 37 | #include <ReferenceTime.h>
|
| 38 |
|
| 39 |
|
| 40 | /* ------------------------------------------------------------------------ */
|
| 41 | /* RRLP Assistance request decoding */
|
| 42 | /* ---------------------------------------------------------------------{{{ */
|
| 43 | /* Decode and validate the assistance data request messages.
|
| 44 | * See section 10.10 of
|
| 45 | * . ETSI TS 149 031 V8.1.0 (2009-01)
|
| 46 | * . 3GPP TS 49.031 version 8.1.0 Release 8
|
| 47 | */
|
| 48 |
|
| 49 | /* Packed structure from 49.031 spec (RGA = Request GPS Assistance) */
|
| 50 |
|
| 51 | #define RRLP_RGA0_ALMANAC (1<<0)
|
| 52 | #define RRLP_RGA0_UTC_MODEL (1<<1)
|
| 53 | #define RRLP_RGA0_IONO_MODEL (1<<2)
|
| 54 | #define RRLP_RGA0_NAV_MODEL (1<<3)
|
| 55 | #define RRLP_RGA0_DGPS (1<<4)
|
| 56 | #define RRLP_RGA0_REF_LOC (1<<5)
|
| 57 | #define RRLP_RGA0_REF_TIME (1<<6)
|
| 58 | #define RRLP_RGA0_ACQ_ASSIST (1<<7)
|
| 59 |
|
| 60 | #define RRLP_RGA1_REALTIME_INT (1<<0)
|
| 61 | #define RRLP_RGA1_EPH_EXT (1<<1)
|
| 62 | #define RRLP_RGA1_EPH_EXT_CHECK (1<<2)
|
| 63 |
|
| 64 | struct rrlp_rga_hdr {
|
| 65 | uint8_t items0;
|
| 66 | uint8_t items1;
|
| 67 | } __attribute__((packed));
|
| 68 |
|
| 69 | struct rrlp_rga_eph_sv {
|
| 70 | uint8_t sv_id; /* [7:6] reserved, [5:0] sv_id */
|
| 71 | uint8_t iode; /* latest eph in the MS memory in hours */
|
| 72 | } __attribute__((packed));
|
| 73 |
|
| 74 | struct rrlp_rga_eph {
|
| 75 | uint8_t wn_hi; /* [7:6] = wn[9:8] */
|
| 76 | uint8_t wn_lo; /* wn[7:0] */
|
| 77 | uint8_t toe; /* latest eph in the MS memory in hours */
|
| 78 | uint8_t nsat_tmtoe; /* [7:4] nstat, [3:0] T-Toe limit */
|
| 79 | struct rrlp_rga_eph_sv svs[0];
|
| 80 | } __attribute__((packed));
|
| 81 |
|
| 82 | struct rrlp_rga_eph_ext {
|
| 83 | uint8_t validity; /* in 4 hours units */
|
| 84 | } __attribute__((packed));
|
| 85 |
|
| 86 | struct rrlp_rga_eph_ext_check {
|
| 87 | /* weeks are in gps week modulo 4 */
|
| 88 | uint8_t wn_begin_end; /* [7:4] begin, [3:0] end */
|
| 89 | uint8_t tow_begin;
|
| 90 | uint8_t tow_end;
|
| 91 | } __attribute__((packed));
|
| 92 |
|
| 93 |
|
| 94 | /* Parsing function */
|
| 95 |
|
| 96 | int
|
| 97 | rrlp_decode_assistance_request(
|
| 98 | struct rrlp_assist_req *ar,
|
| 99 | void *req, int req_len)
|
| 100 | {
|
| 101 | struct rrlp_rga_hdr *hdr = NULL;
|
| 102 | struct rrlp_rga_eph *eph = NULL;
|
| 103 | struct rrlp_rga_eph_ext *eph_ext = NULL;
|
| 104 | struct rrlp_rga_eph_ext_check *eph_ext_check = NULL;
|
| 105 | int p = 0;
|
| 106 | int rc = 0;
|
| 107 |
|
| 108 | /* Reset */
|
| 109 | ar->req_elems = 0;
|
| 110 | ar->eph_svs = 0;
|
| 111 |
|
| 112 | /* Parse message */
|
| 113 | hdr = req;
|
| 114 | p += sizeof(struct rrlp_rga_hdr);
|
| 115 | if (p > req_len)
|
| 116 | return -1;
|
| 117 |
|
| 118 | if (hdr->items0 & RRLP_RGA0_NAV_MODEL) {
|
| 119 | printf("NAV_MODEL\n");
|
| 120 | eph = req + p;
|
| 121 | p += sizeof(struct rrlp_rga_eph);
|
| 122 | if (p > req_len)
|
| 123 | return -1;
|
| 124 | p += (eph->nsat_tmtoe >> 4) * sizeof(struct rrlp_rga_eph_sv);
|
| 125 | if (p > req_len)
|
| 126 | return -1;
|
| 127 |
|
| 128 | printf(" GPS week = %d\n", (eph->wn_hi << 8) + eph->wn_lo);
|
| 129 | printf(" TOE = %d\n", eph->toe);
|
| 130 | printf(" T-TOE limit = %d\n", eph->nsat_tmtoe & 0x0F);
|
| 131 |
|
| 132 | int i;
|
| 133 | for(i = 0; i < (eph->nsat_tmtoe >> 4); i++) {
|
| 134 | printf(" %2d: sv_id = %2d (%d) IODE = %3d\n", i, eph->svs[i].sv_id & 0x3F, eph->svs[i].sv_id >> 6, eph->svs[i].iode);
|
| 135 | if(eph->svs[i].sv_id >> 6) {
|
| 136 | /* most certainly invalid data or have to be interpreted differently */
|
| 137 | rc = -3;
|
| 138 | }
|
| 139 | }
|
| 140 | }
|
| 141 |
|
| 142 | if (hdr->items1 & RRLP_RGA1_EPH_EXT) {
|
| 143 | printf("EPH_EXT\n");
|
| 144 | eph_ext = req + p;
|
| 145 | p += sizeof(struct rrlp_rga_eph_ext);
|
| 146 | if (p > req_len)
|
| 147 | return -1;
|
| 148 | }
|
| 149 |
|
| 150 | if (hdr->items1 & RRLP_RGA1_EPH_EXT_CHECK) {
|
| 151 | printf("EPH_EXT_CHECK\n");
|
| 152 | eph_ext_check = req + p;
|
| 153 | p += sizeof(struct rrlp_rga_eph_ext_check);
|
| 154 | if (p > req_len)
|
| 155 | return -1;
|
| 156 | }
|
| 157 |
|
| 158 | if (p != req_len && (p != 2 || req_len != 6)) { /* P==2 && req_len == 6 might happen */
|
| 159 | fprintf(stderr, "p != req_len (%d %d)\n", p, req_len);
|
| 160 | return -2; /* not all bytes consumed ??? */
|
| 161 | }
|
| 162 |
|
| 163 | /* Print a warning for unsupported requests */
|
| 164 | if ((eph_ext != NULL) ||
|
| 165 | (eph_ext_check != NULL) ||
|
| 166 | (hdr->items0 & (RRLP_RGA0_DGPS | RRLP_RGA0_ACQ_ASSIST)) ||
|
| 167 | #if 0
|
| 168 | (hdr->items1 & RRLP_RGA1_REALTIME_INT)) {
|
| 169 | #else
|
| 170 | 0) {
|
| 171 | #endif
|
| 172 | fprintf(stderr, "[w] Unsupported assistance data requested, ignored ...\n");
|
| 173 |
|
| 174 | if(hdr->items0 & RRLP_RGA0_DGPS)
|
| 175 | printf("Unsupported assistance data requested: RRLP_RGA0_DGPS\n");
|
| 176 | if(hdr->items0 & RRLP_RGA0_ACQ_ASSIST)
|
| 177 | printf("Unsupported assistance data requested: RRLP_RGA0_ACQ_ASSIST\n");
|
| 178 | if(hdr->items1 & RRLP_RGA1_REALTIME_INT)
|
| 179 | printf("Unsupported assistance data requested: RRLP_RGA1_REALTIME_INT\n");
|
| 180 | }
|
| 181 |
|
| 182 | /* Copy the request */
|
| 183 | if (hdr->items0 & RRLP_RGA0_ALMANAC) {
|
| 184 | printf("ALMANAC\n");
|
| 185 | ar->req_elems |= RRLP_AR_ALMANAC;
|
| 186 | }
|
| 187 |
|
| 188 | if (hdr->items0 & RRLP_RGA0_UTC_MODEL) {
|
| 189 | printf("UTC_MODEL\n");
|
| 190 | ar->req_elems |= RRLP_AR_UTC_MODEL;
|
| 191 | }
|
| 192 |
|
| 193 | if (hdr->items0 & RRLP_RGA0_IONO_MODEL) {
|
| 194 | printf("IONO_MODEL\n");
|
| 195 | ar->req_elems |= RRLP_AR_IONO_MODEL;
|
| 196 | }
|
| 197 |
|
| 198 | if (hdr->items0 & RRLP_RGA0_REF_LOC) {
|
| 199 | printf("REF_LOC\n");
|
| 200 | ar->req_elems |= RRLP_AR_REF_LOC;
|
| 201 | }
|
| 202 |
|
| 203 | if (hdr->items0 & RRLP_RGA0_REF_TIME) {
|
| 204 | printf("REF_TIME\n");
|
| 205 | ar->req_elems |= RRLP_AR_REF_TIME;
|
| 206 | }
|
| 207 |
|
| 208 | if (hdr->items1 & RRLP_RGA1_REALTIME_INT) {
|
| 209 | printf("REALTIME_INTEGRITY\n");
|
| 210 | ar->req_elems |= RRLP_AR_REALTIME_INT;
|
| 211 | }
|
| 212 |
|
| 213 | if (hdr->items0 & RRLP_RGA0_NAV_MODEL) {
|
| 214 | printf("NAV_MODEL\n");
|
| 215 | int i, n_svs = eph->nsat_tmtoe >> 4;
|
| 216 | ar->req_elems |= RRLP_AR_EPHEMERIS;
|
| 217 | if(n_svs == 0) {
|
| 218 | ar->eph_svs = 0xFFFFFFFFFFFFFFFFULL;
|
| 219 | }
|
| 220 | else {
|
| 221 | for (i=0; i<n_svs; i++)
|
| 222 | ar->eph_svs |= (1ULL << (eph->svs[i].sv_id - 1)); /* Dieter: CHECK */
|
| 223 | }
|
| 224 | }
|
| 225 |
|
| 226 | return rc;
|
| 227 | }
|
| 228 |
|
| 229 | /* }}} */
|
| 230 |
|
| 231 |
|
| 232 | /* ------------------------------------------------------------------------ */
|
| 233 | /* RRLP elements fill */
|
| 234 | /* ---------------------------------------------------------------------{{{ */
|
| 235 |
|
| 236 | /* Helpers */
|
| 237 |
|
| 238 | static void
|
| 239 | _ts_23_032_store_latitude(double lat, uint8_t *b)
|
| 240 | {
|
| 241 | uint32_t x;
|
| 242 | x = (uint32_t) floor(fabs(lat/90.0) * ((double)(1<<23)));
|
| 243 | if (x >= (1<<23))
|
| 244 | x = (1<<23) - 1;
|
| 245 | if (lat < 0.0)
|
| 246 | x |= (1<<23);
|
| 247 | b[0] = (x >> 16) & 0xff;
|
| 248 | b[1] = (x >> 8) & 0xff;
|
| 249 | b[2] = x & 0xff;
|
| 250 | }
|
| 251 |
|
| 252 | static void
|
| 253 | _ts_23_032_store_longitude(double lon, uint8_t *b)
|
| 254 | {
|
| 255 | int32_t x;
|
| 256 | x = floor((lon/360.0) * ((double)(1<<24)));
|
| 257 | if (x >= (1<<23))
|
| 258 | x = 0x007fffff;
|
| 259 | else if (x < -(1<<23))
|
| 260 | x = 0x00800000;
|
| 261 | b[0] = (x >> 16) & 0xff;
|
| 262 | b[1] = (x >> 8) & 0xff;
|
| 263 | b[2] = x & 0xff;
|
| 264 | }
|
| 265 |
|
| 266 | static void
|
| 267 | _ts_23_032_store_altitude(double alt, uint8_t *b)
|
| 268 | {
|
| 269 | int alt_i = (int)fabs(alt);
|
| 270 | b[0] = ((alt_i >> 8) & 0x7f) | (alt<0.0 ? 0x80 : 0x00);
|
| 271 | b[1] = alt_i & 0xff;
|
| 272 | }
|
| 273 |
|
| 274 |
|
| 275 | /* Fill methods */
|
| 276 |
|
| 277 | static void
|
| 278 | _rrlp_fill_navigation_model_element(
|
| 279 | struct NavModelElement *rrlp_nme,
|
| 280 | struct gps_ephemeris_sv *gps_eph_sv)
|
| 281 | {
|
| 282 | struct UncompressedEphemeris *rrlp_eph;
|
| 283 |
|
| 284 | #if 1
|
| 285 | rrlp_nme->satStatus.present = SatStatus_PR_newSatelliteAndModelUC;
|
| 286 | rrlp_eph = &rrlp_nme->satStatus.choice.newSatelliteAndModelUC;
|
| 287 | #else /* does this make any difference fro the MS !? */
|
| 288 | rrlp_nme->satStatus.present = SatStatus_PR_newNaviModelUC;
|
| 289 | rrlp_eph = &rrlp_nme->satStatus.choice.newNaviModelUC;
|
| 290 | #endif
|
| 291 |
|
| 292 | rrlp_nme->satelliteID = gps_eph_sv->sv_id - 1; /* Dieter: satellite ID is zero based */
|
| 293 |
|
| 294 |
|
| 295 | rrlp_eph->ephemCodeOnL2 = gps_eph_sv->code_on_l2;
|
| 296 | rrlp_eph->ephemURA = gps_eph_sv->sv_ura;
|
| 297 | rrlp_eph->ephemSVhealth = gps_eph_sv->sv_health;
|
| 298 | rrlp_eph->ephemIODC = gps_eph_sv->iodc;
|
| 299 | rrlp_eph->ephemL2Pflag = gps_eph_sv->l2_p_flag;
|
| 300 | rrlp_eph->ephemTgd = gps_eph_sv->t_gd;
|
| 301 | rrlp_eph->ephemToc = gps_eph_sv->t_oc;
|
| 302 | rrlp_eph->ephemAF2 = gps_eph_sv->a_f2;
|
| 303 | rrlp_eph->ephemAF1 = gps_eph_sv->a_f1;
|
| 304 | rrlp_eph->ephemAF0 = gps_eph_sv->a_f0;
|
| 305 | rrlp_eph->ephemCrs = gps_eph_sv->c_rs;
|
| 306 | rrlp_eph->ephemDeltaN = gps_eph_sv->delta_n;
|
| 307 | rrlp_eph->ephemM0 = gps_eph_sv->m_0;
|
| 308 | rrlp_eph->ephemCuc = gps_eph_sv->c_uc;
|
| 309 | rrlp_eph->ephemE = gps_eph_sv->e;
|
| 310 | rrlp_eph->ephemCus = gps_eph_sv->c_us;
|
| 311 | rrlp_eph->ephemAPowerHalf = gps_eph_sv->a_powhalf;
|
| 312 | rrlp_eph->ephemToe = gps_eph_sv->t_oe;
|
| 313 | rrlp_eph->ephemFitFlag = gps_eph_sv->fit_flag;
|
| 314 | rrlp_eph->ephemAODA = gps_eph_sv->aodo;
|
| 315 | rrlp_eph->ephemCic = gps_eph_sv->c_ic;
|
| 316 | rrlp_eph->ephemOmegaA0 = gps_eph_sv->omega_0;
|
| 317 | rrlp_eph->ephemCis = gps_eph_sv->c_is;
|
| 318 | rrlp_eph->ephemI0 = gps_eph_sv->i_0;
|
| 319 | rrlp_eph->ephemCrc = gps_eph_sv->c_rc;
|
| 320 | rrlp_eph->ephemW = gps_eph_sv->w;
|
| 321 | rrlp_eph->ephemOmegaADot = gps_eph_sv->omega_dot;
|
| 322 | rrlp_eph->ephemIDot = gps_eph_sv->idot;
|
| 323 |
|
| 324 | rrlp_eph->ephemSF1Rsvd.reserved1 = gps_eph_sv->_rsvd1;
|
| 325 | rrlp_eph->ephemSF1Rsvd.reserved2 = gps_eph_sv->_rsvd2;
|
| 326 | rrlp_eph->ephemSF1Rsvd.reserved3 = gps_eph_sv->_rsvd3;
|
| 327 | rrlp_eph->ephemSF1Rsvd.reserved4 = gps_eph_sv->_rsvd4;
|
| 328 | }
|
| 329 |
|
| 330 | static void
|
| 331 | _rrlp_fill_almanac_element(
|
| 332 | struct AlmanacElement *rrlp_ae,
|
| 333 | struct gps_almanac_sv *gps_alm_sv)
|
| 334 | {
|
| 335 | rrlp_ae->satelliteID = gps_alm_sv->sv_id - 1; /* Dieter: satellite ID is zero based */
|
| 336 |
|
| 337 | rrlp_ae->almanacE = gps_alm_sv->e;
|
| 338 | rrlp_ae->alamanacToa = gps_alm_sv->t_oa;
|
| 339 | rrlp_ae->almanacKsii = gps_alm_sv->ksii;
|
| 340 | rrlp_ae->almanacOmegaDot = gps_alm_sv->omega_dot;
|
| 341 | rrlp_ae->almanacSVhealth = gps_alm_sv->sv_health;
|
| 342 | rrlp_ae->almanacAPowerHalf = gps_alm_sv->a_powhalf;
|
| 343 | rrlp_ae->almanacOmega0 = gps_alm_sv->omega_0;
|
| 344 | rrlp_ae->almanacW = gps_alm_sv->w;
|
| 345 | rrlp_ae->almanacM0 = gps_alm_sv->m_0;
|
| 346 | rrlp_ae->almanacAF0 = gps_alm_sv->a_f0;
|
| 347 | rrlp_ae->almanacAF1 = gps_alm_sv->a_f1;
|
| 348 |
|
| 349 | }
|
| 350 |
|
| 351 | static void
|
| 352 | _rrlp_fill_ionospheric_model(
|
| 353 | struct IonosphericModel *rrlp_iono,
|
| 354 | struct gps_ionosphere_model *gps_iono)
|
| 355 | {
|
| 356 | rrlp_iono->alfa0 = gps_iono->alpha_0;
|
| 357 | rrlp_iono->alfa1 = gps_iono->alpha_1;
|
| 358 | rrlp_iono->alfa2 = gps_iono->alpha_2;
|
| 359 | rrlp_iono->alfa3 = gps_iono->alpha_3;
|
| 360 | rrlp_iono->beta0 = gps_iono->beta_0;
|
| 361 | rrlp_iono->beta1 = gps_iono->beta_1;
|
| 362 | rrlp_iono->beta2 = gps_iono->beta_2;
|
| 363 | rrlp_iono->beta3 = gps_iono->beta_3;
|
| 364 | }
|
| 365 |
|
| 366 | static void
|
| 367 | _rrlp_fill_utc_model(
|
| 368 | struct UTCModel *rrlp_utc,
|
| 369 | struct gps_utc_model *gps_utc)
|
| 370 | {
|
| 371 | rrlp_utc->utcA1 = gps_utc->a1;
|
| 372 | rrlp_utc->utcA0 = gps_utc->a0;
|
| 373 | rrlp_utc->utcTot = gps_utc->t_ot;
|
| 374 | rrlp_utc->utcWNt = gps_utc->wn_t & 0xff;
|
| 375 | rrlp_utc->utcDeltaTls = gps_utc->delta_t_ls;
|
| 376 | rrlp_utc->utcWNlsf = gps_utc->wn_lsf & 0xff;
|
| 377 | rrlp_utc->utcDN = gps_utc->dn;
|
| 378 | rrlp_utc->utcDeltaTlsf = gps_utc->delta_t_lsf;
|
| 379 |
|
| 380 | printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcA1);
|
| 381 | printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcA0);
|
| 382 | printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcTot);
|
| 383 | printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcWNt);
|
| 384 | printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcDeltaTls);
|
| 385 | printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcWNlsf);
|
| 386 | printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcDN);
|
| 387 | printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcDeltaTlsf);
|
| 388 | }
|
| 389 |
|
| 390 | /* }}} */
|
| 391 |
|
| 392 |
|
| 393 | /* ------------------------------------------------------------------------ */
|
| 394 | /* RRLP Assistance PDU Generation */
|
| 395 | /* ---------------------------------------------------------------------{{{ */
|
| 396 |
|
| 397 | struct PDU *
|
| 398 | _rrlp_create_gps_assist_pdu(int refnum, struct GPS_AssistData **o_gps_ad, int iLast)
|
| 399 | {
|
| 400 | struct PDU *pdu;
|
| 401 | struct GPS_AssistData *gps_ad;
|
| 402 | MoreAssDataToBeSent_t *moreAssDataToBeSent;
|
| 403 |
|
| 404 | pdu = calloc(1, sizeof(*pdu));
|
| 405 | if (!pdu)
|
| 406 | return NULL;
|
| 407 |
|
| 408 | gps_ad = calloc(1, sizeof(*gps_ad));
|
| 409 | if (!gps_ad) {
|
| 410 | free(pdu);
|
| 411 | return NULL;
|
| 412 | }
|
| 413 |
|
| 414 | moreAssDataToBeSent = calloc(1, sizeof(*moreAssDataToBeSent));
|
| 415 | if (!moreAssDataToBeSent) {
|
| 416 | free(gps_ad);
|
| 417 | free(pdu);
|
| 418 | return NULL;
|
| 419 | }
|
| 420 | /* last message or more messages ? */
|
| 421 | if(iLast) {
|
| 422 | if(asn_long2INTEGER(moreAssDataToBeSent, MoreAssDataToBeSent_noMoreMessages) != 0)
|
| 423 | fprintf(stderr, "encode error\n");
|
| 424 | } else {
|
| 425 | if(asn_long2INTEGER(moreAssDataToBeSent, MoreAssDataToBeSent_moreMessagesOnTheWay) != 0)
|
| 426 | fprintf(stderr, "encode error\n");
|
| 427 | }
|
| 428 |
|
| 429 | if (o_gps_ad)
|
| 430 | *o_gps_ad = gps_ad;
|
| 431 |
|
| 432 | pdu->referenceNumber = refnum;
|
| 433 | pdu->component.present = RRLP_Component_PR_assistanceData;
|
| 434 | pdu->component.choice.assistanceData.gps_AssistData = gps_ad;
|
| 435 | pdu->component.choice.assistanceData.moreAssDataToBeSent = moreAssDataToBeSent;
|
| 436 |
|
| 437 | return pdu;
|
| 438 | }
|
| 439 |
|
| 440 | static int
|
| 441 | _rrlp_add_ionospheric_model(
|
| 442 | struct GPS_AssistData *rrlp_gps_ad,
|
| 443 | struct gps_assist_data *gps_ad)
|
| 444 | {
|
| 445 | struct IonosphericModel *rrlp_iono;
|
| 446 |
|
| 447 | if (!(gps_ad->fields & GPS_FIELD_IONOSPHERE))
|
| 448 | return -EINVAL;
|
| 449 |
|
| 450 | rrlp_iono = calloc(1, sizeof(*rrlp_iono));
|
| 451 | if (!rrlp_iono)
|
| 452 | return -ENOMEM;
|
| 453 | rrlp_gps_ad->controlHeader.ionosphericModel = rrlp_iono;
|
| 454 |
|
| 455 | _rrlp_fill_ionospheric_model(rrlp_iono, &gps_ad->ionosphere);
|
| 456 |
|
| 457 | return 0;
|
| 458 | }
|
| 459 |
|
| 460 | static int
|
| 461 | _rrlp_add_utc_model(
|
| 462 | struct GPS_AssistData *rrlp_gps_ad,
|
| 463 | struct gps_assist_data *gps_ad)
|
| 464 | {
|
| 465 | struct UTCModel *rrlp_utc;
|
| 466 |
|
| 467 | if (!(gps_ad->fields & GPS_FIELD_UTC))
|
| 468 | return -EINVAL;
|
| 469 |
|
| 470 | rrlp_utc = calloc(1, sizeof(*rrlp_utc));
|
| 471 | if (!rrlp_utc)
|
| 472 | return -ENOMEM;
|
| 473 | rrlp_gps_ad->controlHeader.utcModel = rrlp_utc;
|
| 474 |
|
| 475 | _rrlp_fill_utc_model(rrlp_utc, &gps_ad->utc);
|
| 476 |
|
| 477 | return 0;
|
| 478 | }
|
| 479 |
|
| 480 | static int
|
| 481 | _rrlp_add_reference_location(
|
| 482 | struct GPS_AssistData *rrlp_gps_ad,
|
| 483 | struct gps_assist_data *gps_ad)
|
| 484 | {
|
| 485 | #if 0 /* old */
|
| 486 | struct RefLocation *rrlp_refloc;
|
| 487 |
|
| 488 | /* FIXME: Check if info is in gps_ad */
|
| 489 |
|
| 490 | rrlp_refloc = calloc(1, sizeof(*rrlp_refloc));
|
| 491 | if (!rrlp_refloc)
|
| 492 | return -ENOMEM;
|
| 493 | rrlp_gps_ad->controlHeader.refLocation = rrlp_refloc;
|
| 494 |
|
| 495 | /* FIXME */
|
| 496 | {
|
| 497 | uint8_t gps_loc[] = {
|
| 498 | 0x80, /* Ellipsoid Point with altitude */
|
| 499 | #if 0
|
| 500 | 0x48, 0x0f, 0x93, /* 50.667778 N */
|
| 501 | 0x03, 0x47, 0x87, /* 4.611667 E */
|
| 502 | 0x00, 0x72, /* 114m */
|
| 503 | #else // Dieter
|
| 504 | 0x44, 0xEF, 0xEB,
|
| 505 | 0x09, 0x33, 0xAD,
|
| 506 | 0x01, 0xEC,
|
| 507 | #endif
|
| 508 | };
|
| 509 | uint8_t *b = malloc(sizeof(gps_loc));
|
| 510 | memcpy(b, gps_loc, sizeof(gps_loc));
|
| 511 | rrlp_refloc->threeDLocation.buf = b;
|
| 512 | rrlp_refloc->threeDLocation.size = sizeof(gps_loc);
|
| 513 | }
|
| 514 |
|
| 515 | return 0;
|
| 516 |
|
| 517 | #else /* new */
|
| 518 |
|
| 519 | struct RefLocation *rrlp_refloc;
|
| 520 | uint8_t *b;
|
| 521 |
|
| 522 | if (!(gps_ad->fields & GPS_FIELD_REFPOS))
|
| 523 | return -EINVAL;
|
| 524 |
|
| 525 | rrlp_refloc = calloc(1, sizeof(*rrlp_refloc));
|
| 526 | if (!rrlp_refloc)
|
| 527 | return -ENOMEM;
|
| 528 | rrlp_gps_ad->controlHeader.refLocation = rrlp_refloc;
|
| 529 |
|
| 530 | b = malloc(9);
|
| 531 |
|
| 532 | b[0] = 0x80; /* Ellipsoid Point with altitude */
|
| 533 | _ts_23_032_store_latitude(gps_ad->ref_pos.latitude, &b[1]);
|
| 534 | _ts_23_032_store_longitude(gps_ad->ref_pos.longitude, &b[4]);
|
| 535 | _ts_23_032_store_altitude(gps_ad->ref_pos.altitude, &b[7]);
|
| 536 |
|
| 537 | rrlp_refloc->threeDLocation.buf = b;
|
| 538 | rrlp_refloc->threeDLocation.size = 9;
|
| 539 |
|
| 540 | return 0;
|
| 541 | #endif
|
| 542 | }
|
| 543 |
|
| 544 | static int
|
| 545 | _rrlp_add_reference_time(
|
| 546 | struct GPS_AssistData *rrlp_gps_ad,
|
| 547 | struct gps_assist_data *gps_ad)
|
| 548 | {
|
| 549 | #if 0 /* old */
|
| 550 | struct ReferenceTime *rrlp_reftime;
|
| 551 |
|
| 552 | /* FIXME: Check if info is in gps_ad */
|
| 553 |
|
| 554 | rrlp_reftime = calloc(1, sizeof(*rrlp_reftime));
|
| 555 | if (!rrlp_reftime)
|
| 556 | return -ENOMEM;
|
| 557 | rrlp_gps_ad->controlHeader.referenceTime = rrlp_reftime;
|
| 558 |
|
| 559 | /* FIXME */
|
| 560 | // rrlp_reftime.gpsTime.gpsTOW23b = g_gps_tow / 80; /* 23 bits */
|
| 561 | // rrlp_reftime.gpsTime.gpsWeek = g_gps_week & 0x3ff; /* 10 bits */
|
| 562 |
|
| 563 | #if 1 // Dieter
|
| 564 | printf("Time %d\n", (int)time(NULL));
|
| 565 | //rrlp_reftime->gpsTime.gpsTOW23b = (time(NULL) - 1261643144) + 375961;
|
| 566 | rrlp_reftime->gpsTime.gpsTOW23b = (time(NULL) - 1281949530) + 119148;
|
| 567 | printf("GPS TOW %d\n", (int)rrlp_reftime->gpsTime.gpsTOW23b);
|
| 568 | rrlp_reftime->gpsTime.gpsTOW23b = (uint32_t)((double)rrlp_reftime->gpsTime.gpsTOW23b * 12.5 + 0.5) & 0x7FFFFF;
|
| 569 | //rrlp_reftime->gpsTime.gpsWeek = 1563 & 0x3FF; // KW52 2009
|
| 570 | //rrlp_reftime->gpsTime.gpsWeek = 1565 & 0x3FF; // KW1 2010
|
| 571 | rrlp_reftime->gpsTime.gpsWeek = 1597 & 0x3FF; // KW33 2010
|
| 572 | #endif
|
| 573 |
|
| 574 | return 0;
|
| 575 | #else /* new */
|
| 576 | struct ReferenceTime *rrlp_reftime;
|
| 577 | double tow;
|
| 578 |
|
| 579 | if (!(gps_ad->fields & GPS_FIELD_REFTIME))
|
| 580 | return -EINVAL;
|
| 581 |
|
| 582 | rrlp_reftime = calloc(1, sizeof(*rrlp_reftime));
|
| 583 | if (!rrlp_reftime)
|
| 584 | return -ENOMEM;
|
| 585 | rrlp_gps_ad->controlHeader.referenceTime = rrlp_reftime;
|
| 586 |
|
| 587 | /* TODO: adjust offset so that MS receives the correct time ? */
|
| 588 | #define MY_OFFSET 0
|
| 589 |
|
| 590 | tow = gps_ad->ref_time.tow + (time(NULL) - gps_ad->ref_time.when) + MY_OFFSET;
|
| 591 | printf("TOW = %f\n", tow);
|
| 592 |
|
| 593 | rrlp_reftime->gpsTime.gpsWeek = gps_ad->ref_time.wn & 0x3ff; /* 10b */
|
| 594 | rrlp_reftime->gpsTime.gpsTOW23b =
|
| 595 | ((int)floor(tow / 0.08)) & 0x7fffff; /* 23b */
|
| 596 |
|
| 597 | return 0;
|
| 598 | #endif
|
| 599 | }
|
| 600 |
|
| 601 | static int
|
| 602 | _rrlp_add_realtime_integrity(
|
| 603 | struct GPS_AssistData *rrlp_gps_ad,
|
| 604 | struct gps_assist_data *gps_ad)
|
| 605 | {
|
| 606 | struct SeqOf_BadSatelliteSet *rrlp_realtime_integrity;
|
| 607 |
|
| 608 | #if 0 /* not yet */
|
| 609 | if (!(gps_ad->fields & GPS_FIELD_REALTIME_INT))
|
| 610 | return -EINVAL;
|
| 611 | #endif
|
| 612 |
|
| 613 | rrlp_realtime_integrity = calloc(1, sizeof(*rrlp_realtime_integrity));
|
| 614 | if (!rrlp_realtime_integrity)
|
| 615 | return -ENOMEM;
|
| 616 |
|
| 617 | rrlp_gps_ad->controlHeader.realTimeIntegrity = rrlp_realtime_integrity;
|
| 618 |
|
| 619 | #if 1 /* TODO */
|
| 620 |
|
| 621 | SatelliteID_t *sa_id;
|
| 622 |
|
| 623 | sa_id = calloc(1, sizeof(*sa_id));
|
| 624 |
|
| 625 | *sa_id = 63;
|
| 626 |
|
| 627 | ASN_SEQUENCE_ADD(&rrlp_realtime_integrity->list, sa_id);
|
| 628 | #endif
|
| 629 |
|
| 630 | return 0;
|
| 631 | }
|
| 632 |
|
| 633 | static int
|
| 634 | _rrlp_add_almanac(
|
| 635 | struct GPS_AssistData *rrlp_gps_ad,
|
| 636 | struct gps_assist_data *gps_ad, int *start, int count)
|
| 637 | {
|
| 638 | int i;
|
| 639 | struct Almanac *rrlp_alm;
|
| 640 | struct gps_almanac *gps_alm = &gps_ad->almanac;
|
| 641 |
|
| 642 | if (!(gps_ad->fields & GPS_FIELD_ALMANAC))
|
| 643 | return -EINVAL;
|
| 644 |
|
| 645 | rrlp_alm = calloc(1, sizeof(*rrlp_alm));
|
| 646 | if (!rrlp_alm)
|
| 647 | return -ENOMEM;
|
| 648 | rrlp_gps_ad->controlHeader.almanac = rrlp_alm;
|
| 649 |
|
| 650 | rrlp_alm->alamanacWNa = gps_alm->wna;
|
| 651 | if (count == -1)
|
| 652 | count = gps_alm->n_sv - *start;
|
| 653 | for (i=*start; (i<*start+count) && (i<gps_alm->n_sv); i++) {
|
| 654 | struct AlmanacElement *ae;
|
| 655 | ae = calloc(1, sizeof(*ae));
|
| 656 | if (!ae)
|
| 657 | return -ENOMEM;
|
| 658 | _rrlp_fill_almanac_element(ae, &gps_alm->svs[i]);
|
| 659 | ASN_SEQUENCE_ADD(&rrlp_alm->almanacList.list, ae);
|
| 660 | }
|
| 661 |
|
| 662 | *start = i;
|
| 663 |
|
| 664 | return i < gps_alm->n_sv;
|
| 665 | }
|
| 666 |
|
| 667 | static int
|
| 668 | _rrlp_add_ephemeris(
|
| 669 | struct GPS_AssistData *rrlp_gps_ad,
|
| 670 | struct gps_assist_data *gps_ad, int *start, int count, uint64_t mask)
|
| 671 | {
|
| 672 | int i, j;
|
| 673 | struct NavigationModel *rrlp_nav;
|
| 674 | struct gps_ephemeris *gps_eph = &gps_ad->ephemeris;
|
| 675 |
|
| 676 | if (!(gps_ad->fields & GPS_FIELD_EPHEMERIS))
|
| 677 | return -EINVAL;
|
| 678 |
|
| 679 | rrlp_nav = calloc(1, sizeof(*rrlp_nav));
|
| 680 | if (!rrlp_nav)
|
| 681 | return -ENOMEM;
|
| 682 | rrlp_gps_ad->controlHeader.navigationModel = rrlp_nav;
|
| 683 |
|
| 684 | if (count == -1)
|
| 685 | count = gps_eph->n_sv - *start;
|
| 686 | for (i=*start,j=0; (j<count) && (i<gps_eph->n_sv); i++) {
|
| 687 | if (!(mask & (1ULL<<(gps_eph->svs[i].sv_id-1)))) /* Dieter: CHECK */
|
| 688 | continue;
|
| 689 | struct NavModelElement *nme;
|
| 690 | nme = calloc(1, sizeof(*nme));
|
| 691 | if (!nme)
|
| 692 | return -ENOMEM;
|
| 693 | _rrlp_fill_navigation_model_element(nme, &gps_eph->svs[i]);
|
| 694 | ASN_SEQUENCE_ADD(&rrlp_nav->navModelList.list, nme);
|
| 695 | j++;
|
| 696 | }
|
| 697 |
|
| 698 | *start = i;
|
| 699 |
|
| 700 | return i < gps_eph->n_sv;
|
| 701 | }
|
| 702 |
|
| 703 |
|
| 704 | #define MAX_PDUS 64
|
| 705 |
|
| 706 | int
|
| 707 | rrlp_gps_assist_pdus(int refnum,
|
| 708 | struct gps_assist_data *gps_ad, struct rrlp_assist_req *req,
|
| 709 | void **o_pdu, int *o_len, int o_max_pdus)
|
| 710 | {
|
| 711 | struct PDU *lst_pdu[MAX_PDUS];
|
| 712 | int lst_cnt = 0;
|
| 713 |
|
| 714 | struct PDU *rrlp_pdu = NULL;
|
| 715 | struct GPS_AssistData *rrlp_gps_ad = NULL;
|
| 716 | uint32_t re = req->req_elems;
|
| 717 | int i, rv = 0;
|
| 718 |
|
| 719 | /* IonosphericModel, UTCModel, RefLocation, ReferenceTime */
|
| 720 | if (re & (RRLP_AR_IONO_MODEL |
|
| 721 | RRLP_AR_UTC_MODEL |
|
| 722 | RRLP_AR_REF_TIME |
|
| 723 | RRLP_AR_REF_LOC |
|
| 724 | RRLP_AR_REALTIME_INT))
|
| 725 | {
|
| 726 | int pdu_has_data = 0;
|
| 727 |
|
| 728 | rrlp_pdu = _rrlp_create_gps_assist_pdu(refnum, &rrlp_gps_ad, 0);
|
| 729 | if (!rrlp_pdu) {
|
| 730 | rv = -ENOMEM;
|
| 731 | goto error;
|
| 732 | }
|
| 733 |
|
| 734 | #if 1 /* enable/disable component */
|
| 735 | if (re & RRLP_AR_IONO_MODEL) {
|
| 736 | printf("+ IONO_MODEL\n");
|
| 737 | if (!_rrlp_add_ionospheric_model(rrlp_gps_ad, gps_ad))
|
| 738 | pdu_has_data = 1;
|
| 739 | }
|
| 740 | #endif
|
| 741 | #if 1 /* enable/disable component */
|
| 742 | if (re & RRLP_AR_UTC_MODEL) {
|
| 743 | printf("+ UTC_MODEL\n");
|
| 744 | if (!_rrlp_add_utc_model(rrlp_gps_ad, gps_ad))
|
| 745 | pdu_has_data = 1;
|
| 746 | }
|
| 747 | #endif
|
| 748 | #if 1 /* enable/disable component */
|
| 749 | if (re & RRLP_AR_REF_TIME) {
|
| 750 | printf("+ REF_TIME\n");
|
| 751 | if (!_rrlp_add_reference_time(rrlp_gps_ad, gps_ad))
|
| 752 | pdu_has_data = 1;
|
| 753 | }
|
| 754 | #endif
|
| 755 | #if 1 /* enable/disable component */
|
| 756 | if (re & RRLP_AR_REF_LOC) {
|
| 757 | printf("+ REF_LOC\n");
|
| 758 | if (!_rrlp_add_reference_location(rrlp_gps_ad, gps_ad))
|
| 759 | pdu_has_data = 1;
|
| 760 | }
|
| 761 | #endif
|
| 762 | #if 0 /* enable/disable component (skip this as it is for now a dummy list only) */
|
| 763 | if (re & RRLP_AR_REALTIME_INT) {
|
| 764 | printf("+ REALTIME_INTEGRITY\n");
|
| 765 | if (!_rrlp_add_realtime_integrity(rrlp_gps_ad, gps_ad))
|
| 766 | pdu_has_data = 1;
|
| 767 | }
|
| 768 | #endif
|
| 769 |
|
| 770 | if (pdu_has_data) {
|
| 771 | lst_pdu[lst_cnt++] = rrlp_pdu;
|
| 772 | rrlp_pdu = NULL;
|
| 773 | }
|
| 774 | }
|
| 775 | #if 1 /* enable/disable component */
|
| 776 | /* Almanac */
|
| 777 | if (re & RRLP_AR_ALMANAC) {
|
| 778 | i = 0;
|
| 779 | do {
|
| 780 | if (!(gps_ad->fields & GPS_FIELD_ALMANAC))
|
| 781 | break;
|
| 782 |
|
| 783 | printf("+ ALMANAC\n");
|
| 784 |
|
| 785 | if (!rrlp_pdu) {
|
| 786 | rrlp_pdu = _rrlp_create_gps_assist_pdu(refnum, &rrlp_gps_ad, 0);
|
| 787 | if (!rrlp_pdu) {
|
| 788 | rv = -ENOMEM;
|
| 789 | goto error;
|
| 790 | }
|
| 791 | }
|
| 792 |
|
| 793 | /* adjust count so that messages fit in a single PDU */
|
| 794 | rv = _rrlp_add_almanac(rrlp_gps_ad, gps_ad, &i, 10);
|
| 795 | if (rv < 0)
|
| 796 | goto error;
|
| 797 |
|
| 798 | lst_pdu[lst_cnt++] = rrlp_pdu;
|
| 799 | rrlp_pdu = NULL;
|
| 800 | } while (rv);
|
| 801 | }
|
| 802 | #endif
|
| 803 | #if 1 /* enable/disable component */
|
| 804 | /* Ephemeris */
|
| 805 | if (re & RRLP_AR_EPHEMERIS) {
|
| 806 | i = 0;
|
| 807 | do {
|
| 808 | if (!(gps_ad->fields & GPS_FIELD_EPHEMERIS))
|
| 809 | break;
|
| 810 |
|
| 811 | printf("+ EPHEMERIS\n");
|
| 812 |
|
| 813 | if (!rrlp_pdu) {
|
| 814 | rrlp_pdu = _rrlp_create_gps_assist_pdu(refnum, &rrlp_gps_ad, 0);
|
| 815 | if (!rrlp_pdu) {
|
| 816 | rv = -ENOMEM;
|
| 817 | goto error;
|
| 818 | }
|
| 819 | }
|
| 820 | #if 1 /* two sets in one PDS to be not larger than maximum message size */
|
| 821 | rv = _rrlp_add_ephemeris(rrlp_gps_ad, gps_ad, &i, 2, req->eph_svs);
|
| 822 | #elif 0 /* three sets in one PDU, too large */
|
| 823 | rv = _rrlp_add_ephemeris(rrlp_gps_ad, gps_ad, &i, 3, req->eph_svs);
|
| 824 | #elif 0 /* all sets in one PDU, too large */
|
| 825 | rv = _rrlp_add_ephemeris(rrlp_gps_ad, gps_ad, &i, -1, req->eph_svs);
|
| 826 | #endif
|
| 827 | lst_pdu[lst_cnt++] = rrlp_pdu;
|
| 828 | rrlp_pdu = NULL;
|
| 829 |
|
| 830 | } while (rv);
|
| 831 | }
|
| 832 | #endif
|
| 833 |
|
| 834 | /* set last message flag */
|
| 835 | if(asn_long2INTEGER(lst_pdu[lst_cnt - 1]->component.choice.assistanceData.moreAssDataToBeSent, MoreAssDataToBeSent_noMoreMessages) != 0)
|
| 836 | fprintf(stderr, "encode error\n");
|
| 837 |
|
| 838 | /* Serialize & Release all PDUs */
|
| 839 | for (i=0; i<lst_cnt && i<o_max_pdus; i++) {
|
| 840 | /* Debugging, dump PDU */
|
| 841 | asn_fprint(stdout, &asn_DEF_PDU, lst_pdu[i]);
|
| 842 | rv = uper_encode_to_new_buffer(&asn_DEF_PDU, NULL, lst_pdu[i], &o_pdu[i]);
|
| 843 | if (rv < 0) {
|
| 844 | printf("uper_encode_to_new_buffer error %d (%d)\n", rv, i);
|
| 845 | goto error;
|
| 846 | }
|
| 847 | o_len[i] = rv;
|
| 848 | }
|
| 849 |
|
| 850 | return lst_cnt;
|
| 851 |
|
| 852 | /* Release ASN.1 objects */
|
| 853 | error:
|
| 854 | if (rrlp_pdu)
|
| 855 | asn_DEF_PDU.free_struct(&asn_DEF_PDU, (void*)rrlp_pdu, 0);
|
| 856 |
|
| 857 | for (i=0; i<lst_cnt; i++)
|
| 858 | asn_DEF_PDU.free_struct(&asn_DEF_PDU, lst_pdu[i], 0);
|
| 859 |
|
| 860 | return rv;
|
| 861 | }
|
| 862 |
|
| 863 | /* }}} */
|
| 864 |
|