mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
HITL/IL2: Fixed calculation of AttitudeRaw.accel[XYZ]
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3025 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
parent
e1eaf4d132
commit
9859829fcc
@ -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;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
@ -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]);
|
||||
};
|
||||
|
||||
}
|
||||
|
@ -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;
|
||||
|
Loading…
x
Reference in New Issue
Block a user