gps.cpp

这是一个GPS接收定位的原代码

#include "gps.h"
#include <errno.h>
#include <unistd.h>
#include <math.h>
#include <fcntl.h>
#include <termios.h>
#include <time.h>


// Defining instances
GPS *Gps;
list_of<highlight> highlightlist;

void Mapwindow::mousePressEvent(QMouseEvent *mep){
	// Mouse clicked somewhere. Get the coords ...
	// If a map is showing, move within it
	if(mymapinfo){
		// Position to this location, using mostly the positioning logic
		Position pos(mappos.latit - (mep->pos().y()-height()/2)/mymapinfo->pixperdegheight,
			mappos.longit + (mep->pos().x()-width()/2)/mymapinfo->pixperdegwidth);
		positionto(pos);
		updateGPSpos(pos);
	}else{
		// This probably highly bogus - need to think through what it
		// means if we have no visible map
		picoffset.setX(picoffset.x()+mep->pos().x()-width()/2);
		picoffset.setY(picoffset.y()+mep->pos().y()-height()/2);
		update();
	}
}

void Mapwindow::load(const char *cp){
	if(currentmapname != cp){
		if(mymap.load(cp)){
			if(verbose) printf("Just loaded map %s\n", cp);
			currentmapname = cp;
		}else{
			if(verbose) printf("No map loaded\n", cp);
			currentmapname = String::NULLSTRING;
		}
	}
}


// Return a heading in degrees, given x/y lengths travelled
double Mapwindow::yxheading(double northdiff, double eastdiff){
	double heading;
	if(fabs(eastdiff) < 1e-9)
		heading = northdiff >= 0 ? 0 : 180;
	else{
		heading =  (atan2(eastdiff,northdiff)/deg2rad+0.5);
		if(heading < 0) heading = 360+heading;
	}
   return heading;
}

// Given Lat/Long & time differences, maintain a smoothed velocity (speed/heading)
int Mapwindow::plotspeed(const Position &cpr, double utctime){
	// approximation to speed over ground. Uses rough-and-ready
	// adjustment for longitude based on cos of current latitude
	// which seems to work OK in the UK

	double milesperNdegree;
	double milesperEdegree;

	// How many metres is one degree of latitude (should be the same over
	// whole globe near enough, so pick one that works in the OSGB range)
	capm(49, 50, &milesperNdegree);
	const double coslat = cos(cpr.latit*deg2rad);
	milesperNdegree *= metres2miles;
	milesperEdegree = milesperNdegree*coslat;

	// printf("plotspeed, utctime %f\n", utctime);

	if(!firstplot){
		double tdiff = utctime-lastplottime;
		if(tdiff == 0)
			return 0;

		if(tdiff < 0) tdiff += 24L*60*60;

		double latdiffrate = (cpr.latit-lastplotpos.latit)/tdiff;
		double longdiffrate = (cpr.longit-lastplotpos.longit)/tdiff;
		double heading = 0;
		// printf("In plotspeed, time difference is %f, latdiffrate %f, longdiffrate %f\n", tdiff, latdiffrate, longdiffrate);

		for(int i = 0; i < npoles; i++){
			xvfilter[i] += fconst * (latdiffrate-xvfilter[i]);
			latdiffrate = xvfilter[i];
			yvfilter[i] += fconst * (longdiffrate-yvfilter[i]);
			longdiffrate = yvfilter[i];
			if(!finite(latdiffrate) || !finite(longdiffrate)){
				printf("Gone non-finite!\n");
				exit(0);
			}
		}
		heading = yxheading(latdiffrate, longdiffrate*coslat);
		double ymps = latdiffrate * milesperNdegree;
		double xmps = longdiffrate * milesperEdegree;
		double mps = sqrt(ymps*ymps + xmps*xmps);
		int mph = int(mps*60*60 + 0.5);
		// printf("Smoothed: latdiffrate %f, longdiffrate %f, heading %f, speed %d\n", latdiffrate, longdiffrate, heading, mph);
		String s;
		s.sprintf("%d MPH", mph);
		Gps->speed->setText(s.cstring());
		s.sprintf("%.0f deg", heading);
		Gps->hdng->setText(s.cstring());
	
	}

	lastplotpos = cpr;
	lastplottime = utctime;
	firstplot = false;
	return 0;
}

void Mapwindow::updateGPSpos(const Position &cpr){
	Gps->mymousey->setText(dmsLat(cpr.latit).cstring());
	Gps->mymousex->setText(dmsLong(cpr.longit).cstring());
	String oslocation;
	double e, n;
	if(mymapinfo->osproj && cvtongr(cpr.latit, cpr.longit, oslocation, e, n)){
		Gps->altpos->setText(oslocation.cstring());
		Gps->altpos->show();
	}else{
		Gps->altpos->setText("");
		Gps->altpos->hide();
	}
}

