aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: support functions for new simulators

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-02 20:29:16 +10:00
parent 437e1bdba0
commit 19147a05f6
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/SITL/SITL.cpp
View File

@ -156,3 +156,28 @@ void SITL::convert_body_frame(double rollDeg, double pitchDeg,
*r = cosf(phi)*psiDot*cosf(theta) - sinf(phi)*thetaDot;
}
/*
convert angular velocities from body frame to
earth frame.
all inputs and outputs are in radians/s
*/
Vector3f SITL::convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro)
{
float p = gyro.x;
float q = gyro.y;
float r = gyro.z;
float phi, theta, psi;
dcm.to_euler(&phi, &theta, &psi);
float phiDot = p + tanf(theta)*(q*sinf(phi) + r*cosf(phi));
float thetaDot = q*cosf(phi) - r*sinf(phi);
if (fabsf(cosf(theta)) < 1.0e-20f) {
theta += 1.0e-10f;
}
float psiDot = (q*sinf(phi) + r*cosf(phi))/cosf(theta);
return Vector3f(phiDot, thetaDot, psiDot);
}

4
libraries/SITL/SITL.h
View File

@ -13,6 +13,7 @@ struct PACKED sitl_fdm {
// this is the packet sent by the simulator
// to the APM executable to update the simulator state
// All values are little-endian
uint64_t timestamp_us;
double latitude, longitude; // degrees
double altitude; // MSL
double heading; // degrees
@ -106,6 +107,9 @@ public:
static void convert_body_frame(double rollDeg, double pitchDeg,
double rollRate, double pitchRate, double yawRate,
double *p, double *q, double *r);
// convert a set of roll rates from body frame to earth frame
static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro);
};
#endif // __SITL_H__