rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-169: HiTL - il2simulator now updates attitudeRaw

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1968 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
corvus 2010-10-16 20:04:46 +00:00 committed by corvus
parent ebae3ffa40
commit c067552f74
2 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ground/src/plugins/hitlnew/il2simulator.cpp
View File

@ -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();
}

10
ground/src/plugins/hitlnew/simulator.h
View File

@ -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;