zr-v4/libraries/AP_GPS/AP_GPS_NMEA.cpp

/*
   This program 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.

   This program 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 program.  If not, see <http://www.gnu.org/licenses/>.
 */

//
// NMEA parser, adapted by Michael Smith from TinyGPS v9:
//
// TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
// Copyright (C) 2008-9 Mikal Hart
// All rights reserved.
//

/// @file	AP_GPS_NMEA.cpp
/// @brief	NMEA protocol parser
///
/// This is a lightweight NMEA parser, derived originally from the
/// TinyGPS parser by Mikal Hart.
///

#include <AP_Common/AP_Common.h>

#include <ctype.h>
#include <stdint.h>
#include <stdlib.h>

#include "AP_GPS_NMEA.h"

#include <AP_Logger/AP_Logger.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;

// optionally log all NMEA data for debug purposes
// #define NMEA_LOG_PATH "nmea.log"

#ifdef NMEA_LOG_PATH
#include <stdio.h>
#endif

// Convenience macros //////////////////////////////////////////////////////////
//
#define DIGIT_TO_VAL(_x)        (_x - '0')
#define hexdigit(x) ((x)>9?'A'+((x)-10):'0'+(x))

bool AP_GPS_NMEA::read(void)
{
    int16_t numc;
    bool parsed = false;

    numc = port->available();
    while (numc--) {
        char c = port->read();
#ifdef NMEA_LOG_PATH
        static FILE *logf = nullptr;
        if (logf == nullptr) {
            logf = fopen(NMEA_LOG_PATH, "wb");
        }
        if (logf != nullptr) {
            ::fwrite(&c, 1, 1, logf);
        }
#endif
        if (_decode(c)) {
            parsed = true;
        }
    }
    return parsed;
}

bool AP_GPS_NMEA::_decode(char c)
{
    bool valid_sentence = false;

    _sentence_length++;
        
    switch (c) {
    case ',': // term terminators
    case ';': // term terminators
        _parity ^= c;
        
        markCRC = aulCrcTable[(markCRC ^ c) & 0xff] ^ (markCRC >> 8);
        FALLTHROUGH;
    case '\r':
    case '\n':
    case '*':
        if (_term_offset < sizeof(_term)) {
            _term[_term_offset] = 0;
            valid_sentence = _term_complete();
        }
        ++_term_number;
        _term_offset = 0;
        _is_checksum_term = c == '*';
        return valid_sentence;

    case '$': // sentence begin
    case '#': // sentence begin
        _term_number = _term_offset = 0;
        _parity = 0;
        markCRC = 0;
        _sentence_type = _GPS_SENTENCE_OTHER;
        _is_checksum_term = false;
        _gps_data_good = false;
        _sentence_length = 1;
        return valid_sentence;
    }

    // ordinary characters
    if (_term_offset < sizeof(_term) - 1)
        _term[_term_offset++] = c;
    if (!_is_checksum_term){
        _parity ^= c;
        
        markCRC = aulCrcTable[(markCRC ^ c) & 0xff] ^ (markCRC >> 8);
    }

    return valid_sentence;
}

int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p)
{
    char *endptr = nullptr;
    long ret = 100 * strtol(p, &endptr, 10);
    int sign = ret < 0 ? -1 : 1;

    if (ret >= (long)INT32_MAX) {
        return INT32_MAX;
    }
    if (ret <= (long)INT32_MIN) {
        return INT32_MIN;
    }
    if (endptr == nullptr || *endptr != '.') {
        return ret;
    }

    if (isdigit(endptr[1])) {
        ret += sign * 10 * DIGIT_TO_VAL(endptr[1]);
        if (isdigit(endptr[2])) {
            ret += sign * DIGIT_TO_VAL(endptr[2]);
            if (isdigit(endptr[3])) {
                ret += sign * (DIGIT_TO_VAL(endptr[3]) >= 5);
            }
        }
    }
    return ret;
}

/*
  parse a NMEA latitude/longitude degree value. The result is in degrees*1e7
 */
