rrlp-ephemeris: Support filling of ReferencePositon IE
Signed-off-by: Sylvain Munaut <tnt@246tNt.com>
This commit is contained in:
parent
755429b7f1
commit
8dbd19e6d8
2
Makefile
2
Makefile
|
@ -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
10
gps.h
|
@ -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
68
rrlp.c
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue