Context Navigation

CPTComponent.cpp@ 125

Last change on this file since 125 was 111, checked in by DHERBOMEZ Gérald, 9 years ago
Delete deprecated call to shared memories. Use I/O mechanism instead. Adjustement of dllimport and dllexport macros;
File size: 43.1 KB

Rev	Line
[59]	1	// *********************************************************************
	2	// created: 20011/06/28 - 18:13
	3	// filename: cptComponent.cpp
	4	//
	5	// author: Sergio Rodriguez
	6	//
	7	// version: $Id: $
	8	//
	9	// purpose: Acquires SPAN CPT data
	10	//
	11	// *********************************************************************
	12
	13	#include "CPTComponent.h"
	14
	15	#include <iostream>
	16
[88]	17	#include <Pacpus/kernel/ComponentFactory.h>
	18	#include <Pacpus/kernel/DbiteFileTypes.h>
	19	#include <Pacpus/kernel/Log.h>
	20	#include <Pacpus/PacpusTools/geodesie.h>
	21	#include <Pacpus/PacpusTools/ShMem.h>
[59]	22
[99]	23
[59]	24	namespace pacpus {
	25
	26	using namespace std;
	27
	28	/// Construct the factory
	29	static ComponentFactory<CPTComponent> sFactory("CPTComponent");
	30
	31	static const size_t AllFramesSize = sizeof(TimestampedBestgpsposaFrame)
	32	+ sizeof(TimestampedRawimusaFrame)
	33	+ sizeof(TimestampedInspvaaFrame)
	34	+ sizeof(TimestampedInscovFrame)
	35	;
	36
	37	DECLARE_STATIC_LOGGER("pacpus.base.CPTComponent");
	38
	39	//////////////////////////////////////////////////////////////////////////
	40	/// Constructor of CPTComponent class
	41	CPTComponent::CPTComponent(QString name)
	42	: ComponentBase(name)
	43	{
	44	LOG_INFO("sizeof(TimestampedBestgpsposaFrame)=" << sizeof(TimestampedBestgpsposaFrame));
	45	LOG_INFO("sizeof(TimestampedRawimusaFrame)=" << sizeof(TimestampedRawimusaFrame));
	46	LOG_INFO("sizeof(TimestampedInspvaaFrame)=" << sizeof(TimestampedInspvaaFrame));
	47	LOG_INFO("sizeof(TimestampedInscovFrame)=" << sizeof(TimestampedInscovFrame));
	48
	49	mVerbose = false;
	50	setRecording( false );
	51	enuRef_ = false;
	52	LAT_REF =0.0;
	53	LON_REF =0.0;
	54	HE_REF =0.0;
	55
	56	currentRoadtime_ = 0;
	57	currentTimerange_ = 0;
	58
	59	//decoding
[78]	60	currentDataFrame_ ="";
[59]	61
	62	frameToDecode = "";
	63	timeOfFrameToDecode = 0;
	64	timerangeOfFrameToDecode = 0;
	65
	66	restOfFrame = "";
	67
	68	newFrameToDecode = false;
	69	startOfFrame = false;
	70	sofIdx_ =0;
	71	endOfFrame = false;
	72	eofIdx_ =0;
	73
	74	//ShMems allocation
[105]	75	// mShMem = new ShMem("SPAN_FRAMES", AllFramesSize);
[59]	76	mAllFramesBuffer = new char[AllFramesSize];
	77	}
	78
	79	/// Destructor of the CPTComponent class
	80	CPTComponent::~CPTComponent()
	81	{
[105]	82	// delete mShMem;
[59]	83	delete[] mAllFramesBuffer;
	84	}
	85
	86	ComponentBase::COMPONENT_CONFIGURATION CPTComponent::configureComponent(XmlComponentConfig /config/)
	87	{
	88	setPortCOM( xmlParameters().getProperty("port").toLatin1() );
	89
[78]	90	if (!xmlParameters().getProperty("recording").isNull()) {
	91	setRecording ( (xmlParameters().getProperty("recording") == "true" ? true : false) );
[59]	92	}
	93	if (!xmlParameters().getProperty("verbose").isNull()) {
	94	mVerbose = (xmlParameters().getProperty("verbose") == "true" ? true : false);
	95	}
	96	if (!xmlParameters().getProperty("ENUref").isNull()) {
	97	enuRef_ = (xmlParameters().getProperty("ENUref") == "true" ? true : false);
	98	if (enuRef_) {
	99	LAT_REF = xmlParameters().getProperty("LatRef").toDouble();
	100	LON_REF = xmlParameters().getProperty("LonRef").toDouble();
	101	HE_REF = xmlParameters().getProperty("HeRef").toDouble();
	102	if (mVerbose) {
	103	LOG_DEBUG("Reference Saint Mande:: Lat: " << LAT_REF << ", Lon: " << LON_REF << " Height: " << HE_REF);
	104	}
	105	}
	106	}
	107	return ComponentBase::CONFIGURED_OK;
	108	}
	109
[88]	110
	111
[99]	112	void CPTComponent::addOutputs()
	113	{
	114	// must inherit from QObject to use addOutput
	115	addOutput<QPointF, CPTComponent>("position2DENU");
	116	}
[88]	117
[99]	118
[59]	119	void CPTComponent::setPortCOM(const char * port)
	120	{
	121	portName = port;
	122	}
	123
	124	/// Log function
	125	void CPTComponent::processing(int /i/)
	126	{
	127	if (!serialPort) {
	128	LOG_WARN("no serial port");
	129	return;
	130	}
	131
	132	//ShMem data variables
	133	TimestampedBestgpsposaFrame a;
	134	TimestampedRawimusaFrame b;
	135	TimestampedInspvaaFrame c;
	136	TimestampedInscovFrame d;
	137
	138	//ShMem buffer allocation
	139	char * allFramesBuffer = new char[AllFramesSize];
	140
	141	//ShMem buffer initialization
	142	initBuffer(&a, &b, &c, &d);
	143	memcpy(allFramesBuffer,
	144	&a, sizeof(TimestampedBestgpsposaFrame));
	145	memcpy(allFramesBuffer
	146	+ sizeof(TimestampedBestgpsposaFrame),
	147	&b,sizeof(TimestampedRawimusaFrame));
	148	memcpy(allFramesBuffer
	149	+ sizeof(TimestampedBestgpsposaFrame) + sizeof(TimestampedRawimusaFrame),
	150	&c, sizeof(TimestampedInspvaaFrame));
	151	memcpy(allFramesBuffer
	152	+ sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame)+sizeof(TimestampedInspvaaFrame),
	153	&d, sizeof(TimestampedInscovFrame));
	154
	155	memcpy(allFramesBuffer, mAllFramesBuffer, AllFramesSize);
	156
	157	// get the frame and remove it in the list
[78]	158	FRAME * currentFrame = serialPort->getNextFrame();
[59]	159
	160	char * currentDataFrame = new char[currentFrame->length];
	161	memcpy(currentDataFrame, currentFrame->data, currentFrame->length);
	162	currentDataFrameLength_ = currentFrame->length;
	163	currentRoadtime_ = currentFrame->t;
[78]	164
	165	// free memory
	166	delete currentFrame;
[59]	167
	168	setState(ComponentBase::MONITOR_OK);
	169
	170	if (isRecording()) {
	171	s8Data_.dataPos = dataFile_.tellp();
	172	s8Data_.length = currentDataFrameLength_;
	173	if (mVerbose) {
	174	LOG_DEBUG("data:" << currentDataFrame << "addr:" << s8Data_.dataPos << "l:" << s8Data_.length);
	175	}
	176	size_t dataSize = sizeof(Stream8Position);
	177	raws8hdFile.writeRecord(currentRoadtime_, 0, reinterpret_cast<const char *>(&s8Data_), dataSize);
	178	dataFile_.write(reinterpret_cast<char*>(currentDataFrame), currentDataFrameLength_);
	179	}
	180
	181	// Decoding
	182	char * buffer = new char[currentDataFrameLength_ + 1];
	183	memcpy(buffer, currentDataFrame, currentDataFrameLength_);
	184	buffer[currentDataFrameLength_] = '\0'; // add a \0 to convert for the conversion in QString
[99]	185	currentDataFrame_ = "";
	186	currentDataFrame_.append(buffer);
[59]	187
[78]	188	delete[] buffer;
	189	delete[] currentDataFrame;
[59]	190
	191	analyzeFrame();
	192
	193	if (newFrameToDecode) {
	194	LOG_DEBUG("got new frame to decode");
	195
	196	type = frameType(frameToDecode);
	197	LOG_DEBUG("frame type=" << type);
	198	if (-1 != type) {
	199	if (-1 == decodeFrame(type)) {
	200	LOG_WARN("cannot decode the dataframe");
	201	} else {
	202	switch(type) {
	203	case TRAME_GGA_DBL:
	204	if (mVerbose) {
	205	cout << "[SPAN-GGA] Lat: " <<
	206	ggaFrame_.lat<<
	207	" Lon: " << ggaFrame_.lon<<
	208	" Hgt: " << ggaFrame_.alt_msl <<
	209	" Quality: " << ggaFrame_.ind_qualite << "\n";}
	210	break;
	211
	212	case TRAME_BESTGPSPOSA:
	213	//ShMem buffer data copy
	214	memcpy(&a.frame, &bestgpsposaFrame_, sizeof(trame_bestgpsposa));
	215	a.time=timeOfFrameToDecode;
	216	a.timerange=timerangeOfFrameToDecode;
	217	memcpy(allFramesBuffer, &a, sizeof(TimestampedBestgpsposaFrame));
	218
	219	if (isRecording()) {
	220	size_t dataSize = sizeof(bestgpsposaFrame_);
	221	bestgpsposaFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
	222	reinterpret_cast<const char *>(&bestgpsposaFrame_), dataSize);
	223	}
	224	if (mVerbose) {
	225	cout << "[SPAN-BESTGPSPOSA]" << "\t"
	226	<< "Lat=" << bestgpsposaFrame_.Lat << "\t"
	227	<< "Lon=" << bestgpsposaFrame_.Lon << "\t"
	228	<< "Hgt=" << bestgpsposaFrame_.Hgt << "\t"
	229	<< "E=" << bestgpsposaFrame_.e << "\t"
	230	<< "N=" << bestgpsposaFrame_.n << "\t"
	231	<< "U=" << bestgpsposaFrame_.u
	232	;
	233	}
[105]	234	//mShMem->write(allFramesBuffer, AllFramesSize);
[88]	235
[99]	236	if (position2DENUOutput_ && position2DENUOutput_->hasConnection())
	237	{
	238	// send ENU 2D position
	239	QPointF pt( bestgpsposaFrame_.e, bestgpsposaFrame_.n);
	240	position2DENUOutput_->send(pt);
[88]	241	}
[59]	242	break;
	243
	244	case TRAME_RAWIMUSA:
	245
	246	//ShMem buffer data copy
	247	memcpy(&b.frame, &rawimuFrame_, sizeof(trame_rawimusa));
	248	b.time=timeOfFrameToDecode;
	249	b.timerange=timerangeOfFrameToDecode;
	250	memcpy(allFramesBuffer+
	251	sizeof(TimestampedBestgpsposaFrame),&b,sizeof(TimestampedRawimusaFrame));
	252
	253	if (isRecording()) {
	254	size_t dataSize = sizeof(rawimuFrame_);
	255	rawimuFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
	256	reinterpret_cast<const char *>(&rawimuFrame_), dataSize);
	257	}
	258	if (mVerbose) {
	259	cout << "[SPAN-RAWIMUSA]"<< endl;
	260	}
	261
	262	//ShMem refresh
[105]	263	/*mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
[59]	264	+sizeof(TimestampedRawimusaFrame)
	265	+sizeof(TimestampedInspvaaFrame)
[105]	266	+sizeof(TimestampedInscovFrame));*/
[59]	267	break;
	268	case TRAME_INSPVAA:
	269
	270	//ShMem buffer data copy
	271	memcpy(&c.frame, &inspvaFrame_, sizeof(trame_inspvaa));
	272	c.time=timeOfFrameToDecode;
	273	c.timerange=timerangeOfFrameToDecode;
	274	memcpy(allFramesBuffer+
	275	sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame),
	276	&c,sizeof(TimestampedInspvaaFrame));
	277
	278	if (isRecording()) {
	279	size_t dataSize = sizeof(inspvaFrame_);
	280	inspvaFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
	281	reinterpret_cast<const char *>(&inspvaFrame_), dataSize);
	282	}
	283	if (mVerbose) {
	284	cout << "[SPAN-INSPVAA]:" << "\t"
	285	<< "Lat: " << inspvaFrame_.Lat << "\t"
	286	<< "Lon: " << inspvaFrame_.Lon << "\t"
	287	<< "Roll: "<< inspvaFrame_.Roll << "\t"
	288	<< "Pitch: " << inspvaFrame_.Azimuth << "\t"
	289	<< "E: " << inspvaFrame_.e << "\t"
	290	<< "N: " << inspvaFrame_.n << "\t"
	291	<< "U: " << inspvaFrame_.u << endl;
	292	}
	293
	294	//ShMem refresh
[105]	295	/* mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
[59]	296	+sizeof(TimestampedRawimusaFrame)
	297	+sizeof(TimestampedInspvaaFrame)
[105]	298	+sizeof(TimestampedInscovFrame));*/
[59]	299	break;
	300	case TRAME_INSCOV:
	301
	302	//ShMem data copy
	303	memcpy(&d.frame, &inscovFrame_, sizeof(trame_inscov));
	304	d.time=timeOfFrameToDecode;
	305	d.timerange=timerangeOfFrameToDecode;
	306	memcpy(allFramesBuffer+
	307	sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame)+sizeof(TimestampedInspvaaFrame),
	308	&d,sizeof(TimestampedInscovFrame));
	309
	310	if (isRecording()) {
	311	size_t dataSize = sizeof(inscovFrame_);
	312	inscovFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
	313	reinterpret_cast<const char *>(&inscovFrame_), dataSize);
	314	// FIXED: was inscovFile_ instead of inscovFrame_ given as buffer
	315	}
	316	if (mVerbose) {
	317	cout << "[SPAN-INSCOV] CovXX: " << inscovFrame_.PosCov[0][0] <<
	318	" CovYY: " << inscovFrame_.PosCov[1][1] <<
	319	" CovZZ: "<< inscovFrame_.PosCov[2][2] << endl;
	320	}
	321
	322	//ShMem refresh
