From e5a936f388d3275c674235649a34ea88b48a9d72 Mon Sep 17 00:00:00 2001 From: Reinhard Arlt Date: Sat, 31 Jan 2015 08:36:04 +0100 Subject: Add new scale routine. --- driver_nmea2000.c | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) (limited to 'driver_nmea2000.c') diff --git a/driver_nmea2000.c b/driver_nmea2000.c index fba3f967..4059dbee 100644 --- a/driver_nmea2000.c +++ b/driver_nmea2000.c @@ -59,6 +59,19 @@ FILE *logFile = NULL; extern bool __attribute__ ((weak)) gpsd_add_device(const char *device_name, bool flag_nowait); +#define SHIFT32 0x100000000l + +static int scale_int(int32_t var, const int64_t factor) +{ + int64_t ret; + + ret = var; + ret *= factor; + ret >>= 32; + + return((int)ret); +} + static void print_data(struct gps_context_t *context, unsigned char *buffer, int len, PGN *pgn) { @@ -485,8 +498,8 @@ static gps_mask_t hnd_129038(unsigned char *bu, int len, PGN *pgn, struct gps_de "pgn %6d(%3d):\n", pgn->pgn, session->driver.nmea2000.unit); if (decode_ais_header(session->context, bu, len, ais, 0xffffffffU) != 0) { - ais->type1.lon = (int) (getles32(bu, 5) * 0.06); - ais->type1.lat = (int) (getles32(bu, 9) * 0.06); + ais->type1.lon = (int) scale_int(getles32(bu, 5), (int64_t)(SHIFT32 *.06L)); + ais->type1.lat = (int) scale_int(getles32(bu, 9), (int64_t)(SHIFT32 *.06L)); ais->type1.accuracy = (bool) ((bu[13] >> 0) & 0x01); ais->type1.raim = (bool) ((bu[13] >> 1) & 0x01); ais->type1.second = (unsigned int) ((bu[13] >> 2) & 0x3f); -- cgit v1.2.1