NMEА0183 sentence parser

nmea0138.c

#include <inttypes.h>
#include <stdbool.h>
#include <stdio.h>
#include <assert.h>
#include <string.h>

const char PROPRIETARY = 'P';
const char MANUFACTURER_IDENTIFIER[] = "UWV";

#define NMEA0183_OUT_OF_RANGE -1
#define NMEA0183_CHECKSUM_ERR -2

typedef struct _nmea0183_t
{
    uint8_t index;
    char sentence[40];
    char checksum;
} nmea0183_t;

int8_t nmea0183_encode_start(nmea0183_t *nmea0183)
{
    nmea0183->index = 0;
    nmea0183->checksum = 0;

    // Add start byte
    nmea0183->sentence[nmea0183->index] = '$';
    nmea0183->index++;

    // Add proprietary
    nmea0183->sentence[nmea0183->index] = PROPRIETARY;
    nmea0183->index++;
    nmea0183->checksum ^= PROPRIETARY;

    // Add identifier
    for (int i = 0; i < sizeof(MANUFACTURER_IDENTIFIER) - 1; i++)
    {
        nmea0183->sentence[nmea0183->index] = MANUFACTURER_IDENTIFIER[i];
        nmea0183->index++;
        nmea0183->checksum ^= MANUFACTURER_IDENTIFIER[i];
    }

    return 0;
}

int8_t nmea0183_add_int(nmea0183_t *nmea0183, int32_t value)
{
    int32_t divider = 1;
    int32_t value_temp = value;

    // Add comma if not first
    if (nmea0183->index > 5)
    {
        nmea0183->sentence[nmea0183->index] = ',';
        nmea0183->index++;
        nmea0183->checksum ^= ',';
    }

    // Calculate divider length
    while (value_temp > 0)
    {
        value_temp /= 10;
        divider *= 10;
    }
    divider /= 10;

    // Int to string
    while (divider > 0)
    {
        char value_char = (((value / divider) % 10UL) + '0');
        nmea0183->sentence[nmea0183->index] = value_char;
        nmea0183->index++;
        nmea0183->checksum ^= value_char;

        divider /= 10;
    }

    // Edge case when `value` == 0
    if (value == 0)
    {
        nmea0183->sentence[nmea0183->index] = '0';
        nmea0183->index++;
        nmea0183->checksum ^= '0';
    }

    return 0;
}

int8_t nmea0183_encode_end(nmea0183_t *nmea0183)
{
    nmea0183->sentence[nmea0183->index] = '*';
    nmea0183->index++;

    // Add checksum
    nmea0183->sentence[nmea0183->index] = '0' + (nmea0183->checksum / 16) % 16;
    nmea0183->index++;
    nmea0183->sentence[nmea0183->index] = '0' + nmea0183->checksum % 16;
    nmea0183->index++;

    // Add line break
    nmea0183->sentence[nmea0183->index] = '\r';
    nmea0183->index++;
    nmea0183->sentence[nmea0183->index] = '\n';
    nmea0183->index++;

    // Friendly print
    nmea0183->sentence[nmea0183->index] = '\0';

    return nmea0183->index;
}

int8_t nmea0183_decode_init(nmea0183_t *nmea0183, char *sentence, uint8_t length)
{
    int8_t i;
    int8_t engraved_checksum;

    nmea0183->index = 0;
    nmea0183->checksum = 0;
    memcpy(nmea0183->sentence, sentence, length);

    // Find sentence start
    while (nmea0183->sentence[nmea0183->index] != '$')
    {
        nmea0183->index++;
        if (nmea0183->index > length - 3)
        {
            return NMEA0183_OUT_OF_RANGE;
        }
    }
    nmea0183->index++;

    // Calculate checksum
    i = nmea0183->index;
    while (nmea0183->sentence[i] != '*')
    {
        nmea0183->checksum ^= nmea0183->sentence[i];
        if (i > length - 3)
        {
            return NMEA0183_OUT_OF_RANGE;
        }
        i++;
    }

    // Verify checksum
    engraved_checksum = (nmea0183->sentence[i + 1] - '0') * 16;
    engraved_checksum += (nmea0183->sentence[i + 2] - '0');
    if (engraved_checksum != nmea0183->checksum)
    {
        return NMEA0183_CHECKSUM_ERR;
    }

    // Skip to arguments
    nmea0183->index += 4;

    return 0;
}

float nmea0183_eat_argument(nmea0183_t *nmea0183)
{
    float argument = 0;
    char arg_char;
    int decimal_divider = 0;
    bool done = false;

    while (!done)
    {
        arg_char = nmea0183->sentence[nmea0183->index];
        nmea0183->index++;

        if (arg_char != ',' && arg_char != '*')
        {
            if (arg_char == '.')
            {
                decimal_divider = 10;
            }
            else if (decimal_divider > 0)
            {
                argument += (arg_char - '0') / (float)decimal_divider;
                decimal_divider *= 10;
            }
            else
            {
                argument = argument * 10 + (arg_char - '0');
            }
        }
        else
        {
            done = true;
        }
    };

    return argument;
}

int main()
{
    nmea0183_t nmea0183;
    int8_t err;

    // Check create message
    nmea0183_encode_start(&nmea0183);
    nmea0183_add_int(&nmea0183, 2);
    nmea0183_add_int(&nmea0183, 0);
    nmea0183_add_int(&nmea0183, 0);
    nmea0183_add_int(&nmea0183, 2);
    nmea0183_encode_end(&nmea0183);
    assert(strcmp(nmea0183.sentence, "$PUWV2,0,0,2*28\r\n") == 0);

    // Check message parse
    err = nmea0183_decode_init(&nmea0183, "$PUWV0,2,0*36\r\n", 15);
    assert(err == 0);
    assert((int)nmea0183_eat_argument(&nmea0183) == 0);
    assert((int)nmea0183_eat_argument(&nmea0183) == 2);
    assert((int)nmea0183_eat_argument(&nmea0183) == 0);

    // Check garbage resilience
    err = nmea0183_decode_init(&nmea0183, "/-d$PUWV0,2,0*36\r\n/]", 20);
    assert(err == 0);
    assert((int)nmea0183_eat_argument(&nmea0183) == 0);
    assert((int)nmea0183_eat_argument(&nmea0183) == 2);
    assert((int)nmea0183_eat_argument(&nmea0183) == 0);

    // Check `out-of-range` error code
    err = nmea0183_decode_init(&nmea0183, "PUWV0,2,0*36\r\n", 15);
    assert(err == NMEA0183_OUT_OF_RANGE);
    err = nmea0183_decode_init(&nmea0183, "$PUWV0,2,036\r\n", 15);
    assert(err == NMEA0183_OUT_OF_RANGE);

    // Check `checksum error`
    err = nmea0183_decode_init(&nmea0183, "$PUWV0,2,0*37\r\n", 15);
    assert(err == NMEA0183_CHECKSUM_ERR);

    return 0;
}