float GetBodyRates(float elapsedMe, float elapsedSim, int counter, void * refcon)
{
union intbb Temp2;
float phi, theta, psi;
float alpha, beta;
float P_flight, Q_flight, R_flight;
float ax, ay, az;
// Angular rates in X-Plane are specified relative to the flight path, not to the aircraft,
// for reasons unknown. So that means we need to rotate by alpha and beta to get angular rates
// in the aircraft body frame, which is what the UDB measures.
// Retrieve rates and slip angles, and convert to radians
P_flight = XPLMGetDataf(drP) / 180 * PI ;
Q_flight = XPLMGetDataf(drQ) / 180 * PI ;
R_flight = XPLMGetDataf(drR) / 180 * PI ;
alpha = XPLMGetDataf(drAlpha) / 180 * PI;
beta = XPLMGetDataf(drBeta) / 180 * PI;
FLIGHTtoBCBF(P_flight, Q_flight, R_flight, alpha, beta);
P_plane = P_flight;
Q_plane = Q_flight;
R_plane = R_flight;
// Angular rate
// multiply by 5632 (constant from UDB code)
// Divide by SCALEGYRO(3.0 for red board)
// 1 * 5632 / 3.0 = 1877.33
Temp2.BB = (int)(P_plane * 1877.33);
Store2LE(&NAV_BODYRATES[6], Temp2);
Temp2.BB = (int)(Q_plane * 1877.33);
Store2LE(&NAV_BODYRATES[8], Temp2);
Temp2.BB = (int)(R_plane * 1877.33);
Store2LE(&NAV_BODYRATES[10], Temp2);
// Our euler angles:
// X-Plane angles are in degrees.
// Phi is roll, roll to right is positive
// Theta is pitch, pitch up is positive
// Psi is yaw, relative to north. North is 0/360.
// Convert these angles to radians first.
phi =(XPLMGetDataf(drPhi)) / 180 * PI * -1.0;
theta = (XPLMGetDataf(drTheta)) / 180 * PI;
psi = (XPLMGetDataf(drPsi)) / 180 * PI * -1.0;
// Get accelerations in OpenGL coordinate frame
//ax = XPLMGetDataf(drLocal_ax);
//ay = XPLMGetDataf(drLocal_ay);
//az = XPLMGetDataf(drLocal_ay);
ax = 0;
ay = 0;
az = 0;
// Gravity is not included in ay, we need to add it. OGL y axis is +ve up,
// so g is -9.8.
ay -= (float)9.8;
// Convert from OGL frame to Aircraft body fixed frame
OGLtoBCBF(ax, ay, az, phi, theta, psi);
ax_plane = ax;
ay_plane = ay;
az_plane = az;
// Lastly we need to convert from X-Plane units (m/s^2) to the arbitrary units used by the UDB
// Accelerations are in m/s^2
// Divide by 9.8 to get g's
// Multiply by 5280 (constant from UDB code)
// Divide by SCALEACCEL (2.64 for red board)
// 1 / 9.8 * 5280 / 2.64 = 204.8
Temp2.BB = (int)(ax * 204.8);
Store2LE(&NAV_BODYRATES[12], Temp2);
Temp2.BB = (int)(ay * 204.8);
Store2LE(&NAV_BODYRATES[14], Temp2);
Temp2.BB = (int)(az * 204.8);
Store2LE(&NAV_BODYRATES[16], Temp2);
CalculateChecksum(NAV_BODYRATES);
SendToComPort(sizeof(NAV_BODYRATES),NAV_BODYRATES);
ReceiveFromComPort();
ServosToControls();
// float ThrottleSetting = 0; //SurfaceDeflections[CHANNEL_THROTTLE];
// float throttle[8] = {ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting,
// ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting};
XPLMSetDatavf(drThro, ThrottleSettings,0,8);
return -1;
}
float GetBodyRates(float elapsedMe, float elapsedSim, int counter, void* refcon)
{
(void)elapsedSim;
(void)counter;
(void)refcon;
pendingElapsedTime += elapsedMe;
ReceiveFromComPort();
if (pendingElapsedTime < 0.025) // Don't run faster than 40Hz
{
return -1;
}
union intbb Temp2;
float phi, theta, psi;
float alpha, beta;
float P_flight, Q_flight, R_flight;
float ax, ay, az;
float gravity_acceleration_x, gravity_acceleration_y, gravity_acceleration_z;
// Angular rates in X-Plane are specified relative to the flight path, not to the aircraft,
// for reasons unknown. So that means we need to rotate by alpha and beta to get angular rates
// in the aircraft body frame, which is what the UDB measures.
// Retrieve rates and slip angles, and convert to radians
P_flight = XPLMGetDataf(drP) / 180 * PI;
Q_flight = XPLMGetDataf(drQ) / 180 * PI;
R_flight = XPLMGetDataf(drR) / 180 * PI;
alpha = XPLMGetDataf(drAlpha) / 180 * PI;
beta = XPLMGetDataf(drBeta) / 180 * PI;
// On 25th Jan 2015, Bill Premerlani confirmed with Austin Meyer, author of X-Plane
// that P, Q and R are rotations in the body frame. So they do not need to be rotated into
// any other frame of reference, other than a small sign correction for the UDB frame conventions.
// Austin Meyer said: "now, i CAN say that P is roll, Q is pitch, and R is yaw, all in degrees per second
//about the aircraft axis,..... (i just looked at the code to confirm this)"
P_plane = P_flight;
Q_plane = -Q_flight; // convert from NED to UDB
R_plane = R_flight;
// Angular rate
// multiply by 5632 (constant from UDB code)
// Divide by SCALEGYRO(3.0 for red board)
// 1 * 5632 / 3.0 = 1877.33
Temp2.BB = (int)(P_plane * 1877.33);
Store2LE(&NAV_BODYRATES[6], Temp2);
Temp2.BB = (int)(Q_plane * 1877.33);
Store2LE(&NAV_BODYRATES[8], Temp2);
Temp2.BB = (int)(R_plane * 1877.33);
Store2LE(&NAV_BODYRATES[10], Temp2);
// Our euler angles:
// X-Plane angles are in degrees.
// Phi is roll, roll to right is positive
// Theta is pitch, pitch up is positive
// Psi is yaw, relative to north. North is 0/360.
// Convert these angles to radians first.
phi = (float)((XPLMGetDataf(drPhi) / 180) * PI);
theta = (float)((XPLMGetDataf(drTheta) / 180) * PI);
psi = (float)((XPLMGetDataf(drPsi) / 180) * PI);
// set up a vertical reference for the plotting computations
// vertical in earth frame:
ax = 0;
ay = -(float)9.8;
az = 0;
// get the acceleration loading (gravity-acceleration) in the body frame in "g"s,
// and convert to meter/sec/sec
// x, y, and z are "UDB" coordinates, x is left wing, y is forward, and z is down.
gravity_acceleration_x = (float)((XPLMGetDataf(drg_side)) * 9.8);
gravity_acceleration_y = (float)((XPLMGetDataf(drg_axil)) * 9.8);
gravity_acceleration_z = (float)((XPLMGetDataf(drg_nrml)) * 9.8);
// Convert from OGL frame to Aircraft body fixed frame
// This produces a vertical reference in body frame
OGLtoBCBF(ax, ay, az, phi, theta, psi);
ax_plane = ax;
ay_plane = -ay; // convert from NED to UDB
az_plane = az;
// Lastly we need to convert from X-Plane units (m/s^2) to the arbitrary units used by the UDB
// Accelerations are in m/s^2
// Divide by 9.8 to get g's
// Multiply by 5280 (constant from UDB code)
// Divide by SCALEACCEL (2.64 for red board)
// 1 / 9.8 * 5280 / 2.64 = 204.8
Temp2.BB = (int)(gravity_acceleration_x * 204.8);
Store2LE(&NAV_BODYRATES[12], Temp2);
Temp2.BB = (int)(gravity_acceleration_y * 204.8);
Store2LE(&NAV_BODYRATES[14], Temp2);
Temp2.BB = (int)(gravity_acceleration_z * 204.8);
Store2LE(&NAV_BODYRATES[16], Temp2);
CalculateChecksum(NAV_BODYRATES);
SendToComPort(sizeof(NAV_BODYRATES), NAV_BODYRATES);
while (pendingElapsedTime >= 0.025) // Don't run slower than 40Hz
{
GPSCount++;
if (!IsConnected())
{
ConnectionCount++;
if (ConnectionCount % 160 == 0) // attempt reconnection every 4 seconds when disconnected
{
AttemptConnection();
ConnectionCount = 0;
}
}
if (GPSCount % 10 == 0)
{
GetGPSData();
GPSCount = 0;
}
pendingElapsedTime -= (float)0.025;
}
ServosToControls();
// float ThrottleSetting = 0; //SurfaceDeflections[CHANNEL_THROTTLE];
// float throttle[8] = {ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting,
// ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting};
XPLMSetDatavf(drThro, ThrottleSettings, 0, 8);
static float prevBrakeSetting = PARKBRAKE_ON;
if (BrakeSetting != prevBrakeSetting)
{
prevBrakeSetting = BrakeSetting;
XPLMSetDataf(drBrake, BrakeSetting);
// LoggingFile.mLogFile << "Set parkbrake to " << BrakeSetting << endl;
}
return -1; // get called back on every frame
}