void Mapwindow::mouseMoveEvent(QMouseEvent *mev){
	QPoint p = mev->pos();
	char buff[512];
	// If no map showing, just show x,y else show lat/long
	if(mymapinfo == 0){
		p.setX(p.x()+picoffset.x()-width()/2);
		p.setY(p.y()+picoffset.y()-height()/2);
		sprintf(buff, "x%d", p.x());
		Gps->mymousex->setText(buff);
		sprintf(buff, "y%d", p.y());
		Gps->mymousey->setText(buff);
	}else{
		Position pos(
			mappos.latit - (p.y()-height()/2)/mymapinfo->pixperdegheight,
			mappos.longit + (p.x()-width()/2)/mymapinfo->pixperdegwidth
		);
		updateGPSpos(pos);

		// DEBUGGING
		// double e, n;	// current east & north
		// String dummy;
		// if(cvtongr(latit, longit, dummy, e, n)){
			// printf("Mousemove for lat %f long %f gives %s\n", latit, longit, dummy.cstring());
		// }
	}
}


void Mapwindow::drawoffsetmap(){
	int xoffset = -picoffset.x() + width()/2;
	int yoffset = -picoffset.y() + height()/2;
	bitBlt(this, xoffset, yoffset, &mymap);
}

void Mapwindow::paintEvent(QPaintEvent *){

	// If we haven't got a real map visible, don't draw it
	if(mymapinfo != &mydummymapinfo){
		// Put the map at the right place
		drawoffsetmap();
	}
	
	QPainter qp(this);
	QPen wblackpen(black, 3);
	QPen nblackpen(black, 1);
	//qp.begin(this);

	// Draw the crosshairs
	qp.setPen(wblackpen);
	const midx = width()/2;
	const midy = height()/2;
	const crosslen = max(width(), height()) / 10;

	qp.drawLine(midx-crosslen*2, midy, midx-crosslen/3, midy);
	qp.drawLine(midx+crosslen*2, midy, midx+crosslen/3, midy);
	qp.drawLine(midx, midy-crosslen*2, midx, midy-crosslen/3);
	qp.drawLine(midx, midy+crosslen*2, midx, midy+crosslen/3);

	qp.setPen(nblackpen);
	qp.drawLine(midx-crosslen*2, midy, midx+crosslen*2, midy);
	qp.drawLine(midx, midy-crosslen*2, midx, midy+crosslen*2);

	// Any highlights to show?
	QPen redpen(red, 2);
	qp.setPen(redpen);
	qp.setRasterOp(NotXorROP);
	qp.setBackgroundMode(OpaqueMode);

	if(mymapinfo && llvalid){
		double minlat, maxlat, minlong, maxlong;
		minlat = mappos.latit - height()/2/mymapinfo->pixperdegheight;
		maxlat = mappos.latit + height()/2/mymapinfo->pixperdegheight;
		minlong = mappos.longit - width()/2/mymapinfo->pixperdegwidth;
		maxlong = mappos.longit + width()/2/mymapinfo->pixperdegwidth;
		// printf("minlong %s maxlong %s minlat %s maxlat %s\n", dmsLong(minlong).cstring(), dmsLong(maxlong).cstring(), dmsLat(minlat).cstring(), dmsLat(maxlat).cstring());
		// Scan the list of highlights for those positioned on the current map
		for(int i = 0; i < highlightlist.element_count(); i++){
			const highlight &chp = highlightlist[i];
			if(chp.pos.latit > minlat && chp.pos.latit < maxlat && chp.pos.longit > minlong &&
chp.pos.longit < maxlong){
				QPoint centre = mappixel_to_displaypixels(pos_to_mappixels(chp.pos));
				const esize = 8; // should be divisble by 2
				qp.drawEllipse(centre.x()-esize/2, centre.y()-esize/2, esize, esize);
				qp.drawText(centre.x()+esize/2+2, centre.y()+esize/2, chp.description.cstring(), chp.description.nchars());
				if(verbose)printf("Found highlight in range, name %s, x %d y %d\n", chp.description.cstring(), centre.x(), centre.y());
			}
		}
	}

	// Redraw the track, ensuring that lasttrackpoint is correctly set
	QPen bluepen(blue, 2);
	qp.setPen(bluepen);
	for(int i = 0; i < track.element_count(); i++){
		QPoint thismappoint = pos_to_mappixels(track[i]);
		if(i > 0){
			qp.drawLine(mappixel_to_displaypixels(lasttrackpoint), mappixel_to_displaypixels(thismappoint));
		}
		lasttrackpoint = thismappoint;
	}


	qp.setRasterOp(CopyROP);
	drawFrame(&qp);
	qp.end();
}


Mapwindow::Mapwindow(GPS *gpsp): QFrame(gpsp), myGpsp(gpsp), picoffset(0,0), llvalid(false), mymapinfo(0), usemapscale(1e6){
	setCursor(crossCursor);
	setMouseTracking(true);
	cleartrack();
	firstplot = true;
}

void Mapwindow::cleartrack(){
	track.clear();
	for(int i = 0; i < npoles; i++){
		xvfilter[i] = yvfilter[i] = 0;
	}
}