[105]	323	/* mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
[59]	324	+sizeof(TimestampedRawimusaFrame)
	325	+sizeof(TimestampedInspvaaFrame)
[105]	326	+sizeof(TimestampedInscovFrame));*/
[59]	327	break;
	328	default:
	329	break;
	330	}
	331	}
	332	}
	333
	334	type = -1;
	335	newFrameToDecode = false;
	336	frameToDecode = "";
	337	}
	338
	339	memcpy(mAllFramesBuffer, allFramesBuffer, AllFramesSize);
	340
	341	delete[] allFramesBuffer;
	342
	343	}
	344
	345	/************************************************************************/
	346	/* Method to stop the thread */
	347	/************************************************************************/
	348	void CPTComponent::stopActivity()
	349	{
	350	serialPort->THREAD_ALIVE = false;
	351	if (!serialPort->wait(2000)) {
	352	serialPort->terminate();
	353	qDebug("The Win32SerialPort thread blocks anormally, it has been killed !!");
	354	}
	355	if ( !serialPort->closePort() )
	356	qDebug("Failed to close the port");
	357	else
	358	qDebug("The port is closed");
	359	delete serialPort;
	360	serialPort = NULL;
	361
	362	if (isRecording()) {
	363	bestgpsposaFile_.close();
	364	rawimuFile_.close();
	365	inspvaFile_.close();
	366	inscovFile_.close();
	367	raws8hdFile.close();
	368	dataFile_.close();
	369	}
	370	}
	371
	372	/************************************************************************/
	373	/* Method to start the thread */
	374	/************************************************************************/
	375	void CPTComponent::startActivity()
	376	{
	377	type = -1;
	378
	379	#if WIN32
	380	serialPort = new Win32SerialPort(portName.toLatin1());
	381	// Asynchrone
	382	serialPort->setMode(FILE_FLAG_OVERLAPPED);
	383	// Synchrone
	384	//serialPort->setMode(0);
	385	#else
	386	serialPort = new PosixSerialPort(portName.toLatin1());
	387	#endif
	388
	389	if (!serialPort->openPort(portName.toLatin1())) {
	390	LOG_ERROR("cannot open the port " << portName);
	391	LOG_INFO("cannot start SPAN CPT Component '" << name() << "'");
	392	return;
	393	}
	394
	395	serialPort->configurePort(xmlParameters().getProperty("baudrate").toLong(),
	396	xmlParameters().getProperty("bytesize").toUInt(),
	397	xmlParameters().getProperty("parity").toShort(),
	398	xmlParameters().getProperty("stopbits").toUInt() - 1);
	399
	400	if (isRecording()) {
	401	bestgpsposaFile_.open("SPAN_bestgpsposa.dbt", WriteMode, TRAME_BESTGPSPOSA, sizeof(trame_bestgpsposa));
	402	rawimuFile_.open("SPAN_rawimusa.dbt", WriteMode, TRAME_RAWIMUSA, sizeof(trame_rawimusa));
	403	inspvaFile_.open("SPAN_inspvaa.dbt", WriteMode, TRAME_INSPVAA, sizeof(trame_inspvaa));
	404	inscovFile_.open("SPAN_inscov.dbt", WriteMode, TRAME_INSCOV, sizeof(trame_inscov));
	405
	406	// raw data file: Dbt header + binary data stream
	407	raws8hdFile.open((name() + ".dbt").toStdString(), WriteMode, STREAM8POSITION, sizeof(Stream8Position));
	408	// FIXME: use ofstream
	409	// open the file with C function to be sure that it will exist
	410	FILE * stream = NULL;
	411	if ( ( stream = fopen((name() + ".utc").toLatin1().data(),"a+") ) == NULL ) {
	412	LOG_FATAL("Error while opening the stream utc file\n press a ENTER to exit");
	413	getchar();
	414	::exit(-1);
	415	} else {
	416	fclose(stream);
	417	}
	418
	419	dataFile_.open((name() + ".utc").toLatin1().data(), ios_base::out\|ios_base::binary\|ios_base::app);
	420	if (!dataFile_)
	421	{
	422	printf("Error while opening the file alasca_data.utc\n press a ENTER to exit\n");
	423	getchar();
	424	::exit(0);
	425	}
	426	}
	427
	428	if (!connect(serialPort,SIGNAL(newDataAvailable(int)),this, SLOT(processing(int)))) {
	429	LOG_WARN("cannot connect SIGNAL(newDataAvailable(int)) with SLOT(unlockProcessing(int)");
	430	}
[88]	431	LOG_INFO("started component '" << name() << "'");
[59]	432
[99]	433	position2DENUOutput_ = getTypedOutput<QPointF, CPTComponent>("position2DENU");
[88]	434
	435
[59]	436	serialPort->THREAD_ALIVE = true;
	437	serialPort->start();
	438
	439	}
	440
	441	int CPTComponent::analyzeFrame()
	442	{
	443	// SOF = start of frame
	444	// EOF = end of frame
	445
	446	// PPS case
	447	//if (currentDataFrame == "PPS")
	448	//{
	449	// frameToDecode = currentDataFrame;
	450	// newFrameToDecode = true;
	451	// timeOfFrameToDecode = currentRoadtime;
	452	// timerangeOfFrameToDecode = currentTimerange;
	453	// return 1;
	454	//}
	455
[78]	456	currentDataFrame_ = restOfFrame + currentDataFrame_;
[59]	457	restOfFrame = "";
	458
[78]	459	if (currentDataFrame_.indexOf(QRegExp("[$#%]"),0)!=-1) {
	460	sofIdx_ = currentDataFrame_.indexOf(QRegExp("[$#%]"),0);
[59]	461	startOfFrame = true;
[78]	462	timeOfFrameToDecode = currentRoadtime_;
[59]	463
[78]	464	if (currentDataFrame_.indexOf("\n",sofIdx_)!=-1) {
	465	eofIdx_ = currentDataFrame_.indexOf("\n",sofIdx_);
[59]	466	endOfFrame = true;
[78]	467	timerangeOfFrameToDecode = currentTimerange_;
[59]	468	}
	469	else
[78]	470	restOfFrame = currentDataFrame_;
[59]	471	}
	472	else
[78]	473	restOfFrame = currentDataFrame_;
[59]	474
	475	if ( (startOfFrame) && (endOfFrame) )
	476	{
	477	newFrameToDecode = true;
	478	for (int i=sofIdx_;i<eofIdx_;i++)
[78]	479	frameToDecode +=currentDataFrame_.at(i);
	480	for (int j=eofIdx_+1; j<currentDataFrame_.size(); j++)
	481	restOfFrame +=currentDataFrame_.at(j);
[59]	482	if (mVerbose) {
	483	LOG_DEBUG("[Frame:] " << frameToDecode);
	484	}
	485	}
	486	startOfFrame = false;
	487	endOfFrame = false;
	488	sofIdx_ =0;
	489	eofIdx_ =0;
	490
	491	return 1;
	492	}
	493
	494	int CPTComponent::frameType(const QString frame)
	495	{
	496	if (( frame[0] == 'P' ) && (frame[1] == 'P')&&( frame[2] == 'S' )) return SIGNAL_PPS ;
	497	if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'G')&&(frame[5] == 'A')) return TRAME_GGA_DBL ;
	498	if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'A')) return TRAME_GSA ;
	499	if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'H' ) && (frame[4] == 'D')&&(frame[5] == 'T')) return TRAME_HDT ;
	500	if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'T')) return TRAME_GST ;
	501	if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'V')) return TRAME_GSV ;
	502	if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'R' ) && (frame[4] == 'M')&&(frame[5] == 'C')) return TRAME_RMC ;
	503	if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'R' ) && (frame[4] == 'O')&&(frame[5] == 'T')) return TRAME_ROT ;
	504	if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'V' ) && (frame[4] == 'T')&&(frame[5] == 'G')) return TRAME_VTG ;
	505	if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'Z' ) && (frame[4] == 'D')&&(frame[5] == 'A')) return TRAME_ZDA ;
	506	if ((frame[0]== '#')\|\|(frame[0]== '%')) { // Span CPT frames
	507	if ((frame.contains("BESTGPSPOSA", Qt::CaseSensitive)))
	508	return TRAME_BESTGPSPOSA;
	509	else if ((frame.contains("RAWIMUSA", Qt::CaseSensitive)))
	510	return TRAME_RAWIMUSA;
	511	else if ((frame.contains("INSPVAA", Qt::CaseSensitive))\|\|(frame.contains("INSPVASA", Qt::CaseSensitive)))
	512	return TRAME_INSPVAA;
	513	else if ((frame.contains("INSCOVA", Qt::CaseSensitive))\|\|(frame.contains("INSCOVSA", Qt::CaseSensitive)))
	514	return TRAME_INSCOV;
	515	else
	516	return UNKNOWN_NMEA_FRAME;
	517	}
	518	return UNKNOWN_NMEA_FRAME;
	519	}
	520
	521	int CPTComponent::decodeFrame(int type)
	522	{
	523	double lat_rad = 0, lon_rad = 0;
	524	int indexGSV = 0;
	525	int indexGSA = 0;
	526	//int pps = 1; // dummy value for pps isRecording()
	527
	528	SENTENCE sentence;
	529	sentence.Sentence = frameToDecode;
	530
	531	QStringList stringList;
	532	QString header;
	533	QString data;
	534
	535	switch(type) {
	536	case UNKNOWN_NMEA_FRAME:
	537	LOG_ERROR("received unknown frame");
	538	break;
	539
	540	case SIGNAL_PPS:
	541	// TODO
	542	LOG_WARN("unimplemented");
	543	break;
	544
	545	case TRAME_GGA_DBL:
	546	if (sentence.Sentence.contains("PPS", Qt::CaseSensitive)) {
	547	LOG_ERROR("Almanac error on SPAN CPT");
	548	return -1;
	549	} else {
	550	if (!nmea0183_.Gga.Parse(sentence)) {
	551	LOG_ERROR("cannot parse the frame " << nmea0183_.Gga.ErrorMessage);
	552	}
	553	lat_rad = Geodesie::Deg2Rad(nmea0183_.Gga.Position.Latitude.GetDecimalDegrees());
	554	lon_rad = Geodesie::Deg2Rad(nmea0183_.Gga.Position.Longitude.GetDecimalDegrees());
	555	ggaFrame_.H = nmea0183_.Gga.Time.time().hour();
	556	ggaFrame_.Mi = nmea0183_.Gga.Time.time().minute();
	557	ggaFrame_.S = nmea0183_.Gga.Time.time().second();
	558	ggaFrame_.Ms = nmea0183_.Gga.Time.time().msec();
	559	ggaFrame_.lon = lon_rad;
	560	ggaFrame_.lat = lat_rad;
	561	ggaFrame_.ind_qualite = nmea0183_.Gga.GPSQuality;
	562	ggaFrame_.nb_sat = nmea0183_.Gga.NumberOfSatellitesInUse;
	563	ggaFrame_.hdop = nmea0183_.Gga.HorizontalDilutionOfPrecision;
	564	ggaFrame_.alt_msl = nmea0183_.Gga.AntennaAltitudeMeters;
	565	ggaFrame_.d_geoidal = nmea0183_.Gga.GeoidalSeparationMeters;
	566	ggaFrame_.age = nmea0183_.Gga.AgeOfDifferentialGPSDataSeconds;
	567	ggaFrame_.dir_lat = ( (nmea0183_.Gga.Position.Latitude.Northing == North) ? 'N' : 'S' );
	568	ggaFrame_.dir_lon = ( (nmea0183_.Gga.Position.Longitude.Easting == East) ? 'E' : 'W' );
	569	ggaFrame_.ref_station_ID = nmea0183_.Gga.DifferentialReferenceStationID;
	570	}
	571	break;
	572
	573	case TRAME_GSA:
	574	if (!nmea0183_.Gsa.Parse(sentence))
	575	qWarning("Failed to parse the frame %s\n", nmea0183_.Gsa.ErrorMessage.toLatin1().data());
	576	gsaFrame_.mode_select = ((nmea0183_.Gsa.OperatingMode == GSA::Manual) ? 'M' : 'A');
	577	gsaFrame_.mode_result = 0;
	578	if (nmea0183_.Gsa.FixMode == GSA::FixUnavailable)
	579	gsaFrame_.mode_result = 1;
	580	if (nmea0183_.Gsa.FixMode == GSA::TwoDimensional)
	581	gsaFrame_.mode_result = 2;
	582	if (nmea0183_.Gsa.FixMode == GSA::ThreeDimensional)
	583	gsaFrame_.mode_result = 3;
	584	for (indexGSA = 0 ; indexGSA<12 ; indexGSA++)
	585	gsaFrame_.SV_PRN[indexGSA] = nmea0183_.Gsa.SatelliteNumber[indexGSA];
	586	gsaFrame_.pdop = nmea0183_.Gsa.PDOP;
	587	gsaFrame_.hdop = nmea0183_.Gsa.HDOP;
	588	gsaFrame_.vdop = nmea0183_.Gsa.VDOP;
	589
	590	break;
	591
	592	case TRAME_GST:
	593	if (!nmea0183_.Gst.Parse( sentence ))
	594	qWarning("Failed to parse the frame %s\n",nmea0183_.Gst.ErrorMessage.toLatin1().data());
	595	gstFrame_.rms = nmea0183_.Gst.RMSvalue;
	596	gstFrame_.a = nmea0183_.Gst.ErrorEllipseMajor;
	597	gstFrame_.b = nmea0183_.Gst.ErrorEllipseMinor;
	598	gstFrame_.phi = nmea0183_.Gst.ErrorEllipseOrientation;
	599	gstFrame_.sigma_lat = nmea0183_.Gst.LatitudeError;
	600	gstFrame_.sigma_lon = nmea0183_.Gst.LongitudeError;
	601	gstFrame_.sigma_alt = nmea0183_.Gst.HeightError;
	602	gstFrame_.H = nmea0183_.Gst.Time.time().hour();
	603	gstFrame_.Mi = nmea0183_.Gst.Time.time().minute();
	604	gstFrame_.S = nmea0183_.Gst.Time.time().second();
	605	gstFrame_.Ms = nmea0183_.Gst.Time.time().msec();
	606
	607	break;
	608
	609	case TRAME_GSV:
	610	indexGSV = 0;
	611	if (!nmea0183_.Gsv.Parse( sentence ))
	612	qWarning("Failed to parse the frame %s\n",nmea0183_.Gsv.ErrorMessage.toLatin1().data());
	613	// it's a new frame, reset stored value in case of the number of satellites
	614	// in view has decreased
	615	if (nmea0183_.Gsv.message_number == 1)
	616	{
	617	while (indexGSV < 36)
	618	{
	619	gsvFrame_.SatellitesInView[ indexGSV ][ 0 ] = 0;
	620	gsvFrame_.SatellitesInView[ indexGSV ][ 1 ] = 0;
	621	gsvFrame_.SatellitesInView[ indexGSV ][ 2 ] = 0;
	622	gsvFrame_.SatellitesInView[ indexGSV ][ 3 ] = 0;
	623	indexGSV++;
	624	}
	625	}
	626	gsvFrame_.NumberOfSatellites = nmea0183_.Gsv.NumberOfSatellites;
	627	gsvFrame_.Totalmessages = nmea0183_.Gsv.Totalmessages;
	628
	629	for ( indexGSV=4(nmea0183_.Gsv.message_number-1); indexGSV<=(4nmea0183_.Gsv.message_number)-1; indexGSV++ )
	630	{
	631	gsvFrame_.SatellitesInView[ indexGSV ][ 0 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SatelliteNumber;
	632	gsvFrame_.SatellitesInView[ indexGSV ][ 1 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].ElevationDegrees;
	633	gsvFrame_.SatellitesInView[ indexGSV ][ 2 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].AzimuthDegreesTrue;
	634	gsvFrame_.SatellitesInView[ indexGSV ][ 3 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SignalToNoiseRatio;
	635	}
	636
	637	break;
	638
	639	case TRAME_HDT:
	640	if (!nmea0183_.Hdt.Parse( sentence ))
	641	qWarning("Failed to parse the frame %s\n",nmea0183_.Hdt.ErrorMessage.toLatin1().data());
	642	hdtFrame_.DegreesTrue = nmea0183_.Hdt.DegreesTrue;
	643
	644	break;
	645
	646	case TRAME_RMC:
	647	if (!nmea0183_.Rmc.Parse( sentence ))
	648	qWarning("Failed to parse the frame %s\n",nmea0183_.Rmc.ErrorMessage.toLatin1().data());
	649	rmcFrame_.H = nmea0183_.Rmc.Time.time().hour();
	650	rmcFrame_.Mi = nmea0183_.Rmc.Time.time().minute();
	651	rmcFrame_.S = nmea0183_.Rmc.Time.time().second();
	652	rmcFrame_.Ms = nmea0183_.Rmc.Time.time().msec();
	653	rmcFrame_.AA = nmea0183_.Rmc.Time.date().year();
	654	rmcFrame_.MM = nmea0183_.Rmc.Time.date().month();
	655	rmcFrame_.JJ = nmea0183_.Rmc.Time.date().day();
	656	rmcFrame_.lat = Geodesie::Deg2Rad(nmea0183_.Rmc.Position.Latitude.GetDecimalDegrees());
	657	rmcFrame_.dir_lat = ( (nmea0183_.Rmc.Position.Latitude.Northing == North) ? 'N' : 'S');
	658	rmcFrame_.lon = Geodesie::Deg2Rad(nmea0183_.Rmc.Position.Longitude.GetDecimalDegrees());
	659	rmcFrame_.dir_lon = ( (nmea0183_.Rmc.Position.Longitude.Easting == East) ? 'E' : 'W' );
	660	rmcFrame_.magnet_var = nmea0183_.Rmc.MagneticVariation;
	661	rmcFrame_.dir_magnet_var = ( (nmea0183_.Rmc.MagneticVariationDirection == East) ? 'E' : 'W');
	662	rmcFrame_.mode = -1;
	663	if (nmea0183_.Rmc.ModeIndication == "A")
	664	rmcFrame_.mode = 1;
	665	if (nmea0183_.Rmc.ModeIndication == "D")
	666	rmcFrame_.mode = 2;
	667	if (nmea0183_.Rmc.ModeIndication == "N")
	668	rmcFrame_.mode = 0;
	669	rmcFrame_.track_true_north = nmea0183_.Rmc.TrackMadeGoodDegreesTrue;
	670	rmcFrame_.valid_data = ( (nmea0183_.Rmc.IsDataValid == True) ? 1 : 0 );
	671	rmcFrame_.vitesse = nmea0183_.Rmc.SpeedOverGroundKnots * 1852.0 / 3600.0; // 1 knot = 1852 m/h
	672
	673	break;
	674
	675	case TRAME_ROT:
	676	if (!nmea0183_.Rot.Parse( sentence ))
	677	qWarning("Failed to parse the frame %s\n",nmea0183_.Rot.ErrorMessage.toLatin1().data());
	678	rotFrame_.RateOfTurn = nmea0183_.Rot.RateOfTurn;
	679	rotFrame_.valid_data = ( (nmea0183_.Rot.IsDataValid == True) ? 1 : 0 );
	680
	681	break;
	682
	683	case TRAME_VTG:
	684	if (!nmea0183_.Vtg.Parse( sentence ))
	685	qWarning("Failed to parse the frame %s\n",nmea0183_.Vtg.ErrorMessage.toLatin1().data());
	686	vtgFrame_.v = nmea0183_.Vtg.SpeedKilometersPerHour;
	687	vtgFrame_.track_true_north = nmea0183_.Vtg.TrackDegreesTrue;
	688	vtgFrame_.track_magnet_north = nmea0183_.Vtg.TrackDegreesMagnetic;
	689
	690	break;
	691
	692	case TRAME_ZDA:
	693	if (!nmea0183_.Zda.Parse( sentence ))
	694	qWarning("Failed to parse the frame %s\n",nmea0183_.Zda.ErrorMessage.toLatin1().data());
	695	zdaFrame_.H = nmea0183_.Zda.Time.time().hour();
	696	zdaFrame_.Mi = nmea0183_.Zda.Time.time().minute();
	697	zdaFrame_.S = nmea0183_.Zda.Time.time().second();
	698	zdaFrame_.Ms = nmea0183_.Zda.Time.time().msec();
	699	zdaFrame_.AA = nmea0183_.Zda.Time.date().year();
	700	zdaFrame_.MM = nmea0183_.Zda.Time.date().month();
	701	zdaFrame_.JJ = nmea0183_.Zda.Time.date().day();
	702	zdaFrame_.H_offset = nmea0183_.Zda.LocalHourDeviation;
	703	zdaFrame_.Mi_offset = nmea0183_.Zda.LocalMinutesDeviation;
	704
	705	break;
	706
	707	case TRAME_BESTGPSPOSA:
	708	stringList = sentence.Sentence.split(";");
	709	if (stringList.size()==2) {
	710	header = stringList.at(0);
	711	data = stringList.at(1);
	712
	713	if (mVerbose) {
	714	LOG_DEBUG("Header " << stringList.at(0));
	715	LOG_DEBUG("Data " << stringList.at(1));
	716	}
	717
	718	if (parseCPTbestgpsposa(data)) {
	719	if (mVerbose) {
	720	LOG_DEBUG("parsed TRAME_BESTGPSPOSA");
	721	}
	722	}
	723	}
	724	break;
	725
	726	case TRAME_RAWIMUSA:
	727	stringList = sentence.Sentence.split(";");
[111]	728	LOG_DEBUG(sentence.Sentence.toStdString());
[59]	729	if (stringList.size()==2) {
	730	header = stringList.at(0);
	731	data = stringList.at(1);
	732
	733	if (mVerbose) {
	734	LOG_DEBUG("Header " << stringList.at(0));
	735	LOG_DEBUG("Data " << stringList.at(1));
	736	}
	737
	738	if (parseCPTrawimusa(data)) {
	739	if (mVerbose) {
	740	LOG_DEBUG("parsed TRAME_RAWIMUSA");
	741	}
	742	}
	743	}
	744	break;
	745	case TRAME_INSPVAA:
	746	stringList = sentence.Sentence.split(";");
[111]	747	LOG_DEBUG(sentence.Sentence.toStdString());
[59]	748	if (stringList.size()==2) {
	749	header = stringList.at(0);
	750	data = stringList.at(1);
	751	if (mVerbose) {
	752	LOG_DEBUG("Header " << stringList.at(0));
	753	LOG_DEBUG("Data " << stringList.at(1));
	754	}
	755	if (parseCPTinspvaa(data)) {
	756	if (mVerbose) {
	757	LOG_DEBUG("parsed TRAME_INSPVAA");
	758	}
	759	}
	760	}
	761	break;
	762
	763	case TRAME_INSCOV:
	764	stringList = sentence.Sentence.split(";");
[111]	765	LOG_DEBUG(sentence.Sentence.toStdString());
[59]	766	if (stringList.size()==2) {
	767	header = stringList.at(0);
	768	data = stringList.at(1);
	769	if (mVerbose) {
	770	LOG_DEBUG("Header " << stringList.at(0));
	771	LOG_DEBUG("Data " << stringList.at(1));
	772	}
	773	if (parseCPTinscov(data)) {
	774	LOG_DEBUG("parsed TRAME_INSCOV");
	775	}
	776	}
	777	break;
	778
	779	default:
	780	LOG_ERROR("unknown frame type=" << type);
	781	return 0;
	782	}
	783
	784	return 1;
	785	}
	786
	787	int CPTComponent::parseCPTbestgpsposa(QString data)
	788	{
	789	double x=0.0;
	790	double y=0.0;
	791	double z=0.0;
	792
	793	QStringList stringList = data.split(",");
	794	if (21 == stringList.size()) {
	795	if (stringList.at(0).compare("SOL_COMPUTED")==0)
	796	bestgpsposaFrame_.Status = SOL_COMPUTED;
	797	else if (stringList.at(0).compare("INSUFFICIENT_OBS")==0)
	798	bestgpsposaFrame_.Status = INSUFFICIENT_OBS;
	799	else if (stringList.at(0).compare("NO_CONVERGENCE")==0)
	800	bestgpsposaFrame_.Status = INSUFFICIENT_OBS;
	801	else if (stringList.at(0).compare("SINGULARITY")==0)
	802	bestgpsposaFrame_.Status = SINGULARITY;
	803	else if (stringList.at(0).compare("COV_TRACE ")==0)
	804	bestgpsposaFrame_.Status = COV_TRACE;
	805	else if (stringList.at(0).compare("TEST_DIST")==0)
	806	bestgpsposaFrame_.Status = TEST_DIST;
	807	else if (stringList.at(0).compare("COLD_START")==0)
	808	bestgpsposaFrame_.Status = COLD_START;
	809	else if (stringList.at(0).compare("V_H_LIMIT")==0)
	810	bestgpsposaFrame_.Status = V_H_LIMIT;
	811	else if (stringList.at(0).compare("VARIANCE")==0)
	812	bestgpsposaFrame_.Status = VARIANCE;
	813	else if (stringList.at(0).compare("RESIDUALS")==0)
	814	bestgpsposaFrame_.Status = RESIDUALS;
	815	else if (stringList.at(0).compare("DELTA_POS")==0)
	816	bestgpsposaFrame_.Status = DELTA_POS;
	817	else if (stringList.at(0).compare("NEGATIVE_VAR")==0)
	818	bestgpsposaFrame_.Status = NEGATIVE_VAR;
	819	else if (stringList.at(0).compare("INTEGRITY_WARNING")==0)
	820	bestgpsposaFrame_.Status = INTEGRITY_WARNING;
	821	else if (stringList.at(0).compare("IMU_UNPLUGGED")==0)
	822	bestgpsposaFrame_.Status = IMU_UNPLUGGED;
	823	else if (stringList.at(0).compare("PENDING")==0)
	824	bestgpsposaFrame_.Status = PENDING;
	825	else {
	826	LOG_ERROR("cannot parse BESTGPSPOSA");
	827	return 0;
	828	}
	829
	830	if (stringList.at(1).compare("NONE")==0) {
	831	bestgpsposaFrame_.posType = NONE;
	832	} else if (stringList.at(1).compare("FIXEDPOS")==0) {
	833	bestgpsposaFrame_.posType = FIXEDPOS;
	834	} else if (stringList.at(1).compare("FIXEDHEIGHT")==0) {
	835	bestgpsposaFrame_.posType = FIXEDHEIGHT;
	836	} else if (stringList.at(1).compare("FLOATCONV")==0) {
	837	bestgpsposaFrame_.posType = FLOATCONV;
	838	} else if (stringList.at(1).compare("WIDELANE")==0) {
	839	bestgpsposaFrame_.posType = WIDELANE;
	840	} else if (stringList.at(1).compare("NARROWLANE")==0) {
	841	bestgpsposaFrame_.posType = NARROWLANE;
	842	} else if (stringList.at(1).compare("DOPPLER_VELOCITY")==0) {
	843	bestgpsposaFrame_.posType = DOPPLER_VELOCITY;
	844	} else if (stringList.at(1).compare("SINGLE")==0) {
	845	bestgpsposaFrame_.posType = SINGLE;
	846	} else if (stringList.at(1).compare("PSRDIFF")==0) {
	847	bestgpsposaFrame_.posType = PSRDIFF;
	848	} else if (stringList.at(1).compare("WAAS")==0) {
	849	bestgpsposaFrame_.posType = WAAS;
	850	} else if (stringList.at(1).compare("PROPAGATED")==0) {
	851	bestgpsposaFrame_.posType = PROPAGATED;
	852	} else if (stringList.at(1).compare("OMNISTAR")==0) {
	853	bestgpsposaFrame_.posType = OMNISTAR;
	854	} else if (stringList.at(1).compare("L1_FLOAT")==0) {
	855	bestgpsposaFrame_.posType = L1_FLOAT;
	856	} else if (stringList.at(1).compare("IONOFREE_FLOAT")==0) {
	857	bestgpsposaFrame_.posType = IONOFREE_FLOAT;
	858	} else if (stringList.at(1).compare("NARROW_FLOAT")==0) {
	859	bestgpsposaFrame_.posType = NARROW_FLOAT;
	860	} else if (stringList.at(1).compare("L1_INT")==0) {
	861	bestgpsposaFrame_.posType = L1_INT;
	862	} else if (stringList.at(1).compare("WIDE_INT")==0) {
	863	bestgpsposaFrame_.posType = WIDE_INT;
	864	} else if (stringList.at(1).compare("NARROW_INT")==0) {
	865	bestgpsposaFrame_.posType = NARROW_INT;
	866	} else if (stringList.at(1).compare("RTK_DIRECT_INS")==0) {
	867	bestgpsposaFrame_.posType = RTK_DIRECT_INS;
	868	} else if (stringList.at(1).compare("INS")==0) {
	869	bestgpsposaFrame_.posType = INS;
	870	} else if (stringList.at(1).compare("INS_PSRSP")==0) {
	871	bestgpsposaFrame_.posType = INS_PSRSP;
	872	} else if (stringList.at(1).compare("INS_PSRDIFF")==0) {
	873	bestgpsposaFrame_.posType = INS_PSRDIFF;
	874	} else if (stringList.at(1).compare("INS_RTKFLOAT")==0) {
	875	bestgpsposaFrame_.posType = INS_RTKFLOAT;
	876	} else if (stringList.at(1).compare("INS_RTKFIXED")==0) {
	877	bestgpsposaFrame_.posType = INS_RTKFIXED;
	878	} else if (stringList.at(1).compare("INS_OMNISTAR")==0) {
	879	bestgpsposaFrame_.posType = INS_OMNISTAR;
	880	} else if (stringList.at(1).compare("INS_OMNISTAR_HP")==0) {
	881	bestgpsposaFrame_.posType = INS_OMNISTAR_HP;
	882	} else if (stringList.at(1).compare("INS_OMNISTAR_XP")==0) {
	883	bestgpsposaFrame_.posType = INS_OMNISTAR_XP;
	884	} else if (stringList.at(1).compare("OMNISTAR_HP")==0) {
	885	bestgpsposaFrame_.posType = OMNISTAR_HP;
	886	} else if (stringList.at(1).compare("OMNISTAR_XP")==0) {
	887	bestgpsposaFrame_.posType = OMNISTAR_XP;
	888	} else if (stringList.at(1).compare("CDGPS")==0) {
	889	bestgpsposaFrame_.posType = CDGPS;
	890	} else {
	891	LOG_ERROR("cannot parse BESTGPSPOSA");
	892	return 0;
	893	}
	894
	895	bestgpsposaFrame_.Lat = stringList.at(2).toDouble();
	896	bestgpsposaFrame_.Lon = stringList.at(3).toDouble();
	897	bestgpsposaFrame_.Hgt = stringList.at(4).toDouble();
	898	bestgpsposaFrame_.Undulation = stringList.at(5).toFloat();
	899	bestgpsposaFrame_.LatStd = stringList.at(7).toFloat();
	900	bestgpsposaFrame_.LonStd = stringList.at(8).toFloat();
	901	bestgpsposaFrame_.HgtStd = stringList.at(9).toFloat();
	902
	903	// Geo to ECEF
	904	Geodesie::Geographique_2_ECEF(
	905	Geodesie::Deg2Rad(bestgpsposaFrame_.Lon), Geodesie::Deg2Rad(bestgpsposaFrame_.Lat), bestgpsposaFrame_.Hgt,
	906	x, y, z);
	907	// ECEF to ENU
	908	Geodesie::ECEF_2_ENU(x, y, z,
	909	bestgpsposaFrame_.e, bestgpsposaFrame_.n, bestgpsposaFrame_.u,
	910	Geodesie::Deg2Rad(LON_REF), Geodesie::Deg2Rad(LAT_REF), HE_REF);
	911	return 1;
	912	}
	913	return 0;
	914	}
	915
	916	int CPTComponent::parseCPTrawimusa(QString data)
	917	{
	918	QStringList stringList = data.split(",");
	919	QStringList stringList2;
	920	if (9 == stringList.size()) {
	921	rawimuFrame_.Week = stringList.at(0).toULong();
	922	rawimuFrame_.Seconds = stringList.at(1).toDouble();
	923
	924	rawimuFrame_.ZAccel = stringList.at(3).toLong();
	925	rawimuFrame_.YAccel = stringList.at(4).toLong();
	926	rawimuFrame_.XAccel = stringList.at(5).toLong();
	927
	928	rawimuFrame_.ZGyro = stringList.at(6).toLong();
	929	rawimuFrame_.YGyro = stringList.at(7).toLong();
	930
	931	stringList2 = stringList.at(8).split("*");
	932	if (2 == stringList2.size()) {
	933	rawimuFrame_.XGyro = stringList2.at(0).toLong();
	934	}
	935	return 1;
	936	}
	937	return 0;
	938	}
	939
	940	int CPTComponent::parseCPTinspvaa(QString data)
	941	{
	942	QStringList stringList = data.split(",");
	943	QStringList stringList2;
	944
	945	double x=0.0;
	946	double y=0.0;
	947	double z=0.0;
	948
	949	if (12 == stringList.size()) {
	950	inspvaFrame_.Week = stringList.at(0).toULong();
	951	inspvaFrame_.Seconds = stringList.at(1).toDouble();
	952	inspvaFrame_.Lat = stringList.at(2).toDouble();
	953	inspvaFrame_.Lon = stringList.at(3).toDouble();
	954	inspvaFrame_.Hgt = stringList.at(4).toDouble();
	955	inspvaFrame_.NorthVel = stringList.at(5).toFloat();
	956	inspvaFrame_.EastVel = stringList.at(6).toFloat();
	957	inspvaFrame_.UpVel = stringList.at(7).toFloat();
	958	inspvaFrame_.Roll = stringList.at(8).toFloat();
	959	inspvaFrame_.Pitch = stringList.at(9).toFloat();
	960	inspvaFrame_.Azimuth = stringList.at(10).toFloat();
	961
	962	// Geo to ECEF
	963	Geodesie::Geographique_2_ECEF(
	964	Geodesie::Deg2Rad(inspvaFrame_.Lon), Geodesie::Deg2Rad(inspvaFrame_.Lat), inspvaFrame_.Hgt,
	965	x, y, z);
	966	// ECEF to ENU
	967	Geodesie::ECEF_2_ENU(x, y, z,
	968	inspvaFrame_.e, inspvaFrame_.n, inspvaFrame_.u,
	969	Geodesie::Deg2Rad(LON_REF), Geodesie::Deg2Rad(LAT_REF), HE_REF);
	970
	971	stringList2 = stringList.at(11).split("*");
	972	if (2 == stringList2.size()) {
	973	if (stringList2.at(0).compare("INS_INACTIVE")==0)
	974	inspvaFrame_.Status = INS_INACTIVE;
	975	else if (stringList2.at(0).compare("INS_ALIGNING")==0)
	976	inspvaFrame_.Status = INS_ALIGNING;
	977	else if (stringList2.at(0).compare("INS_SOLUTION_NOT_GOOD")==0)
	978	inspvaFrame_.Status = INS_SOLUTION_NOT_GOOD;
	979	else if (stringList2.at(0).compare("INS_SOLUTION_GOOD")==0)
	980	inspvaFrame_.Status = INS_SOLUTION_GOOD;
	981	else if (stringList2.at(0).compare("INS_BAD_GPS_AGREEMENT")==0)
	982	inspvaFrame_.Status = INS_BAD_GPS_AGREEMENT;
	983	else if (stringList2.at(0).compare("INS_ALIGNMENT_COMPLETE")==0)
	984	inspvaFrame_.Status = INS_ALIGNMENT_COMPLETE;
	985
	986	return 1;
	987	}
	988	}
	989	return 0;
	990	}
	991
	992	int CPTComponent::parseCPTinscov(QString data)
	993	{
	994	QStringList stringList = data.split(",");
	995	QStringList stringList2;
	996	if (29 == stringList.size()) {
	997	inscovFrame_.Week = stringList.at(0).toULong();
	998	inscovFrame_.Seconds = stringList.at(1).toDouble();
	999	//PosCov
	1000	inscovFrame_.PosCov[0][0]=stringList.at(2).toDouble();
	1001	inscovFrame_.PosCov[0][1]=stringList.at(3).toDouble();
	1002	inscovFrame_.PosCov[0][2]=stringList.at(4).toDouble();
	1003
	1004	inscovFrame_.PosCov[1][0]=stringList.at(5).toDouble();
	1005	inscovFrame_.PosCov[1][1]=stringList.at(6).toDouble();
	1006	inscovFrame_.PosCov[1][2]=stringList.at(7).toDouble();
	1007
	1008	inscovFrame_.PosCov[2][0]=stringList.at(8).toDouble();
	1009	inscovFrame_.PosCov[2][1]=stringList.at(9).toDouble();
	1010	inscovFrame_.PosCov[2][2]=stringList.at(10).toDouble();
	1011	//AttCov
	1012	inscovFrame_.AttCov[0][0]=stringList.at(11).toDouble();
	1013	inscovFrame_.AttCov[0][1]=stringList.at(12).toDouble();
	1014	inscovFrame_.AttCov[0][2]=stringList.at(13).toDouble();
	1015
	1016	inscovFrame_.AttCov[1][0]=stringList.at(14).toDouble();
	1017	inscovFrame_.AttCov[1][1]=stringList.at(15).toDouble();
	1018	inscovFrame_.AttCov[1][2]=stringList.at(16).toDouble();
	1019
	1020	inscovFrame_.AttCov[2][0]=stringList.at(17).toDouble();
	1021	inscovFrame_.AttCov[2][1]=stringList.at(18).toDouble();
	1022	inscovFrame_.AttCov[2][2]=stringList.at(19).toDouble();
	1023	//VelCov
	1024	inscovFrame_.VelCov[0][0]=stringList.at(20).toDouble();
	1025	inscovFrame_.VelCov[0][1]=stringList.at(21).toDouble();
	1026	inscovFrame_.VelCov[0][2]=stringList.at(22).toDouble();
	1027
	1028	inscovFrame_.VelCov[1][0]=stringList.at(23).toDouble();
	1029	inscovFrame_.VelCov[1][1]=stringList.at(24).toDouble();
	1030	inscovFrame_.VelCov[1][2]=stringList.at(25).toDouble();
	1031
	1032	inscovFrame_.VelCov[2][0]=stringList.at(26).toDouble();
	1033	inscovFrame_.VelCov[2][1]=stringList.at(27).toDouble();
	1034
	1035	stringList2 = stringList.at(11).split("*");
	1036	if (2 == stringList2.size()) {
	1037	inscovFrame_.VelCov[2][2]=stringList2.at(0).toDouble();
	1038	return 1;
	1039	}
	1040	}
	1041	return 0;
	1042	}
	1043
	1044	void CPTComponent::initBuffer(TimestampedBestgpsposaFrame* a,
	1045	TimestampedRawimusaFrame* b,
	1046	TimestampedInspvaaFrame* c,
	1047	TimestampedInscovFrame* d)
	1048	{
	1049	a->time=0;
	1050	a->timerange=0;
	1051	a->frame.Status=INSUFFICIENT_OBS;
	1052	a->frame.posType=NONE;
	1053	a->frame.Lat=0.0;
	1054	a->frame.Lon=0.0;
	1055	a->frame.Hgt=0.0;
	1056	a->frame.Undulation=0.0;
	1057	a->frame.LatStd=0.0;
	1058	a->frame.LonStd=0.0;
	1059	a->frame.HgtStd=0.0;
	1060	a->frame.e=0.0;
	1061	a->frame.n=0.0;
	1062	a->frame.u=0.0;
	1063
	1064	b->time=0;
	1065	b->timerange=0;
	1066	b->frame.Week=0;
	1067	b->frame.Seconds=0.0;
	1068	b->frame.ZAccel=0.0;
	1069	b->frame.YAccel=0.0;
	1070	b->frame.XAccel=0.0;
	1071	b->frame.ZGyro=0.0;
	1072	b->frame.YGyro=0.0;
	1073	b->frame.XGyro=0.0;
	1074
	1075	c->time=0;
	1076	c->timerange=0;
	1077	c->frame.Week =0;
	1078	c->frame.Seconds=0.0;
	1079	c->frame.Lat=0.0;
	1080	c->frame.Lon=0.0;
	1081	c->frame.Hgt=0.0;
	1082	c->frame.NorthVel=0.0;
	1083	c->frame.EastVel=0.0;
	1084	c->frame.UpVel=0.0;
	1085	c->frame.Roll=0.0;
	1086	c->frame.Pitch=0.0;
	1087	c->frame.Azimuth=0.0;
	1088	c->frame.Status=INS_INACTIVE;
	1089	c->frame.e=0.0;
	1090	c->frame.n=0.0;
	1091	c->frame.u=0.0;
	1092
	1093	d->time=0;
	1094	d->timerange=0;
	1095	d->frame.Week=0;
	1096	d->frame.Seconds=0.0;
	1097	for (register int i=0;i<3;i++) {
	1098	for (register int j=0;j<3;j++) {
	1099	d->frame.PosCov[i][j]=0.0;
	1100	d->frame.AttCov[i][j]=0.0;
	1101	d->frame.VelCov[i][j]=0.0;
	1102	}
	1103	}
	1104	}
	1105
	1106	} // namespace pacpus

Note: See TracBrowser for help on using the repository browser.

Context Navigation

source: pacpussensors/trunk/SpanCPTComponent/CPTComponent.cpp@ 125

Download in other formats: