LibrePilot/flight/Modules/GPS/UBX.c

/**
 ******************************************************************************
 * @addtogroup OpenPilotModules OpenPilot Modules
 * @{
 * @addtogroup GSPModule GPS Module
 * @brief Process GPS information (UBX binary format)
 * @{
 *
 * @file       UBX.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
 * @brief      GPS module, handles GPS and NMEA stream
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/
/*
 * 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, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include "openpilot.h"
#include "pios.h"

#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)

#include "UBX.h"
#include "gps.h"

static uint32_t gpsRxReceived = 0;
static uint32_t gpsRxChkSumError = 0;
static uint32_t gpsRxOverflow = 0;

// parse incoming character stream for messages in UBX binary format

int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData)
{
	enum proto_states {START,UBX_SY2,UBX_CLASS,UBX_ID,UBX_LEN1,
		UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2,FINISHED};
	static enum proto_states proto_state = START;
	static uint8_t rx_count = 0;
	UBXPacket *ubx = (UBXPacket *)gps_rx_buffer;

	switch (proto_state) {
		case START: // detect protocol
			if (c ==  UBX_SYNC1) // first UBX sync char found
				proto_state = UBX_SY2;
			break;
		case UBX_SY2:
			if (c == UBX_SYNC2) // second UBX sync char found
				proto_state = UBX_CLASS;
			else
				proto_state = START; // reset state
			break;
		case UBX_CLASS:
			ubx->header.class = c;
			proto_state = UBX_ID;
			break;
		case UBX_ID:
			ubx->header.id = c;
			proto_state = UBX_LEN1;
			break;
		case UBX_LEN1:
			ubx->header.len = c;
			proto_state = UBX_LEN2;
			break;
		case UBX_LEN2:
			ubx->header.len += (c << 8);
			if (ubx->header.len > sizeof(UBXPayload)) {
				gpsRxOverflow++;
				proto_state = START;
			} else {
				rx_count = 0;
				proto_state = UBX_PAYLOAD;
			}
			break;
		case UBX_PAYLOAD:
			if (rx_count < ubx->header.len) {
				ubx->payload.payload[rx_count] = c;
				if (++rx_count == ubx->header.len)
					proto_state = UBX_CHK1;
			} else {
				gpsRxOverflow++;
				proto_state = START;
			}
			break;
		case UBX_CHK1:
			ubx->header.ck_a = c;
			proto_state = UBX_CHK2;
			break;
		case UBX_CHK2:
			ubx->header.ck_b = c;
			if (checksum_ubx_message(ubx)) { // message complete and valid
				parse_ubx_message(ubx, GpsData);
				proto_state = FINISHED;
			} else {
				gpsRxChkSumError++;
				proto_state = START;
			}
			break;
		default: break;
	}

	if (proto_state == START)
		return PARSER_ERROR;	// parser couldn't use this byte
	else if (proto_state == FINISHED) {
		gpsRxReceived++;
		proto_state = START;
		return PARSER_COMPLETE;	// message complete & processed
	}

	return PARSER_INCOMPLETE; // message not (yet) complete
}


// Keep track of various GPS messages needed to make up a single UAVO update
// time-of-week timestamp is used to correlate matching messages

#define POSLLH_RECEIVED	(1 << 0)
#define STATUS_RECEIVED	(1 << 1)
#define DOP_RECEIVED	(1 << 2)
#define VELNED_RECEIVED	(1 << 3)
#define SOL_RECEIVED	(1 << 4)
#define	ALL_RECEIVED	(SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED)
#define NONE_RECEIVED	0

static struct msgtracker{
		uint32_t	currentTOW;		// TOW of the message set currently in progress
		uint8_t		msg_received;	// keep track of received message types
	} msgtracker;

// Check if a message belongs to the current data set and register it as 'received'
bool check_msgtracker (uint32_t tow, uint8_t msg_flag)
{

	if (tow > msgtracker.currentTOW ? true                  // start of a new message set
		: (msgtracker.currentTOW - tow > 6*24*3600*1000)) { // 6 days, TOW wrap around occured
		msgtracker.currentTOW = tow;
		msgtracker.msg_received = NONE_RECEIVED;
	} else if (tow < msgtracker.currentTOW)	// message outdated (don't process)
				return false;

	msgtracker.msg_received |= msg_flag; // register reception of this msg type
	return true;
}

bool checksum_ubx_message (UBXPacket *ubx)
{
	int i;
	uint8_t ck_a, ck_b;

	ck_a = ubx->header.class;
	ck_b = ck_a;

	ck_a += ubx->header.id;
	ck_b += ck_a;

	ck_a += ubx->header.len & 0xff;
	ck_b += ck_a;

	ck_a += ubx->header.len >> 8;
	ck_b += ck_a;

	for (i = 0; i < ubx->header.len; i++) {
		ck_a += ubx->payload.payload[i];
		ck_b += ck_a;
	}

	if (ubx->header.ck_a == ck_a &&
			ubx->header.ck_b == ck_b)
		return true;
	else
		return false;

}

void parse_ubx_nav_posllh (UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition)
{
	if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) {
		if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) {
			GpsPosition->Altitude = (float)posllh->hMSL*0.001f;
			GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL)*0.001f;
			GpsPosition->Latitude = posllh->lat;
			GpsPosition->Longitude = posllh->lon;
		}
	}
}

void parse_ubx_nav_status (UBX_NAV_STATUS *status, GPSPositionData *GpsPosition)
{
#if 0 // we already get this info from NAV_SOL
	if (check_msgtracker(status->iTOW, STATUS_RECEIVED)) {
		switch (status->gpsFix)	{
			case STATUS_GPSFIX_2DFIX:
				GpsPosition->Status = GPSPOSITION_STATUS_FIX2D;
				break;
			case STATUS_GPSFIX_3DFIX:
				GpsPosition->Status = GPSPOSITION_STATUS_FIX3D;
				break;
			default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX;
		}
	}
#endif
}

void parse_ubx_nav_sol (UBX_NAV_SOL *sol, GPSPositionData *GpsPosition)
{
	if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) {
		GpsPosition->Satellites = sol->numSV;

		if (sol->flags & STATUS_FLAGS_GPSFIX_OK) {
			switch (sol->gpsFix) {
				case STATUS_GPSFIX_2DFIX:
					GpsPosition->Status = GPSPOSITION_STATUS_FIX2D;
					break;
				case STATUS_GPSFIX_3DFIX:
					GpsPosition->Status = GPSPOSITION_STATUS_FIX3D;
					break;
				default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX;
			}
		}
		else // fix is not valid so we make sure to treat is as NOFIX
			GpsPosition->Status = GPSPOSITION_STATUS_NOFIX;
	}
}

void parse_ubx_nav_dop (UBX_NAV_DOP *dop, GPSPositionData *GpsPosition)
{
	if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) {
		GpsPosition->HDOP = (float)dop->hDOP * 0.01f;
		GpsPosition->VDOP = (float)dop->vDOP * 0.01f;
		GpsPosition->PDOP = (float)dop->pDOP * 0.01f;
	}
}

void parse_ubx_nav_velned (UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition)
{
	GPSVelocityData GpsVelocity;

	if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) {
		if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) {
			GpsVelocity.North	= (float)velned->velN/100.0f;
			GpsVelocity.East	= (float)velned->velE/100.0f;
			GpsVelocity.Down	= (float)velned->velD/100.0f;
			GPSVelocitySet(&GpsVelocity);
			GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f;
			GpsPosition->Heading = (float)velned->heading * 1.0e-5f;
		}
	}
}

#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_timeutc (UBX_NAV_TIMEUTC *timeutc)
{
	if (!(timeutc->valid & TIMEUTC_VALIDUTC))
		return;

	GPSTimeData GpsTime;

	GpsTime.Year = timeutc->year;
	GpsTime.Month = timeutc->month;
	GpsTime.Day = timeutc->day;
	GpsTime.Hour = timeutc->hour;
	GpsTime.Minute = timeutc->min;
	GpsTime.Second = timeutc->sec;

	GPSTimeSet(&GpsTime);
}
#endif

#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_svinfo (UBX_NAV_SVINFO *svinfo)
{
	uint8_t chan;
	GPSSatellitesData svdata;

	svdata.SatsInView = 0;
	for (chan = 0; chan < svinfo->numCh;	chan++) {
		if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) {
			svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim;
			svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev;
			svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
			svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
			svdata.SatsInView++;
		}

	}
	// fill remaining slots (if any)
	for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
		svdata.Azimuth[chan] = (float)0.0f;
		svdata.Elevation[chan] = (float)0.0f;
		svdata.PRN[chan] = 0;
		svdata.SNR[chan] = 0;
	}

	GPSSatellitesSet(&svdata);
}
#endif

// UBX message parser
// returns UAVObjectID if a UAVObject structure is ready for further processing

uint32_t parse_ubx_message (UBXPacket *ubx, GPSPositionData *GpsPosition)
{
	uint32_t id = 0;

	switch (ubx->header.class) {
		case UBX_CLASS_NAV:
			switch (ubx->header.id) {
				case UBX_ID_POSLLH:
					parse_ubx_nav_posllh (&ubx->payload.nav_posllh, GpsPosition);
					break;
				case UBX_ID_STATUS:
					parse_ubx_nav_status (&ubx->payload.nav_status, GpsPosition);
					break;
				case UBX_ID_DOP:
					parse_ubx_nav_dop (&ubx->payload.nav_dop, GpsPosition);
					break;
				case UBX_ID_SOL:
					parse_ubx_nav_sol (&ubx->payload.nav_sol, GpsPosition);
					break;
				case UBX_ID_VELNED:
					parse_ubx_nav_velned (&ubx->payload.nav_velned, GpsPosition);
					break;
#if !defined(PIOS_GPS_MINIMAL)
				case UBX_ID_TIMEUTC:
					parse_ubx_nav_timeutc (&ubx->payload.nav_timeutc);
					break;
				case UBX_ID_SVINFO:
					parse_ubx_nav_svinfo (&ubx->payload.nav_svinfo);
					break;
#endif
			}
			break;
	}
	if (msgtracker.msg_received == ALL_RECEIVED) {
		GPSPositionSet(GpsPosition);
		msgtracker.msg_received = NONE_RECEIVED;
		id = GPSPOSITION_OBJID;
	}
	return id;
}

#endif // PIOS_INCLUDE_GPS_UBX_PARSER