uint32_t AP_GPS_NMEA::_parse_degrees()
{
    char *p, *q;
    uint8_t deg = 0, min = 0;
    float frac_min = 0;
    int32_t ret = 0;

    // scan for decimal point or end of field
    for (p = _term; *p && isdigit(*p); p++)
        ;
    q = _term;

    // convert degrees
    while ((p - q) > 2 && *q) {
        if (deg)
            deg *= 10;
        deg += DIGIT_TO_VAL(*q++);
    }

    // convert minutes
    while (p > q && *q) {
        if (min)
            min *= 10;
        min += DIGIT_TO_VAL(*q++);
    }

    // convert fractional minutes
    if (*p == '.') {
        q = p + 1;
        float frac_scale = 0.1f;
        while (*q && isdigit(*q)) {
            frac_min += DIGIT_TO_VAL(*q) * frac_scale;
            q++;
            frac_scale *= 0.1f;
        }
    }
    ret = (deg * (int32_t)10000000UL);
    ret += (min * (int32_t)10000000UL / 60);
    ret += (int32_t) (frac_min * (1.0e7f / 60.0f));
    return ret;
}

/*
  see if we have a new set of NMEA messages
 */
bool AP_GPS_NMEA::_have_new_message()
{
    if (_last_RMC_ms == 0 ||
        _last_GGA_ms == 0) {
        return false;
    }
    uint32_t now = AP_HAL::millis();
    if (now - _last_RMC_ms > 150 ||
        now - _last_GGA_ms > 150) {
        return false;
    }
    if (_last_VTG_ms != 0 && 
        now - _last_VTG_ms > 150) {
        return false;
    }
    // prevent these messages being used again
    if (_last_VTG_ms != 0) {
        _last_VTG_ms = 1;
    }

    if (now - _last_HDT_ms > 300) {
        // we have lost GPS yaw
        state.have_gps_yaw = false;
    }

    _last_GGA_ms = 1;
    _last_RMC_ms = 1;
    return true;
}

// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
bool AP_GPS_NMEA::_term_complete()
{
    // handle the last term in a message
    if (_is_checksum_term) {
        uint8_t nibble_high = 0;
        uint8_t nibble_low  = 0;
        if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) {
            return false;
        }
        const uint8_t checksum = (nibble_high << 4u) | nibble_low;
        if(_gps_data_good && (_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT )){
        if(get_new_mark){                        
            const uint8_t mark_crc = markCRC >> 24;

            // gcs().send_text(MAV_SEVERITY_INFO, "week: %d, %f,crc:0x%02x--0x%02x--0x%02x",gps_mark.week,gps_mark.second,checksum,_parity,mark_crc);
            // gcs().send_text(MAV_SEVERITY_INFO, "llh: %f , %f , %f\n",gps_mark.lat,gps_mark.lon,gps_mark.hgt);
            if(checksum == mark_crc){
                switch (_sentence_type) {
                case _GPS_SENTENCE_MARK:
                case _GPS_SENTENCE_EVENT:
                    if(get_new_mark){                        
                        gcs().send_text(MAV_SEVERITY_INFO, "%d, %.4f, %f , %f , %f\n",gps_mark.week,gps_mark.second,gps_mark.lat,gps_mark.lon,gps_mark.hgt);
                        get_new_mark = false;
                            AP_Logger *logger = AP_Logger::get_singleton();

                            if ( logger!=nullptr )
                            {
                                Vector3d  llh = Vector3d(gps_mark.lat,gps_mark.lon, gps_mark.hgt);
                                Vector3f  sigma = Vector3f(gps_mark.lat_sigma,gps_mark.lon_sigma, gps_mark.hgt_sigma);
                                    AP::logger().Write_RTK_Mark_Pos(
                                    gps_mark.week,
                                    gps_mark.second,
                                    llh,
                                    gps_mark.undulation,
                                    sigma,
                                    gps_mark.diff_age,
                                    gps_mark.sol_age
                                    );
                            }
                    }
                }
                
                return _have_new_message();
            }else{
                
                gcs().send_text(MAV_SEVERITY_INFO, "crc err:0x%02x--0x%02x--0x%02x",checksum,_parity,mark_crc);
            }
            get_new_mark = false;
        }
        }
        if (checksum == _parity ) {
            if (_gps_data_good) {
                uint32_t now = AP_HAL::millis();
                switch (_sentence_type) {
                case _GPS_SENTENCE_RMC:
                    _last_RMC_ms = now;
                    //time                        = _new_time;
                    //date                        = _new_date;
                    state.location.lat     = _new_latitude;
                    state.location.lng     = _new_longitude;
                    state.ground_speed     = _new_speed*0.01f;
                    state.ground_course    = wrap_360(_new_course*0.01f);
                    make_gps_time(_new_date, _new_time * 10);
                    set_uart_timestamp(_sentence_length);
                    state.last_gps_time_ms = now;
                    fill_3d_velocity();
                    break;
                case _GPS_SENTENCE_GGA:
                    _last_GGA_ms = now;
                    state.location.alt  = _new_altitude;
                    state.location.lat  = _new_latitude;
                    state.location.lng  = _new_longitude;
                    state.num_sats      = _new_satellite_count;
                    state.hdop          = _new_hdop;
                    switch(_new_quality_indicator) {
                    case 0: // Fix not available or invalid
                        state.status = AP_GPS::NO_FIX;
                        break;
                    case 1: // GPS SPS Mode, fix valid
                        state.status = AP_GPS::GPS_OK_FIX_3D;
                        break;
                    case 2: // Differential GPS, SPS Mode, fix valid
                        state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
                        break;
                    case 3: // GPS PPS Mode, fix valid
                        state.status = AP_GPS::GPS_OK_FIX_3D;
                        break;
                    case 4: // Real Time Kinematic. System used in RTK mode with fixed integers
                        state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
                        break;
                    case 5: // Float RTK. Satellite system used in RTK mode, floating integers
                        state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
                        break;
                    case 6: // Estimated (dead reckoning) Mode
                        state.status = AP_GPS::NO_FIX;
                        break;
                    default://to maintain compatibility with MAV_GPS_INPUT and others
                        state.status = AP_GPS::GPS_OK_FIX_3D;
                        break;
                    }
                    break;
                case _GPS_SENTENCE_VTG:
                    _last_VTG_ms = now;
                    state.ground_speed  = _new_speed*0.01f;
                    state.ground_course = wrap_360(_new_course*0.01f);
                    fill_3d_velocity();
                    // VTG has no fix indicator, can't change fix status
                    break;
                case _GPS_SENTENCE_HDT:
                    _last_HDT_ms = now;
                    state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
                    state.have_gps_yaw = true;
                    break;
                // case _GPS_SENTENCE_MARK:
                // case _GPS_SENTENCE_EVENT:
                //     if(get_new_mark){                        
                //         gcs().send_text(MAV_SEVERITY_INFO, "week: %d, %f",gps_mark.week,gps_mark.second);
                //         gcs().send_text(MAV_SEVERITY_INFO, "llh: %f , %f , %f\n",gps_mark.lat,gps_mark.lon,gps_mark.hgt);
                //         get_new_mark = false;
                //     }
                //     break;
                }
            } else {
                switch (_sentence_type) {
                case _GPS_SENTENCE_RMC:
                case _GPS_SENTENCE_GGA:
                    // Only these sentences give us information about
                    // fix status.
                    state.status = AP_GPS::NO_FIX;
                }
            }
            // see if we got a good message
            return _have_new_message();
        }
        // we got a bad message, ignore it
        return false;
    }

    // the first term determines the sentence type
    if (_term_number == 0) {
        /*
          The first two letters of the NMEA term are the talker
          ID. The most common is 'GP' but there are a bunch of others
          that are valid. We accept any two characters here.
         */
        if (_term[0] < 'A' || _term[0] > 'Z' ||
            _term[1] < 'A' || _term[1] > 'Z') {
            _sentence_type = _GPS_SENTENCE_OTHER;
            return false;
        }
        const char *term_type = &_term[2];
        if (strcmp(term_type, "RMC") == 0) {
            _sentence_type = _GPS_SENTENCE_RMC;
            // gcs().send_text(MAV_SEVERITY_INFO, "get: %s\n",term_type);
        } else if (strcmp(term_type, "GGA") == 0) {
            _sentence_type = _GPS_SENTENCE_GGA;
            // gcs().send_text(MAV_SEVERITY_INFO, "get: %s\n",term_type);
        } else if (strcmp(term_type, "HDT") == 0) {
            _sentence_type = _GPS_SENTENCE_HDT;
            // HDT doesn't have a data qualifier
            _gps_data_good = true;
        } else if (strcmp(term_type, "VTG") == 0) {
            _sentence_type = _GPS_SENTENCE_VTG;
            // VTG may not contain a data qualifier, presume the solution is good
            // unless it tells us otherwise.
            _gps_data_good = true;
            // gcs().send_text(MAV_SEVERITY_INFO, "get: %s\n",term_type);
        } else if (strcmp(term_type, "ENTALLA") == 0) {
            _sentence_type = _GPS_SENTENCE_EVENT;
            // VTG may not contain a data qualifier, presume the solution is good
            // unless it tells us otherwise.
            _gps_data_good = true;
            // gcs().send_text(MAV_SEVERITY_INFO, "get: %s",term_type);
        } else if (strcmp(term_type, "RKPOSA") == 0) {
            _sentence_type = _GPS_SENTENCE_MARK;
            // VTG may not contain a data qualifier, presume the solution is good
            // unless it tells us otherwise.
            
            // gcs().send_text(MAV_SEVERITY_INFO, "get: %s",term_type);
            _gps_data_good = true;
        } else {
            _sentence_type = _GPS_SENTENCE_OTHER;
        }
        return false;
    }

    // 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT
    if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
        switch (_sentence_type + _term_number) {
        // operational status
        //
        case _GPS_SENTENCE_RMC + 2: // validity (RMC)
            _gps_data_good = _term[0] == 'A';
            break;
        case _GPS_SENTENCE_GGA + 6: // Fix data (GGA)
            _gps_data_good = _term[0] > '0';
            _new_quality_indicator = _term[0] - '0';
            break;
        case _GPS_SENTENCE_VTG + 9: // validity (VTG) (we may not see this field)
            _gps_data_good = _term[0] != 'N';
            break;
        case _GPS_SENTENCE_GGA + 7: // satellite count (GGA)
            _new_satellite_count = atol(_term);
            break;
        case _GPS_SENTENCE_GGA + 8: // HDOP (GGA)
            _new_hdop = (uint16_t)_parse_decimal_100(_term);
            break;

        // time and date
        //
        case _GPS_SENTENCE_RMC + 1: // Time (RMC)
        case _GPS_SENTENCE_GGA + 1: // Time (GGA)
            _new_time = _parse_decimal_100(_term);
            break;
        case _GPS_SENTENCE_RMC + 9: // Date (GPRMC)
            _new_date = atol(_term);
            break;

        // location
        //
        case _GPS_SENTENCE_RMC + 3: // Latitude
        case _GPS_SENTENCE_GGA + 2:
            _new_latitude = _parse_degrees();
            break;
        case _GPS_SENTENCE_RMC + 4: // N/S
        case _GPS_SENTENCE_GGA + 3:
            if (_term[0] == 'S')
                _new_latitude = -_new_latitude;
            break;
        case _GPS_SENTENCE_RMC + 5: // Longitude
        case _GPS_SENTENCE_GGA + 4:
            _new_longitude = _parse_degrees();
            break;
        case _GPS_SENTENCE_RMC + 6: // E/W
        case _GPS_SENTENCE_GGA + 5:
            if (_term[0] == 'W')
                _new_longitude = -_new_longitude;
            break;
        case _GPS_SENTENCE_GGA + 9: // Altitude (GPGGA)
            _new_altitude = _parse_decimal_100(_term);
            break;

        // course and speed
        //
        case _GPS_SENTENCE_RMC + 7: // Speed (GPRMC)
        case _GPS_SENTENCE_VTG + 5: // Speed (VTG)
            _new_speed = (_parse_decimal_100(_term) * 514) / 1000;       // knots-> m/sec, approximiates * 0.514
            break;
        case _GPS_SENTENCE_HDT + 1: // Course (HDT)
            _new_gps_yaw = _parse_decimal_100(_term);
            break;
        case _GPS_SENTENCE_RMC + 8: // Course (GPRMC)
        case _GPS_SENTENCE_VTG + 1: // Course (VTG)
            _new_course = _parse_decimal_100(_term);
            break;
        }
    }
           