void Mapwindow::trackto(double latit, double longit, double utctime, int offcentrerange){

	// printf("Trackto %f %f\n", latit, longit);

	if(track.element_count() >= maxtrack){
		// Too big. Throw away first item
		track.delete_this_element(0);
	}
	Position p(latit, longit);
	track += p;

	updateGPSpos(p);
	plotspeed(p, utctime);

	long secs = long(utctime);
	// int hundredths = int((utctime-secs)*100);
	String utcs;
	utcs.sprintf("%.2ld:%.2ld:%.2ld UTC", secs / 3600, secs % 3600 / 60, secs %60);
	Gps->utc->setText(utcs.cstring());

	// If this is the first track position, get the right map
	if(track.element_count() == 1){
		positionto(p);
	}

	// On the grounds that this may one day throw an
	// exception, let's be ready
	try{
		QPoint qp = pos_to_mappixels(p);

		// If this is the first track position, don't try to draw any track
		if(track.element_count() == 1){
			lasttrackpoint = qp;
			return;
		}

		if(qp != picoffset){

			// A different point ... make something of it
			int xdiff = picoffset.x() - qp.x();
			int ydiff = picoffset.y() - qp.y();
			if(xdiff*xdiff + ydiff*ydiff > offcentrerange*offcentrerange){
				// printf("Redraw entire track\n");
				// This triggers a repaint and a full track redraw
				positionto(p);
			}else{
				QPainter qptr;
				QPen pen(blue, 2);
				qptr.begin(this);
				qptr.setPen(pen);
				qptr.setRasterOp(NotXorROP);

				// Just draw from previous to current
				qptr.drawLine(mappixel_to_displaypixels(lasttrackpoint), mappixel_to_displaypixels(qp));
				lasttrackpoint = qp;
				qptr.end();
			}
		}
	}
	catch(char *cp){
		printwhinge("Mapwindow::trackto caught exception %s", cp);
	}
}

// Highly bogus function - should check to see if the position is mappable and
// throw an exception if not, but I haven't figured out what 'mappable' means,
// but a start has been made
QPoint Mapwindow::pos_to_mappixels(const Position &cpr){
	int xcoord, ycoord;
	if(mymapinfo == 0){
		throw("Mapwindow::pos_to_mappixels called with no map valid");
	}

	if(mymapinfo->osproj){
		double e, n;	// current east & north
		String dummy;
		if(cvtongr(cpr.latit, cpr.longit, dummy, e, n) == 0){
			printf("Mapwindow::pos_to_mappixels requests lat %f long %f in OSGB mode, but not convertible\n", cpr.latit, cpr.longit);
			// What should we do here?
			throw("Mapwindow::pos_to_mappixels: could not convert position to OSGB");
		}
		xcoord = int((e-mymapinfo->eastmin)*mymapinfo->pixpermetrewidth+0.5);
		ycoord = int((mymapinfo->northmax-n)*mymapinfo->pixpermetreheight+0.5);
	}else{
		xcoord = int((cpr.longit-mymapinfo->longmin)*mymapinfo->pixperdegwidth+0.5);
		ycoord = int((mymapinfo->latmax-cpr.latit)*mymapinfo->pixperdegheight+0.5);
		// printf("pos_to_mappixels for non-OS-projection of %f %f gives %d %d\n", cpr.latit, cpr.longit, xcoord, ycoord);
	}
	return QPoint(xcoord, ycoord);
}

// And now turn a mappixel into a displaypixel
QPoint Mapwindow::mappixel_to_displaypixels(const QPoint &cpr){
	return QPoint(cpr.x()-picoffset.x()+width()/2, cpr.y()-picoffset.y()+height()/2);
}

void Mapwindow::setpreferredmapscale(double s){
	usemapscale = s;
	// Call positionto if we have a current position.
	// This locates the nearest map and forces a redraw
	// of everything
	if(llvalid){
		positionto(mappos);
	}
}

bool Mapwindow::positionto(const Position &cpr){
	mappos=cpr; llvalid = true;

	locatescale(usemapscale);
	mymapinfo = locatemapinfo(cpr.latit, cpr.longit);

	if(verbose) printf("Positionto lat%f long%f foundmap=%s\n", cpr.latit, cpr.longit, mymapinfo != 0 ?  mymapinfo->name.cstring() : "None");
	if(mymapinfo){
		enumeratescales(myGpsp->mapscales);
		// printf("Look for map scales\n");
		// for(int i = 0; i < myGpsp->mapscales.element_count(); i++) printf("Found map scale %f\n", myGpsp->mapscales[i]);
		// Tell the outer thingy to show the available scales and check
		// the relevant one
		myGpsp->showmapscales(mymapinfo);




		if(verbose){
			printf("Found map %s, nscale %f escale %f\n", mymapinfo->name.cstring(), mymapinfo->pixperdegheight, mymapinfo->pixperdegwidth);
			mymapinfo->print();
		}