Autopilot-CVS: downlink/ground/src main.cpp,1.18,1.19

[Dennis D'A. <den...@us...>]

Update of /cvsroot/autopilot/downlink/ground/src
In directory sc8-pr-cvs1:/tmp/cvs-serv6747

Modified Files:
	main.cpp 
Log Message:
Added conversion from NED to latitude/longitude as per Aaron's algorithm.  GSC is now initialized to lat/long and vehicle moves relative to intialized position.


Index: main.cpp
===================================================================
RCS file: /cvsroot/autopilot/downlink/ground/src/main.cpp,v
retrieving revision 1.18
retrieving revision 1.19
diff -u -d -r1.18 -r1.19
--- main.cpp	7 Feb 2003 04:34:12 -0000	1.18
+++ main.cpp	27 Feb 2003 01:23:47 -0000	1.19
@@ -41,6 +41,7 @@
 #include "state/udp.h"
 #include "mat/Conversions.h"
 
+
 using namespace std;
 using namespace libstate;
 using namespace util;
@@ -52,6 +53,7 @@
 int				server_port	= 2002;
 int				udp_fd		= -1;
 int				packets		= 0;
+static const double	pi		= 3.141519;
 
 
 static void
@@ -173,6 +175,16 @@
 {
 	state_t			state;
 	
+	double 			a=6378137.0;
+	double			lat;
+	double			lon;
+	double			alt;
+	
+	// starting position (474 Wando Park Blvd)
+	double			lat0=32.8315392;
+	double			lon0=-79.8542328;
+	double			alt0=3;
+	
 	gui->packets->value( packets++ );
 
 	//send joystick
@@ -204,6 +216,24 @@
 	gui->rates_q->value( state.q );
 	gui->rates_r->value( state.r );
 
+	// Aaron's method from email
+	// TODO: move to Nav.cpp as Vector<3> NED2lla(Vector<3>)
+	//
+	// X=N points towards the north pole
+	// Y=E towards the east
+	// Z=D towards the earth center
+
+	// convert starting position to radians
+	lat0=(lat0*pi)/180;
+	lon0=(lon0*pi)/180;
+
+	lat = state.x/(a-state.z) + lat0;
+	lon = state.y/((a-state.z)*cos(lat))+lon0;
+	alt = alt0 - state.z;
+	
+	gui->latitude->value((lat*180)/pi);
+	gui->longitude->value((lon*180)/pi);
+	gui->altitude_llh->value(alt);
 
 	// Setup the artificial horizon object
 	Horizon *		ai = gui->horizon;
@@ -212,10 +242,11 @@
 	ai->speed	= sqrt( sqr(state.vx) + sqr(state.vy) + sqr(state.vz) );
 	ai->message	= "MAN";
 
-
 	double angles[3] = { state.phi, state.theta, state.psi };
 
 	update_angles( angles );
+
+	return;
 }
 
 
@@ -355,6 +386,7 @@
 	void
 )
 {
+	gui->guiJoyID->value(0);
 	gui->rollAxes->value(0);
 	gui->pitchAxes->value(1);
 	gui->collAxes->value(2);