rrlp-ephemeris: Support filling of ReferencePositon IE

Signed-off-by: Sylvain Munaut <tnt@246tNt.com>
This commit is contained in:
Sylvain Munaut 2009-12-28 14:31:41 +01:00 committed by Harald Welte
parent 755429b7f1
commit 8dbd19e6d8
3 changed files with 65 additions and 15 deletions

View File

@ -11,7 +11,7 @@ all: rrlp-test
rrlp-test: libgsm-asn1.a gps.o ubx.o ubx-parse.o rrlp.o main.o rrlp-test: libgsm-asn1.a gps.o ubx.o ubx-parse.o rrlp.o main.o
$(CC) -o $@ gps.o ubx.o ubx-parse.o rrlp.o main.o -L. -lgsm-asn1 $(CC) -o $@ gps.o ubx.o ubx-parse.o rrlp.o main.o -L. -lgsm-asn1 -lm
# #

10
gps.h
View File

@ -143,11 +143,20 @@ struct gps_ephemeris {
}; };
/* Reference position */
struct gps_ref_pos { /* WSG84 ellipsoid */
double latitude; /* deg */
double longitude; /* deg */
double altitude; /* m above ellipsoid */
};
/* All assist data */ /* All assist data */
#define GPS_FIELD_IONOSPHERE (1<<0) #define GPS_FIELD_IONOSPHERE (1<<0)
#define GPS_FIELD_UTC (1<<1) #define GPS_FIELD_UTC (1<<1)
#define GPS_FIELD_ALMANAC (1<<2) #define GPS_FIELD_ALMANAC (1<<2)
#define GPS_FIELD_EPHEMERIS (1<<3) #define GPS_FIELD_EPHEMERIS (1<<3)
#define GPS_FIELD_REFPOS (1<<4)
struct gps_assist_data { struct gps_assist_data {
int fields; int fields;
@ -155,6 +164,7 @@ struct gps_assist_data {
struct gps_utc_model utc; struct gps_utc_model utc;
struct gps_almanac almanac; struct gps_almanac almanac;
struct gps_ephemeris ephemeris; struct gps_ephemeris ephemeris;
struct gps_ref_pos ref_pos;
}; };

68
rrlp.c
View File

@ -22,6 +22,7 @@
#include <errno.h> #include <errno.h>
#include <math.h>
#include "gps.h" #include "gps.h"
#include "rrlp.h" #include "rrlp.h"
@ -181,6 +182,47 @@ rrlp_decode_assistance_request(
/* RRLP elements fill */ /* 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 static void
_rrlp_fill_navigation_model_element( _rrlp_fill_navigation_model_element(
struct NavModelElement *rrlp_nme, struct NavModelElement *rrlp_nme,
@ -358,27 +400,25 @@ _rrlp_add_reference_location(
struct gps_assist_data *gps_ad) struct gps_assist_data *gps_ad)
{ {
struct RefLocation *rrlp_refloc; struct RefLocation *rrlp_refloc;
uint8_t *b;
/* FIXME: Check if info is in gps_ad */ if (!(gps_ad->fields & GPS_FIELD_REFPOS))
return -EINVAL;
rrlp_refloc = calloc(1, sizeof(*rrlp_refloc)); rrlp_refloc = calloc(1, sizeof(*rrlp_refloc));
if (!rrlp_refloc) if (!rrlp_refloc)
return -ENOMEM; return -ENOMEM;
rrlp_gps_ad->controlHeader.refLocation = rrlp_refloc; rrlp_gps_ad->controlHeader.refLocation = rrlp_refloc;
/* FIXME */ b = malloc(9);
{
uint8_t gps_loc[] = { b[0] = 0x80; /* Ellipsoid Point with altitude */
0x80, /* Ellipsoid Point with altitude */ _ts_23_032_store_latitude(gps_ad->ref_pos.latitude, &b[1]);
0x48, 0x0f, 0x93, /* 50.667778 N */ _ts_23_032_store_longitude(gps_ad->ref_pos.longitude, &b[4]);
0x03, 0x47, 0x87, /* 4.611667 E */ _ts_23_032_store_altitude(gps_ad->ref_pos.altitude, &b[7]);
0x00, 0x72, /* 114m */
}; rrlp_refloc->threeDLocation.buf = b;
uint8_t *b = malloc(sizeof(gps_loc)); rrlp_refloc->threeDLocation.size = 9;
memcpy(b, gps_loc, sizeof(gps_loc));
rrlp_refloc->threeDLocation.buf = b;
rrlp_refloc->threeDLocation.size = sizeof(gps_loc);
}
return 0; return 0;
} }