mtk.cpp

/****************************************************************************
 *
 *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file mtk.cpp
 *
 * @author Thomas Gubler <thomasgubler@student.ethz.ch>
 * @author Julian Oes <julian@oes.ch>
 */

#include "mtk.h"

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <ctime>
#include <math.h>


GPSDriverMTK::GPSDriverMTK(GPSCallbackPtr callback, void *callback_user, sensor_gps_s *gps_position) :
	GPSHelper(callback, callback_user),
	_gps_position(gps_position)
{
	decodeInit();
}

int
GPSDriverMTK::configure(unsigned &baudrate, const GPSConfig &config)
{
	if (config.output_mode != OutputMode::GPS) {
		GPS_WARN("MTK: Unsupported Output Mode %i", (int)config.output_mode);
		return -1;
	}

	if (baudrate > 0 && baudrate != MTK_BAUDRATE) {
		return -1;
	}

	/* set baudrate first */
	if (GPSHelper::setBaudrate(MTK_BAUDRATE) != 0) {
		return -1;
	}

	baudrate = MTK_BAUDRATE;

	/* Write config messages, don't wait for an answer */
	if (strlen(MTK_OUTPUT_5HZ) != write(MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
		goto errout;
	}

	gps_usleep(10000);

	if (strlen(MTK_SET_BINARY) != write(MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
		goto errout;
	}

	gps_usleep(10000);

	if (strlen(MTK_SBAS_ON) != write(MTK_SBAS_ON, strlen(MTK_SBAS_ON))) {
		goto errout;
	}

	gps_usleep(10000);

	if (strlen(MTK_WAAS_ON) != write(MTK_WAAS_ON, strlen(MTK_WAAS_ON))) {
		goto errout;
	}

	gps_usleep(10000);

	if (strlen(MTK_NAVTHRES_OFF) != write(MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
		goto errout;
	}

	return 0;

errout:
	GPS_WARN("mtk: config write failed");
	return -1;
}

int
GPSDriverMTK::receive(unsigned timeout)
{
	uint8_t buf[GPS_READ_BUFFER_SIZE];
	gps_mtk_packet_t packet{};

	/* timeout additional to poll */
	gps_abstime time_started = gps_absolute_time();

	int j = 0;

	while (true) {

		int ret = read(buf, sizeof(buf), timeout);

		if (ret > 0) {
			/* first read whatever is left */
			if (j < ret) {
				/* pass received bytes to the packet decoder */
				while (j < ret) {
					if (parseChar(buf[j], packet) > 0) {
						handleMessage(packet);
						return 1;
					}

					j++;
				}

				/* everything is read */
				j = 0;
			}

		} else {
			gps_usleep(20000);
		}

		/* in case we keep trying but only get crap from GPS */
		if (time_started + timeout * 1000 < gps_absolute_time()) {
			return -1;
		}
	}
}

void
GPSDriverMTK::decodeInit()
{
	_rx_ck_a = 0;
	_rx_ck_b = 0;
	_rx_count = 0;
	_decode_state = MTK_DECODE_UNINIT;
}
int
GPSDriverMTK::parseChar(uint8_t b, gps_mtk_packet_t &packet)
{
	int ret = 0;

	if (_decode_state == MTK_DECODE_UNINIT) {

		if (b == MTK_SYNC1_V16) {
			_decode_state = MTK_DECODE_GOT_CK_A;
			_mtk_revision = 16;

		} else if (b == MTK_SYNC1_V19) {
			_decode_state = MTK_DECODE_GOT_CK_A;
			_mtk_revision = 19;
		}

	} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
		if (b == MTK_SYNC2) {
			_decode_state = MTK_DECODE_GOT_CK_B;

		} else {
			// Second start symbol was wrong, reset state machine
			decodeInit();
		}

	} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
		// Add to checksum
		if (_rx_count < 33) {
			addByteToChecksum(b);
		}

		// Fill packet buffer
		((uint8_t *)(&packet))[_rx_count] = b;
		_rx_count++;

		/* Packet size minus checksum, XXX ? */
		if (_rx_count >= sizeof(packet)) {
			/* Compare checksum */
			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
				ret = 1;

			} else {
				ret = -1;
			}

			// Reset state machine to decode next packet
			decodeInit();
		}
	}

	return ret;
}

void
GPSDriverMTK::handleMessage(gps_mtk_packet_t &packet)
{
	if (_mtk_revision == 16) {
		_gps_position->latitude_deg = packet.latitude / 1e6;   // from degrees*1e6 to degrees
		_gps_position->longitude_deg = packet.longitude / 1e6; // from degrees*1e6 to degrees

	} else if (_mtk_revision == 19) {
		_gps_position->latitude_deg = packet.latitude / 1e7;   // from degrees*1e7 to degrees
		_gps_position->longitude_deg = packet.longitude / 1e7; // from degrees*1e7 to degrees

	} else {
		GPS_WARN("mtk: unknown revision");
		_gps_position->latitude_deg = 0.0;
		_gps_position->longitude_deg = 0.0;

		// Indicate this data is not usable and bail out
		_gps_position->eph = 1000.0f;
		_gps_position->epv = 1000.0f;
		_gps_position->fix_type = 0;
		return;
	}

	_gps_position->altitude_msl_m = packet.msl_altitude / 100.0; // from cm to meters
	_gps_position->fix_type = packet.fix_type;
	_gps_position->eph = packet.hdop / 100.0f; // from cm to m
	_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
	_gps_position->hdop = packet.hdop / 100.0f;
	_gps_position->vdop = _gps_position->hdop;
	_gps_position->vel_m_s = ((float)packet.ground_speed) / 100.0f; // from cm/s to m/s
	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
	_gps_position->satellites_used = packet.satellites;

#ifndef NO_MKTIME
	/* convert time and date information to unix timestamp */
	struct tm timeinfo = {};
	uint32_t timeinfo_conversion_temp;

	timeinfo.tm_mday = packet.date / 10000;
	timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 10000;
	timeinfo.tm_mon = (timeinfo_conversion_temp / 100) - 1;
	timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 100) + 100;

	timeinfo.tm_hour = (packet.utc_time / 10000000);
	timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 10000000;
	timeinfo.tm_min = timeinfo_conversion_temp / 100000;
	timeinfo_conversion_temp -= timeinfo.tm_min * 100000;
	timeinfo.tm_sec = timeinfo_conversion_temp / 1000;
	timeinfo_conversion_temp -= timeinfo.tm_sec * 1000;

	timeinfo.tm_isdst = 0;


	time_t epoch = mktime(&timeinfo);

	if (epoch > GPS_EPOCH_SECS) {
		// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
		// and control its drift. Since we rely on the HRT for our monotonic
		// clock, updating it from time to time is safe.

		timespec ts{};
		ts.tv_sec = epoch;
		ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL;

		setClock(ts);

		_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
		_gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL;

	} else {
		_gps_position->time_utc_usec = 0;
	}

#else
	_gps_position->time_utc_usec = 0;
#endif

	_gps_position->timestamp = gps_absolute_time();
	_gps_position->timestamp_time_relative = 0;

	// Position and velocity update always at the same time
	_rate_count_vel++;
	_rate_count_lat_lon++;
}

void
GPSDriverMTK::addByteToChecksum(uint8_t b)
{
	_rx_ck_a = _rx_ck_a + b;
	_rx_ck_b = _rx_ck_b + _rx_ck_a;
}