GPSSerial.c

/*
 *  Copyright (C) 2023 Swansea University
 *
 *  This file is part of the SELKIELogger suite of tools.
 *
 *  SELKIELogger is free software: you can redistribute it and/or modify it
 *  under the terms of the GNU General Public License as published by the Free
 *  Software Foundation, either version 3 of the License, or (at your option)
 *  any later version.
 *
 *  SELKIELogger is distributed in the hope that it will be useful, but WITHOUT
 *  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 *  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 *  more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with this SELKIELogger product.
 *  If not, see <http://www.gnu.org/licenses/>.
*/
 
// Open serial port and communicate with UBlox GPS
#include <errno.h>
#include <fcntl.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>
 
#include "GPSCommands.h"
#include "GPSMessages.h"
#include "GPSSerial.h"
#include "GPSTypes.h"
 
int ubx_openConnection(const char *port, const int initialBaud) {
    // Use base library function to get initial connection
    int handle = openSerialConnection(port, initialBaud);
 
    // The set baud rate command also disables NMEA and enabled UBX output on UART1
    if (!ubx_setBaudRate(handle, 115200)) {
        fprintf(stderr, "Unable to command baud rate change");
        perror("openConnection");
        return -1;
    }
 
    fsync(handle);
    usleep(5E4);
 
    // Assuming that we're now configured for 115200 baud, adjust serial settings to match
    struct termios options;
    tcgetattr(handle, &options);
    cfsetispeed(&options, B115200);
    cfsetospeed(&options, B115200);
 
    // Set options using TCSADRAIN in case commands not yet set
    if (tcsetattr(handle, TCSADRAIN, &options)) { fprintf(stderr, "tcsetattr() failed!\n"); }
 
    // The GPS module will stop listening for 1 second if the wrong baud rate is used
    // i.e. It was already in high rate mode
    sleep(1);
 
    {
        struct termios check;
        tcgetattr(handle, &check);
        if (cfgetispeed(&check) != B115200) {
            fprintf(stderr, "Unable to set target baud. Wanted %d, got %d\n", 115200,
                    flag_to_baud(cfgetispeed(&check)));
    //      return -1;
        }
    }
 
    /* Send the command again, to make sure the protocol settings get applied
     *
     * If we were already in high rate mode the initial rate change command
     * would fail silently, and the GPS might be transmitting at the right
     * baud but with the wrong protocols
     */
    if (!ubx_setBaudRate(handle, 115200)) {
        fprintf(stderr, "Unable to command baud rate change");
        perror("openConnection");
        return -1;
    }
    usleep(5E4);
 
    return handle;
}
 
void ubx_closeConnection(int handle) {
    close(handle);
}
 
bool ubx_readMessage(int handle, ubx_message *out) {
    static uint8_t buf[UBX_SERIAL_BUFF];
    static int index = 0; // Current read position
    static int hw = 0;    // Current array end
    return ubx_readMessage_buf(handle, out, buf, &index, &hw);
}
 
