rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

HITL/IL2: Fixed calculation of AttitudeRaw.accel[XYZ]

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3025 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
corvus 2011-03-11 20:16:31 +00:00 committed by corvus
parent e1eaf4d132
commit 9859829fcc
3 changed files with 35 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
View File

@ -218,4 +218,22 @@ void CoordinateConversions::RPY2Quaternion(const float rpy[3], float q[4])
}
}
//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void CoordinateConversions::Quaternion2R(const float q[4], float Rbe[3][3])
{
float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
Rbe[0][0] = q0s + q1s - q2s - q3s;
Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]);
Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]);
Rbe[1][1] = q0s - q1s + q2s - q3s;
Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]);
Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]);
Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]);
Rbe[2][2] = q0s - q1s - q2s + q3s;
}
}

1
ground/openpilotgcs/src/libs/utils/coordinateconversions.h
View File

@ -48,6 +48,7 @@ public:
void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
void Quaternion2RPY(const float q[4], float rpy[3]);
void RPY2Quaternion(const float rpy[3], float q[4]);
void Quaternion2R(const float q[4], float Rbe[3][3]);
};
}

24
ground/openpilotgcs/src/plugins/hitlnew/il2simulator.cpp
View File

@ -272,18 +272,26 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
memset(&velData, 0, sizeof(VelocityActual::DataFields));
velData.North = current.dY*100;
velData.East = current.dX*100;
velData.Down = current.dZ*100;
velData.Down = current.dZ*-100;
// Update AttitudeRaw object (filtered gyros only for now)
// Update AttitudeRaw object (filtered gyros and accels only for now)
AttitudeRaw::DataFields rawData;
memset(&rawData, 0, sizeof(AttitudeRaw::DataFields));
rawData = attRaw->getData();
rawData.gyros[0] = current.dRoll;
rawData.gyros[1] = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
rawData.gyros[2] = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
rawData.accels[0] = current.ddX;
rawData.accels[1] = current.ddY;
rawData.accels[2] = current.ddZ;
// rotate turn rates and accelerations into body frame
// (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!)
rawData.gyros[0] = current.dRoll;
rawData.gyros[1] = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
rawData.gyros[2] = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;
// accels are in NED and can be converted using standard ned->local rotation matrix
float Rbe[3][3];
Utils::CoordinateConversions().Quaternion2R(quat,Rbe);
for (int t=0;t<3;t++) {
rawData.accels[t]=current.ddX*Rbe[t][0]
+current.ddY*Rbe[t][1]
+(current.ddZ+9.81)*Rbe[t][2];
}
rawData.accels[2]=-rawData.accels[2];
// Update homelocation
HomeLocation::DataFields homeData;