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

IL2 add quaternion attitude and accels

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2802 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
corvus 2011-02-17 01:51:45 +00:00 committed by corvus
parent 949617e236
commit a91a35685a
3 changed files with 66 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -167,4 +167,55 @@ void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rn
NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
}
// ****** find roll, pitch, yaw from quaternion ********
void CoordinateConversions::Quaternion2RPY(const float q[4], float rpy[3])
{
float R13, R11, R12, R23, R33;
float q0s = q[0] * q[0];
float q1s = q[1] * q[1];
float q2s = q[2] * q[2];
float q3s = q[3] * q[3];
R13 = 2 * (q[1] * q[3] - q[0] * q[2]);
R11 = q0s + q1s - q2s - q3s;
R12 = 2 * (q[1] * q[2] + q[0] * q[3]);
R23 = 2 * (q[2] * q[3] + q[0] * q[1]);
R33 = q0s - q1s - q2s + q3s;
rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
rpy[2] = RAD2DEG * atan2f(R12, R11);
rpy[0] = RAD2DEG * atan2f(R23, R33);
//TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2
}
// ****** find quaternion from roll, pitch, yaw ********
void CoordinateConversions::RPY2Quaternion(const float rpy[3], float q[4])
{
float phi, theta, psi;
float cphi, sphi, ctheta, stheta, cpsi, spsi;
phi = DEG2RAD * rpy[0] / 2;
theta = DEG2RAD * rpy[1] / 2;
psi = DEG2RAD * rpy[2] / 2;
cphi = cosf(phi);
sphi = sinf(phi);
ctheta = cosf(theta);
stheta = sinf(theta);
cpsi = cosf(psi);
spsi = sinf(psi);
q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi;
q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi;
q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi;
q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi;
if (q[0] < 0) { // q0 always positive for uniqueness
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
}
}

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

@ -46,6 +46,8 @@ public:
void LLA2ECEF(double LLA[3], double ECEF[3]);
int ECEF2LLA(double ECEF[3], double LLA[3]);
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]);
};
}

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

@ -249,10 +249,16 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
attActualData.Roll = current.roll;
attActualData.Pitch = current.pitch;
attActualData.Yaw = current.azimuth;
attActualData.q1 = 0;
attActualData.q2 = 0;
attActualData.q3 = 0;
attActualData.q4 = 0;
float rpy[3];
float quat[4];
rpy[0]=current.roll;
rpy[1]=current.pitch;
rpy[2]=current.yaw;
Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
attActualData.q1 = quat[0];
attActualData.q2 = quat[1];
attActualData.q3 = quat[2];
attActualData.q4 = quat[3];
// Update positionActual objects
PositionActual::DataFields posData;
@ -275,6 +281,9 @@ void IL2Simulator::processUpdate(const QByteArray& inp)
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;
rawDara.accels[0] = current.ddX;
rawDara.accels[1] = current.ddY;
rawDara.accels[2] = current.ddZ;
// Update homelocation
HomeLocation::DataFields homeData;