rrlp-ephemeris: Support filling of ReferencePositon IE
Signed-off-by: Sylvain Munaut <tnt@246tNt.com>
diff --git a/Makefile b/Makefile
index a1f2265..742cac7 100644
--- a/Makefile
+++ b/Makefile
@@ -11,7 +11,7 @@
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
#
diff --git a/gps.h b/gps.h
index 50a7e5e..03d643f 100644
--- a/gps.h
+++ b/gps.h
@@ -143,11 +143,20 @@
};
+/* Reference position */
+struct gps_ref_pos { /* WSG84 ellipsoid */
+ double latitude; /* deg */
+ double longitude; /* deg */
+ double altitude; /* m above ellipsoid */
+};
+
+
/* 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)
struct gps_assist_data {
int fields;
@@ -155,6 +164,7 @@
struct gps_utc_model utc;
struct gps_almanac almanac;
struct gps_ephemeris ephemeris;
+ struct gps_ref_pos ref_pos;
};
diff --git a/rrlp.c b/rrlp.c
index 0ff9321..5e55d1d 100644
--- a/rrlp.c
+++ b/rrlp.c
@@ -22,6 +22,7 @@
#include <errno.h>
+#include <math.h>
#include "gps.h"
#include "rrlp.h"
@@ -181,6 +182,47 @@
/* 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,
@@ -358,27 +400,25 @@
struct gps_assist_data *gps_ad)
{
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));
if (!rrlp_refloc)
return -ENOMEM;
rrlp_gps_ad->controlHeader.refLocation = rrlp_refloc;
- /* FIXME */
- {
- uint8_t gps_loc[] = {
- 0x80, /* Ellipsoid Point with altitude */
- 0x48, 0x0f, 0x93, /* 50.667778 N */
- 0x03, 0x47, 0x87, /* 4.611667 E */
- 0x00, 0x72, /* 114m */
- };
- 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);
- }
+ 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;
}