From c067552f74ada3954b92b364d1e770573bfcb8cf Mon Sep 17 00:00:00 2001
From: corvus <corvus>
Date: Sat, 16 Oct 2010 20:04:46 +0000
Subject: [PATCH] OP-169: HiTL - il2simulator now updates attitudeRaw

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1968 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 ground/src/plugins/hitlnew/il2simulator.cpp | 20 ++++++++++++++++++++
 ground/src/plugins/hitlnew/simulator.h      | 10 ++++++++++
 2 files changed, 30 insertions(+)

diff --git a/ground/src/plugins/hitlnew/il2simulator.cpp b/ground/src/plugins/hitlnew/il2simulator.cpp
index 3266d338f..d6208cfe0 100644
--- a/ground/src/plugins/hitlnew/il2simulator.cpp
+++ b/ground/src/plugins/hitlnew/il2simulator.cpp
@@ -209,6 +209,18 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
 	current.X = old.X + (current.dX*current.dT);
 	current.Y = old.Y + (current.dY*current.dT);
 
+	// accelerations (filtered)
+#define SPEED_FILTER 2
+	current.ddX = ((current.dX-old.dX) + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER+1);
+	current.ddY = ((current.dY-old.dY) + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER+1);
+	current.ddZ = ((current.dZ-old.dZ) + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER+1);
+
+#define TURN_FILTER 2
+	// turn speeds (filtered)
+	current.dAzimuth = ((current.azimuth-old.azimuth) + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER+1);
+	current.dPitch   = ((current.pitch-old.pitch)     + TURN_FILTER * (old.dPitch))   / (TURN_FILTER+1);
+	current.dRoll    = ((current.roll-old.roll)       + TURN_FILTER * (old.dRoll))    / (TURN_FILTER+1);
+
 	// Update AltitudeActual object
 	BaroAltitude::DataFields altActualData;
 	memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
@@ -243,6 +255,13 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
 	velData.Down = current.dZ*100;
 	velActual->setData(velData);
 
+	// Update AttitudeRaw object (filtered gyros only for now)
+	AttitudeRaw::DataFields rawData;
+	rawData.gyros_filtered[0] = current.dRoll;
+	rawData.gyros_filtered[1] = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
+	rawData.gyros_filtered[2] = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
+	attRaw->setData(rawData);
+
 	// Update homelocation
 	HomeLocation::DataFields homeData;
 	homeData.Latitude = settings.latitude.toFloat() * 10e6;
@@ -287,5 +306,6 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
 	velActual->updated();
         posHome->updated();
         gpsPos->updated();
+        attRaw->updated();
 }
 
diff --git a/ground/src/plugins/hitlnew/simulator.h b/ground/src/plugins/hitlnew/simulator.h
index 64dbd06d6..ae02bd1d7 100644
--- a/ground/src/plugins/hitlnew/simulator.h
+++ b/ground/src/plugins/hitlnew/simulator.h
@@ -72,10 +72,20 @@
 	float dY;
 	float dZ;
 
+	// acceleration (absolute)
+	float ddX;
+	float ddY;
+	float ddZ;
+
 	//angle
 	float azimuth;
 	float pitch;
 	float roll;
+	
+	//rotation speed
+	float dAzimuth;
+	float dPitch;
+	float dRoll;
 
 } FLIGHT_PARAM;