Updated Hitl module and FlightGear to Provide VelocityActual.

Tested with FlightGear 2.0, But should not break 1.9.1, please provide feedback if anithing odd happens.


git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1923 ebee16cc-31ac-478f-84a7-5cbb03baadba

ground/src/plugins/hitlnew/fgsimulator.cpp

@@ -33,6 +33,8 @@
 const float FGSimulator::FT2M = 0.3048;
 const float FGSimulator::KT2MPS = 0.514444444;
 const float FGSimulator::INHG2KPA = 3.386;
+const float FGSimulator::FPS2CMPS = 30.48;
+
 
 //FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) :
 //		Simulator(hostAddr, outPort, inPort,  manual, binPath, dataPath),
@@ -102,11 +104,11 @@ bool FGSimulator::setupProcess()
 	// Note: The input generic protocol is set to update at a much higher rate than the actual updates are sent by the GCS.
 	// If this is not done then a lag will be introduced by FlightGear, likelly because the receive socket buffer builds up during startup.
         QString args("--fg-root=\"" + settings.dataPath + "\" " +
-                     "--timeofday=dusk " +
+                     "--timeofday=noon " +
                      "--httpd=5400 " +
                      "--enable-hud " +
                      "--in-air " +
-                     "--altitude=2000 " +
+                     "--altitude=3000 " +
                      "--vc=100 " +
                      "--generic=socket,out,50,localhost," + QString::number(settings.inPort) + ",udp,opfgprotocol");
 	if(!settings.manual)
@@ -198,7 +200,21 @@ void FGSimulator::processUpdate(const QByteArray& inp)
 	float temperature = fields[19].toFloat();
 	// Get pressure (kpa)
 	float pressure = fields[20].toFloat() * INHG2KPA;
-
+	// Get VelocityActual Down (cm/s)
+	float velocityActualDown = fields[21].toFloat() * FPS2CMPS;
+	// Get VelocityActual East (cm/s)
+	float velocityActualEast = fields[22].toFloat() * FPS2CMPS;	
+	// Get VelocityActual Down (cm/s)
+	float velocityActualNorth = fields[23].toFloat() * FPS2CMPS;
+	
+	// Update VelocityActual.{Nort,East,Down}
+	VelocityActual::DataFields velocityActualData;
+	memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
+	velocityActualData.North = velocityActualNorth;
+	velocityActualData.East = velocityActualEast;
+	velocityActualData.Down = velocityActualDown;
+	velActual->setData(velocityActualData);
+	
 	// Update AltitudeActual object
         BaroAltitude::DataFields altActualData;
         memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));

ground/src/plugins/hitlnew/fgsimulator.h

@@ -50,6 +50,7 @@ private:
     static const float FT2M;
     static const float KT2MPS;
     static const float INHG2KPA;
+    static const float FPS2CMPS;
 
 	void processUpdate(const QByteArray& data);
 };

ground/src/plugins/hitlnew/isimulator.h

@@ -11,6 +11,7 @@
 #include "uavobjects/actuatordesired.h"
 #include "uavobjects/altitudeactual.h"
 #include "uavobjects/attitudeactual.h"
+#include "uavobjects/velocityactual.h"
 #include "uavobjects/positionactual.h"
 #include "uavobjects/gcstelemetrystats.h"
 
@@ -49,6 +50,7 @@ private:
 	QUdpSocket* outSocket;
 	ActuatorDesired* actDesired;
 	AltitudeActual* altActual;
+	VelocityActual* velActual;
 	AttitudeActual* attActual;
 	PositionActual* posActual;
 	GCSTelemetryStats* telStats;

ground/src/plugins/hitlnew/opfgprotocol.xml

@@ -182,15 +182,29 @@
       </chunk>
 
       <chunk>
-         <name>Temperature</name>
-         <node>/environment/temperature-degc</node>
+         <name>Pressure</name>
+         <node>/environment/pressure-inhg</node>
          <type>float</type>
          <format>%f</format>
       </chunk>
 
       <chunk>
-         <name>Pressure</name>
-         <node>/environment/pressure-inhg</node>
+         <name>velocityActualDown</name>
+         <node>velocities/speed-down-fps</node>
+         <type>float</type>
+         <format>%f</format>
+      </chunk>
+
+      <chunk>
+         <name>velocityActualEast</name>
+         <node>velocities/speed-east-fps</node>
+         <type>float</type>
+         <format>%f</format>
+      </chunk>
+
+      <chunk>
+         <name>velocityActualNorth</name>
+         <node>velocities/speed-north-fps</node>
          <type>float</type>
          <format>%f</format>
       </chunk>

ground/src/plugins/hitlnew/simulator.cpp

@@ -116,6 +116,7 @@ void Simulator::onStart()
 	ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance();
 	UAVObjectManager* objManager = pm->getObject<UAVObjectManager>();
 	actDesired = ActuatorDesired::GetInstance(objManager);
+	velActual = VelocityActual::GetInstance(objManager);
         altActual = BaroAltitude::GetInstance(objManager);
 	attActual = AttitudeActual::GetInstance(objManager);
         gpsPos = GPSPosition::GetInstance(objManager);

ground/src/plugins/hitlnew/simulator.h

@@ -37,6 +37,7 @@
 #include "uavobjects/uavobjectmanager.h"
 #include "uavobjects/actuatordesired.h"
 // #include "uavobjects/altitudeactual.h"
+#include "uavobjects/velocityactual.h"
 #include "uavobjects/baroaltitude.h"
 #include "uavobjects/attitudeactual.h"
 #include "uavobjects/gpsposition.h"
@@ -145,6 +146,7 @@ protected:
 	ActuatorDesired* actDesired;
         BaroAltitude* altActual;
 	AttitudeActual* attActual;
+	VelocityActual* velActual;
         GPSPosition* gpsPos;
 	GCSTelemetryStats* telStats;