bool ubx_readMessage_buf(int handle, ubx_message *out, uint8_t buf[UBX_SERIAL_BUFF], int *index, int *hw) {
    int ti = 0;
    if ((*hw) < UBX_SERIAL_BUFF - 1) {
        errno = 0;
        ti = read(handle, &(buf[(*hw)]), UBX_SERIAL_BUFF - (*hw));
        if (ti >= 0) {
            (*hw) += ti;
        } else {
            if (errno != EAGAIN) {
                fprintf(stderr, "Unexpected error while reading from serial port (handle ID: 0x%02x)\n",
                        handle);
                fprintf(stderr, "read returned \"%s\" in readMessage\n", strerror(errno));
                out->sync1 = 0xAA;
                return false;
            }
        }
    }
 
    // Check buf[index] is valid ID
    while (!(buf[(*index)] == 0xB5) && (*index) < (*hw)) {
        (*index)++; // Current byte cannot be start of a message, so advance
    }
    if ((*index) == (*hw)) {
        if ((*hw) > 0 && (*index) > 0) {
            // Move data from index back to zero position
            memmove(buf, &(buf[(*index)]), UBX_SERIAL_BUFF - (*index));
            (*hw) -= (*index);
            (*index) = 0;
        }
        // fprintf(stderr, "Buffer empty - returning\n");
        out->sync1 = 0xFF;
        if (ti == 0) { out->sync1 = 0xFD; }
        return false;
    }
 
    if (((*hw) - (*index)) < 8) {
        // Not enough data for any valid message, come back later
        out->sync1 = 0xFF;
        return false;
    }
 
    // Set length of data required for message
 
    out->sync1 = buf[(*index)];
    out->sync2 = buf[(*index) + 1];
    out->msgClass = buf[(*index) + 2];
    out->msgID = buf[(*index) + 3];
    out->length = buf[(*index) + 4] + (buf[(*index) + 5] << 8);
 
    if (out->sync2 != 0x62) {
        // Found first sync byte, but second not valid
        // Advance the index so we skip this message and go back around
        (*index)++;
        out->sync1 = 0xFF;
        return false;
    }
 
    if (((*hw) - (*index)) < (out->length + 8)) {
        // Not enough data for this message yet, so mark output invalid
        out->sync1 = 0xFF;
        if (ti == 0) { out->sync1 = 0xFD; }
        // Go back around, but leave index where it is so we will pick up
        // from the same point in the buffer
        return false;
    }
 
    out->extdata = NULL;
    if (out->length <= 256) {
        for (uint8_t i = 0; i < out->length; i++) {
            out->data[i] = buf[(*index) + 6 + i];
        }
    } else {
        out->extdata = calloc(out->length, 1);
        for (uint16_t i = 0; i < out->length; i++) {
            out->extdata[i] = buf[(*index) + 6 + i];
        }
    }
 
    out->csumA = buf[(*index) + 6 + out->length];
    out->csumB = buf[(*index) + 7 + out->length];
 
    bool valid = ubx_check_checksum(out);
    if (valid) {
        (*index) += 8 + out->length;
    } else {
        out->sync1 = 0xEE; // Use 0xEE as "Found, but invalid", leaving 0xFF as
                           // "No message"
        if (out->extdata) {
            free(out->extdata);
            out->extdata = NULL;
        }
        (*index)++;
    }
    if ((*hw) > 0) {
        // Move data from index back to zero position
        memmove(buf, &(buf[(*index)]), UBX_SERIAL_BUFF - (*index));
        (*hw) -= (*index);
        (*index) = 0;
    }
    return valid;
}
 
bool ubx_waitForMessage(const int handle, const uint8_t msgClass, const uint8_t msgID, const int maxDelay,
                        ubx_message *out) {
    const time_t deadline = time(NULL) + maxDelay;
    while (time(NULL) < deadline) {
        bool rms = ubx_readMessage(handle, out);
        if (rms) {
            if ((out->msgClass == msgClass) && (out->msgID == msgID)) { return true; }
        } else {
            usleep(50);
        }
    }
    return false;
}
 
bool ubx_writeMessage(int handle, const ubx_message *out) {
    if (!ubx_check_checksum(out) || (out->sync1 != 0xB5) || (out->sync2 != 0x62)) {
        fprintf(stderr, "Attempted to send invalid message\n");
        return false;
    }
 
    uint8_t head[6] = {out->sync1,
                       out->sync2,
                       out->msgClass,
                       out->msgID,
                       (uint8_t)(out->length & 0xFF),
                       (uint8_t)(out->length >> 8)};
    uint8_t tail[2] = {out->csumA, out->csumB};
    ssize_t ret = write(handle, head, 6);
    if (ret != 6) {
        perror("writeMessage:head");
        return false;
    }
 
    /*
     * The results of a zero length write are undefined for anything that
     * isn't a regular file. Zero length messages are valid, so guard this
     * whole section.
     */
    if (out->length > 0) {
        if (out->length <= 256) {
            ret = write(handle, out->data, out->length);
        } else {
            ret = write(handle, out->extdata, out->length);
        }
        if (ret != out->length) {
            perror("writeMessage:data");
            return false;
        }
    }
 
    ret = write(handle, tail, 2);
    if (ret != 2) {
        perror("writeMessage:tail");
        return false;
    }
    return true;
}