From 47ab210af93a909e5eb8168d1ff58d2401519570 Mon Sep 17 00:00:00 2001 From: Harald Welte Date: Wed, 18 Jul 2012 22:04:09 +0200 Subject: [PATCH] import Dieter's rrlpd based on Sylvain's code from August 10, 2011 --- rrlpd/README | 46 + rrlpd/patches_OpenBSC/diff_gsm_04_08 | 27 + rrlpd/patches_OpenBSC/diff_rrlp | 289 ++++ .../01_fix_per_encoding_dieter.diff | 17 + rrlpd/src/Makefile | 41 + rrlpd/src/asn1/rrlp.asn | 1055 ++++++++++++++ rrlpd/src/gps.c | 131 ++ rrlpd/src/gps.h | 193 +++ rrlpd/src/main.c | 1298 +++++++++++++++++ rrlpd/src/rrlp.c | 864 +++++++++++ rrlpd/src/rrlp.h | 65 + rrlpd/src/ubx-parse.c | 246 ++++ rrlpd/src/ubx-parse.h | 45 + rrlpd/src/ubx.c | 96 ++ rrlpd/src/ubx.h | 240 +++ 15 files changed, 4653 insertions(+) create mode 100644 rrlpd/README create mode 100644 rrlpd/patches_OpenBSC/diff_gsm_04_08 create mode 100644 rrlpd/patches_OpenBSC/diff_rrlp create mode 100644 rrlpd/patches_asn1c/01_fix_per_encoding_dieter.diff create mode 100644 rrlpd/src/Makefile create mode 100644 rrlpd/src/asn1/rrlp.asn create mode 100644 rrlpd/src/gps.c create mode 100644 rrlpd/src/gps.h create mode 100644 rrlpd/src/main.c create mode 100644 rrlpd/src/rrlp.c create mode 100644 rrlpd/src/rrlp.h create mode 100644 rrlpd/src/ubx-parse.c create mode 100644 rrlpd/src/ubx-parse.h create mode 100644 rrlpd/src/ubx.c create mode 100644 rrlpd/src/ubx.h diff --git a/rrlpd/README b/rrlpd/README new file mode 100644 index 0000000..d49748c --- /dev/null +++ b/rrlpd/README @@ -0,0 +1,46 @@ +RRLP Server +------------ + +- Adjust ASN1C paths in the makefile (maybe adjust the makefile too) + +- ** VERY IMPORTANT **: apply ASN1C patch, otherwise invalid PDUs will + be generated + +- Patches for OpenBSC are "TODO" + +- Adjust IP Address of RRLP Server in OpenBSC rrlp.c (TODO: ajust code to + get this setting from config file) + +- Requires a u-Blox GPS receiver. The receiver is supposed to be connected + over its USB port. Some changes are problably required if the reciver is + connected through its UART port (see "#define GPS_USB ..." in main.c) + +- To work properly, the GPS receiver should already have a GPS fix + +- Enable RRLP in OpenBSC config file ("rrlp mode ms-based") + +- How to run: (Parameter: interface IP Address where to listen, + GPS receiver port), e.g.: + + ./rrlp-serv 192.168.1.1 /dev/ttyS0 + + +Issues: + + - "Work in Progress": code not yet properly organized and cleaned up + + - very verbose output for debugging/testing + + - rrlp.c: find out if data channel is slow (SDCCH) so that long assistance + data will not be sent + + - send an RRLP request not just when paging a phone + + - paging: sometimes no RRLP response is reveicved !? + + - Location update: response of the phone got lost, channel is closed too + early !? + + - GPS reference time: do we need an offset so that the time is correct when + the phone receives it ? + diff --git a/rrlpd/patches_OpenBSC/diff_gsm_04_08 b/rrlpd/patches_OpenBSC/diff_gsm_04_08 new file mode 100644 index 0000000..4ccd003 --- /dev/null +++ b/rrlpd/patches_OpenBSC/diff_gsm_04_08 @@ -0,0 +1,27 @@ +--- libmsc/gsm_04_08.c Mon Jul 18 11:19:21 2011 ++++ r:libmsc/gsm_04_08.c Tue Aug 09 14:34:30 2011 +@@ -1136,16 +1135,24 @@ + static int gsm48_rx_rr_app_info(struct gsm_subscriber_connection *conn, struct msgb *msg) + { + struct gsm48_hdr *gh = msgb_l3(msg); + u_int8_t apdu_id_flags; + u_int8_t apdu_len; + u_int8_t *apdu_data; + + apdu_id_flags = gh->data[0]; + apdu_len = gh->data[1]; + apdu_data = gh->data+2; + + DEBUGP(DNM, "RX APPLICATION INFO id/flags=0x%02x apdu_len=%u apdu=%s", + apdu_id_flags, apdu_len, osmo_hexdump(apdu_data, apdu_len)); ++ ++#if 1 /* RRLP Server */ ++ if(apdu_id_flags == 0x00) { /* RRLP */ ++ extern int handle_rrlp(struct gsm_subscriber_connection *conn, uint8_t *data, int len); ++ ++ handle_rrlp(conn, apdu_data, apdu_len); ++ } ++#endif + + return db_apdu_blob_store(conn->subscr, apdu_id_flags, apdu_len, apdu_data); + } diff --git a/rrlpd/patches_OpenBSC/diff_rrlp b/rrlpd/patches_OpenBSC/diff_rrlp new file mode 100644 index 0000000..6de751a --- /dev/null +++ b/rrlpd/patches_OpenBSC/diff_rrlp @@ -0,0 +1,289 @@ +--- libmsc/rrlp.c Mon Jul 18 11:19:21 2011 ++++ r:libmsc/rrlp.c Wed Aug 10 07:34:26 2011 +@@ -20,37 +20,317 @@ + */ + + + #include + + #include + #include + #include + #include + ++/* ----------------------------------------------- */ ++ ++/* TODO: move in a separate file ? */ ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#define RRLP_SERV_PORT 7890 ++#define RRLP_SERV_IP "192.168.5.250" /* TODO: from config file */ ++ ++#define MAX_RRLP_DATA 256 ++ ++/* Server cmds */ ++ ++#define RRLP_CMD_MS_DATA 1 /* data from MS */ ++#define RRLP_CMD_MS_DATA_SLOW 2 /* data from MS, slow channel */ ++ ++/* Server response */ ++ ++#define RRLP_RSP_ASSIST_DATA 1 /* assitance data, send to MS */ ++#define RRLP_RSP_RRLP_ERROR 2 /* RRLP error */ ++#define RRLP_RSP_RRLP_POSITION 3 /* RRLP position */ ++#define RRLP_RSP_ERROR 4 /* something went wrong */ ++ ++/* TODO: adjust error messages, use logging */ ++ ++static int rrlp_serv_cmd(struct gsm_subscriber_connection *conn, ++ uint8_t cmd, uint8_t *data, int len_data, ++ uint8_t *cmd_reply, uint8_t *reply, int *len_reply) ++{ ++ static int fd = -1; ++ static struct sockaddr_in sa; ++ int len; ++ uint8_t buf[2 + 1 + 8 + MAX_RRLP_DATA]; /* len, cmd, subscriber ID, data */ ++ int len_pkt, offs; ++ int rc; ++ long long unsigned int id; ++ struct sockaddr_in from; ++ int from_len; ++ fd_set readset; ++ struct timeval tv; ++ ++ if(len_data > MAX_RRLP_DATA) { ++ fprintf(stderr, "len_data > MAX_RRLP_DATA: %d\n", len_data); ++ return -1; ++ } ++ if(fd == -1) { ++ fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); ++ if(fd < 0) { ++ fprintf(stderr, "socket() failed: (%d) %s\n", fd, strerror(errno)); ++ return -1; ++ } ++ ++ sa.sin_family = AF_INET; ++ sa.sin_port = htons(RRLP_SERV_PORT); ++ if(inet_aton(RRLP_SERV_IP, &sa.sin_addr) != 1) { ++ fprintf(stderr, "inet_aton() failed: %s\n", strerror(errno)); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ ++ rc = connect(fd, (struct sockaddr *)&sa, sizeof(sa)); ++ if(rc < 0) { ++ fprintf(stderr, "connect() failed: (%d) %s\n", rc, strerror(errno)); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ } ++ ++ /* we are now connected */ ++ ++ id = conn->subscr->id; ++ ++ /* build cmd packet */ ++ ++ len_pkt = 2 + 1 + 8 + len_data; ++ buf[0] = len_pkt & 0xFF; ++ buf[1] = (len_pkt >> 8) & 0xFF; ++ ++ buf[2] = cmd; ++ ++ buf[3] = id & 0xFF; ++ buf[4] = (id >> 8) & 0xFF; ++ buf[5] = (id >> 16) & 0xFF; ++ buf[6] = (id >> 24) & 0xFF; ++ buf[7] = (id >> 32) & 0xFF; ++ buf[8] = (id >> 40) & 0xFF; ++ buf[9] = (id >> 48) & 0xFF; ++ buf[10] = (id >> 56) & 0xFF; ++ /* data */ ++ memcpy(&buf[11], data, len_data); ++ ++ /* send cmd */ ++ ++ len = sendto(fd, buf, len_pkt, 0, (struct sockaddr*)&sa, sizeof(sa)); ++ if(len < 0) { ++ fprintf(stderr, "sendto() failed: (%d) %s\n", len, strerror(errno)); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ if(len != len_pkt) { ++ fprintf(stderr, "sendto: len != len_pkt: %d %d\n", len, len_pkt); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ ++ /* wait at most 500 ms for a reply */ ++ ++ FD_ZERO(&readset); ++ FD_SET(fd, &readset); ++ tv.tv_sec = 0; ++ tv.tv_usec = 500 * 1000; ++ ++ /* this creates another UDP socket on Cygwin !? */ ++ rc = select(fd + 1, &readset, NULL, NULL, &tv); ++ if(rc < 0) { ++ fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno)); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ ++ if(!FD_ISSET(fd, &readset)) { ++ fprintf(stderr, "timeout select()\n"); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ ++ /* read packet */ ++ from_len = sizeof(from); ++ len = recvfrom(fd, buf, sizeof(buf), 0, (struct sockaddr*)&from, &from_len); ++ if(len < 0) { ++ fprintf(stderr, "recvfrom() failed: (%d) %s\n", len, strerror(errno)); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ if(len < 2) { ++ fprintf(stderr, "len < 2: %d\n", len); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ len_pkt = buf[0] + (buf[1] << 8); ++ if(len_pkt < 2 + 1) { ++ fprintf(stderr, "len_pkt < 2 + 1: %d\n", len_pkt); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ if(len != len_pkt) { ++ fprintf(stderr, "recvfrom: len != len_pkt: %d %d\n", len, len_pkt); ++ close(fd); ++ fd = -1; ++ return -1; ++ } ++ len_pkt -= 2; ++ offs = 2; ++ ++#if 0 /* dump packet */ ++ { ++ int i; ++ for(i = 0; i < len_pkt; i++) ++ printf("%02X ", buf[offs + i]); ++ printf("\n"); ++ } ++#endif ++ ++ /* process packet */ ++ ++ *len_reply = len_pkt - 1; ++ *cmd_reply = buf[offs]; ++ memcpy(reply, &buf[offs + 1], *len_reply); ++ ++ return 0; ++} ++ ++/* ----------------------------------------------- */ ++ ++static int send_rrlp_req(struct gsm_subscriber_connection *conn); ++ ++/* TODO: adjust error messages, use logging */ ++ ++int handle_rrlp(struct gsm_subscriber_connection *conn, uint8_t *data, int len) ++{ ++ struct gsm_network *net = conn->bts->network; ++ int rc; ++ uint8_t cmd_reply; ++ uint8_t reply[MAX_RRLP_DATA]; ++ int len_reply; ++ uint8_t cmd; ++ ++ if (net->rrlp.mode == RRLP_MODE_NONE) ++ return 0; ++ ++ if(len > MAX_RRLP_DATA) { ++ fprintf(stderr, "too many data for handle_rrlp (%d)\n", len); ++ return -1; ++ } ++ ++ /* TODO: decide if channel is slow (SDCCH), for slow channels ++ only short assistance data should be sent */ ++ ++ if(1) ++ cmd = RRLP_CMD_MS_DATA; ++ else ++ cmd = RRLP_CMD_MS_DATA_SLOW; ++ ++ rc = rrlp_serv_cmd(conn, cmd, data, len, &cmd_reply, reply, &len_reply); ++ if(rc != 0) { ++ fprintf(stderr, "rrlp_serv_cmd failed (%d)\n", rc); ++ return rc; ++ } ++ ++ if(cmd_reply == RRLP_RSP_ERROR) { ++ printf("RRLP Server error (general): %s\n", reply); ++ return 0; ++ } ++ ++ if(cmd_reply == RRLP_RSP_RRLP_ERROR) { ++ printf("RRLP Server error (RRLP): %s\n", reply); ++ return 0; ++ } ++ ++ if(cmd_reply == RRLP_RSP_RRLP_POSITION) { ++ long latitude; ++ long longitude; ++ long altitude; ++ ++ if(len_reply != 12) { ++ fprintf(stderr, "invalid RRLP position length (%d)\n", len_reply); ++ return -1; ++ } ++ ++ latitude = reply[0] + (reply[1] << 8) + (reply[2] << 16) + (reply[3] << 24); ++ longitude = reply[4] + (reply[5] << 8) + (reply[6] << 16) + (reply[7] << 24); ++ altitude = reply[8] + (reply[9] << 8) + (reply[10] << 16) + (reply[11] << 24); ++ ++ /* TODO: do something useful with the position */ ++ ++ printf("RRLP Server position: "); ++ printf("latitude = %f ", ((double)latitude * 90.0) / 0x800000L); ++ printf("longitude = %f ", ((double)longitude * 360.0) / 0x1000000L); ++ printf("altitude = %ld\n", altitude); ++ ++ return 0; ++ } ++ ++ if(cmd_reply == RRLP_RSP_ASSIST_DATA) { ++ printf("Assistance data, len %d\n", len_reply); ++ ++ /* ++ If there are assistance data, send them. If there are no more, ++ repeat the measurement request ++ */ ++ ++ if(len_reply) ++ return gsm48_send_rr_app_info(conn, 0x00, len_reply, reply); ++ else ++ send_rrlp_req(conn); ++ } ++ ++ return 0; ++} ++ diff --git a/rrlpd/patches_asn1c/01_fix_per_encoding_dieter.diff b/rrlpd/patches_asn1c/01_fix_per_encoding_dieter.diff new file mode 100644 index 0000000..a09c201 --- /dev/null +++ b/rrlpd/patches_asn1c/01_fix_per_encoding_dieter.diff @@ -0,0 +1,17 @@ +Index: skeletons/per_support.c +=================================================================== +--- skeletons/per_support.c (revision 1407) ++++ skeletons/per_support.c (working copy) +@@ -336,7 +336,12 @@ + buf[3] = bits; + else { + ASN_DEBUG("->[PER out split %d]", obits); ++#if 1 // Dieter ++ po->nboff -= obits; // undo incrementation from a few lines above ++ per_put_few_bits(po, bits >> (obits - 24), 24); // shift according to the rest of the bits ++#else + per_put_few_bits(po, bits >> 8, 24); ++#endif + per_put_few_bits(po, bits, obits - 24); + ASN_DEBUG("<-[PER out split %d]", obits); + } diff --git a/rrlpd/src/Makefile b/rrlpd/src/Makefile new file mode 100644 index 0000000..774a19d --- /dev/null +++ b/rrlpd/src/Makefile @@ -0,0 +1,41 @@ +# !! adjust as needed !! +ASN1C=/usr/local/bin/asn1c +ASN1_INCLUDE=/usr/src/asn1c/skeletons + +CC=gcc +# -DEMIT_ASN_DEBUG=1 ?? +CFLAGS=-I$(ASN1_INCLUDE) -I./asn1_gen -O3 -Wall + +ASN1_FILES=$(wildcard asn1/*.asn) + +all: rrlp-serv + +rrlp-serv: librrlp-asn1.a main.o rrlp.o ubx.o ubx-parse.o gps.o + $(CC) $(CFLAGS) -o $@ main.o rrlp.o ubx.o ubx-parse.o gps.o -L. -lrrlp-asn1 + +# +# ASN1 file autogeneration (need recursive makefile call) +# + +ASN1_SOURCES = $(wildcard asn1_gen/*.c) +ASN1_OBJECTS = $(ASN1_SOURCES:.c=.o) + +# -fnative-types ?? + +librrlp-asn1.a: $(ASN1_FILES) + mkdir -p asn1_gen && \ + cd asn1_gen && \ + $(ASN1C) -fskeletons-copy -gen-PER $(addprefix ../,$^) && \ + rm converter-sample.c Makefile.am.sample && \ + $(ASN1C) -gen-PER $(addprefix ../,$^) + @$(MAKE) librrlp-asn1.a.submake + +librrlp-asn1.a.submake: $(ASN1_OBJECTS) + $(AR) rcs librrlp-asn1.a $^ + +.PHONY: librrlp-asn1.a.submake + +clean: + rm -Rf asn1_gen + rm -f *.o rrlp-serv rrlp-serv.exe test-clnt test-clnt.exe + diff --git a/rrlpd/src/asn1/rrlp.asn b/rrlpd/src/asn1/rrlp.asn new file mode 100644 index 0000000..f7f5535 --- /dev/null +++ b/rrlpd/src/asn1/rrlp.asn @@ -0,0 +1,1055 @@ +RRLP-Messages +-- { RRLP-messages } + +DEFINITIONS AUTOMATIC TAGS ::= + +BEGIN + +--IMPORTS + +-- MsrPosition-Req, MsrPosition-Rsp, AssistanceData, +-- ProtocolError + +--FROM +-- RRLP-Components +-- { RRLP-Components } +--; + +--IMPORTS +-- Ext-GeographicalInformation +--FROM +-- MAP-LCS-DataTypes { +-- ccitt identified-organization (4) etsi (0) mobileDomain (0) +-- gsm-Network (1) modules (3) map-LCS-DataTypes (25) version5 (5)} + +-- ExtensionContainer +--FROM MAP-ExtensionDataTypes { +-- ccitt identified-organization (4) etsi (0) mobileDomain (0) +-- gsm-Network (1) modules (3) map-ExtensionDataTypes (21) version4 (4)} +-- ; +-- local import +-- maxExt-GeographicalInformation INTEGER ::= 20 +-- Ext-GeographicalInformation ::= OCTET STRING (SIZE (1..20)) +-- ExtensionContainer ::= OCTET STRING + +--ExtensionContainer ::= SEQUENCE { +-- privateExtensionList [0] IMPLICIT PrivateExtensionList OPTIONAL, +-- pcsExtensions [1] IMPLICIT PcsExtensions OPTIONAL, +-- ... } + +--PrivateExtensionList ::= SEQUENCE OF PrivateExtension + +--PrivateExtension ::= SEQUENCE { +-- extId OBJECT IDENTIFIER, MAP-EXTENSION .&extensionId +-- extType ANY OPTIONAL { @extId } +--} + +--PcsExtensions ::= SEQUENCE { +-- ... +-- } + + +PDU ::= SEQUENCE { + referenceNumber INTEGER (0..7), + component RRLP-Component +} + +RRLP-Component ::= CHOICE { + msrPositionReq MsrPosition-Req, + msrPositionRsp MsrPosition-Rsp, + assistanceData AssistanceData, + assistanceDataAck NULL, + protocolError ProtocolError, + ... +} + +--RRLP-Components +-- { RRLP-Components } +-- ETSI TS 144 031 V6.8.0 +--DEFINITIONS AUTOMATIC TAGS ::= + +--BEGIN + +--IMPORTS + +-- Ext-GeographicalInformation +--FROM +-- MAP-LCS-DataTypes { +-- ccitt identified-organization (4) etsi (0) mobileDomain (0) +-- gsm-Network (1) modules (3) map-LCS-DataTypes (25) version5 (5)} + +-- ExtensionContainer +--FROM MAP-ExtensionDataTypes { +-- ccitt identified-organization (4) etsi (0) mobileDomain (0) +-- gsm-Network (1) modules (3) map-ExtensionDataTypes (21) version4 (4)} +-- ; +-- Add here other ASN.1 definitions presented below +-- in chapters 4 and 5. + +-- add this definition to RRLP-Components module +-- Measurement Position request component + +MsrPosition-Req ::= SEQUENCE { + positionInstruct PositionInstruct, + referenceAssistData ReferenceAssistData OPTIONAL, + msrAssistData MsrAssistData OPTIONAL, + systemInfoAssistData SystemInfoAssistData OPTIONAL, + gps-AssistData GPS-AssistData OPTIONAL, + extensionContainer ExtensionContainer OPTIONAL, + ..., +-- Release 98 extension element + rel98-MsrPosition-Req-extension Rel98-MsrPosition-Req-Extension OPTIONAL, +-- Release 5 extension element + rel5-MsrPosition-Req-extension Rel5-MsrPosition-Req-Extension OPTIONAL +} + +-- add this defintion to RRLP-Components module +-- Measurement Position response component + +MsrPosition-Rsp ::= SEQUENCE { + multipleSets MultipleSets OPTIONAL, + referenceIdentity ReferenceIdentity OPTIONAL, + otd-MeasureInfo OTD-MeasureInfo OPTIONAL, + locationInfo LocationInfo OPTIONAL, + gps-MeasureInfo GPS-MeasureInfo OPTIONAL, + locationError LocationError OPTIONAL, + extensionContainer ExtensionContainer OPTIONAL, + ..., +-- Release extension here + rel-98-MsrPosition-Rsp-Extension Rel-98-MsrPosition-Rsp-Extension OPTIONAL, + rel-5-MsrPosition-Rsp-Extension Rel-5-MsrPosition-Rsp-Extension OPTIONAL +-- rel-5-MsrPosition-Rsp-Extension and other possible future extensions +-- are the only information elements that may be included in the 2nd +-- MsrPosition-Rsp component when RRLP pseudo-segmentation is used +} + +-- add this defintion to RRLP-Components module +-- Assistance Data component + +AssistanceData ::= SEQUENCE { + referenceAssistData ReferenceAssistData OPTIONAL, + msrAssistData MsrAssistData OPTIONAL, + systemInfoAssistData SystemInfoAssistData OPTIONAL, + gps-AssistData GPS-AssistData OPTIONAL, + moreAssDataToBeSent MoreAssDataToBeSent OPTIONAL, -- If not present, interpret as only +-- Assistance Data component used to +-- deliver entire set of assistance +-- data. + extensionContainer ExtensionContainer OPTIONAL, + ..., +-- Release extension here + rel98-AssistanceData-Extension Rel98-AssistanceData-Extension OPTIONAL, + rel5-AssistanceData-Extension Rel5-AssistanceData-Extension OPTIONAL +} + +-- add this defintion to RRLP-Components module +-- Protocol Error component + ProtocolError ::= SEQUENCE { + errorCause ErrorCodes, + extensionContainer ExtensionContainer OPTIONAL, + ..., +-- Release extensions here + rel-5-ProtocolError-Extension Rel-5-ProtocolError-Extension OPTIONAL +} + +PositionInstruct ::= SEQUENCE { +-- Method type + methodType MethodType, + positionMethod PositionMethod, + measureResponseTime MeasureResponseTime, + useMultipleSets UseMultipleSets, + environmentCharacter EnvironmentCharacter OPTIONAL +} + +-- +MethodType ::= CHOICE { + msAssisted AccuracyOpt, -- accuracy is optional + msBased Accuracy, -- accuracy is mandatory + msBasedPref Accuracy, -- accuracy is mandatory + msAssistedPref Accuracy -- accuracy is mandatory +} + +-- Accuracy of the location estimation +AccuracyOpt ::= SEQUENCE { + accuracy Accuracy OPTIONAL +} + +-- The values of this field are defined in 3GPP TS 23.032 (Uncertainty code) +Accuracy ::= INTEGER (0..127) + +-- Position Method +PositionMethod ::= ENUMERATED { + eotd (0), + gps (1), + gpsOrEOTD (2) +} + +-- Measurement request response time +MeasureResponseTime ::= INTEGER (0..7) + +-- useMultiple Sets, FFS! +UseMultipleSets ::= ENUMERATED { + multipleSets (0), -- multiple sets are allowed + oneSet (1) -- sending of multiple is not allowed +} + +-- Environment characterization +EnvironmentCharacter ::= ENUMERATED { + badArea (0), -- bad urban or suburban, heavy multipath and NLOS + notBadArea (1), -- light multipath and NLOS + mixedArea (2), -- not defined or mixed environment + ... +} +-- E-OTD reference BTS for Assitance data IE +ReferenceAssistData ::= SEQUENCE { + bcchCarrier BCCHCarrier, -- BCCH carrier + bsic BSIC, -- BSIC + timeSlotScheme TimeSlotScheme, -- Timeslot scheme + btsPosition BTSPosition OPTIONAL +} + +-- ellipsoid point and +-- ellipsoid point with altitude and uncertainty ellipsoid shapes are supported +BTSPosition ::= Ext-GeographicalInformation + +-- RF channel number of BCCH +BCCHCarrier ::= INTEGER (0..1023) + +-- Base station Identity Code +BSIC ::= INTEGER (0..63) + +-- Timeslot scheme +TimeSlotScheme ::= ENUMERATED { + equalLength (0), + variousLength (1) +} + +-- Time slot (modulo) +ModuloTimeSlot ::= INTEGER (0..3) + +-- E-OTD measurement assistance data IE +-- The total number of neighbors in this element (MsrAssistData) +-- and in SystemInfoAssistData element (presented neighbors +-- can be at a maximum 15!) +MsrAssistData ::= SEQUENCE { + msrAssistList SeqOfMsrAssistBTS +} + +SeqOfMsrAssistBTS ::= SEQUENCE (SIZE(1..15)) OF MsrAssistBTS + +MsrAssistBTS ::= SEQUENCE { + bcchCarrier BCCHCarrier, -- BCCH carrier + bsic BSIC, -- BSIC + multiFrameOffset MultiFrameOffset, -- multiframe offset + timeSlotScheme TimeSlotScheme, -- Timeslot scheme + roughRTD RoughRTD, -- rough RTD value +-- Location Calculation Assistance data is moved here + calcAssistanceBTS CalcAssistanceBTS OPTIONAL +} +-- Multiframe offset +MultiFrameOffset ::= INTEGER (0..51) +-- The Multiframe Offset value 51 shall not be encoded by the transmitting entity and +-- shall be treated by the receiving entity as 0. +-- Rough RTD value between one base station and reference BTS + +RoughRTD ::= INTEGER (0..1250) +-- The RoughRTD value 1250 shall not be encoded by the transmitting entity and shall +-- be treated by the receiving entity as 0. +-- E-OTD Measurement assistance data for system information List IE +-- The total number of base stations in this element (SystemInfoAssistData +-- presented neighbors) and in MsrAssistData element can be at a maximum 15. + +SystemInfoAssistData ::= SEQUENCE { + systemInfoAssistList SeqOfSystemInfoAssistBTS +} + +SeqOfSystemInfoAssistBTS::= SEQUENCE (SIZE(1..32)) OF SystemInfoAssistBTS +-- whether n.th is present or not ? + +SystemInfoAssistBTS ::= CHOICE { + notPresent NULL, + present AssistBTSData +} + +-- Actual assistance data for system information base station +AssistBTSData ::= SEQUENCE { + bsic BSIC, -- BSIC + multiFrameOffset MultiFrameOffset, -- multiframe offset + timeSlotScheme TimeSlotScheme, -- Timeslot scheme + roughRTD RoughRTD, -- rough RTD value +-- Location Calculation Assistance data + calcAssistanceBTS CalcAssistanceBTS OPTIONAL +} + +-- E-OTD Location calculation assistance data, +-- CalcAssistanceBTS element is optional not subfields +CalcAssistanceBTS ::= SEQUENCE { + fineRTD FineRTD, -- fine RTD value between base stations + referenceWGS84 ReferenceWGS84 -- reference coordinates +} + +-- Coordinates of neighbour BTS, WGS-84 ellipsoid +ReferenceWGS84 ::= SEQUENCE { + relativeNorth RelDistance, -- relative distance (south negative) + relativeEast RelDistance, -- relative distance (west negative) +-- Relative Altitude is not always known + relativeAlt RelativeAlt OPTIONAL -- relative altitude +} + +-- Fine RTD value between this BTS and the reference BTS +FineRTD ::= INTEGER (0..255) + +-- Relative north/east distance +RelDistance ::= INTEGER (-200000..200000) + +-- Relative altitude + +RelativeAlt ::= INTEGER (-4000..4000) +-- Measure position response IEs +-- Reference Identity +-- Multiple sets + +MultipleSets ::= SEQUENCE { + -- number of reference sets + nbrOfSets INTEGER (2..3), + -- This field actually tells the number of reference BTSs + nbrOfReferenceBTSs INTEGER (1..3), + -- This field is conditional and included optionally only if + -- nbrOfSets is 3 and number of reference BTSs is 2. + referenceRelation ReferenceRelation OPTIONAL +} + +-- Relation between refence BTSs and sets +ReferenceRelation ::= ENUMERATED { + secondBTSThirdSet (0), -- 1st BTS related to 1st and 2nd sets + secondBTSSecondSet (1), -- 1st BTS related to 1st and 3rd sets + firstBTSFirstSet (2) -- 1st BTS related to 1st set +} + +-- Reference BTS Identity, this element contains number of +-- BTSs told nbrOfReferenceBTSs field in Multiple sets element) + +ReferenceIdentity ::= SEQUENCE { + -- Reference BTS list + refBTSList SeqOfReferenceIdentityType +} +SeqOfReferenceIdentityType ::= SEQUENCE (SIZE(1..3)) OF ReferenceIdentityType + +-- Cell identity +ReferenceIdentityType ::= CHOICE { + bsicAndCarrier BSICAndCarrier, -- BSIC and Carrier + ci CellID, -- Cell ID, LAC not needed + requestIndex RequestIndex, -- Index to Requested Neighbor List + systemInfoIndex SystemInfoIndex, -- Index to System info list, this type of ref. identity + -- shall not be used by the MS unless it has received + -- the SystemInfoAssistData from the SMLC for this cell. + ciAndLAC CellIDAndLAC -- CI and LAC +} + +BSICAndCarrier ::= SEQUENCE { + carrier BCCHCarrier, + bsic BSIC +} + +RequestIndex ::= INTEGER (1..16) + +SystemInfoIndex ::= INTEGER (1..32) + +CellIDAndLAC ::= SEQUENCE { + referenceLAC LAC, -- Location area code + referenceCI CellID -- Cell identity +} + +CellID ::= INTEGER (0..65535) + +LAC ::= INTEGER (0..65535) + +-- OTD-MeasureInfo + +OTD-MeasureInfo ::= SEQUENCE { + -- Measurement info elements, OTD-MsrElement is repeated number of times + -- told in nbrOfReferenceBTSs in MultipleSets, default value is 1 + otdMsrFirstSets OTD-MsrElementFirst, + -- if more than one sets are present this element is repeated + -- NumberOfSets - 1 (-1 = first set) + otdMsrRestSets SeqOfOTD-MsrElementRest OPTIONAL +} + +SeqOfOTD-MsrElementRest ::= SEQUENCE (SIZE(1..2)) OF OTD-MsrElementRest + +-- OTD measurent information for 1 set +OTD-MsrElementFirst ::= SEQUENCE { + refFrameNumber INTEGER (0..42431), -- Frame number modulo 42432 + referenceTimeSlot ModuloTimeSlot, + toaMeasurementsOfRef TOA-MeasurementsOfRef OPTIONAL, + stdResolution StdResolution, + taCorrection INTEGER (0..960) OPTIONAL, -- TA correction +-- measured neighbors in OTD measurements + otd-FirstSetMsrs SeqOfOTD-FirstSetMsrs OPTIONAL +} + +SeqOfOTD-FirstSetMsrs ::= SEQUENCE (SIZE(1..10)) OF OTD-FirstSetMsrs + +-- OTD measurent information 2 and 3 sets if exist +OTD-MsrElementRest ::= SEQUENCE { + refFrameNumber INTEGER (0..42431), -- Frame number modulo 42432 + referenceTimeSlot ModuloTimeSlot, + toaMeasurementsOfRef TOA-MeasurementsOfRef OPTIONAL, + stdResolution StdResolution, + taCorrection INTEGER (0..960) OPTIONAL, -- TA correction + -- measured neighbors in OTD measurements + otd-MsrsOfOtherSets SeqOfOTD-MsrsOfOtherSets OPTIONAL +} + +SeqOfOTD-MsrsOfOtherSets ::= SEQUENCE (SIZE(1..10)) OF OTD-MsrsOfOtherSets + +-- Standard deviation of the TOA measurements from the reference BTS +TOA-MeasurementsOfRef ::= SEQUENCE { + refQuality RefQuality, + numOfMeasurements NumOfMeasurements +} + +RefQuality ::= INTEGER (0..31) -- St Dev of TOA of reference as defined in annex + +NumOfMeasurements ::= INTEGER (0..7) -- No. of measurements for RefQuality as defined in annex + +StdResolution ::= INTEGER (0..3) -- Values of resolution are defined in annex + +OTD-FirstSetMsrs ::= OTD-MeasurementWithID + +-- Neighbour info in OTD measurements 0-10 times in TD measurement info +OTD-MsrsOfOtherSets ::= CHOICE { + identityNotPresent OTD-Measurement, + identityPresent OTD-MeasurementWithID +} + +-- For this OTD measurement identity is same as the identity of BTS +-- in the first set with same sequence number +OTD-Measurement ::= SEQUENCE { + nborTimeSlot ModuloTimeSlot, + eotdQuality EOTDQuality, + otdValue OTDValue +} + +-- This measurement contains the BTS identity and measurement +OTD-MeasurementWithID ::=SEQUENCE { + neighborIdentity NeighborIdentity, + nborTimeSlot ModuloTimeSlot, + eotdQuality EOTDQuality, + otdValue OTDValue +} + +EOTDQuality ::= SEQUENCE { + nbrOfMeasurements INTEGER (0..7), + stdOfEOTD INTEGER (0..31) +} + +NeighborIdentity ::= CHOICE { + bsicAndCarrier BSICAndCarrier, -- BSIC and Carrier + ci CellID, -- Cell ID, LAC not needed + multiFrameCarrier MultiFrameCarrier, -- MultiFrameOffest and BSIC + requestIndex RequestIndex, -- Index to Requested Neighbor List + systemInfoIndex SystemInfoIndex, -- Index to System info list, this type of neighbour + -- identity shall not be used by the MS unless it has + -- received the SystemInfoAssistData from the SMLC for + -- this cell. + ciAndLAC CellIDAndLAC -- CI and LAC +} + +-- Multiframe and carrier +MultiFrameCarrier ::= SEQUENCE { + bcchCarrier BCCHCarrier, + multiFrameOffset MultiFrameOffset +} + +-- OTD measurement value for neighbour +OTDValue ::= INTEGER (0..39999) + +-- Location information IE +LocationInfo ::= SEQUENCE { + refFrame INTEGER (0..65535), -- Reference Frame number + -- If refFrame is within (42432..65535), it shall be ignored by the receiver + -- in that case the MS should provide GPS TOW if available + gpsTOW INTEGER (0..14399999) OPTIONAL, -- GPS TOW + fixType FixType, + -- Note that applicable range for refFrame is 0 - 42431 + -- Possible shapes carried in posEstimate are + -- ellipsoid point, + -- ellipsoid point with uncertainty circle + -- ellipsoid point with uncertainty ellipse + -- ellipsoid point with altitude and uncertainty ellipsoid + posEstimate Ext-GeographicalInformation +} + +FixType ::= INTEGER { + twoDFix (0), + threeDFix (1) +} (0..1) + +-- GPS-Measurement information +GPS-MeasureInfo ::= SEQUENCE { + -- Measurement info elements + -- user has to make sure that in this element is number of elements + -- defined in reference BTS identity + gpsMsrSetList SeqOfGPS-MsrSetElement +} + +SeqOfGPS-MsrSetElement ::= SEQUENCE (SIZE(1..3)) OF GPS-MsrSetElement + +-- OTD measurent information 1-3 times in message +GPS-MsrSetElement ::= SEQUENCE { + refFrame INTEGER (0..65535) OPTIONAL, -- Reference Frame number + gpsTOW GPSTOW24b, + -- GPS TOW + -- Note that applicable range for refFrame is 0 - 42431 + --N_SAT can be read from number of elements of gps-msrList + gps-msrList SeqOfGPS-MsrElement +} + +-- 24 bit presentation for GPSTOW +GPSTOW24b ::= INTEGER (0..14399999) + +-- measured elements in measurement parameters field +SeqOfGPS-MsrElement ::= SEQUENCE (SIZE(1..16)) OF GPS-MsrElement + +GPS-MsrElement ::= SEQUENCE { + satelliteID SatelliteID, -- Satellite identifier + cNo INTEGER (0..63), -- carrier noise ratio + doppler INTEGER (-32768..32767), -- doppler, mulltiply by 0.2 + wholeChips INTEGER (0..1022), -- whole value of the code phase measurement + fracChips INTEGER (0..1024), -- fractional value of the code phase measurement + -- a value of 1024 shall not be encoded by the sender + -- the receiver shall consider a value of 1024 to be + -- invalid data + mpathIndic MpathIndic, -- multipath indicator + pseuRangeRMSErr INTEGER (0..63) -- index +} + +-- Multipath indicator +MpathIndic ::= ENUMERATED { + notMeasured (0), + low (1), + medium (2), + high (3) +} + +-- Location error IE +LocationError ::= SEQUENCE { + locErrorReason LocErrorReason, + additionalAssistanceData AdditionalAssistanceData OPTIONAL, + ... +} + +LocErrorReason ::= ENUMERATED { + unDefined (0), + notEnoughBTSs (1), + notEnoughSats (2), + eotdLocCalAssDataMissing (3), + eotdAssDataMissing (4), + gpsLocCalAssDataMissing (5), + gpsAssDataMissing (6), + methodNotSupported (7), + notProcessed (8), + refBTSForGPSNotServingBTS (9), + refBTSForEOTDNotServingBTS (10), + ... +} + +-- exception handling: +-- an unrecognized value shall be treated the same as value 0 +-- defines additional assistance data needed for any new location attempt +-- MS shall retain any assistance data already received +AdditionalAssistanceData ::= SEQUENCE { + gpsAssistanceData GPSAssistanceData OPTIONAL, + extensionContainer ExtensionContainer OPTIONAL, +... +} + +GPSAssistanceData ::= OCTET STRING (SIZE (1..maxGPSAssistanceData)) +-- GPSAssistanceData has identical structure and encoding to octets 3 to n of the +-- GPS Assistance Data IE in 3GPP TS 49.031 +maxGPSAssistanceData INTEGER ::= 40 +-- Protocol Error Causes +ErrorCodes ::= ENUMERATED { + unDefined (0), + missingComponet (1), + incorrectData (2), + missingIEorComponentElement (3), + messageTooShort (4), + unknowReferenceNumber (5), + ... +} + +-- exception handling: +-- an unrecognized value shall be treated the same as value 0 +-- GPS assistance data IE +GPS-AssistData ::= SEQUENCE { + controlHeader ControlHeader +} + +-- More Assistance Data To Be Sent IE +-- More Assistance Data Components On the Way indication for delivery of an entire set of assistance +-- data in multiple Assistance Data components. +MoreAssDataToBeSent ::= ENUMERATED { + noMoreMessages (0), -- This is the only or last Assistance Data message used to deliver + -- the entire set of assistance data. + moreMessagesOnTheWay (1) -- The SMLC will send more Assistance Data messages or a final RRLP + -- Measure Position Request message to deliver the + -- the entire set of assistance data. +} + +-- Control header of the GPS assistance data +ControlHeader ::= SEQUENCE { + -- Field type Present information + referenceTime ReferenceTime OPTIONAL, + refLocation RefLocation OPTIONAL, + dgpsCorrections DGPSCorrections OPTIONAL, + navigationModel NavigationModel OPTIONAL, + ionosphericModel IonosphericModel OPTIONAL, + utcModel UTCModel OPTIONAL, + almanac Almanac OPTIONAL, + acquisAssist AcquisAssist OPTIONAL, + realTimeIntegrity SeqOf-BadSatelliteSet OPTIONAL +} + +ReferenceTime ::= SEQUENCE { + gpsTime GPSTime, + gsmTime GSMTime OPTIONAL, + gpsTowAssist GPSTOWAssist OPTIONAL +} + +-- GPS Time includes week number and time-of-week (TOW) +GPSTime ::= SEQUENCE { + gpsTOW23b GPSTOW23b, + gpsWeek GPSWeek +} + +-- GPSTOW, range 0-604799.92, resolution 0.08 sec, 23-bit presentation +GPSTOW23b ::= INTEGER (0..7559999) + +-- GPS week number +GPSWeek ::= INTEGER (0..1023) +-- GPSTOWAssist consists of TLM message, Anti-spoof flag, Alert flag, and 2 reserved bits in TLM Word +-- for each visible satellite. +-- N_SAT can be read from number of elements in GPSTOWAssist + +GPSTOWAssist ::= SEQUENCE (SIZE(1..12)) OF GPSTOWAssistElement + +GPSTOWAssistElement ::= SEQUENCE { + satelliteID SatelliteID, + tlmWord TLMWord, + antiSpoof AntiSpoofFlag, + alert AlertFlag, + tlmRsvdBits TLMReservedBits +} + +-- TLM Word, 14 bits +TLMWord ::= INTEGER (0..16383) + +-- Anti-Spoof flag +AntiSpoofFlag ::= INTEGER (0..1) + +-- Alert flag +AlertFlag ::= INTEGER (0..1) + +-- Reserved bits in TLM word, MSB occurs earlier in TLM Word transmitted by satellite +TLMReservedBits ::= INTEGER (0..3) + +GSMTime ::= SEQUENCE { + bcchCarrier BCCHCarrier, -- BCCH carrier + bsic BSIC, -- BSIC + frameNumber FrameNumber, + timeSlot TimeSlot, + bitNumber BitNumber +} + +-- Frame number +FrameNumber ::= INTEGER (0..2097151) + +-- Time slot number +TimeSlot ::= INTEGER (0..7) + +-- Bit number +BitNumber ::= INTEGER (0..156) + +-- Reference Location IE +RefLocation ::= SEQUENCE { + threeDLocation Ext-GeographicalInformation +} + +-- DGPS Corrections IE +DGPSCorrections ::= SEQUENCE { + gpsTOW INTEGER (0..604799), -- DGPS reference time + status INTEGER (0..7), +-- N_SAT can be read from number of elements of satList + satList SeqOfSatElement +} +SeqOfSatElement ::= SEQUENCE (SIZE (1..16)) OF SatElement + +-- number of correction for satellites +SatElement ::= SEQUENCE { + satelliteID SatelliteID, +--- Sequence number for ephemeris + iode INTEGER (0..239), +-- User Differential Range Error + udre INTEGER (0..3), +-- Pseudo Range Correction, range is +-- -655.04 - +655.04, + pseudoRangeCor INTEGER (-2047..2047), +-- Pseudo Range Rate Correction, range is +-- -4.064 - +4.064, + rangeRateCor INTEGER (-127..127), +-- Delta Pseudo Range Correction 2 + deltaPseudoRangeCor2 INTEGER (-127..127), -- This IE shall be ignored by the receiver and +-- set to zero by the sender +-- Delta Pseudo Range Correction 2 + deltaRangeRateCor2 INTEGER (-7..7), -- This IE shall be ignored by the receiver and +-- set to zero by the sender +-- Delta Pseudo Range Correction 3 + deltaPseudoRangeCor3 INTEGER (-127..127), -- This IE shall be ignored by the receiver and +-- set to zero by the sender +-- Delta Pseudo Range Correction 3 + deltaRangeRateCor3 INTEGER (-7..7) -- This IE shall be ignored by the receiver and +-- set to zero by the sender +} + +SatelliteID ::= INTEGER (0..63) -- identifies satellite + +-- Navigation Model IE +NavigationModel ::= SEQUENCE { + navModelList SeqOfNavModelElement +} + +-- navigation model satellite list +SeqOfNavModelElement ::= SEQUENCE (SIZE(1..16)) OF NavModelElement + +NavModelElement ::= SEQUENCE { + satelliteID SatelliteID, + satStatus SatStatus -- satellite status +} + +-- the Status of the navigation model +SatStatus ::= CHOICE { +-- New satellite, new Navigation Model + newSatelliteAndModelUC UncompressedEphemeris, +-- Existing satellite, Existing Navigation Model + oldSatelliteAndModel NULL, +-- Existing satellite, new Navigation Model + newNaviModelUC UncompressedEphemeris, + ... +} + +-- Uncompressed satellite emhemeris and clock corrections +UncompressedEphemeris ::= SEQUENCE { + ephemCodeOnL2 INTEGER (0..3), + ephemURA INTEGER (0..15), + ephemSVhealth INTEGER (0..63), + ephemIODC INTEGER (0..1023), + ephemL2Pflag INTEGER (0..1), + ephemSF1Rsvd EphemerisSubframe1Reserved, + ephemTgd INTEGER (-128..127), + ephemToc INTEGER (0..37799), + ephemAF2 INTEGER (-128..127), + ephemAF1 INTEGER (-32768..32767), + ephemAF0 INTEGER (-2097152..2097151), + ephemCrs INTEGER (-32768..32767), + ephemDeltaN INTEGER (-32768..32767), + ephemM0 INTEGER (-2147483648..2147483647), + ephemCuc INTEGER (-32768..32767), + ephemE INTEGER (0..4294967295), + ephemCus INTEGER (-32768..32767), + ephemAPowerHalf INTEGER (0..4294967295), + ephemToe INTEGER (0..37799), + ephemFitFlag INTEGER (0..1), + ephemAODA INTEGER (0..31), + ephemCic INTEGER (-32768..32767), + ephemOmegaA0 INTEGER (-2147483648..2147483647), + ephemCis INTEGER (-32768..32767), + ephemI0 INTEGER (-2147483648..2147483647), + ephemCrc INTEGER (-32768..32767), + ephemW INTEGER (-2147483648..2147483647), + ephemOmegaADot INTEGER (-8388608..8388607), + ephemIDot INTEGER (-8192..8191) +} + +-- Reserved bits in subframe 1 of navigation message +EphemerisSubframe1Reserved ::= SEQUENCE { + reserved1 INTEGER (0..8388607), -- 23-bit field + reserved2 INTEGER (0..16777215), -- 24-bit field + reserved3 INTEGER (0..16777215), -- 24-bit field + reserved4 INTEGER (0..65535) -- 16-bit field +} + +-- Ionospheric Model IE +IonosphericModel ::= SEQUENCE { + alfa0 INTEGER (-128..127), + alfa1 INTEGER (-128..127), + alfa2 INTEGER (-128..127), + alfa3 INTEGER (-128..127), + beta0 INTEGER (-128..127), + beta1 INTEGER (-128..127), + beta2 INTEGER (-128..127), + beta3 INTEGER (-128..127) +} + +-- Universal Time Coordinate Model +UTCModel ::= SEQUENCE { + utcA1 INTEGER (-8388608..8388607), + utcA0 INTEGER (-2147483648..2147483647), + utcTot INTEGER (0..255), + utcWNt INTEGER (0..255), + utcDeltaTls INTEGER (-128..127), + utcWNlsf INTEGER (0..255), + utcDN INTEGER (-128..127), + utcDeltaTlsf INTEGER (-128..127) +} + +-- Almanac, Long term model +-- NOTE: These are parameters are subset of the ephemeris +-- NOTE: But with reduced resolution and accuracy +Almanac ::= SEQUENCE { + alamanacWNa INTEGER (0..255), -- Once per message +-- navigation model satellite list. +-- The size of almanacList is actually Nums_Sats_Total field + almanacList SeqOfAlmanacElement +} + +SeqOfAlmanacElement ::= SEQUENCE (SIZE(1..64)) OF AlmanacElement + +-- Almanac info once per satellite +AlmanacElement ::= SEQUENCE { + satelliteID SatelliteID, + almanacE INTEGER (0..65535), + alamanacToa INTEGER (0..255), + almanacKsii INTEGER (-32768..32767), + almanacOmegaDot INTEGER (-32768..32767), + almanacSVhealth INTEGER (0..255), + almanacAPowerHalf INTEGER (0..16777215), + almanacOmega0 INTEGER (-8388608..8388607), + almanacW INTEGER (-8388608..8388607), + almanacM0 INTEGER (-8388608..8388607), + almanacAF0 INTEGER (-1024..1023), + almanacAF1 INTEGER (-1024..1023) +} + +-- Acquisition Assistance +AcquisAssist ::= SEQUENCE { +-- Number of Satellites can be read from acquistList + timeRelation TimeRelation, +-- Acquisition assistance list +-- The size of Number of Satellites is actually Number of Satellites field + acquisList SeqOfAcquisElement +} + +SeqOfAcquisElement ::= SEQUENCE (SIZE(1..16)) OF AcquisElement + +-- the relationship between GPS time and air-interface timing +TimeRelation ::= SEQUENCE { +-- + gpsTOW GPSTOW23b, -- 23b presentation + gsmTime GSMTime OPTIONAL +} + +-- data occuring per number of satellites +AcquisElement ::= SEQUENCE { + svid SatelliteID, +-- Doppler 0th order term, +-- -5120.0 - 5117.5 Hz (= -2048 - 2047 with 2.5 Hz resolution) + doppler0 INTEGER (-2048..2047), + addionalDoppler AddionalDopplerFields OPTIONAL, + codePhase INTEGER (0..1022), -- Code Phase + intCodePhase INTEGER (0..19), -- Integer Code Phase + gpsBitNumber INTEGER (0..3), -- GPS bit number + codePhaseSearchWindow INTEGER (0..15), -- Code Phase Search Window + addionalAngle AddionalAngleFields OPTIONAL +} + +AddionalDopplerFields ::= SEQUENCE { +-- Doppler 1st order term, -1.0 - +0.5 Hz/sec +-- (= -42 + (0 to 63) with 1/42 Hz/sec. resolution) + doppler1 INTEGER (0..63), + dopplerUncertainty INTEGER (0..7) +-- a sender shall not encode any DopplerUncertainty value in the range 5 to 7 +-- a receiver shall ignore any value between 5 and 7. +} + +AddionalAngleFields ::= SEQUENCE { +-- azimuth angle, 0 - 348.75 deg (= 0 - 31 with 11.25 deg resolution) + azimuth INTEGER (0..31), +-- elevation angle, 0 - 78.75 deg (= 0 - 7 with 11.25 deg resolution) + elevation INTEGER (0..7) +} + +-- Real-Time Integrity +-- number of bad satellites can be read from this element +SeqOf-BadSatelliteSet ::= SEQUENCE (SIZE(1..16)) OF SatelliteID + +-- Extension Elements +-- Release 98 Extensions here +Rel98-MsrPosition-Req-Extension ::= SEQUENCE { + rel98-Ext-ExpOTD Rel98-Ext-ExpOTD OPTIONAL, -- ExpectedOTD extension + ..., + gpsTimeAssistanceMeasurementRequest NULL OPTIONAL, + gpsReferenceTimeUncertainty GPSReferenceTimeUncertainty OPTIONAL +-- Further R98 extensions here +} + +Rel98-AssistanceData-Extension ::= SEQUENCE { + rel98-Ext-ExpOTD Rel98-Ext-ExpOTD OPTIONAL, -- ExpectedOTD extension + ..., + gpsTimeAssistanceMeasurementRequest NULL OPTIONAL, + gpsReferenceTimeUncertainty GPSReferenceTimeUncertainty OPTIONAL +-- Further R98 extensions here + +} +-- Release 98 ExpOTD extension +Rel98-Ext-ExpOTD ::= SEQUENCE { +-- If MsrAssistBTS is included in message, msrAssistData-R98-ExpOTD shall be included. + msrAssistData-R98-ExpOTD MsrAssistData-R98-ExpOTD OPTIONAL, +-- If SystemInfoAssistaData is included in message, systemInfoAssistData-R98-ExpOTD shall be +-- included. + systemInfoAssistData-R98-ExpOTD SystemInfoAssistData-R98-ExpOTD OPTIONAL +} + +-- MsrAssistData R98 extension +MsrAssistData-R98-ExpOTD ::= SEQUENCE { + msrAssistList-R98-ExpOTD SeqOfMsrAssistBTS-R98-ExpOTD +} + +-- Indexes in SeqOfMsrAssistBTS-R98-ExpOTD refer to SeqOfMsrAssistBTS +-- If the index exceeds the SegOfMsrAssistBTS range or if there is other +-- inconsistencies between the BTS indices, the MS shall apply protocol +-- error cause incorrectData +SeqOfMsrAssistBTS-R98-ExpOTD ::= SEQUENCE (SIZE(1..15)) OF MsrAssistBTS-R98-ExpOTD + +-- This element completes MsrAssistBTS IE +MsrAssistBTS-R98-ExpOTD ::= SEQUENCE { + expectedOTD ExpectedOTD, + expOTDUncertainty ExpOTDUncertainty +} + +-- SystemInfoAssistData R98 extension +SystemInfoAssistData-R98-ExpOTD ::= SEQUENCE { + systemInfoAssistListR98-ExpOTD SeqOfSystemInfoAssistBTS-R98-ExpOTD +} + +-- SeqOfSystemInfoAssistBTS-R98-ExpOTD index refer to SeqOfSystemInfoAssistBTS +-- If the index exceeds the SegOfSystemInfoAssistBTS range or if there is other +-- inconsistencies between the BTS indices, the MS shall apply protocol +-- error cause incorrectData + +SeqOfSystemInfoAssistBTS-R98-ExpOTD ::= SEQUENCE (SIZE(1..32)) OF SystemInfoAssistBTS-R98-ExpOTD + +-- whether n.th is present or not ? +SystemInfoAssistBTS-R98-ExpOTD ::= CHOICE { + notPresent NULL, + present AssistBTSData-R98-ExpOTD +} + +-- This element completes AssistBTSData IE +AssistBTSData-R98-ExpOTD ::= SEQUENCE { + expectedOTD ExpectedOTD, + expOTDuncertainty ExpOTDUncertainty -- Uncertainty of expected OTD +} + +-- Expected OTD value between nbor base station and reference BTS +-- at MS's current estimated location. +ExpectedOTD ::= INTEGER (0..1250) + +-- The ExpectedOTD value 1250 shall not be encoded by the transmitting entity and +-- shall be treated by the receiving entity as 0. +-- Uncertainty of Exptected OTD in bits +ExpOTDUncertainty ::= INTEGER(0..7) + +-- Release 98 extensions +GPSReferenceTimeUncertainty ::= INTEGER (0 .. 127) -- Coding according to Annex + +GPSTimeAssistanceMeasurements ::= SEQUENCE { + referenceFrameMSB INTEGER (0 .. 63), -- MSB of frame number + gpsTowSubms INTEGER (0 .. 9999) OPTIONAL, -- in units of 100ns, for MS based AGPS + deltaTow INTEGER (0 .. 127) OPTIONAL, -- for MS assisted AGPS + gpsReferenceTimeUncertainty GPSReferenceTimeUncertainty OPTIONAL +} + +Rel-98-MsrPosition-Rsp-Extension ::= SEQUENCE { +-- First extension to Release 98 + rel-98-Ext-MeasureInfo SEQUENCE { + otd-MeasureInfo-R98-Ext OTD-MeasureInfo-R98-Ext OPTIONAL + }, + ..., + timeAssistanceMeasurements GPSTimeAssistanceMeasurements OPTIONAL +-- Further R98 extensions here +} + +-- This is an addition to OTD-MeasureInfo element defined in original message, +-- If OTD-MeasureInfo is absent, or if one or more OTD-MsrElementRest are present +-- OTD-MeasureInfo-R98-Ext shall be absent. +-- OTD-MeasureInfo-R98-Ext +OTD-MeasureInfo-R98-Ext ::= SEQUENCE { +-- Measurement info elements + otdMsrFirstSets-R98-Ext OTD-MsrElementFirst-R98-Ext +} + +-- OTD measurement information Ext for the first set only +OTD-MsrElementFirst-R98-Ext ::= SEQUENCE { +-- additional measured neighbors in OTD measurements + otd-FirstSetMsrs-R98-Ext SeqOfOTD-FirstSetMsrs-R98-Ext OPTIONAL +} + +SeqOfOTD-FirstSetMsrs-R98-Ext ::= SEQUENCE (SIZE(1..5)) OF OTD-FirstSetMsrs + +Rel-5-MsrPosition-Rsp-Extension ::= SEQUENCE { + extended-reference Extended-reference OPTIONAL, +-- The extended-reference shall be included by the MS if and only if previously +-- received from the SMLC in a Measure Position Request. When included, the value sent +-- by the MS shall equal the value received from the SMLC. +-- extension to Release 5, for RRLP pseudo-segmentation here + otd-MeasureInfo-5-Ext OTD-MeasureInfo-5-Ext OPTIONAL, + ulPseudoSegInd UlPseudoSegInd OPTIONAL, -- Included when uplink RRLP +-- Pseudo-segmentation is used, not included when no uplink pseudo-segmentation is used + ... +-- Possibly more extensions for Release 5 here later +} + +Extended-reference ::= SEQUENCE { + smlc-code INTEGER (0..63), + transaction-ID INTEGER (0..262143) +} + +OTD-MeasureInfo-5-Ext ::= SeqOfOTD-MsrElementRest +-- if more than one measurement sets are present this element is repeated +-- NumberOfSets - 1 (-1 = first set) combined in OTD-MeasureInfo-5-Ext and +-- OTD-MeasureInfo (e.g. if NumberOfSets is 3, then one otdMsrRestSets may +-- be sent in OTD-MeasureInfo-5-Ext and one in OTD-MeasureInfo) +-- First part of Uplink RRLP Pseudo-segmentation indication, possibly more may be defined +-- in the future for segmentation with more than two segments. +UlPseudoSegInd ::= ENUMERATED { + firstOfMany (0), + secondOfMany(1) +} + +Rel5-MsrPosition-Req-Extension ::= SEQUENCE { + extended-reference Extended-reference, + ... +-- Possibly more extensions for Release 5 here later +} + +Rel5-AssistanceData-Extension ::= SEQUENCE { + extended-reference Extended-reference, + ... +-- Possibly more extensions for Release 5 here later +} + +Rel-5-ProtocolError-Extension::= SEQUENCE { + extended-reference Extended-reference OPTIONAL, +-- The extended-reference shall be included by the MS if and only if previously +-- received from the SMLC. +-- When included, the value sent by the MS shall equal the value received from the SMLC. + ... +-- Possibly more extensions for Release 5 here later +} + +Ext-GeographicalInformation ::= OCTET STRING (SIZE (1..20)) +ExtensionContainer ::= OCTET STRING + +END \ No newline at end of file diff --git a/rrlpd/src/gps.c b/rrlpd/src/gps.c new file mode 100644 index 0000000..e8b6848 --- /dev/null +++ b/rrlpd/src/gps.c @@ -0,0 +1,131 @@ +/* + * gps.c + * + * A few utility functions to deal with low level GPS data + * + * + * Copyright (C) 2009 Sylvain Munaut + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#include "gps.h" + + +#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1)) +#define GET_FIELD_S(w, nb, pos) (((int)((w) << (32-(nb)-(pos)))) >> (32-(nb))) + +/* + * Unpacks GPS Subframe 1,2,3 payloads (3 * 8 words) + * + * Note: eph->sv_id is not filled here since not present in those subframes + * + * (no parity bit checking is done, only the lower 24 bits of each word + * are used) + */ +int +gps_unpack_sf123(uint32_t *sf, struct gps_ephemeris_sv *eph) +{ + uint32_t *sf1 = &sf[0]; + uint32_t *sf2 = &sf[8]; + uint32_t *sf3 = &sf[16]; + + int iode1, iode2; + + eph->week_no = GET_FIELD_U(sf1[0], 10, 14); + eph->code_on_l2 = GET_FIELD_U(sf1[0], 2, 12); + eph->sv_ura = GET_FIELD_U(sf1[0], 4, 8); + eph->sv_health = GET_FIELD_U(sf1[0], 6, 2); + eph->l2_p_flag = GET_FIELD_U(sf1[1], 1, 23); + eph->t_gd = GET_FIELD_S(sf1[4], 8, 0); + eph->iodc = (GET_FIELD_U(sf1[0], 2, 0) << 8) | \ + GET_FIELD_U(sf1[5], 8, 16); + eph->t_oc = GET_FIELD_U(sf1[5], 16, 0); + eph->a_f2 = GET_FIELD_S(sf1[6], 8, 16); + eph->a_f1 = GET_FIELD_S(sf1[6], 16, 0); + eph->a_f0 = GET_FIELD_S(sf1[7], 22, 2); + + iode1 = GET_FIELD_U(sf2[0], 8, 16); + eph->c_rs = GET_FIELD_S(sf2[0], 16, 0); + eph->delta_n = GET_FIELD_S(sf2[1], 16, 8); + eph->m_0 = (GET_FIELD_S(sf2[1], 8, 0) << 24) | \ + GET_FIELD_U(sf2[2], 24, 0); + eph->c_uc = GET_FIELD_S(sf2[3], 16, 8); + eph->e = (GET_FIELD_U(sf2[3], 8, 0) << 24) | \ + GET_FIELD_U(sf2[4], 24, 0); + eph->c_us = GET_FIELD_S(sf2[5], 16, 8); + eph->a_powhalf = (GET_FIELD_U(sf2[5], 8, 0) << 24) | \ + GET_FIELD_U(sf2[6], 24, 0); + eph->t_oe = GET_FIELD_U(sf2[7], 16, 8); + eph->fit_flag = GET_FIELD_U(sf2[7], 1, 7); + + eph->c_ic = GET_FIELD_S(sf3[0], 16, 8); + eph->omega_0 = (GET_FIELD_S(sf3[0], 8, 0) << 24) | \ + GET_FIELD_U(sf3[1], 24, 0); + eph->c_is = GET_FIELD_S(sf3[2], 16, 8); + eph->i_0 = (GET_FIELD_S(sf3[2], 8, 0) << 24) | \ + GET_FIELD_U(sf3[3], 24, 0); + eph->c_rc = GET_FIELD_S(sf3[4], 16, 8); + eph->w = (GET_FIELD_S(sf3[4], 8, 0) << 24) | \ + GET_FIELD_U(sf3[5], 24, 0); + eph->omega_dot = GET_FIELD_S(sf3[6], 24, 0); + iode2 = GET_FIELD_U(sf3[7], 8, 16); + eph->idot = GET_FIELD_S(sf3[7], 14, 2); + + eph->_rsvd1 = GET_FIELD_U(sf1[1], 23, 0); + eph->_rsvd2 = GET_FIELD_U(sf1[2], 24, 0); + eph->_rsvd3 = GET_FIELD_U(sf1[3], 24, 0); + eph->_rsvd4 = GET_FIELD_U(sf1[4], 16, 8); + eph->aodo = GET_FIELD_U(sf2[7], 5, 2); + + /* Check & cross-validate iodc[7:0], iode1, iode2 */ + if ((iode1 != iode2) || (iode1 != (eph->iodc & 0xff))) { + fprintf(stderr, "gps_unpack_sf123 failed\n"); + return -1; + } + + return 0; +} + + +/* + * Unpacks GPS Subframe 4 or 5 Almanac pages payload (8 words) + * + * (no parity bit checking is done, only the lower 24 bits of each word + * are used) + */ +int +gps_unpack_sf45_almanac(uint32_t *sf, struct gps_almanac_sv *alm) +{ + /* this is the page ID but not the satellite ID, its wrong in subframe 5 */ + alm->sv_id = GET_FIELD_U(sf[0], 6, 16); + + alm->e = GET_FIELD_U(sf[0], 16, 0); + alm->t_oa = GET_FIELD_U(sf[1], 8, 16); + alm->ksii = GET_FIELD_S(sf[1], 16, 0); + alm->omega_dot = GET_FIELD_S(sf[2], 16, 8); + alm->sv_health = GET_FIELD_U(sf[2], 8, 0); + alm->a_powhalf = GET_FIELD_U(sf[3], 24, 0); + alm->omega_0 = GET_FIELD_S(sf[4], 24, 0); + alm->w = GET_FIELD_S(sf[5], 24, 0); + alm->m_0 = GET_FIELD_S(sf[6], 24, 0); + alm->a_f0 = (GET_FIELD_S(sf[7], 8, 16) << 3) | \ + GET_FIELD_U(sf[7], 3, 2); + alm->a_f1 = GET_FIELD_S(sf[7], 11, 5); + + return 0; +} + diff --git a/rrlpd/src/gps.h b/rrlpd/src/gps.h new file mode 100644 index 0000000..424fb7a --- /dev/null +++ b/rrlpd/src/gps.h @@ -0,0 +1,193 @@ +/* + * gps.h + * + * Header to deal with low level GPS data + * + * + * Copyright (C) 2009 Sylvain Munaut + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef __GPS_H__ +#define __GPS_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + + +#define MAX_SV 64 + + +/* Ionosperic model data */ +struct gps_ionosphere_model { + /* #bits Scale factor Effective Units */ + /* (LSB) range */ + + int alpha_0; /* s 8 2^-30 seconds */ + int alpha_1; /* s 8 2^-27 s / semi-circles */ + int alpha_2; /* s 8 2^-24 s / (semi-circles)^2 */ + int alpha_3; /* s 8 2^-24 s / (semi-circles)^3 */ + int beta_0; /* s 8 2^11 seconds */ + int beta_1; /* s 8 2^14 s / semi-circles */ + int beta_2; /* s 8 2^16 s / (semi-circles)^2 */ + int beta_3; /* s 8 2^16 s / (semi-circles)^3 */ +}; + + +/* UTC model data */ +struct gps_utc_model { + /* #bits Scale factor Effective Units */ + /* (LSB) range */ + + int a0; /* s 32 2^-30 seconds */ + int a1; /* s 24 2^-50 seconds / seconds */ + int delta_t_ls; /* s 8 1 seconds */ + int t_ot; /* u 8 2^12 602,112 seconds */ + int wn_t; /* u 8 1 weeks */ + int wn_lsf; /* u 8 1 weeks */ + int dn; /* u 8 1 7 days */ + int delta_t_lsf;/* s 8 1 seconds */ +}; + + +/* Almanach data */ +struct gps_almanac_sv { + int sv_id; + int sv_health; + + /* #bits Scale factor Effective Units */ + /* (LSB) range */ + + int e; /* u 16 2^-21 */ + int t_oa; /* u 8 2^12 602,112 seconds */ + int ksii; /* s 16 2^-19 semi-circles */ + int omega_dot; /* s 16 2^-38 semi-circles / s */ + int a_powhalf; /* u 24 2^-11 meters */ + int omega_0; /* s 24 2^-23 semi-circles */ + int w; /* s 24 2^-23 semi-circles */ + int m_0; /* s 24 2^-23 semi-circles */ + int a_f0; /* s 11 2^-20 seconds */ + int a_f1; /* s 11 2^-38 seconds / seconds */ +}; + +struct gps_almanac { + int wna; + int n_sv; + struct gps_almanac_sv svs[MAX_SV]; +}; + + +/* Ephemeris data */ +struct gps_ephemeris_sv { + int sv_id; + + /* #bits Scale factor Effective Units */ + /* (LSB) range */ + + int code_on_l2; /* u 2 1 / */ + int week_no; /* u 10 1 week */ + int l2_p_flag; /* u 1 1 / */ + int sv_ura; /* u 4 / / */ + int sv_health; /* u 6 / / */ + int t_gd; /* s 8 2^-31 seconds */ + int iodc; /* u 10 / / */ + int t_oc; /* u 16 2^4 604,784 seconds */ + int a_f2; /* s 8 2^-55 sec / sec^2 */ + int a_f1; /* s 16 2^-43 sec / sec */ + int a_f0; /* s 22 2^-31 seconds */ + + int c_rs; /* s 16 2^-5 meters */ + int delta_n; /* s 16 2^-43 semi-circles / s */ + int m_0; /* s 32 2^-31 semi-circles */ + int c_uc; /* s 16 2^-29 radians */ + unsigned int e; /* u 32 2^-33 0.03 / */ + int c_us; /* s 16 2^-29 radians */ + unsigned int a_powhalf; /* u 32 2^-19 meters^(1/2) */ + int t_oe; /* u 16 2^4 604,784 seconds */ + int fit_flag; /* u 1 / / */ + + int c_ic; /* s 16 2^-29 radians */ + int omega_0; /* s 32 2^-31 semi-circles */ + int c_is; /* s 16 2^-29 radians */ + int i_0; /* s 32 2^-31 semi-circles */ + int c_rc; /* s 16 2^-5 meters */ + int w; /* s 32 2^-31 semi-circles */ + int omega_dot; /* s 24 2^-43 semi-circles / s */ + int idot; /* s 14 2^-43 semi-circles / s */ + + int _rsvd1; /* 23 bits */ + int _rsvd2; /* 24 bits */ + int _rsvd3; /* 24 bits */ + int _rsvd4; /* 16 bits */ + int aodo; /* 8 bits Not sure it needs to be here ... */ +}; + +struct gps_ephemeris { + int n_sv; + struct gps_ephemeris_sv svs[MAX_SV]; +}; + + +/* Reference position */ +struct gps_ref_pos { /* WSG84 ellipsoid */ + double latitude; /* deg */ + double longitude; /* deg */ + double altitude; /* m above ellipsoid */ +}; + + +/* Reference time */ +struct gps_ref_time { + int wn; /* GPS week number */ + double tow; /* in seconds */ + time_t when; /* time in seconds when time was set */ +}; + + +/* All assist data */ +#define GPS_FIELD_IONOSPHERE (1<<0) +#define GPS_FIELD_UTC (1<<1) +#define GPS_FIELD_ALMANAC (1<<2) +#define GPS_FIELD_EPHEMERIS (1<<3) +#define GPS_FIELD_REFPOS (1<<4) +#define GPS_FIELD_REFTIME (1<<5) +#define GPS_FIELD_REALTIME_INT (1<<6) + +struct gps_assist_data { + int fields; + struct gps_ionosphere_model ionosphere; + struct gps_utc_model utc; + struct gps_almanac almanac; + struct gps_ephemeris ephemeris; + struct gps_ref_pos ref_pos; + struct gps_ref_time ref_time; +}; + + +/* GPS Subframe utility methods (see gps.c for details) */ +int gps_unpack_sf123(uint32_t *sf, struct gps_ephemeris_sv *eph); +int gps_unpack_sf45_almanac(uint32_t *sf, struct gps_almanac_sv *alm); + + +#ifdef __cplusplus +} +#endif + +#endif /* __GPS_H__ */ + diff --git a/rrlpd/src/main.c b/rrlpd/src/main.c new file mode 100644 index 0000000..eae2d0c --- /dev/null +++ b/rrlpd/src/main.c @@ -0,0 +1,1298 @@ +/* (C) 2011 by Dieter Spaar + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + */ + +/* + + The idea behind the RRLP server is as follows: + + - Every RRLP related response from the MS is sent to the RRLP server + + - If it is a position, the decoded position is returned + + - If there is an error, an error message is returned + + - If assistance data are requested, a set of PDUs with the required + data is generated. A set of PDUs is used to not exceed the maximum + size for a RRLP message. + + - If an assitance data ACK is received, the next assitance data PDU + is sent (no data are returned if it is the final message). + + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "gps.h" +#include "ubx.h" +#include "ubx-parse.h" +#include "rrlp.h" + +#define DEBUG 1 +#if DEBUG + #define printd(x, args ...) printf(x, ## args) +#else + #define printd(x, args ...) +#endif + +#define DEBUG_GPS 1 +#if DEBUG_GPD + #define prints(x, args ...) printf(x, ## args) +#else + #define prints(x, args ...) +#endif + +//#define PDU RRLP_Component + +/* Convert "Type" defined by -DPDU into "asn_DEF_Type" */ +#define ASN_DEF_PDU(t) asn_DEF_ ## t +#define DEF_PDU_Type(t) ASN_DEF_PDU(t) +#define PDU_Type DEF_PDU_Type(PDU) + +/* TODO */ + +#define RRLP_SERV_PORT 7890 + +#define MAX_RRLP_DATA 256 + +/* Server cmds */ + +#define RRLP_CMD_MS_DATA 1 /* data from MS */ +#define RRLP_CMD_MS_DATA_SLOW 2 /* data from MS, slow channel */ + +/* Server response */ + +#define RRLP_RSP_ASSIST_DATA 1 /* assitance data, send to MS */ +#define RRLP_RSP_RRLP_ERROR 2 /* RRLP error */ +#define RRLP_RSP_RRLP_POSITION 3 /* RRLP position */ +#define RRLP_RSP_ERROR 4 /* something went wrong */ + + +#define SUBSC_ID_NONE 0 + +#define N_MAX_AD_RECORDS 10 +#define N_MAX_AD_RECORDS_SLOW 1 /* maximum number of messages used on slow channel */ + +enum rrlp_state { RRLP_NONE = 0, RRLP_ASSIST, RRLP_ERROR, RRLP_POSITION, RRLP_WAIT_POSITION }; + +/* record for the state of a subscriber */ + +struct rrlp_sub_state { + long long unsigned int id; /* subscriber ID from OpenBSC */ + time_t time_first; /* time when record was used the first time */ + + int reference_num; /* RRLP reference number */ + + enum rrlp_state state; + + int error; /* error */ + + /* a large assistance data APDU is split in smaller parts */ + int cur_assist_record; /* current assistance data record */ + uint8_t assist_data_len[N_MAX_AD_RECORDS]; /* length of a record */ + uint8_t assist_data[N_MAX_AD_RECORDS][MAX_RRLP_DATA]; /* record */ + + /* GPS position */ + long latitude; /* */ + long longitude; /* */ + long altitude; /* in meter */ +}; + +void free_rrlp_subscriber(struct rrlp_sub_state *rrlp_sub_state); + +/* static allocation, might change that for a large number of queries at once */ + +#define N_MAX_RRLP_STATES 3000 + +struct rrlp_sub_state g_rrlp_states[N_MAX_RRLP_STATES]; + +/* assistance data from the GPS receiver */ + +struct gps_assist_data g_gps; + +/* + According to TS 23.032 + + So far only "Ellipsoid point with altitude and uncertainty Ellipsoid" + seems to be sent from the MS. +*/ + +int decode_pos_estimate(struct rrlp_sub_state *rrlp_sub_state, uint8_t *data, int len) +{ + long latitude; + long longitude; + long altitude; + + if(len < 1) { + fprintf(stderr, "decode_pos_estimate: len too short (%d)\n", len); + return -1; + } + + /* type of shape */ + switch((data[0] & 0xF0) >> 4) { + case 0x00: + printd("Ellipsoid point\n"); + break; + + case 0x01: + printd("Ellipsoid point with uncertainty Circle\n"); + break; + + case 0x03: + printd("Ellipsoid point with uncertainty Ellipse\n"); + break; + + case 0x05: + printd("Polygon\n"); + break; + + case 0x08: + printd("Ellipsoid point with altitude\n"); + break; + + case 0x09: + printd("Ellipsoid point with altitude and uncertainty Ellipsoid\n"); + if(len != 14) { + fprintf(stderr, "decode_pos_estimate: invalid len (%d)\n", len); + return -1; + } + /* Deggrees of latitude */ + latitude = ((data[1] & 0x7F) << 16) + (data[2] << 8) + data[3]; + if(data[1] & 0x80) + latitude = -latitude; + printd("latitude = %f\n", ((double)latitude * 90.0) / 0x800000L); + /* Deggrees of longitude */ + longitude = (data[4] << 16) + (data[5] << 8) + data[6]; + if(data[4] & 0x80) + longitude |= 0xFF000000; + printd("longitude = %f\n", ((double)longitude * 360.0) / 0x1000000L); + /* Altitude */ + altitude = ((data[7] & 0x7F) << 8) + data[8]; + if(data[7] & 0x80) + altitude = -altitude; + printd("altitude = %ld\n", altitude); + /* Uncertainty semi-major */ + /* Uncertainty semi-minor */ + /* Orientation of major axis */ + /* Uncertainty Altitude */ + /* Confidence */ + + rrlp_sub_state->latitude = latitude; + rrlp_sub_state->longitude = longitude; + rrlp_sub_state->altitude = altitude; + + return 0; + break; + + case 0x0A: + printd("Ellipsoid Arc\n"); + break; + + default: + printd("Unknown shape type: %d\n", (data[0] & 0xF0) >> 4); + break; + } + + fprintf(stderr, "cannot decode shape type (%d)\n", (data[0] & 0xF0) >> 4); + + return -1; +} + +int decode_rrlp_data(struct rrlp_sub_state *rrlp_sub_state, int max_reply_msgs, + uint8_t *data, int len, + uint8_t *cmd_reply, uint8_t *reply, int *len_reply) +{ + static asn_TYPE_descriptor_t *pduType = &PDU_Type; + void *structure; /* Decoded structure */ + asn_codec_ctx_t *opt_codec_ctx = 0; + asn_dec_rval_t rval; + int rc; + + structure = 0; + + rval = uper_decode(opt_codec_ctx, pduType, (void **)&structure, data, len, 0, 0); + + if(!structure) { + if(errno) { + fprintf(stderr, "Error: errno = %d\n", errno); + return -1; + } else { + /* EOF */ + fprintf(stderr, "EOF\n"); + return -1; + } + } else { + /* dump PDU */ + asn_fprint(stdout, pduType, structure); + + long l; + PDU_t *pdu = (PDU_t*)structure; + + printd("\n"); + printd("-- referenceNumber = %ld\n", pdu->referenceNumber); + + rrlp_sub_state->reference_num = pdu->referenceNumber; + + /* CHOICE */ + switch(pdu->component.present) { + + case RRLP_Component_PR_msrPositionReq: + printd("-- msrPositionReq\n"); + + *cmd_reply = RRLP_RSP_ERROR; + strcpy(reply, "APU unsupported: msrPositionReq"); + *len_reply = strlen(reply) + 1; + break; + + case RRLP_Component_PR_msrPositionRsp: + printd("-- msrPositionRsp\n"); + + /* "MsrPosition-Rsp": process either "locationInfo" or "locationError" */ + + /* SEQUENCE with OPTIONAL components */ + MsrPosition_Rsp_t *msrPositionRsp = &pdu->component.choice.msrPositionRsp; + + if(msrPositionRsp->multipleSets) { + printd("---- multipleSets\n"); + } + + if(msrPositionRsp->referenceIdentity) { + printd("---- referenceIdentity\n"); + } + + if(msrPositionRsp->otd_MeasureInfo) { + printd("---- otd_MeasureInfo\n"); + } + + if(msrPositionRsp->locationInfo) { + printd("---- locationInfo\n"); + printd("------ refFrame = %ld\n", msrPositionRsp->locationInfo->refFrame); + if(msrPositionRsp->locationInfo->gpsTOW) { + printd("------ gpsTOW = %ld\n", *msrPositionRsp->locationInfo->gpsTOW); + /* in ms */ + } + printd("---- fixType = %ld\n", msrPositionRsp->locationInfo->fixType); + printd("---- posEstimate\n"); + int n = msrPositionRsp->locationInfo->posEstimate.size; + uint8_t *buf = msrPositionRsp->locationInfo->posEstimate.buf; + #if 1 /* dump data */ + int i; + for(i = 0; i < n; i++) + printd("%02X ", buf[i]); + printd("\n"); + #endif + /* TODO */ + if(decode_pos_estimate(rrlp_sub_state, buf, n) != 0) { + fprintf(stderr, "decode_pos_estimate failed\n"); + + *cmd_reply = RRLP_RSP_ERROR; + strcpy(reply, "decode_pos_estimate failed"); + *len_reply = strlen(reply) + 1; + } else { + *cmd_reply = RRLP_RSP_RRLP_POSITION; + + rrlp_sub_state->state = RRLP_POSITION; + + reply[0] = rrlp_sub_state->latitude & 0xFF; + reply[1] = (rrlp_sub_state->latitude >> 8) & 0xFF; + reply[2] = (rrlp_sub_state->latitude >> 16) & 0xFF; + reply[3] = (rrlp_sub_state->latitude >> 24) & 0xFF; + + reply[4] = rrlp_sub_state->longitude & 0xFF; + reply[5] = (rrlp_sub_state->longitude >> 8) & 0xFF; + reply[6] = (rrlp_sub_state->longitude >> 16) & 0xFF; + reply[7] = (rrlp_sub_state->longitude >> 24) & 0xFF; + + reply[ 8] = rrlp_sub_state->altitude & 0xFF; + reply[ 9] = (rrlp_sub_state->altitude >> 8) & 0xFF; + reply[10] = (rrlp_sub_state->altitude >> 16) & 0xFF; + reply[11] = (rrlp_sub_state->altitude >> 24) & 0xFF; + + *len_reply = 3 * 4; + } + /* we got a location, ignore anything else */ + goto done; + } + + if(msrPositionRsp->gps_MeasureInfo) { + printd("---- gps_MeasureInfo\n"); + } + + if(msrPositionRsp->locationError) { + printd("---- locationError\n"); + if(asn_INTEGER2long(&msrPositionRsp->locationError->locErrorReason, &l) == 0) { + asn_INTEGER_specifics_t *specs = (asn_INTEGER_specifics_t *)asn_DEF_LocErrorReason.specifics; + const asn_INTEGER_enum_map_t *el; + el = INTEGER_map_value2enum(specs, l); + if(el) { + printd("---- locErrorReason = %ld (%s)\n", l, el->enum_name); + sprintf(reply, "locationError %ld (%s)", l, el->enum_name); + } else { + printd("---- locErrorReason = %ld\n", l); + sprintf(reply, "locationError %ld", l); + } + } else { + printd("---- locErrorReason: decode error\n"); + strcpy(reply, "locationError ??"); + l = 9999; + } + if(msrPositionRsp->locationError->additionalAssistanceData) { + printd("------ additionalAssistanceData\n"); + if(msrPositionRsp->locationError->additionalAssistanceData->gpsAssistanceData) { + printd("------ gpsAssistanceData\n"); + int n = msrPositionRsp->locationError->additionalAssistanceData->gpsAssistanceData->size; + uint8_t *buf = msrPositionRsp->locationError->additionalAssistanceData->gpsAssistanceData->buf; + struct rrlp_assist_req ar; + #if 1 /* dump data */ + int i; + for(i = 0; i < n; i++) + printd("%02X ", buf[i]); + printd("\n"); + #endif + /* check state */ + if(rrlp_sub_state->state == RRLP_ASSIST) { + fprintf(stderr, "assistanceData in invalid state\n"); + + *cmd_reply = RRLP_RSP_ERROR; + strcpy(reply, "assistanceData in invalid state"); + *len_reply = strlen(reply) + 1; + goto done; + } + if(rrlp_sub_state->state == RRLP_WAIT_POSITION) { + fprintf(stderr, "assistanceData already sent\n"); + + *cmd_reply = RRLP_RSP_ERROR; + strcpy(reply, "assistanceData already sent"); + *len_reply = strlen(reply) + 1; + goto done; + } + /* decode the requested assistance data */ + rc = rrlp_decode_assistance_request(&ar, buf, n); + if(rc != 0) { + fprintf(stderr, "rrlp_decode_assistance_request failed: %d\n", rc); + + *cmd_reply = RRLP_RSP_ERROR; + sprintf(reply, "rrlp_decode_assistance_request failed: %d", rc); + *len_reply = strlen(reply) + 1; + } else if(l == LocErrorReason_gpsAssDataMissing) { + void *pdus[N_MAX_AD_RECORDS]; + int lens[N_MAX_AD_RECORDS]; + int i; + int error = 0; + int ref_num; + + /* take a new reference number for the assistance data */ + ref_num = rrlp_sub_state->reference_num; + ref_num++; + if(ref_num > 7) /* 0..7 */ + ref_num = 7; + if(ref_num == 0) /* 0 is special */ + ref_num = 1; + + /* generate PDUs */ + rc = rrlp_gps_assist_pdus(ref_num, &g_gps, &ar, pdus, lens, N_MAX_AD_RECORDS); + if(rc < 0) { + fprintf(stderr, "rrlp_gps_assist_pdus: %d\n", rc); + + *cmd_reply = RRLP_RSP_ERROR; + sprintf(reply, "rrlp_gps_assist_pdus failed: %d", rc); + *len_reply = strlen(reply) + 1; + goto done; + } + + /* clear messages */ + for(i = 0; i < N_MAX_AD_RECORDS; i++) { + rrlp_sub_state->assist_data_len[i] = 0; + memset(rrlp_sub_state->assist_data[i], 0, MAX_RRLP_DATA); + } + + /* check if maximum number of messages exceeded */ + if(rc > max_reply_msgs) { + for(i = 0; i < rc; i++) { + if(pdus[i]) + asn_DEF_PDU.free_struct(&asn_DEF_PDU, pdus[i], 0); + } + + fprintf(stderr, "too many assitance messages: (%d > %d)\n", rc, max_reply_msgs); + + *cmd_reply = RRLP_RSP_ERROR; + sprintf(reply, "too many assitance messages: (%d > %d)\n", rc, max_reply_msgs); + *len_reply = strlen(reply) + 1; + goto done; + } + + /* copy PDUs to subscriber state */ + for(i = 0; i < rc; i++) { + printd("%p %d\n", pdus[i], lens[i]); + if(pdus[i] == NULL) { + fprintf(stderr, "pdus[%d] == NULL\n", i); + sprintf(reply, "pdus[%d] == NULL\n", i); + error = -1; + } + #if 0 /* Debug */ + { + int rv, fd; + char filename[50]; + sprintf(filename, "pdu%02d.uper", i); + fd = open(filename, O_TRUNC | O_CREAT | O_RDWR, S_IREAD | S_IWRITE); + if (fd < 0) + printf("File error\n"); + rv = write(fd, pdus[i], lens[i]); + if(rv != lens[i]) + printf("Write error\n"); + close(fd); + } + #endif + #if 0 /* Debug */ + int j; + for(j = 0; j < lens[i]; j++) + printf("%02X ", ((uint8_t*)pdus[i])[j]); + printf("\n"); + #endif + + if(lens[i] > MAX_RRLP_DATA) { + fprintf(stderr, "lens[%d] > MAX_RRLP_DATA (%d)\n", i, lens[i]); + sprintf(reply, "lens[%d] > MAX_RRLP_DATA (%d)\n", i, lens[i]); + error = -2; + } + + if(error == 0) { + rrlp_sub_state->assist_data_len[i] = lens[i]; + memcpy(rrlp_sub_state->assist_data[i], pdus[i], lens[i]); + + } + + if(pdus[i]) + free(pdus[i]); + } + + if(error) { + *cmd_reply = RRLP_RSP_ERROR; + *len_reply = strlen(reply) + 1; + goto done; + } + + *cmd_reply = RRLP_RSP_ASSIST_DATA; + + rrlp_sub_state->state = RRLP_ASSIST; + rrlp_sub_state->cur_assist_record = 0; + + /* send first assistent data block */ + i = rrlp_sub_state->cur_assist_record; + memcpy(reply, rrlp_sub_state->assist_data[i], rrlp_sub_state->assist_data_len[i]); + *len_reply = rrlp_sub_state->assist_data_len[i]; + /* advance if not already at the end */ + if(rrlp_sub_state->assist_data_len[i]) + rrlp_sub_state->cur_assist_record++; + + /* we got assistance data, ignore anything else */ + goto done; + } + } + } + rrlp_sub_state->state = RRLP_ERROR; + + *cmd_reply = RRLP_RSP_RRLP_ERROR; + *len_reply = strlen(reply) + 1; + /* we got an error, ignore anything else */ + goto done; + } + + if(msrPositionRsp->extensionContainer) { + printd("---- extensionContainer\n"); + } + + /* no "locationInfo" or "locationError" */ + *cmd_reply = RRLP_RSP_ERROR; + strcpy(reply, "unexpected MsrPosition-Rsp component"); + *len_reply = strlen(reply) + 1; + break; + + case RRLP_Component_PR_assistanceData: + printd("-- assistanceData\n"); + + *cmd_reply = RRLP_RSP_ERROR; + strcpy(reply, "APDU unsupported: assistanceData"); + *len_reply = strlen(reply) + 1; + break; + + case RRLP_Component_PR_assistanceDataAck: + printd("-- assistanceDataAck\n"); + + /* check state */ + if(rrlp_sub_state->state != RRLP_ASSIST) { + *cmd_reply = RRLP_RSP_ERROR; + fprintf(stderr, "assistanceData Ack in invalid state\n"); + strcpy(reply, "assistanceData Ack in invalid state"); + *len_reply = strlen(reply) + 1; + goto done; + } + + /* send next assitance data reply or done */ + + *cmd_reply = RRLP_RSP_ASSIST_DATA; + + int i = rrlp_sub_state->cur_assist_record; + if(i < N_MAX_AD_RECORDS) { + memcpy(reply, rrlp_sub_state->assist_data[i], rrlp_sub_state->assist_data_len[i]); + *len_reply = rrlp_sub_state->assist_data_len[i]; + /* advance if not already at the end */ + if(rrlp_sub_state->assist_data_len[i]) + rrlp_sub_state->cur_assist_record++; + else { + rrlp_sub_state->state = RRLP_NONE; + } + } else { + *len_reply = 0; + rrlp_sub_state->state = RRLP_WAIT_POSITION; + } + + break; + + case RRLP_Component_PR_protocolError: + printd("-- protocolError\n"); + ProtocolError_t *protocolError = &pdu->component.choice.protocolError; + if(asn_INTEGER2long(&protocolError->errorCause, &l) == 0) { + asn_INTEGER_specifics_t *specs = (asn_INTEGER_specifics_t *)asn_DEF_ErrorCodes.specifics; + const asn_INTEGER_enum_map_t *el; + el = INTEGER_map_value2enum(specs, l); + if(el) { + printd("---- errorCause = %ld (%s)\n", l, el->enum_name); + sprintf(reply, "protocolError %ld (%s)", l, el->enum_name); + } else { + printd("---- errorCause = %ld\n", l); + sprintf(reply, "protocolError %ld", l); + } + } else { + printd("---- errorCause: decode error\n"); + strcpy(reply, "protocolError ??"); + } + + rrlp_sub_state->state = RRLP_ERROR; + + *cmd_reply = RRLP_RSP_RRLP_ERROR; + *len_reply = strlen(reply) + 1; + break; + + default: + printd("-- unknown %d\n", pdu->component.present); + + *cmd_reply = RRLP_RSP_ERROR; + sprintf(reply, "unknown PDU conponent %d", pdu->component.present); + *len_reply = strlen(reply) + 1; + break; + } + +done: + /* free structure */ + + asn_DEF_PDU.free_struct(&asn_DEF_PDU, (void*)structure, 0); + } + + return 0; +} + +/***********************************************************************/ + +static int ubx_parse(struct gps_assist_data *gps, uint8_t *data, int len) +{ + int rv = 0, i; + + /* Parse each message */ + for(i = 0; i < len; ) { + rv = ubx_msg_dispatch(ubx_parse_dt, data + i, len - i, gps); + if (rv < 0) + i++; /* Invalid message: try one byte later */ + else + i += rv; + } + + return (rv >= 0) ? 0 : rv; +} + +/***********************************************************************/ + +/* taken from OsmocomBB */ + +static int serial_init(const char *serial_port, speed_t baudrate) +{ + int rc, serial_fd; + struct termios tio; + + serial_fd = open(serial_port, O_RDWR | O_NOCTTY | O_NDELAY); + if (serial_fd < 0) { + perror("cannot open serial port"); + return serial_fd; + } + + //fcntl(serial_fd, F_SETFL, 0); + + /* Configure serial interface */ + rc = tcgetattr(serial_fd, &tio); + if (rc < 0) { + close(serial_fd); + perror("tcgetattr()"); + return rc; + } + rc = cfsetispeed(&tio, baudrate); + if (rc < 0) { + close(serial_fd); + perror("cfsetispeed()"); + return rc; + } + rc = cfsetospeed(&tio, baudrate); + if (rc < 0) { + close(serial_fd); + perror("cfsetospeed()"); + return rc; + } + tio.c_cflag &= ~(PARENB | CSTOPB | CSIZE | CRTSCTS); + tio.c_cflag |= (CREAD | CLOCAL | CS8); + tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tio.c_iflag |= (INPCK | ISTRIP); + tio.c_iflag &= ~(ISTRIP | IXON | IXOFF | IGNBRK | INLCR | ICRNL | IGNCR); + tio.c_oflag &= ~(OPOST | ONLCR); + rc = tcsetattr(serial_fd, TCSANOW, &tio); + if (rc < 0) { + close(serial_fd); + perror("tcsetattr()"); + return rc; + } + + return serial_fd; +} + +#define GPS_USB 1 /* set to 1 if uBlox GPS receiver is connected to USB */ + +#if GPS_USB + #define GPS_BAUDRATE B115200 +#else + #define GPS_BAUDRATE B9600 +#endif + +/* + +USB: switch back to NMEA output + +"$PUBX,41,3,0003,0002,9600,0*17\r\n" + +*/ + +static int set_ubx_protocol(int fd) +{ + int len; + /* Set Protocol and baudrate (switch to UBX output, still allow NMEA input) */ +#if GPS_USB /* if uBlox is connected through USB */ + char upx_chg_mode[] = "$PUBX,41,3,0003,0001,115200,0*1C\r\n"; +#else /* if uBlox is connected through the serial port */ + char upx_chg_mode[] = "$PUBX,41,1,0003,0001,9600,0*16\r\n"; +#endif + + len = write(fd, upx_chg_mode, sizeof(upx_chg_mode) - 1); + if(len != sizeof(upx_chg_mode) - 1) { + perror("write()"); + return len; + } + + /* a short delay */ + + sleep(1); + + /* flush the input buffer (there is probably are more elegant solution) */ + + for(;;) { + fd_set readset; + struct timeval tv; + int rc; + int len; + uint8_t buf[256]; + + /* check for data */ + + FD_ZERO(&readset); + FD_SET(fd, &readset); + tv.tv_sec = 0; + tv.tv_usec = 300 * 1000; + + rc = select(fd + 1, &readset, NULL, NULL, &tv); + if(rc < 0) { + if(errno != EINTR) { + fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno)); + break; + } + } + if(!FD_ISSET(fd, &readset)) + break; + + /* read data */ + + len = read(fd, buf, sizeof(buf)); + if(len <= 0) { + fprintf(stderr, "serial read() failed: (%d) %s\n", len, strerror(errno)); + break; + } + + } + + return 0; +} + +static int ubx_get_data(int fd) +{ + int len; + char upx_msg1[] = "\xb5\x62\x01\x02\x00\x00\x03\x0a"; /* NAV-POSLLH */ + char upx_msg2[] = "\xb5\x62\x0b\x10\x00\x00\x1b\x5c"; /* AID-DATA */ + char upx_msg3[] = "\xB5\x62\x06\x01\x03\x00\x01\x20\x01\x2C\x83"; /* CFG-MSG, send NAV-TIMEGPS every second */ + + len = write(fd, upx_msg1, sizeof(upx_msg1) - 1); + if(len != sizeof(upx_msg1) - 1) { + perror("write()"); + return len; + } + + len = write(fd, upx_msg2, sizeof(upx_msg2) - 1); + if(len != sizeof(upx_msg2) - 1) { + perror("write()"); + return len; + } + +#if 1 + len = write(fd, upx_msg3, sizeof(upx_msg3) - 1); + if(len != sizeof(upx_msg3) - 1) { + perror("write()"); + return len; + } +#endif + + return 0; +} + +static int ubx_get_time(int fd) +{ + int len; + char upx_msg1[] = "\xb5\x62\x01\x20\x00\x00\x21\x64"; /* NAV-TIMEGPS */ + //char upx_msg1[] = "\xb5\x62\x01\x22\x00\x00\x23\x6A"; /* NAV-CLOCK */ + fd_set readset; + struct timeval tv; + int rc; + uint8_t buf[256]; + struct gps_assist_data gps; + + len = write(fd, upx_msg1, sizeof(upx_msg1) - 1); + if(len != sizeof(upx_msg1) - 1) { + perror("write()"); + return len; + } + + /* wait for reply (does it really take nearly a second ?) */ + + FD_ZERO(&readset); + FD_SET(fd, &readset); + tv.tv_sec = 0; + tv.tv_usec = 1000 * 1000; + + rc = select(fd + 1, &readset, NULL, NULL, &tv); + if(rc < 0) { + if(errno != EINTR) { + fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno)); + return -1; + } + } + if(!FD_ISSET(fd, &readset)) { + fprintf(stderr, "timeout waiting for GPS time\n"); + return -2; + } + + /* read data */ + + len = read(fd, buf, sizeof(buf)); + if(len <= 0) { + fprintf(stderr, "serial read() failed: (%d) %s\n", len, strerror(errno)); + return -1; + } + + /* parse data */ + + memset(&gps, 0, sizeof(gps)); + if(ubx_parse(&gps, buf, len) != 0) { + fprintf(stderr, "ubx_parse failed\n"); + return -1; + } + + return 0; +} + +/***********************************************************************/ + +void init_rrlp_states(void) +{ + int i; + memset(&g_rrlp_states, 0, sizeof(g_rrlp_states)); + + for(i = 0; i < N_MAX_RRLP_STATES; i++) { + g_rrlp_states[i].id = SUBSC_ID_NONE; + g_rrlp_states[i].state = RRLP_NONE; + } +} + +/* find record with this ID or get an unused one */ + +struct rrlp_sub_state *get_rrlp_subscriber(long long unsigned int id) +{ + int i; + + /* search for this ID */ + for(i = 0; i < N_MAX_RRLP_STATES; i++) { + if(g_rrlp_states[i].id == id) + return &g_rrlp_states[i]; + } + + /* search for an unused record */ + for(i = 0; i < N_MAX_RRLP_STATES; i++) { + if(g_rrlp_states[i].id == SUBSC_ID_NONE) { + g_rrlp_states[i].id = id; + g_rrlp_states[i].time_first = time(NULL); + return &g_rrlp_states[i]; + } + } + + return NULL; +} + +void free_rrlp_subscriber(struct rrlp_sub_state *rrlp_sub_state) +{ + memset(rrlp_sub_state, 0, sizeof(struct rrlp_sub_state)); + rrlp_sub_state->id = SUBSC_ID_NONE; + rrlp_sub_state->state = RRLP_NONE; +} + +/* find expired records and free them */ + +#define SUBS_RECORD_LIFETIME 300 /* in seconds */ + +void free_expired_rrlp_subscribers(void) +{ + int i; + + /* search for this ID */ + for(i = 0; i < N_MAX_RRLP_STATES; i++) { + if(g_rrlp_states[i].id != SUBSC_ID_NONE && time(NULL) - g_rrlp_states[i].time_first > SUBS_RECORD_LIFETIME) { + free_rrlp_subscriber(&g_rrlp_states[i]); + } + } +} + +#define GPS_POLL_TIME 100 /* in seconds */ + +#define MAX(a, b) ((a > b) ? a : b) + +int rrlp_serv(int fd, int fd_serial) +{ + fd_set readset; + struct timeval tv; + int rc; + int len; + uint8_t buf[2 + 1 + 8 + MAX_RRLP_DATA]; /* len, cmd, subscriber ID, data */ + uint8_t buf_reply[MAX_RRLP_DATA]; + uint8_t buf_serial[1024 * 8]; /* should be large enough for the complete data set */ + int len_pkt, offs, len_reply, len_data; + uint8_t cmd, cmd_reply; + long long unsigned int id; + struct sockaddr_in from; + int from_len; + time_t last_gps_query = 0; + time_t last_subs_expiration = time(NULL); + struct rrlp_sub_state *rrlp_sub_state; + + memset(&g_gps, 0, sizeof(g_gps)); + + init_rrlp_states(); + + for(;;) { + + /* new GPS poll ? */ + + if(time(NULL) - last_gps_query > GPS_POLL_TIME) { + printd("GPS poll\n"); + ubx_get_data(fd_serial); + last_gps_query = time(NULL); + } + + /* any data from RRLP client or GPS Receicer ? */ + + FD_ZERO(&readset); + FD_SET(fd, &readset); + if(fd_serial != -1) + FD_SET(fd_serial, &readset); + tv.tv_sec = 1; + tv.tv_usec = 0; + + /* this creates another UDP socket on Cygwin !? */ +#if 0 + rc = select(sizeof(readset) * 8, &readset, NULL, NULL, &tv); +#else + rc = select(MAX(fd, fd_serial) + 1, &readset, NULL, NULL, &tv); +#endif + if(rc < 0) { + if(errno == EINTR) + continue; + fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno)); + return -1; + } + + /* data from RRLP client ? */ + + if(FD_ISSET(fd, &readset)) { + + /* read packet */ + from_len = sizeof(from); + len = recvfrom(fd, buf, sizeof(buf), 0, (struct sockaddr*)&from, &from_len); + if(len < 0) { + fprintf(stderr, "recvfrom() failed: (%d) %s\n", len, strerror(errno)); + return -1; + } + if(len < 2) { + fprintf(stderr, "len < 2: %d\n", len); + return -1; + } + len_pkt = buf[0] + (buf[1] << 8); + if(len_pkt < 2 + 1 + 8) { + fprintf(stderr, "len_pkt < 2 + 8 + 1: %d\n", len_pkt); + return -1; + } + if(len != len_pkt) { + /* TODO: we might have received more than one packet */ + fprintf(stderr, "recvfrom: len != len_pkt: %d %d\n", len, len_pkt); + return -1; + } + len_pkt -= 2; + offs = 2; + +#if 0 /* dump packet */ + { + int i; + for(i = 0; i < len_pkt; i++) + printd("%02X ", buf[offs + i]); + printd("\n"); + } +#endif + + cmd = buf[offs + 0]; + /* get subscriber ID */ + id = buf[offs + 1]; + id += ((long long unsigned int)buf[offs + 2] << 8); + id += ((long long unsigned int)buf[offs + 3] << 16); + id += ((long long unsigned int)buf[offs + 4] << 24); + id += ((long long unsigned int)buf[offs + 5] << 32); + id += ((long long unsigned int)buf[offs + 6] << 40); + id += ((long long unsigned int)buf[offs + 7] << 48); + id += ((long long unsigned int)buf[offs + 8] << 56); + + printd("cmd = 0x%02X ID = %llu\n", cmd, id); + + len_data = len_pkt - (1 + 8); + offs += (1 + 8); + +#if 1 /* dump data */ + { + int i; + for(i = 0; i < len_data; i++) + printd("%02X ", buf[offs + i]); + printd("\n"); + } +#endif + + /* process packet */ + + len_reply = 0; + cmd_reply = 0x00; + + /* search subscriber record with this ID or allocate a new one */ + + rrlp_sub_state = get_rrlp_subscriber(id); + if(rrlp_sub_state == NULL) { + fprintf(stderr, "cannot get subscriber reccord\n"); + + cmd_reply = RRLP_RSP_ERROR; + strcpy(buf_reply, "cannot get subscriber reccord"); + len_reply = strlen(buf_reply) + 1; + } else { + if(cmd == RRLP_CMD_MS_DATA) { + if(decode_rrlp_data(rrlp_sub_state, N_MAX_AD_RECORDS, &buf[offs], len_data, &cmd_reply, buf_reply, &len_reply) != 0) { + cmd_reply = RRLP_RSP_ERROR; + strcpy(buf_reply, "decode_rrlp_data failed"); + len_reply = strlen(buf_reply) + 1; + } + } else if(cmd == RRLP_CMD_MS_DATA_SLOW) { + if(decode_rrlp_data(rrlp_sub_state, N_MAX_AD_RECORDS_SLOW, &buf[offs], len_data, &cmd_reply, buf_reply, &len_reply) != 0) { + cmd_reply = RRLP_RSP_ERROR; + strcpy(buf_reply, "decode_rrlp_data failed"); + len_reply = strlen(buf_reply) + 1; + } + } else { + cmd_reply = RRLP_RSP_ERROR; + strcpy(buf_reply, "invalid command"); + len_reply = strlen(buf_reply) + 1; + } + } + + /* send reply, build packet */ + len_pkt = 2 + 1 + len_reply; + buf[0] = len_pkt & 0xFF; + buf[1] = (len_pkt >> 8) & 0xFF; + buf[2] = cmd_reply; /* cmd */ + /* data */ + memcpy(&buf[3], buf_reply, len_reply); + + len = sendto(fd, buf, len_pkt, 0, (struct sockaddr*)&from, sizeof(from)); + if(len < 0) { + fprintf(stderr, "sendto() failed: (%d) %s\n", len, strerror(errno)); + return -1; + } + if(len != len_pkt) { + fprintf(stderr, "sendto: len != len_pkt: %d %d\n", len, len_pkt); + return -1; + } + } + + /* data from the GPS receiver ? */ + + if(fd_serial != -1 && FD_ISSET(fd_serial, &readset)) { + + int buf_offset = 0; + int total_len = 0; + struct gps_assist_data gps; + + for(;;) { + len = read(fd_serial, buf_serial + buf_offset, sizeof(buf_serial) - buf_offset); + if(len <= 0) { + fprintf(stderr, "serial read() failed: (%d) %s\n", len, strerror(errno)); + break; + } + prints("serial data: %d\n", len); + + total_len += len; + + buf_offset += len; + if(buf_offset >= sizeof(buf_serial)) { + fprintf(stderr, "serial buffer full\n"); + break; + } + + /* check for more data */ + + FD_ZERO(&readset); + FD_SET(fd_serial, &readset); + tv.tv_sec = 0; + tv.tv_usec = 300 * 1000; + + rc = select(fd_serial + 1, &readset, NULL, NULL, &tv); + if(rc < 0) { + if(errno != EINTR) { + fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno)); + break; + } + } + if(!FD_ISSET(fd_serial, &readset)) + break; + } + + prints("total_len: %d\n", total_len); + +#if 0 /* dump hex data */ + { + int i; + for(i = 0; i < total_len; i++) + prints("%02X ", buf_serial[i]); + prints("\n"); + } +#endif +#if 0 /* dump ASCII data */ + { + int i; + for(i = 0; i < total_len; i++) + prints("%c", buf_serial[i]); + } +#endif + + memset(&gps, 0, sizeof(gps)); + if(ubx_parse(&gps, buf_serial, total_len) != 0) { + fprintf(stderr, "ubx_parse failed\n"); + } + else { + if(gps.fields == GPS_FIELD_REFTIME) { + g_gps.ref_time = gps.ref_time; + } + else if (gps.fields != 0) { + g_gps = gps; + } + } + + + #if 0 /* for time testing */ + close(fd_serial); + fd_serial = -1; + #endif + } + + /* expire subscribers ? */ + + if(time(NULL) - last_subs_expiration > SUBS_RECORD_LIFETIME) { + printd("Subscriber expiration\n"); + free_expired_rrlp_subscribers(); + last_subs_expiration = time(NULL); + } + +#if 0 /* for testing only */ + ubx_get_time(fd_serial); +#endif + } + + return 0; +} + +static int fd = -1; +static int fd_serial = -1; + +static void signal_handler(int signal) +{ + int rc; + + fprintf(stdout, "signal %u received\n", signal); + + switch (signal) { + case SIGINT: + if(fd != -1) { + printd("close\n"); + rc = close(fd); + if(rc != 0) { + fprintf(stderr, "close() failed: (%d) %s\n", rc, strerror(errno)); + } + } + if(fd_serial != -1) { + printd("close serial\n"); + rc = close(fd_serial); + if(rc != 0) { + fprintf(stderr, "close() failed: (%d) %s\n", rc, strerror(errno)); + } + } + exit(0); + break; + default: + break; + } +} + +int main(int argc, char *argv[]) +{ + int rc; + struct sockaddr_in sa; + int on; + + printf("RRLP Server\n"); + + if(argc != 3) { + fprintf(stderr, "usage: rrlp-serv .\n"); + return -1; + } + + fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if(fd < 0) { + fprintf(stderr, "socket() failed: (%d) %s\n", fd, strerror(errno)); + return -1; + } + + /* This is for Cygwin/Windows, for Linux take the interface name */ + + sa.sin_family = AF_INET; + sa.sin_port = htons(RRLP_SERV_PORT); + sa.sin_addr.s_addr = INADDR_ANY; + if(inet_aton(argv[1], &sa.sin_addr) != 1) { + fprintf(stderr, "inet_aton() failed: %s\n", strerror(errno)); + close(fd); + return -1; + } + + if(sa.sin_addr.s_addr == INADDR_NONE) + { + fprintf(stderr, "Invalid local address\n"); + close(fd); + return -1; + } + + on = 1; + rc = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)); + if(rc != 0) { + fprintf(stderr, "setsockopt() failed: (%d) %s\n", rc, strerror(errno)); + } + + rc = bind(fd, (struct sockaddr *)&sa, sizeof(sa)); + if(rc < 0) { + fprintf(stderr, "bind() failed: (%d) %s\n", rc, strerror(errno)); + close(fd); + return -1; + } + + /* serial port for GPS receiver */ + + fd_serial= serial_init(argv[2], GPS_BAUDRATE); + if(fd_serial < 0) { + fprintf(stderr, "serial_init failed: (%d) %s\n", fd_serial, strerror(errno)); + close(fd); + return -1; + } + + set_ubx_protocol(fd_serial); + + signal(SIGINT, &signal_handler); + signal(SIGABRT, &signal_handler); + signal(SIGUSR1, &signal_handler); + + printf("Waiting for incoming data....\n"); + + rrlp_serv(fd, fd_serial); + + rc = close(fd); + if(rc != 0) { + fprintf(stderr, "close() failed: (%d) %s\n", rc, strerror(errno)); + } + fd = -1; + + rc = close(fd_serial); + if(rc != 0) { + fprintf(stderr, "close() serial failed: (%d) %s\n", rc, strerror(errno)); + } + fd_serial = -1; + + return 0; +} diff --git a/rrlpd/src/rrlp.c b/rrlpd/src/rrlp.c new file mode 100644 index 0000000..541c0a0 --- /dev/null +++ b/rrlpd/src/rrlp.c @@ -0,0 +1,864 @@ +/* + * rrlp.c + * + * RRLP implementation + * + * + * Copyright (C) 2009 Sylvain Munaut + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + + +#include +#include + +#include "gps.h" +#include "rrlp.h" + +#include +#include +#include +#include +#include +#include +#include +#include + + +/* ------------------------------------------------------------------------ */ +/* RRLP Assistance request decoding */ +/* ---------------------------------------------------------------------{{{ */ +/* Decode and validate the assistance data request messages. + * See section 10.10 of + * . ETSI TS 149 031 V8.1.0 (2009-01) + * . 3GPP TS 49.031 version 8.1.0 Release 8 + */ + +/* Packed structure from 49.031 spec (RGA = Request GPS Assistance) */ + +#define RRLP_RGA0_ALMANAC (1<<0) +#define RRLP_RGA0_UTC_MODEL (1<<1) +#define RRLP_RGA0_IONO_MODEL (1<<2) +#define RRLP_RGA0_NAV_MODEL (1<<3) +#define RRLP_RGA0_DGPS (1<<4) +#define RRLP_RGA0_REF_LOC (1<<5) +#define RRLP_RGA0_REF_TIME (1<<6) +#define RRLP_RGA0_ACQ_ASSIST (1<<7) + +#define RRLP_RGA1_REALTIME_INT (1<<0) +#define RRLP_RGA1_EPH_EXT (1<<1) +#define RRLP_RGA1_EPH_EXT_CHECK (1<<2) + +struct rrlp_rga_hdr { + uint8_t items0; + uint8_t items1; +} __attribute__((packed)); + +struct rrlp_rga_eph_sv { + uint8_t sv_id; /* [7:6] reserved, [5:0] sv_id */ + uint8_t iode; /* latest eph in the MS memory in hours */ +} __attribute__((packed)); + +struct rrlp_rga_eph { + uint8_t wn_hi; /* [7:6] = wn[9:8] */ + uint8_t wn_lo; /* wn[7:0] */ + uint8_t toe; /* latest eph in the MS memory in hours */ + uint8_t nsat_tmtoe; /* [7:4] nstat, [3:0] T-Toe limit */ + struct rrlp_rga_eph_sv svs[0]; +} __attribute__((packed)); + +struct rrlp_rga_eph_ext { + uint8_t validity; /* in 4 hours units */ +} __attribute__((packed)); + +struct rrlp_rga_eph_ext_check { + /* weeks are in gps week modulo 4 */ + uint8_t wn_begin_end; /* [7:4] begin, [3:0] end */ + uint8_t tow_begin; + uint8_t tow_end; +} __attribute__((packed)); + + +/* Parsing function */ + +int +rrlp_decode_assistance_request( + struct rrlp_assist_req *ar, + void *req, int req_len) +{ + struct rrlp_rga_hdr *hdr = NULL; + struct rrlp_rga_eph *eph = NULL; + struct rrlp_rga_eph_ext *eph_ext = NULL; + struct rrlp_rga_eph_ext_check *eph_ext_check = NULL; + int p = 0; + int rc = 0; + + /* Reset */ + ar->req_elems = 0; + ar->eph_svs = 0; + + /* Parse message */ + hdr = req; + p += sizeof(struct rrlp_rga_hdr); + if (p > req_len) + return -1; + + if (hdr->items0 & RRLP_RGA0_NAV_MODEL) { + printf("NAV_MODEL\n"); + eph = req + p; + p += sizeof(struct rrlp_rga_eph); + if (p > req_len) + return -1; + p += (eph->nsat_tmtoe >> 4) * sizeof(struct rrlp_rga_eph_sv); + if (p > req_len) + return -1; + + printf(" GPS week = %d\n", (eph->wn_hi << 8) + eph->wn_lo); + printf(" TOE = %d\n", eph->toe); + printf(" T-TOE limit = %d\n", eph->nsat_tmtoe & 0x0F); + + int i; + for(i = 0; i < (eph->nsat_tmtoe >> 4); i++) { + 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); + if(eph->svs[i].sv_id >> 6) { + /* most certainly invalid data or have to be interpreted differently */ + rc = -3; + } + } + } + + if (hdr->items1 & RRLP_RGA1_EPH_EXT) { + printf("EPH_EXT\n"); + eph_ext = req + p; + p += sizeof(struct rrlp_rga_eph_ext); + if (p > req_len) + return -1; + } + + if (hdr->items1 & RRLP_RGA1_EPH_EXT_CHECK) { + printf("EPH_EXT_CHECK\n"); + eph_ext_check = req + p; + p += sizeof(struct rrlp_rga_eph_ext_check); + if (p > req_len) + return -1; + } + + if (p != req_len && (p != 2 || req_len != 6)) { /* P==2 && req_len == 6 might happen */ + fprintf(stderr, "p != req_len (%d %d)\n", p, req_len); + return -2; /* not all bytes consumed ??? */ + } + + /* Print a warning for unsupported requests */ + if ((eph_ext != NULL) || + (eph_ext_check != NULL) || + (hdr->items0 & (RRLP_RGA0_DGPS | RRLP_RGA0_ACQ_ASSIST)) || + #if 0 + (hdr->items1 & RRLP_RGA1_REALTIME_INT)) { + #else + 0) { + #endif + fprintf(stderr, "[w] Unsupported assistance data requested, ignored ...\n"); + + if(hdr->items0 & RRLP_RGA0_DGPS) + printf("Unsupported assistance data requested: RRLP_RGA0_DGPS\n"); + if(hdr->items0 & RRLP_RGA0_ACQ_ASSIST) + printf("Unsupported assistance data requested: RRLP_RGA0_ACQ_ASSIST\n"); + if(hdr->items1 & RRLP_RGA1_REALTIME_INT) + printf("Unsupported assistance data requested: RRLP_RGA1_REALTIME_INT\n"); + } + + /* Copy the request */ + if (hdr->items0 & RRLP_RGA0_ALMANAC) { + printf("ALMANAC\n"); + ar->req_elems |= RRLP_AR_ALMANAC; + } + + if (hdr->items0 & RRLP_RGA0_UTC_MODEL) { + printf("UTC_MODEL\n"); + ar->req_elems |= RRLP_AR_UTC_MODEL; + } + + if (hdr->items0 & RRLP_RGA0_IONO_MODEL) { + printf("IONO_MODEL\n"); + ar->req_elems |= RRLP_AR_IONO_MODEL; + } + + if (hdr->items0 & RRLP_RGA0_REF_LOC) { + printf("REF_LOC\n"); + ar->req_elems |= RRLP_AR_REF_LOC; + } + + if (hdr->items0 & RRLP_RGA0_REF_TIME) { + printf("REF_TIME\n"); + ar->req_elems |= RRLP_AR_REF_TIME; + } + + if (hdr->items1 & RRLP_RGA1_REALTIME_INT) { + printf("REALTIME_INTEGRITY\n"); + ar->req_elems |= RRLP_AR_REALTIME_INT; + } + + if (hdr->items0 & RRLP_RGA0_NAV_MODEL) { + printf("NAV_MODEL\n"); + int i, n_svs = eph->nsat_tmtoe >> 4; + ar->req_elems |= RRLP_AR_EPHEMERIS; + if(n_svs == 0) { + ar->eph_svs = 0xFFFFFFFFFFFFFFFFULL; + } + else { + for (i=0; ieph_svs |= (1ULL << (eph->svs[i].sv_id - 1)); /* Dieter: CHECK */ + } + } + + return rc; +} + +/* }}} */ + + +/* ------------------------------------------------------------------------ */ +/* RRLP elements fill */ +/* ---------------------------------------------------------------------{{{ */ + + /* Helpers */ + +static void +_ts_23_032_store_latitude(double lat, uint8_t *b) +{ + uint32_t x; + x = (uint32_t) floor(fabs(lat/90.0) * ((double)(1<<23))); + if (x >= (1<<23)) + x = (1<<23) - 1; + if (lat < 0.0) + x |= (1<<23); + b[0] = (x >> 16) & 0xff; + b[1] = (x >> 8) & 0xff; + b[2] = x & 0xff; +} + +static void +_ts_23_032_store_longitude(double lon, uint8_t *b) +{ + int32_t x; + x = floor((lon/360.0) * ((double)(1<<24))); + if (x >= (1<<23)) + x = 0x007fffff; + else if (x < -(1<<23)) + x = 0x00800000; + b[0] = (x >> 16) & 0xff; + b[1] = (x >> 8) & 0xff; + b[2] = x & 0xff; +} + +static void +_ts_23_032_store_altitude(double alt, uint8_t *b) +{ + int alt_i = (int)fabs(alt); + b[0] = ((alt_i >> 8) & 0x7f) | (alt<0.0 ? 0x80 : 0x00); + b[1] = alt_i & 0xff; +} + + + /* Fill methods */ + +static void +_rrlp_fill_navigation_model_element( + struct NavModelElement *rrlp_nme, + struct gps_ephemeris_sv *gps_eph_sv) +{ + struct UncompressedEphemeris *rrlp_eph; + + #if 1 + rrlp_nme->satStatus.present = SatStatus_PR_newSatelliteAndModelUC; + rrlp_eph = &rrlp_nme->satStatus.choice.newSatelliteAndModelUC; + #else /* does this make any difference fro the MS !? */ + rrlp_nme->satStatus.present = SatStatus_PR_newNaviModelUC; + rrlp_eph = &rrlp_nme->satStatus.choice.newNaviModelUC; + #endif + + rrlp_nme->satelliteID = gps_eph_sv->sv_id - 1; /* Dieter: satellite ID is zero based */ + + + rrlp_eph->ephemCodeOnL2 = gps_eph_sv->code_on_l2; + rrlp_eph->ephemURA = gps_eph_sv->sv_ura; + rrlp_eph->ephemSVhealth = gps_eph_sv->sv_health; + rrlp_eph->ephemIODC = gps_eph_sv->iodc; + rrlp_eph->ephemL2Pflag = gps_eph_sv->l2_p_flag; + rrlp_eph->ephemTgd = gps_eph_sv->t_gd; + rrlp_eph->ephemToc = gps_eph_sv->t_oc; + rrlp_eph->ephemAF2 = gps_eph_sv->a_f2; + rrlp_eph->ephemAF1 = gps_eph_sv->a_f1; + rrlp_eph->ephemAF0 = gps_eph_sv->a_f0; + rrlp_eph->ephemCrs = gps_eph_sv->c_rs; + rrlp_eph->ephemDeltaN = gps_eph_sv->delta_n; + rrlp_eph->ephemM0 = gps_eph_sv->m_0; + rrlp_eph->ephemCuc = gps_eph_sv->c_uc; + rrlp_eph->ephemE = gps_eph_sv->e; + rrlp_eph->ephemCus = gps_eph_sv->c_us; + rrlp_eph->ephemAPowerHalf = gps_eph_sv->a_powhalf; + rrlp_eph->ephemToe = gps_eph_sv->t_oe; + rrlp_eph->ephemFitFlag = gps_eph_sv->fit_flag; + rrlp_eph->ephemAODA = gps_eph_sv->aodo; + rrlp_eph->ephemCic = gps_eph_sv->c_ic; + rrlp_eph->ephemOmegaA0 = gps_eph_sv->omega_0; + rrlp_eph->ephemCis = gps_eph_sv->c_is; + rrlp_eph->ephemI0 = gps_eph_sv->i_0; + rrlp_eph->ephemCrc = gps_eph_sv->c_rc; + rrlp_eph->ephemW = gps_eph_sv->w; + rrlp_eph->ephemOmegaADot = gps_eph_sv->omega_dot; + rrlp_eph->ephemIDot = gps_eph_sv->idot; + + rrlp_eph->ephemSF1Rsvd.reserved1 = gps_eph_sv->_rsvd1; + rrlp_eph->ephemSF1Rsvd.reserved2 = gps_eph_sv->_rsvd2; + rrlp_eph->ephemSF1Rsvd.reserved3 = gps_eph_sv->_rsvd3; + rrlp_eph->ephemSF1Rsvd.reserved4 = gps_eph_sv->_rsvd4; +} + +static void +_rrlp_fill_almanac_element( + struct AlmanacElement *rrlp_ae, + struct gps_almanac_sv *gps_alm_sv) +{ + rrlp_ae->satelliteID = gps_alm_sv->sv_id - 1; /* Dieter: satellite ID is zero based */ + + rrlp_ae->almanacE = gps_alm_sv->e; + rrlp_ae->alamanacToa = gps_alm_sv->t_oa; + rrlp_ae->almanacKsii = gps_alm_sv->ksii; + rrlp_ae->almanacOmegaDot = gps_alm_sv->omega_dot; + rrlp_ae->almanacSVhealth = gps_alm_sv->sv_health; + rrlp_ae->almanacAPowerHalf = gps_alm_sv->a_powhalf; + rrlp_ae->almanacOmega0 = gps_alm_sv->omega_0; + rrlp_ae->almanacW = gps_alm_sv->w; + rrlp_ae->almanacM0 = gps_alm_sv->m_0; + rrlp_ae->almanacAF0 = gps_alm_sv->a_f0; + rrlp_ae->almanacAF1 = gps_alm_sv->a_f1; + +} + +static void +_rrlp_fill_ionospheric_model( + struct IonosphericModel *rrlp_iono, + struct gps_ionosphere_model *gps_iono) +{ + rrlp_iono->alfa0 = gps_iono->alpha_0; + rrlp_iono->alfa1 = gps_iono->alpha_1; + rrlp_iono->alfa2 = gps_iono->alpha_2; + rrlp_iono->alfa3 = gps_iono->alpha_3; + rrlp_iono->beta0 = gps_iono->beta_0; + rrlp_iono->beta1 = gps_iono->beta_1; + rrlp_iono->beta2 = gps_iono->beta_2; + rrlp_iono->beta3 = gps_iono->beta_3; +} + +static void +_rrlp_fill_utc_model( + struct UTCModel *rrlp_utc, + struct gps_utc_model *gps_utc) +{ + rrlp_utc->utcA1 = gps_utc->a1; + rrlp_utc->utcA0 = gps_utc->a0; + rrlp_utc->utcTot = gps_utc->t_ot; + rrlp_utc->utcWNt = gps_utc->wn_t & 0xff; + rrlp_utc->utcDeltaTls = gps_utc->delta_t_ls; + rrlp_utc->utcWNlsf = gps_utc->wn_lsf & 0xff; + rrlp_utc->utcDN = gps_utc->dn; + rrlp_utc->utcDeltaTlsf = gps_utc->delta_t_lsf; + + printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcA1); + printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcA0); + printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcTot); + printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcWNt); + printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcDeltaTls); + printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcWNlsf); + printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcDN); + printf("UTC: 0x%X\n", (unsigned)rrlp_utc->utcDeltaTlsf); +} + +/* }}} */ + + +/* ------------------------------------------------------------------------ */ +/* RRLP Assistance PDU Generation */ +/* ---------------------------------------------------------------------{{{ */ + +struct PDU * +_rrlp_create_gps_assist_pdu(int refnum, struct GPS_AssistData **o_gps_ad, int iLast) +{ + struct PDU *pdu; + struct GPS_AssistData *gps_ad; + MoreAssDataToBeSent_t *moreAssDataToBeSent; + + pdu = calloc(1, sizeof(*pdu)); + if (!pdu) + return NULL; + + gps_ad = calloc(1, sizeof(*gps_ad)); + if (!gps_ad) { + free(pdu); + return NULL; + } + + moreAssDataToBeSent = calloc(1, sizeof(*moreAssDataToBeSent)); + if (!moreAssDataToBeSent) { + free(gps_ad); + free(pdu); + return NULL; + } + /* last message or more messages ? */ + if(iLast) { + if(asn_long2INTEGER(moreAssDataToBeSent, MoreAssDataToBeSent_noMoreMessages) != 0) + fprintf(stderr, "encode error\n"); + } else { + if(asn_long2INTEGER(moreAssDataToBeSent, MoreAssDataToBeSent_moreMessagesOnTheWay) != 0) + fprintf(stderr, "encode error\n"); + } + + if (o_gps_ad) + *o_gps_ad = gps_ad; + + pdu->referenceNumber = refnum; + pdu->component.present = RRLP_Component_PR_assistanceData; + pdu->component.choice.assistanceData.gps_AssistData = gps_ad; + pdu->component.choice.assistanceData.moreAssDataToBeSent = moreAssDataToBeSent; + + return pdu; +} + +static int +_rrlp_add_ionospheric_model( + struct GPS_AssistData *rrlp_gps_ad, + struct gps_assist_data *gps_ad) +{ + struct IonosphericModel *rrlp_iono; + + if (!(gps_ad->fields & GPS_FIELD_IONOSPHERE)) + return -EINVAL; + + rrlp_iono = calloc(1, sizeof(*rrlp_iono)); + if (!rrlp_iono) + return -ENOMEM; + rrlp_gps_ad->controlHeader.ionosphericModel = rrlp_iono; + + _rrlp_fill_ionospheric_model(rrlp_iono, &gps_ad->ionosphere); + + return 0; +} + +static int +_rrlp_add_utc_model( + struct GPS_AssistData *rrlp_gps_ad, + struct gps_assist_data *gps_ad) +{ + struct UTCModel *rrlp_utc; + + if (!(gps_ad->fields & GPS_FIELD_UTC)) + return -EINVAL; + + rrlp_utc = calloc(1, sizeof(*rrlp_utc)); + if (!rrlp_utc) + return -ENOMEM; + rrlp_gps_ad->controlHeader.utcModel = rrlp_utc; + + _rrlp_fill_utc_model(rrlp_utc, &gps_ad->utc); + + return 0; +} + +static int +_rrlp_add_reference_location( + struct GPS_AssistData *rrlp_gps_ad, + struct gps_assist_data *gps_ad) +{ +#if 0 /* old */ + struct RefLocation *rrlp_refloc; + + /* FIXME: Check if info is in gps_ad */ + + rrlp_refloc = calloc(1, sizeof(*rrlp_refloc)); + if (!rrlp_refloc) + return -ENOMEM; + rrlp_gps_ad->controlHeader.refLocation = rrlp_refloc; + + /* FIXME */ + { + uint8_t gps_loc[] = { + 0x80, /* Ellipsoid Point with altitude */ + #if 0 + 0x48, 0x0f, 0x93, /* 50.667778 N */ + 0x03, 0x47, 0x87, /* 4.611667 E */ + 0x00, 0x72, /* 114m */ + #else // Dieter + 0x44, 0xEF, 0xEB, + 0x09, 0x33, 0xAD, + 0x01, 0xEC, + #endif + }; + uint8_t *b = malloc(sizeof(gps_loc)); + memcpy(b, gps_loc, sizeof(gps_loc)); + rrlp_refloc->threeDLocation.buf = b; + rrlp_refloc->threeDLocation.size = sizeof(gps_loc); + } + + return 0; + +#else /* new */ + + struct RefLocation *rrlp_refloc; + uint8_t *b; + + if (!(gps_ad->fields & GPS_FIELD_REFPOS)) + return -EINVAL; + + rrlp_refloc = calloc(1, sizeof(*rrlp_refloc)); + if (!rrlp_refloc) + return -ENOMEM; + rrlp_gps_ad->controlHeader.refLocation = rrlp_refloc; + + b = malloc(9); + + b[0] = 0x80; /* Ellipsoid Point with altitude */ + _ts_23_032_store_latitude(gps_ad->ref_pos.latitude, &b[1]); + _ts_23_032_store_longitude(gps_ad->ref_pos.longitude, &b[4]); + _ts_23_032_store_altitude(gps_ad->ref_pos.altitude, &b[7]); + + rrlp_refloc->threeDLocation.buf = b; + rrlp_refloc->threeDLocation.size = 9; + + return 0; +#endif +} + +static int +_rrlp_add_reference_time( + struct GPS_AssistData *rrlp_gps_ad, + struct gps_assist_data *gps_ad) +{ +#if 0 /* old */ + struct ReferenceTime *rrlp_reftime; + + /* FIXME: Check if info is in gps_ad */ + + rrlp_reftime = calloc(1, sizeof(*rrlp_reftime)); + if (!rrlp_reftime) + return -ENOMEM; + rrlp_gps_ad->controlHeader.referenceTime = rrlp_reftime; + + /* FIXME */ +// rrlp_reftime.gpsTime.gpsTOW23b = g_gps_tow / 80; /* 23 bits */ +// rrlp_reftime.gpsTime.gpsWeek = g_gps_week & 0x3ff; /* 10 bits */ + + #if 1 // Dieter + printf("Time %d\n", (int)time(NULL)); + //rrlp_reftime->gpsTime.gpsTOW23b = (time(NULL) - 1261643144) + 375961; + rrlp_reftime->gpsTime.gpsTOW23b = (time(NULL) - 1281949530) + 119148; + printf("GPS TOW %d\n", (int)rrlp_reftime->gpsTime.gpsTOW23b); + rrlp_reftime->gpsTime.gpsTOW23b = (uint32_t)((double)rrlp_reftime->gpsTime.gpsTOW23b * 12.5 + 0.5) & 0x7FFFFF; + //rrlp_reftime->gpsTime.gpsWeek = 1563 & 0x3FF; // KW52 2009 + //rrlp_reftime->gpsTime.gpsWeek = 1565 & 0x3FF; // KW1 2010 + rrlp_reftime->gpsTime.gpsWeek = 1597 & 0x3FF; // KW33 2010 + #endif + + return 0; +#else /* new */ + struct ReferenceTime *rrlp_reftime; + double tow; + + if (!(gps_ad->fields & GPS_FIELD_REFTIME)) + return -EINVAL; + + rrlp_reftime = calloc(1, sizeof(*rrlp_reftime)); + if (!rrlp_reftime) + return -ENOMEM; + rrlp_gps_ad->controlHeader.referenceTime = rrlp_reftime; + + /* TODO: adjust offset so that MS receives the correct time ? */ + #define MY_OFFSET 0 + + tow = gps_ad->ref_time.tow + (time(NULL) - gps_ad->ref_time.when) + MY_OFFSET; + printf("TOW = %f\n", tow); + + rrlp_reftime->gpsTime.gpsWeek = gps_ad->ref_time.wn & 0x3ff; /* 10b */ + rrlp_reftime->gpsTime.gpsTOW23b = + ((int)floor(tow / 0.08)) & 0x7fffff; /* 23b */ + + return 0; +#endif +} + +static int +_rrlp_add_realtime_integrity( + struct GPS_AssistData *rrlp_gps_ad, + struct gps_assist_data *gps_ad) +{ + struct SeqOf_BadSatelliteSet *rrlp_realtime_integrity; + +#if 0 /* not yet */ + if (!(gps_ad->fields & GPS_FIELD_REALTIME_INT)) + return -EINVAL; +#endif + + rrlp_realtime_integrity = calloc(1, sizeof(*rrlp_realtime_integrity)); + if (!rrlp_realtime_integrity) + return -ENOMEM; + + rrlp_gps_ad->controlHeader.realTimeIntegrity = rrlp_realtime_integrity; + +#if 1 /* TODO */ + + SatelliteID_t *sa_id; + + sa_id = calloc(1, sizeof(*sa_id)); + + *sa_id = 63; + + ASN_SEQUENCE_ADD(&rrlp_realtime_integrity->list, sa_id); +#endif + + return 0; +} + +static int +_rrlp_add_almanac( + struct GPS_AssistData *rrlp_gps_ad, + struct gps_assist_data *gps_ad, int *start, int count) +{ + int i; + struct Almanac *rrlp_alm; + struct gps_almanac *gps_alm = &gps_ad->almanac; + + if (!(gps_ad->fields & GPS_FIELD_ALMANAC)) + return -EINVAL; + + rrlp_alm = calloc(1, sizeof(*rrlp_alm)); + if (!rrlp_alm) + return -ENOMEM; + rrlp_gps_ad->controlHeader.almanac = rrlp_alm; + + rrlp_alm->alamanacWNa = gps_alm->wna; + if (count == -1) + count = gps_alm->n_sv - *start; + for (i=*start; (i<*start+count) && (in_sv); i++) { + struct AlmanacElement *ae; + ae = calloc(1, sizeof(*ae)); + if (!ae) + return -ENOMEM; + _rrlp_fill_almanac_element(ae, &gps_alm->svs[i]); + ASN_SEQUENCE_ADD(&rrlp_alm->almanacList.list, ae); + } + + *start = i; + + return i < gps_alm->n_sv; +} + +static int +_rrlp_add_ephemeris( + struct GPS_AssistData *rrlp_gps_ad, + struct gps_assist_data *gps_ad, int *start, int count, uint64_t mask) +{ + int i, j; + struct NavigationModel *rrlp_nav; + struct gps_ephemeris *gps_eph = &gps_ad->ephemeris; + + if (!(gps_ad->fields & GPS_FIELD_EPHEMERIS)) + return -EINVAL; + + rrlp_nav = calloc(1, sizeof(*rrlp_nav)); + if (!rrlp_nav) + return -ENOMEM; + rrlp_gps_ad->controlHeader.navigationModel = rrlp_nav; + + if (count == -1) + count = gps_eph->n_sv - *start; + for (i=*start,j=0; (jn_sv); i++) { + if (!(mask & (1ULL<<(gps_eph->svs[i].sv_id-1)))) /* Dieter: CHECK */ + continue; + struct NavModelElement *nme; + nme = calloc(1, sizeof(*nme)); + if (!nme) + return -ENOMEM; + _rrlp_fill_navigation_model_element(nme, &gps_eph->svs[i]); + ASN_SEQUENCE_ADD(&rrlp_nav->navModelList.list, nme); + j++; + } + + *start = i; + + return i < gps_eph->n_sv; +} + + +#define MAX_PDUS 64 + +int +rrlp_gps_assist_pdus(int refnum, + struct gps_assist_data *gps_ad, struct rrlp_assist_req *req, + void **o_pdu, int *o_len, int o_max_pdus) +{ + struct PDU *lst_pdu[MAX_PDUS]; + int lst_cnt = 0; + + struct PDU *rrlp_pdu = NULL; + struct GPS_AssistData *rrlp_gps_ad = NULL; + uint32_t re = req->req_elems; + int i, rv = 0; + + /* IonosphericModel, UTCModel, RefLocation, ReferenceTime */ + if (re & (RRLP_AR_IONO_MODEL | + RRLP_AR_UTC_MODEL | + RRLP_AR_REF_TIME | + RRLP_AR_REF_LOC | + RRLP_AR_REALTIME_INT)) + { + int pdu_has_data = 0; + + rrlp_pdu = _rrlp_create_gps_assist_pdu(refnum, &rrlp_gps_ad, 0); + if (!rrlp_pdu) { + rv = -ENOMEM; + goto error; + } + +#if 1 /* enable/disable component */ + if (re & RRLP_AR_IONO_MODEL) { + printf("+ IONO_MODEL\n"); + if (!_rrlp_add_ionospheric_model(rrlp_gps_ad, gps_ad)) + pdu_has_data = 1; + } +#endif +#if 1 /* enable/disable component */ + if (re & RRLP_AR_UTC_MODEL) { + printf("+ UTC_MODEL\n"); + if (!_rrlp_add_utc_model(rrlp_gps_ad, gps_ad)) + pdu_has_data = 1; + } +#endif +#if 1 /* enable/disable component */ + if (re & RRLP_AR_REF_TIME) { + printf("+ REF_TIME\n"); + if (!_rrlp_add_reference_time(rrlp_gps_ad, gps_ad)) + pdu_has_data = 1; + } +#endif +#if 1 /* enable/disable component */ + if (re & RRLP_AR_REF_LOC) { + printf("+ REF_LOC\n"); + if (!_rrlp_add_reference_location(rrlp_gps_ad, gps_ad)) + pdu_has_data = 1; + } +#endif +#if 0 /* enable/disable component (skip this as it is for now a dummy list only) */ + if (re & RRLP_AR_REALTIME_INT) { + printf("+ REALTIME_INTEGRITY\n"); + if (!_rrlp_add_realtime_integrity(rrlp_gps_ad, gps_ad)) + pdu_has_data = 1; + } +#endif + + if (pdu_has_data) { + lst_pdu[lst_cnt++] = rrlp_pdu; + rrlp_pdu = NULL; + } + } +#if 1 /* enable/disable component */ + /* Almanac */ + if (re & RRLP_AR_ALMANAC) { + i = 0; + do { + if (!(gps_ad->fields & GPS_FIELD_ALMANAC)) + break; + + printf("+ ALMANAC\n"); + + if (!rrlp_pdu) { + rrlp_pdu = _rrlp_create_gps_assist_pdu(refnum, &rrlp_gps_ad, 0); + if (!rrlp_pdu) { + rv = -ENOMEM; + goto error; + } + } + + /* adjust count so that messages fit in a single PDU */ + rv = _rrlp_add_almanac(rrlp_gps_ad, gps_ad, &i, 10); + if (rv < 0) + goto error; + + lst_pdu[lst_cnt++] = rrlp_pdu; + rrlp_pdu = NULL; + } while (rv); + } +#endif +#if 1 /* enable/disable component */ + /* Ephemeris */ + if (re & RRLP_AR_EPHEMERIS) { + i = 0; + do { + if (!(gps_ad->fields & GPS_FIELD_EPHEMERIS)) + break; + + printf("+ EPHEMERIS\n"); + + if (!rrlp_pdu) { + rrlp_pdu = _rrlp_create_gps_assist_pdu(refnum, &rrlp_gps_ad, 0); + if (!rrlp_pdu) { + rv = -ENOMEM; + goto error; + } + } +#if 1 /* two sets in one PDS to be not larger than maximum message size */ + rv = _rrlp_add_ephemeris(rrlp_gps_ad, gps_ad, &i, 2, req->eph_svs); +#elif 0 /* three sets in one PDU, too large */ + rv = _rrlp_add_ephemeris(rrlp_gps_ad, gps_ad, &i, 3, req->eph_svs); +#elif 0 /* all sets in one PDU, too large */ + rv = _rrlp_add_ephemeris(rrlp_gps_ad, gps_ad, &i, -1, req->eph_svs); +#endif + lst_pdu[lst_cnt++] = rrlp_pdu; + rrlp_pdu = NULL; + + } while (rv); + } +#endif + + /* set last message flag */ + if(asn_long2INTEGER(lst_pdu[lst_cnt - 1]->component.choice.assistanceData.moreAssDataToBeSent, MoreAssDataToBeSent_noMoreMessages) != 0) + fprintf(stderr, "encode error\n"); + + /* Serialize & Release all PDUs */ + for (i=0; i + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef __RRLP_H__ +#define __RRLP_H__ + +#include + +#include "gps.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +/* Our internal simplified structure for requests */ + +#define RRLP_AR_REF_LOC (1<<0) +#define RRLP_AR_REF_TIME (1<<1) +#define RRLP_AR_UTC_MODEL (1<<2) +#define RRLP_AR_IONO_MODEL (1<<3) +#define RRLP_AR_ALMANAC (1<<4) +#define RRLP_AR_EPHEMERIS (1<<5) +#define RRLP_AR_REALTIME_INT (1<<6) + +struct rrlp_assist_req { + uint32_t req_elems; + uint64_t eph_svs; +}; + + +/* Methods */ +int rrlp_decode_assistance_request(struct rrlp_assist_req *ar, + void *req, int req_len); + +int rrlp_gps_assist_pdus(int refnum, + struct gps_assist_data *gps_ad, struct rrlp_assist_req *req, + void **o_pdu, int *o_len, int o_max_pdus); + + +#ifdef __cplusplus +} +#endif + +#endif /* __RRLP_H__ */ + diff --git a/rrlpd/src/ubx-parse.c b/rrlpd/src/ubx-parse.c new file mode 100644 index 0000000..0f6e816 --- /dev/null +++ b/rrlpd/src/ubx-parse.c @@ -0,0 +1,246 @@ +/* + * ubx-parse.c + * + * Implementation of parsing code converting UBX messages to GPS assist + * data + * + * + * Copyright (C) 2009 Sylvain Munaut + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#include "gps.h" +#include "ubx.h" +#include "ubx-parse.h" + +#define DEBUG 1 +#if DEBUG + #define printd(x, args ...) printf(x, ## args) +#else + #define printd(x, args ...) +#endif + +#define DEBUG1 0 +#if DEBUG1 + #define printd1(x, args ...) printf(x, ## args) +#else + #define printd1(x, args ...) +#endif + +/* Helpers */ + +static int +float_to_fixedpoint(float f, int sf) +{ + if (sf < 0) { + while (sf++ < 0) + f *= 2.0f; + } else { + while (sf-- > 0) + f *= 0.5f; + } + + return (int)f; +} + +static inline int +double_to_fixedpoint(double d, int sf) +{ + if (sf < 0) { + while (sf++ < 0) + d *= 2.0; + } else { + while (sf-- > 0) + d *= 0.5; + } + + return (int)d; +} + + +/* UBX message parsing to fill gps assist data */ + +static void +_ubx_msg_parse_nav_posllh(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) +{ + struct ubx_nav_posllh *nav_posllh = pl; + struct gps_assist_data *gps = ud; + + printd("[.] NAV_POSLLH\n"); + + gps->fields |= GPS_FIELD_REFPOS; + + gps->ref_pos.latitude = (double)(nav_posllh->lat) * 1e-7; + gps->ref_pos.longitude = (double)(nav_posllh->lon) * 1e-7; + gps->ref_pos.altitude = (double)(nav_posllh->height) * 1e-3; + + printd(" TOW %lu\n", nav_posllh->itow); + printd(" latitude %f\n", gps->ref_pos.latitude); + printd(" longitude %f\n", gps->ref_pos.longitude); + printd(" altitude %f\n", gps->ref_pos.altitude); +} + +static void +_ubx_msg_parse_aid_ini(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) +{ + struct ubx_aid_ini *aid_ini = pl; + struct gps_assist_data *gps = ud; + + printd("[.] AID_INI\n"); + + /* Extract info for "Reference Time" */ + gps->fields |= GPS_FIELD_REFTIME; + + gps->ref_time.wn = aid_ini->wn; + gps->ref_time.tow = (double)aid_ini->tow * 1e-3; + gps->ref_time.when = time(NULL); + + printd(" WN %d\n", gps->ref_time.wn); + printd(" TOW %ld\n", aid_ini->tow); + + if((aid_ini->flags & 0x03) != 0x03) { /* time and pos valid ? */ + fprintf(stderr, "Postion and/or time not valid (0x%lx)", aid_ini->flags); + } + + // FIXME: We could extract ref position as well but we need it in + // WGS84 geodetic coordinates and it's provided as ecef, so + // we need a lot of math ... +} + +static void +_ubx_msg_parse_aid_hui(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) +{ + struct ubx_aid_hui *aid_hui = pl; + struct gps_assist_data *gps = ud; + + printd("[.] AID_HUI\n"); + + if (aid_hui->flags & 0x2) { /* UTC parameters valid */ + struct gps_utc_model *utc = &gps->utc; + + printd(" UTC\n"); + + gps->fields |= GPS_FIELD_UTC; + + utc->a0 = double_to_fixedpoint(aid_hui->utc_a0, -30); + utc->a1 = double_to_fixedpoint(aid_hui->utc_a1, -50); + utc->delta_t_ls = aid_hui->utc_ls; + utc->t_ot = aid_hui->utc_tot >> 12; + utc->wn_t = aid_hui->utc_wnt; + utc->wn_lsf = aid_hui->utc_wnf; + utc->dn = aid_hui->utc_dn; + utc->delta_t_lsf = aid_hui->utc_lsf; + } + + if (aid_hui->flags & 0x04) { /* Klobuchar parameters valid */ + struct gps_ionosphere_model *iono = &gps->ionosphere; + + printd(" IONOSPHERE\n"); + + gps->fields |= GPS_FIELD_IONOSPHERE; + + iono->alpha_0 = float_to_fixedpoint(aid_hui->klob_a0, -30); + iono->alpha_1 = float_to_fixedpoint(aid_hui->klob_a1, -27); + iono->alpha_2 = float_to_fixedpoint(aid_hui->klob_a2, -24); + iono->alpha_3 = float_to_fixedpoint(aid_hui->klob_a3, -24); + iono->beta_0 = float_to_fixedpoint(aid_hui->klob_b0, 11); + iono->beta_1 = float_to_fixedpoint(aid_hui->klob_b1, 14); + iono->beta_2 = float_to_fixedpoint(aid_hui->klob_b2, 16); + iono->beta_3 = float_to_fixedpoint(aid_hui->klob_b3, 16); + } +} + +static void +_ubx_msg_parse_aid_alm(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) +{ + struct ubx_aid_alm *aid_alm = pl; + struct gps_assist_data *gps = ud; + + if(pl_len == 8) /* length if not available */ + return; + + if(pl_len != sizeof(struct ubx_aid_alm)) { + fprintf(stderr, "pl_len != sizeof(struct ubx_aid_alm) (%d)\n", pl_len); + return; + } + + printd("[.] AID_ALM %2ld - %ld (nsv = %d)\n", aid_alm->sv_id, aid_alm->gps_week, gps->almanac.n_sv); + + if (aid_alm->gps_week) { + int i = gps->almanac.n_sv++; + gps->fields |= GPS_FIELD_ALMANAC; + gps->almanac.wna = aid_alm->gps_week & 0xff; + gps_unpack_sf45_almanac(aid_alm->alm_words, &gps->almanac.svs[i]); + /* set satellite ID this way, otherwise it will be wrong */ + gps->almanac.svs[i].sv_id = aid_alm->sv_id; + } +} + +static void +_ubx_msg_parse_aid_eph(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) +{ + struct ubx_aid_eph *aid_eph = pl; + struct gps_assist_data *gps = ud; + + if(pl_len == 8) /* length if not available */ + return; + + if(pl_len != sizeof(struct ubx_aid_eph)) { + fprintf(stderr, "pl_len != sizeof(struct ubx_aid_eph) (%d)\n", pl_len); + return; + } + + printd("[.] AID_EPH %2ld - %s (nsv = %d)\n", aid_eph->sv_id, aid_eph->present ? "present" : "", gps->ephemeris.n_sv); + + if (aid_eph->present) { + int i = gps->ephemeris.n_sv++; + gps->fields |= GPS_FIELD_EPHEMERIS; + gps->ephemeris.svs[i].sv_id = aid_eph->sv_id; + gps_unpack_sf123(aid_eph->eph_words, &gps->ephemeris.svs[i]); + } +} + + +static void +_ubx_msg_parse_nav_timegps(struct ubx_hdr *hdr, void *pl, int pl_len, void *ud) +{ + struct ubx_nav_timegps *nav_timegps = pl; + struct gps_assist_data *gps = ud; + + printd1("[.] NAV_TIMEGPS\n"); + + /* Extract info for "Reference Time" */ + gps->fields |= GPS_FIELD_REFTIME; + + gps->ref_time.wn = nav_timegps->week; + gps->ref_time.tow = (double)nav_timegps->itow * 1e-3; + gps->ref_time.when = time(NULL); + + printd1(" WN %d\n", nav_timegps->week); + printd1(" TOW %ld\n", nav_timegps->itow); +} + +/* Dispatch table */ +struct ubx_dispatch_entry ubx_parse_dt[] = { + UBX_DISPATCH(NAV, POSLLH, _ubx_msg_parse_nav_posllh), + UBX_DISPATCH(AID, INI, _ubx_msg_parse_aid_ini), + UBX_DISPATCH(AID, HUI, _ubx_msg_parse_aid_hui), + UBX_DISPATCH(AID, ALM, _ubx_msg_parse_aid_alm), + UBX_DISPATCH(AID, EPH, _ubx_msg_parse_aid_eph), + UBX_DISPATCH(NAV, TIMEGPS, _ubx_msg_parse_nav_timegps), +}; + diff --git a/rrlpd/src/ubx-parse.h b/rrlpd/src/ubx-parse.h new file mode 100644 index 0000000..621475d --- /dev/null +++ b/rrlpd/src/ubx-parse.h @@ -0,0 +1,45 @@ +/* + * ubx-parse.h + * + * Header for parsing code converting UBX messages to GPS assist data + * + * + * Copyright (C) 2009 Sylvain Munaut + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef __UBX_PARSE_H__ +#define __UBX_PARSE_H__ + + +#include "gps.h" +#include "ubx.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +/* Dispatch table */ +extern struct ubx_dispatch_entry ubx_parse_dt[]; + + +#ifdef __cplusplus +} +#endif + +#endif /* __UBX_PARSE_H__ */ + diff --git a/rrlpd/src/ubx.c b/rrlpd/src/ubx.c new file mode 100644 index 0000000..29baa61 --- /dev/null +++ b/rrlpd/src/ubx.c @@ -0,0 +1,96 @@ +/* + * ubx.c + * + * Implementation of generic UBX helpers + * + * + * Copyright (C) 2009 Sylvain Munaut + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include + +#include "ubx.h" + + +static void +ubx_checksum(uint8_t *data, int len, uint8_t *cksum) +{ + int i; + uint8_t ck0 = 0, ck1 = 0; + for (i=0; ihandler) { + if ((dt->msg_class == msg_class) && (dt->msg_id == msg_id)) + return dt->handler; + dt++; + } + return NULL; +} + + +int +ubx_msg_dispatch(struct ubx_dispatch_entry *dt, + void *msg, int len, void *userdata) +{ + struct ubx_hdr *hdr = msg; + uint8_t cksum[2], *cksum_ptr; + ubx_msg_handler_t h; + + if(len < 2) { + fprintf(stderr, "[!] Length too small (%d)\n", len); + return -1; + } + + if ((hdr->sync[0] != UBX_SYNC0) || (hdr->sync[1] != UBX_SYNC1)) { + fprintf(stderr, "[!] Invalid sync bytes\n"); + return -1; + } + + if(len < sizeof(struct ubx_hdr)) { + fprintf(stderr, "[!] Length too small for UBX header (%d)\n", len); + return -1; + } + + if(len < sizeof(struct ubx_hdr) + hdr->payload_len - 2) { + fprintf(stderr, "[!] Length too small for UBX header and payload (%d)\n", len); + return -1; + } + + ubx_checksum(msg + 2, sizeof(struct ubx_hdr) + hdr->payload_len - 2, cksum); + cksum_ptr = msg + (sizeof(struct ubx_hdr) + hdr->payload_len); + if ((cksum_ptr[0] != cksum[0]) || (cksum_ptr[1] != cksum[1])) { + fprintf(stderr, "[!] Invalid checksum\n"); + return -1; + } + + h = ubx_find_handler(dt, hdr->msg_class, hdr->msg_id); + if (h) + h(hdr, msg + sizeof(struct ubx_hdr), hdr->payload_len, userdata); + + return sizeof(struct ubx_hdr) + hdr->payload_len + 2; +} + diff --git a/rrlpd/src/ubx.h b/rrlpd/src/ubx.h new file mode 100644 index 0000000..13ae5fa --- /dev/null +++ b/rrlpd/src/ubx.h @@ -0,0 +1,240 @@ +/* + * ubx.h + * + * Header for UBX related stuff + * + * + * Copyright (C) 2009 Sylvain Munaut + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef __UBX_H__ +#define __UBX_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* Constants used in UBX */ + + /* Sync bytes (two first bytes of each message) */ +#define UBX_SYNC0 0xb5 +#define UBX_SYNC1 0x62 + + /* UBX messages classes */ +#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_RXM 0x02 +#define UBX_CLASS_INF 0x04 +#define UBX_CLASS_ACK 0x05 +#define UBX_CLASS_CFG 0x06 +#define UBX_CLASS_UPD 0x09 +#define UBX_CLASS_MON 0x0a +#define UBX_CLASS_AID 0x0b +#define UBX_CLASS_TIM 0x0d + + /* UBX messages type ID (by class) */ +#define UBX_NAV_POSECEF 0x01 +#define UBX_NAV_POSLLH 0x02 +#define UBX_NAV_STATUS 0x03 +#define UBX_NAV_DOP 0x04 +#define UBX_NAV_SOL 0x06 +#define UBX_NAV_POSUTM 0x08 +#define UBX_NAV_VELECEF 0x11 +#define UBX_NAV_VELNED 0x12 +#define UBX_NAV_TIMEGPS 0x20 +#define UBX_NAV_TIMEUTC 0x21 +#define UBX_NAV_CLOCK 0x22 +#define UBX_NAV_SVINFO 0x30 +#define UBX_NAV_DGPS 0x31 +#define UBX_NAV_SBAS 0x32 +#define UBX_NAV_EKFSTATUS 0x40 + +#define UBX_RXM_RAW 0x10 +#define UBX_RXM_SFRB 0x11 +#define UBX_RXM_SVSI 0x20 +#define UBX_RXM_SVSI_GPS 0x20 +#define UBX_RXM_ALM 0x30 +#define UBX_RXM_EPH 0x31 +#define UBX_RXM_POSREQ 0x40 + +#define UBX_INF_ERROR 0x00 +#define UBX_INF_WARNING 0x01 +#define UBX_INF_NOTICE 0x02 +#define UBX_INF_TEST 0x03 +#define UBX_INF_DEBUG 0x04 +#define UBX_INF_USER 0x07 + +#define UBX_ACK_NAK 0x00 +#define UBX_ACK_ACK 0x01 + +#define UBX_CFG_PRT 0x00 +#define UBX_CFG_USB 0x1b +#define UBX_CFG_MSG 0x01 +#define UBX_CFG_NMEA 0x17 +#define UBX_CFG_RATE 0x08 +#define UBX_CFG_CFG 0x09 +#define UBX_CFG_TP 0x07 +#define UBX_CFG_NAV2 0x1a +#define UBX_CFG_DAT 0x06 +#define UBX_CFG_INF 0x02 +#define UBX_CFG_RST 0x04 +#define UBX_CFG_RXM 0x11 +#define UBX_CFG_ANT 0x13 +#define UBX_CFG_FXN 0x0e +#define UBX_CFG_SBAS 0x16 +#define UBX_CFG_LIC 0x80 +#define UBX_CFG_TM 0x10 +#define UBX_CFG_TM2 0x19 +#define UBX_CFG_TMODE 0x1d +#define UBX_CFG_EKF 0x12 + +#define UBX_UPD_DOWNL 0x01 +#define UBX_UPD_UPLOAD 0x02 +#define UBX_UPD_EXEC 0x03 +#define UBX_UPD_MEMCPY 0x04 + +#define UBX_MON_SCHD 0x01 +#define UBX_MON_IO 0x02 +#define UBX_MON_IPC 0x03 +#define UBX_MON_VER 0x04 +#define UBX_MON_EXCEPT 0x05 +#define UBX_MON_MSGPP 0x06 +#define UBX_MON_RXBUF 0x07 +#define UBX_MON_TXBUF 0x08 +#define UBX_MON_HW 0x09 +#define UBX_MON_USB 0x0a + +#define UBX_AID_REQ 0x00 +#define UBX_AID_INI 0x01 +#define UBX_AID_HUI 0x02 +#define UBX_AID_DATA 0x10 +#define UBX_AID_ALM 0x30 +#define UBX_AID_EPH 0x31 + +#define UBX_TIM_TP 0x01 +#define UBX_TIM_TM 0x02 +#define UBX_TIM_TM2 0x03 +#define UBX_TIM_SVIN 0x04 + + +/* Header */ +struct ubx_hdr { + uint8_t sync[2]; + uint8_t msg_class; + uint8_t msg_id; + uint16_t payload_len; +} __attribute__((packed)); + + +/* Payload formats (some of them) */ +struct ubx_nav_posllh { + uint32_t itow; /* ms */ + int32_t lon; /* scaling 1e-7 */ + int32_t lat; /* scaling 1e-7 */ + int32_t height;/* mm */ + int32_t hsl; /* mm */ + uint32_t hacc; /* mm */ + uint32_t vacc; /* mm */ +} __attribute__((packed)); + +struct ubx_aid_ini { + int32_t x; + int32_t y; + int32_t z; + uint32_t posacc; + uint16_t tm_cfg; + uint16_t wn; + uint32_t tow; /* ms */ + int32_t tow_ns; + uint32_t tacc_ms; + uint32_t tacc_ns; + int32_t clkd; + uint32_t clkdacc; + uint32_t flags; +} __attribute__((packed)); + +struct ubx_aid_hui { + uint32_t health; + double utc_a1; + double utc_a0; + int32_t utc_tot; + int16_t utc_wnt; + int16_t utc_ls; + int16_t utc_wnf; + int16_t utc_dn; + int16_t utc_lsf; + int16_t utc_spare; + float klob_a0; + float klob_a1; + float klob_a2; + float klob_a3; + float klob_b0; + float klob_b1; + float klob_b2; + float klob_b3; + uint32_t flags; +} __attribute__((packed)); + +struct ubx_aid_alm { + uint32_t sv_id; + uint32_t gps_week; + uint32_t alm_words[8]; /* Present only if 'gps_week' != 0 */ +} __attribute__((packed)); + +struct ubx_aid_eph { + uint32_t sv_id; + uint32_t present; + uint32_t eph_words[24]; /* Present only if 'present' != 0 */ +} __attribute__((packed)); + +struct ubx_nav_timegps { + uint32_t itow; /* ms */ + int32_t ftow; /* ns */ + int16_t week; + uint8_t leaps; + uint8_t valid; + uint32_t tacc; /* ns */ +} __attribute__((packed)); + +/* Message handler */ +typedef void (*ubx_msg_handler_t)( + struct ubx_hdr *hdr, void *payload, int payload_len, void *userdata); + +struct ubx_dispatch_entry { + uint8_t msg_class; + uint8_t msg_id; + ubx_msg_handler_t handler; +}; + +#define UBX_DISPATCH(kls,id,hdl) { \ + .msg_class = UBX_CLASS_ ## kls , \ + .msg_id = UBX_ ## kls ## _ ## id, \ + .handler = (hdl), \ +} + + +/* Methods */ +int ubx_msg_dispatch(struct ubx_dispatch_entry *dt, + void *msg, int len, void *userdata); + + +#ifdef __cplusplus +} +#endif + +#endif /* __UBX_H__ */ +