#if 1
static uint32_t cnt;
cnt++;
// if ((_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT ))
//         gcs().send_text(MAV_SEVERITY_INFO, "%d,t:%d, n:%d,%d: %s",cnt,_sentence_type,_term_number,_term[0],_term);
    // rtk event mark decode
    if ((_sentence_type == _GPS_SENTENCE_MARK || _sentence_type == _GPS_SENTENCE_EVENT )) {
        switch (_sentence_type + _term_number) {
        // operational status
        //
        case _GPS_SENTENCE_MARK + 5: // week
        case _GPS_SENTENCE_EVENT + 5: // week
            gps_mark.week = atol(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%d",_term,gps_mark.week);
            break; 
        case _GPS_SENTENCE_MARK + 6: // second
        case _GPS_SENTENCE_EVENT + 6: // second
            gps_mark.second = atof(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.second);
            break;
        case _GPS_SENTENCE_MARK + 12: // lat
        case _GPS_SENTENCE_EVENT + 22: // lat
            gps_mark.lat = atof(_term);  
            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.lat);
            break;
        case _GPS_SENTENCE_MARK + 13: // lon
        case _GPS_SENTENCE_EVENT + 23:
            gps_mark.lon = atof(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.lon);
            break;
        case _GPS_SENTENCE_MARK + 14: // hgt
        case _GPS_SENTENCE_EVENT + 24:
            gps_mark.hgt = atof(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%s:%f",_term,gps_mark.lon);
            break;
        case _GPS_SENTENCE_MARK + 15: // undulation
        case _GPS_SENTENCE_EVENT + 25:
            gps_mark.undulation = atof(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
            break;
        case _GPS_SENTENCE_MARK + 17: // lat_sigma
        case _GPS_SENTENCE_EVENT + 27:
            gps_mark.lat_sigma = atof(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
            break;
        case _GPS_SENTENCE_MARK + 18: // lon_sigma
        case _GPS_SENTENCE_EVENT + 28:
            gps_mark.lon_sigma = atof(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
            break;
        case _GPS_SENTENCE_MARK + 19: // hgt_sigma
        case _GPS_SENTENCE_EVENT + 29:
            gps_mark.hgt_sigma = atof(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
            break;
        case _GPS_SENTENCE_MARK + 21: // diff_age
        case _GPS_SENTENCE_EVENT + 31:
            gps_mark.diff_age = atof(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
            break;
        case _GPS_SENTENCE_MARK + 22: // sol_age
        case _GPS_SENTENCE_EVENT + 32:
            gps_mark.sol_age = atof(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
            break;
        case _GPS_SENTENCE_MARK + 23: // SVs
        case _GPS_SENTENCE_EVENT + 33:
            gps_mark.SVs = atol(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "%d,%s",_term_number,_term);
        case _GPS_SENTENCE_MARK + 24: // solnSVs
        case _GPS_SENTENCE_EVENT + 34:
            gps_mark.solnSVs = atol(_term); 
            // gcs().send_text(MAV_SEVERITY_INFO, "--- %d,%s \n",_term_number,_term);
            get_new_mark = true;
            
            break;
        }
    }
#endif
    return false;
}

bool
AP_GPS_NMEA::_send_message( void *msg, uint16_t size)
{
    if (port->txspace() < size) {
        return false;
    }

    port->write((const uint8_t *)msg, size);
    return true;
}


const char AP_GPS_NMEA::_initialisation_blob[] = AP_GPS_NMEA_HEMISPHERE_INIT_STRING;

void AP_GPS_NMEA::send_init_blob(uint8_t instance, AP_GPS &gps)
{
    gps.send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}

/*
  detect a NMEA GPS. Adds one byte, and returns true if the stream
  matches a NMEA string
 */
bool
AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
{
	switch (state.step) {
	case 0:
		state.ck = 0;
		if ('$' == data) {
			state.step++;
		}
		break;
	case 1:
		if ('*' == data) {
			state.step++;
		} else {
			state.ck ^= data;
		}
		break;
	case 2:
		if (hexdigit(state.ck>>4) == data) {
			state.step++;
		} else {
			state.step = 0;
		}
		break;
	case 3:
		if (hexdigit(state.ck&0xF) == data) {
            state.step = 0;
			return true;
		}
		state.step = 0;
		break;
    }
    return false;
}


// Calculate and return the CRC for usA binary buffer
// unsigned long CalculateCRC32(u_char *szBuf, int iSize)
// {
//     int iIndex;
//     u_long ulCRC = 0;
//     for (iIndex=0; iIndex<iSize; iIndex++)
//     {
//         ulCRC = aulCrcTable[(ulCRC ^ szBuf[iIndex]) & 0xff] ^ (ulCRC >> 8);
//     }
//     return ulCRC;
// }