Skip to content

Use double/int_least64_t for coordinates to support GPS RTK #83

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Use double/int_least64_t for coordinates to support GPS RTK #83

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9afcfa1
Add minmea_double
rovo89 Jan 27, 2025
7de3f2f
Use minmea_double for coordinates
rovo89 Jan 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions example.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down
22 changes: 14 additions & 8 deletions minmea.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Expand All @@ -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 */
Expand Down Expand Up @@ -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).
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down
58 changes: 45 additions & 13 deletions minmea.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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 *)
Expand Down Expand Up @@ -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.
Expand All @@ -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);
}

/**
Expand Down
46 changes: 38 additions & 8 deletions tests.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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 };
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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

Expand All @@ -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);
Expand Down Expand Up @@ -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);

Expand Down