diff --git a/example.c b/example.c index 53ace67..476fd4d 100644 --- a/example.c +++ b/example.c @@ -22,13 +22,13 @@ int main(void) case MINMEA_SENTENCE_RMC: { struct minmea_sentence_rmc frame; if (minmea_parse_rmc(&frame, line)) { - printf(INDENT_SPACES "$xxRMC: raw coordinates and speed: (%d/%d,%d/%d) %d/%d\n", + printf(INDENT_SPACES "$xxRMC: raw coordinates and speed: (%ld/%ld,%ld/%ld) %d/%d\n", frame.latitude.value, frame.latitude.scale, frame.longitude.value, frame.longitude.scale, frame.speed.value, frame.speed.scale); - printf(INDENT_SPACES "$xxRMC fixed-point coordinates and speed scaled to three decimal places: (%d,%d) %d\n", - minmea_rescale(&frame.latitude, 1000), - minmea_rescale(&frame.longitude, 1000), + printf(INDENT_SPACES "$xxRMC fixed-point coordinates and speed scaled to three decimal places: (%ld,%ld) %d\n", + minmea_rescale_double(&frame.latitude, 1000), + minmea_rescale_double(&frame.longitude, 1000), minmea_rescale(&frame.speed, 1000)); printf(INDENT_SPACES "$xxRMC floating point degree coordinates and speed: (%f,%f) %f\n", minmea_tocoord(&frame.latitude), diff --git a/minmea.c b/minmea.c index c96b392..688a05e 100644 --- a/minmea.c +++ b/minmea.c @@ -155,10 +155,12 @@ bool minmea_scan(const char *sentence, const char *format, ...) *va_arg(ap, int *) = value; } break; - case 'f': { // Fractional value with scale (struct minmea_float). + case 'f': + case 'F': { // Fractional value with scale (struct minmea_float/minmea_double). int sign = 0; - int_least32_t value = -1; - int_least32_t scale = 0; + int_least64_t value = -1; + int_least64_t scale = 0; + int_least64_t max = type == 'f' ? INT_LEAST32_MAX : INT_LEAST64_MAX; if (field) { while (minmea_isfield(*field)) { @@ -170,7 +172,7 @@ bool minmea_scan(const char *sentence, const char *format, ...) int digit = *field - '0'; if (value == -1) value = 0; - if (value > (INT_LEAST32_MAX-digit) / 10) { + if (value > (max-digit) / 10) { /* we ran out of bits, what do we do? */ if (scale) { /* truncate extra precision */ @@ -211,7 +213,11 @@ bool minmea_scan(const char *sentence, const char *format, ...) if (sign) value *= sign; - *va_arg(ap, struct minmea_float *) = (struct minmea_float) {value, scale}; + if (type == 'f') { + *va_arg(ap, struct minmea_float *) = (struct minmea_float) {value, scale}; + } else { + *va_arg(ap, struct minmea_double *) = (struct minmea_double) {value, scale}; + } } break; case 'i': { // Integer value, default 0 (int). @@ -409,7 +415,7 @@ bool minmea_parse_rmc(struct minmea_sentence_rmc *frame, const char *sentence) int latitude_direction; int longitude_direction; int variation_direction; - if (!minmea_scan(sentence, "tTcfdfdffDfd", + if (!minmea_scan(sentence, "tTcFdFdffDfd", type, &frame->time, &validity, @@ -438,7 +444,7 @@ bool minmea_parse_gga(struct minmea_sentence_gga *frame, const char *sentence) int latitude_direction; int longitude_direction; - if (!minmea_scan(sentence, "tTfdfdiiffcfcf_", + if (!minmea_scan(sentence, "tTFdFdiiffcfcf_", type, &frame->time, &frame->latitude, &latitude_direction, @@ -497,7 +503,7 @@ bool minmea_parse_gll(struct minmea_sentence_gll *frame, const char *sentence) int latitude_direction; int longitude_direction; - if (!minmea_scan(sentence, "tfdfdTc;c", + if (!minmea_scan(sentence, "tFdFdTc;c", type, &frame->latitude, &latitude_direction, &frame->longitude, &longitude_direction, diff --git a/minmea.h b/minmea.h index 494b218..736c034 100644 --- a/minmea.h +++ b/minmea.h @@ -45,6 +45,11 @@ struct minmea_float { int_least32_t scale; }; +struct minmea_double { + int_least64_t value; + int_least64_t scale; +}; + struct minmea_date { int day; int month; @@ -72,8 +77,8 @@ struct minmea_sentence_gbs { struct minmea_sentence_rmc { struct minmea_time time; bool valid; - struct minmea_float latitude; - struct minmea_float longitude; + struct minmea_double latitude; + struct minmea_double longitude; struct minmea_float speed; struct minmea_float course; struct minmea_date date; @@ -82,8 +87,8 @@ struct minmea_sentence_rmc { struct minmea_sentence_gga { struct minmea_time time; - struct minmea_float latitude; - struct minmea_float longitude; + struct minmea_double latitude; + struct minmea_double longitude; int fix_quality; int satellites_tracked; struct minmea_float hdop; @@ -109,8 +114,8 @@ enum minmea_faa_mode { }; struct minmea_sentence_gll { - struct minmea_float latitude; - struct minmea_float longitude; + struct minmea_double latitude; + struct minmea_double longitude; struct minmea_time time; char status; char mode; @@ -201,6 +206,7 @@ enum minmea_sentence_id minmea_sentence_id(const char *sentence, bool strict); * c - single character (char *) * d - direction, returned as 1/-1, default 0 (int *) * f - fractional, returned as value + scale (struct minmea_float *) + * F - double fractional, returned as value + scale (struct minmea_double *) * i - decimal, default zero (int *) * s - string (char *) * t - talker identifier and type (char *) @@ -250,6 +256,21 @@ static inline int_least32_t minmea_rescale(const struct minmea_float *f, int_lea return f->value * (new_scale/f->scale); } +/** + * Rescale a double-precision fixed-point value to a different scale. Rounds towards zero. + */ +static inline int_least64_t minmea_rescale_double(const struct minmea_double *d, int_least64_t new_scale) +{ + if (d->scale == 0) + return 0; + if (d->scale == new_scale) + return d->value; + if (d->scale > new_scale) + return (d->value + ((d->value > 0) - (d->value < 0)) * d->scale/new_scale/2) / (d->scale/new_scale); + else + return d->value * (new_scale/d->scale); +} + /** * Convert a fixed-point value to a floating-point value. * Returns NaN for "unknown" values. @@ -261,21 +282,32 @@ static inline float minmea_tofloat(const struct minmea_float *f) return (float) f->value / (float) f->scale; } +/** + * Convert a fixed-point value to a floating-point value. + * Returns NaN for "unknown" values. + */ +static inline double minmea_todouble(const struct minmea_double *d) +{ + if (d->scale == 0) + return NAN; + return (double) d->value / (double) d->scale; +} + /** * Convert a raw coordinate to a floating point DD.DDD... value. * Returns NaN for "unknown" values. */ -static inline float minmea_tocoord(const struct minmea_float *f) +static inline double minmea_tocoord(const struct minmea_double *d) { - if (f->scale == 0) + if (d->scale == 0) return NAN; - if (f->scale > (INT_LEAST32_MAX / 100)) + if (d->scale > (INT_LEAST64_MAX / 100)) return NAN; - if (f->scale < (INT_LEAST32_MIN / 100)) + if (d->scale < (INT_LEAST64_MIN / 100)) return NAN; - int_least32_t degrees = f->value / (f->scale * 100); - int_least32_t minutes = f->value % (f->scale * 100); - return (float) degrees + (float) minutes / (60 * f->scale); + int_least64_t degrees = d->value / (d->scale * 100); + int_least64_t minutes = d->value % (d->scale * 100); + return (double) degrees + (double) minutes / (60 * d->scale); } /** diff --git a/tests.c b/tests.c index b5e067e..6923e20 100644 --- a/tests.c +++ b/tests.c @@ -236,6 +236,23 @@ START_TEST(test_minmea_scan_f) } END_TEST + +START_TEST(test_minmea_scan_F) +{ + struct minmea_double f; + + ck_assert(minmea_scan("9223372036854775807", "F", &f) == true); + ck_assert_int_eq((int64_t) f.value, 9223372036854775807LL); + ck_assert_int_eq(f.scale, 1); + /* doesn't fit, truncate precision */ + ck_assert(minmea_scan("9223372036854775.808", "F", &f) == true); + ck_assert_int_eq((int64_t) f.value, 922337203685477580LL); + ck_assert_int_eq(f.scale, 100); + /* doesn't fit, bail out */ + ck_assert(minmea_scan("9223372036854775808", "F", &f) == false); +} +END_TEST + START_TEST(test_minmea_scan_i) { int value, extra; @@ -560,8 +577,8 @@ START_TEST(test_minmea_parse_gga1) struct minmea_sentence_gga frame = {}; struct minmea_sentence_gga expected = {}; expected.time = (struct minmea_time) { 12, 35, 19, 0 }; - expected.latitude = (struct minmea_float) { 4807038, 1000 }; - expected.longitude = (struct minmea_float) { 1131000, 1000 }; + expected.latitude = (struct minmea_double) { 4807038, 1000 }; + expected.longitude = (struct minmea_double) { 1131000, 1000 }; expected.fix_quality = 1; expected.satellites_tracked = 8; expected.hdop = (struct minmea_float) { 9, 10 }; @@ -628,8 +645,8 @@ START_TEST(test_minmea_parse_gll1) memset(&frame, 0, sizeof(frame)); memset(&expected, 0, sizeof(expected)); - expected.latitude = (struct minmea_float){ 37232475, 10000 }; - expected.longitude = (struct minmea_float){ -121583416, 10000 }; + expected.latitude = (struct minmea_double){ 37232475, 10000 }; + expected.longitude = (struct minmea_double){ -121583416, 10000 }; expected.time = (struct minmea_time){ 16, 12, 29, 487000 }; expected.status = MINMEA_GLL_STATUS_DATA_VALID; expected.mode = MINMEA_FAA_MODE_AUTONOMOUS; @@ -1123,12 +1140,23 @@ START_TEST(test_minmea_float) } END_TEST +#define assert_double_eq(x, y) ck_assert(fabs((x) - (y)) <= 0.0) + +START_TEST(test_minmea_double) +{ + ck_assert(isnan(minmea_todouble(&(struct minmea_double) { 42, 0 }))); + assert_double_eq(minmea_todouble(&(struct minmea_double) { 7, 1 }), 7.0); + assert_double_eq(minmea_todouble(&(struct minmea_double) { -200, 100 }), -2.0); + assert_double_eq(minmea_todouble(&(struct minmea_double) { 15, 10 }), 1.5); +} +END_TEST + START_TEST(test_minmea_coord) { - ck_assert(isnan(minmea_tocoord(&(struct minmea_float) { 42, 0 }))); - assert_float_eq(minmea_tocoord(&(struct minmea_float) { 4200, 1 }), 42.0f); - assert_float_eq(minmea_tocoord(&(struct minmea_float) { 420000, 100 }), 42.0f); - assert_float_eq(minmea_tocoord(&(struct minmea_float) { 423000, 100 }), 42.5f); + ck_assert(isnan(minmea_tocoord(&(struct minmea_double) { 42, 0 }))); + assert_double_eq(minmea_tocoord(&(struct minmea_double) { 4200, 1 }), 42.0); + assert_double_eq(minmea_tocoord(&(struct minmea_double) { 420000, 100 }), 42.0); + assert_double_eq(minmea_tocoord(&(struct minmea_double) { 423000, 100 }), 42.5); } END_TEST @@ -1148,6 +1176,7 @@ static Suite *minmea_suite(void) tcase_add_test(tc_scan, test_minmea_scan_c); tcase_add_test(tc_scan, test_minmea_scan_d); tcase_add_test(tc_scan, test_minmea_scan_f); + tcase_add_test(tc_scan, test_minmea_scan_F); tcase_add_test(tc_scan, test_minmea_scan_i); tcase_add_test(tc_scan, test_minmea_scan_s); tcase_add_test(tc_scan, test_minmea_scan_t); @@ -1187,6 +1216,7 @@ static Suite *minmea_suite(void) tcase_add_test(tc_utils, test_minmea_gettime); tcase_add_test(tc_utils, test_minmea_rescale); tcase_add_test(tc_utils, test_minmea_float); + tcase_add_test(tc_utils, test_minmea_double); tcase_add_test(tc_utils, test_minmea_coord); suite_add_tcase(s, tc_utils);