void GetGPSData(void)
{
union longbbbb Temp4;
union intbb Temp2;
int LocalDays = XPLMGetDatai(drLocalDays);
float LocalSecsFloat = XPLMGetDataf(drLocalSecs) * 1000;
LocalDays += 5;
int Week = (int)(LocalDays / 7) + 1564;
LocalDays = (LocalDays % 7);
Week = (Week * 10) + LocalDays;
int LocalSecsInt = (int)LocalSecsFloat + (LocalDays * 86400000);
LocalSecsFloat = (LocalSecsFloat - (int)LocalSecsFloat) * 1000000;
Temp2.BB = Week;
Store2LE(&NAV_SOL[14], Temp2);
Temp4.WW = LocalSecsInt;
Store4LE(&NAV_SOL[6], Temp4);
Store4LE(&NAV_DOP[6], Temp4);
Store4LE(&NAV_POSLLH[6], Temp4);
Store4LE(&NAV_VELNED[6], Temp4);
Temp4.WW = (int)LocalSecsFloat;
Store4LE(&NAV_SOL[10], Temp4);
double latitude = XPLMGetDataf(drLat);
double longitude = XPLMGetDataf(drLon);
double elevation = XPLMGetDataf(drElev);
double local_x = XPLMGetDataf(drLocal_x);
double local_y = XPLMGetDataf(drLocal_y);
double local_z = XPLMGetDataf(drLocal_z);
double local_vx = XPLMGetDataf(drLocal_vx);
double local_vy = XPLMGetDataf(drLocal_vy);
double local_vz = XPLMGetDataf(drLocal_vz);
Temp4.WW = (int)(local_vx * 100);
Store4LE(&NAV_VELNED[14], Temp4);
Temp4.WW = (int)(local_vy * -100);
Store4LE(&NAV_VELNED[18], Temp4);
Temp4.WW = (int)(local_vz * -100);
Store4LE(&NAV_VELNED[10], Temp4);
Temp4.WW = (int)(XPLMGetDataf(drAirSpeedTrue) * 100);
Store4LE(&NAV_VELNED[22], Temp4);
Temp4.WW = (int)(XPLMGetDataf(drGroundSpeed) * 100);
Store4LE(&NAV_VELNED[26], Temp4);
Temp4.WW = (int)(XPLMGetDataf(drHeading) * 100000);
Store4LE(&NAV_VELNED[30], Temp4);
Temp4.WW = (int)(latitude * 10000000);
Store4LE(&NAV_POSLLH[14], Temp4);
Temp4.WW = (int)(longitude * 10000000);
Store4LE(&NAV_POSLLH[10], Temp4);
Temp4.WW = (int)(elevation * 1000);
Store4LE(&NAV_POSLLH[18], Temp4);
Store4LE(&NAV_POSLLH[22], Temp4);
double ac_pos_lat, ac_pos_lon, ac_pos_elev;
double ac_vel_lat, ac_vel_lon, ac_vel_elev;
// Get AC pos in LLA
XPLMLocalToWorld( local_x,
local_y,
local_z,
&ac_pos_lat,
&ac_pos_lon,
&ac_pos_elev );
// Get AC pos + velocity vector in LLA
XPLMLocalToWorld( local_x + local_vx,
local_y + local_vy,
local_z + local_vz,
&ac_vel_lat,
&ac_vel_lon,
&ac_vel_elev );
// convert to ECEF
LLAtoECEF(ac_pos_lat, ac_pos_lon, ac_pos_elev, local_x, local_y, local_z);
LLAtoECEF(ac_vel_lat, ac_vel_lon, ac_vel_elev, local_vx, local_vy, local_vz);
// AC pos stays as is
// subtract to get velocity vector in ECEF
local_vy -= local_y;
local_vx -= local_x;
local_vz -= local_z;
Temp4.WW = (int)(local_x * 100);
Store4LE(&NAV_SOL[18], Temp4);
Temp4.WW = (int)(local_y * 100);
Store4LE(&NAV_SOL[22], Temp4);
Temp4.WW = (int)(local_z * 100);
Store4LE(&NAV_SOL[26], Temp4);
Temp4.WW = (int)(local_vx * 100);
Store4LE(&NAV_SOL[34], Temp4);
Temp4.WW = (int)(local_vy * 100);
Store4LE(&NAV_SOL[38], Temp4);
Temp4.WW = (int)(local_vz * 100);
Store4LE(&NAV_SOL[42], Temp4);
CalculateChecksum(NAV_SOL);
SendToComPort(sizeof(NAV_SOL),NAV_SOL);
CalculateChecksum(NAV_DOP);
SendToComPort(sizeof(NAV_DOP),NAV_DOP);
CalculateChecksum(NAV_POSLLH);
SendToComPort(sizeof(NAV_POSLLH),NAV_POSLLH);
CalculateChecksum(NAV_VELNED);
SendToComPort(sizeof(NAV_VELNED),NAV_VELNED);
}
void GetGPSData(void)
{
union longbbbb Temp4;
union intbb Temp2;
float phi, theta, psi;
phi = (float)((XPLMGetDataf(drPhi) / 180) * PI);
theta = (float)((XPLMGetDataf(drTheta) / 180) * PI);
psi = (float)((XPLMGetDataf(drPsi) / 180) * PI);
int LocalDays = XPLMGetDatai(drLocalDays);
float LocalSecsFloat = XPLMGetDataf(drLocalSecs) * 1000;
LocalDays += 5;
int Week = (int)(LocalDays / 7) + 1564;
LocalDays = (LocalDays % 7);
Week = (Week * 10) + LocalDays;
int LocalSecsInt = (int)LocalSecsFloat + (LocalDays * 86400000);
LocalSecsFloat = (LocalSecsFloat - (int)LocalSecsFloat) * 1000000;
Temp2.BB = Week;
Store2LE(&NAV_SOL[14], Temp2);
Temp4.WW = LocalSecsInt;
Store4LE(&NAV_SOL[6], Temp4);
Store4LE(&NAV_DOP[6], Temp4);
Store4LE(&NAV_POSLLH[6], Temp4);
Store4LE(&NAV_VELNED[6], Temp4);
Temp4.WW = (int)LocalSecsFloat;
Store4LE(&NAV_SOL[10], Temp4);
double latitude = XPLMGetDataf(drLat);
double longitude = XPLMGetDataf(drLon);
double elevation = XPLMGetDataf(drElev);
double local_x = XPLMGetDataf(drLocal_x);
double local_y = XPLMGetDataf(drLocal_y);
double local_z = XPLMGetDataf(drLocal_z);
double local_vx = XPLMGetDataf(drLocal_vx);
double local_vy = XPLMGetDataf(drLocal_vy);
double local_vz = XPLMGetDataf(drLocal_vz);
Temp4.WW = (int)(local_vx * 100);
Store4LE(&NAV_VELNED[14], Temp4);
Temp4.WW = (int)(local_vy * -100);
Store4LE(&NAV_VELNED[18], Temp4);
Temp4.WW = (int)(local_vz * -100);
Store4LE(&NAV_VELNED[10], Temp4);
Temp4.WW = (int)(XPLMGetDataf(drAirSpeedTrue) * 100);
Store4LE(&NAV_VELNED[22], Temp4);
// note: xplane ground speed is not GPS speed over ground,
// it is 3D ground speed. we need horizontal ground speed for GPS,
// which is computed from the horizontal local velocity components:
double speed_over_ground = 100 * sqrt(local_vx*local_vx + local_vz*local_vz);
Temp4.WW = (int)speed_over_ground;
Store4LE(&NAV_VELNED[26], Temp4);
// Compute course over ground, in degrees,
// from horizontal earth frame velocities,
// which are in OGL frame of reference.
// local_vx is east, local_vz is south.
double course_over_ground = (atan2(local_vx, -local_vz) / PI * 180.0);
// MatrixPilot is expecting an angle between 0 and 360 degrees.
if (course_over_ground < 0.0) course_over_ground += 360.0;
Temp4.WW = (int)(100000.0 * course_over_ground);
Store4LE(&NAV_VELNED[30], Temp4);
Temp4.WW = (int)(latitude * 10000000);
Store4LE(&NAV_POSLLH[14], Temp4);
Temp4.WW = (int)(longitude * 10000000);
Store4LE(&NAV_POSLLH[10], Temp4);
Temp4.WW = (int)(elevation * 1000);
Store4LE(&NAV_POSLLH[18], Temp4);
Store4LE(&NAV_POSLLH[22], Temp4);
double ac_pos_lat, ac_pos_lon, ac_pos_elev;
double ac_vel_lat, ac_vel_lon, ac_vel_elev;
// Get AC pos in LLA
XPLMLocalToWorld(local_x,
local_y,
local_z,
&ac_pos_lat,
&ac_pos_lon,
&ac_pos_elev);
// Get AC pos + velocity vector in LLA
XPLMLocalToWorld(local_x + local_vx,
local_y + local_vy,
local_z + local_vz,
&ac_vel_lat,
&ac_vel_lon,
&ac_vel_elev);
// convert to ECEF
LLAtoECEF(ac_pos_lat, ac_pos_lon, ac_pos_elev, local_x, local_y, local_z);
LLAtoECEF(ac_vel_lat, ac_vel_lon, ac_vel_elev, local_vx, local_vy, local_vz);
// AC pos stays as is
// subtract to get velocity vector in ECEF
local_vy -= local_y;
local_vx -= local_x;
local_vz -= local_z;
Temp4.WW = (int)(local_x * 100);
Store4LE(&NAV_SOL[18], Temp4);
Temp4.WW = (int)(local_y * 100);
Store4LE(&NAV_SOL[22], Temp4);
Temp4.WW = (int)(local_z * 100);
Store4LE(&NAV_SOL[26], Temp4);
Temp4.WW = (int)(local_vx * 100);
Store4LE(&NAV_SOL[34], Temp4);
Temp4.WW = (int)(local_vy * 100);
Store4LE(&NAV_SOL[38], Temp4);
Temp4.WW = (int)(local_vz * 100);
Store4LE(&NAV_SOL[42], Temp4);
// simulate the magnetometer, and place in slots 30,32 and 46 of NAV_SOL
// these slots are used by Ublox, but not by HILSIM
// computation is based on zero declination, and zero inclination
float mag_field_x = 0.0; // earth OGL x mag field (east)
float mag_field_y = 0.0; // earth OGL y mag field (up)
float mag_field_z = -MAG_FIELD; // earth OGL z mag field (south)
// note, the "north pole" of the earth is really a south magnetic pole
// convert to NED body frame
OGLtoBCBF(mag_field_x, mag_field_y, mag_field_z, phi, theta, psi);
// convert from NED body to UDB body frame
double mag_field_body_udb_x = -mag_field_y;
double mag_field_body_udb_y = mag_field_x;
double mag_field_body_udb_z = mag_field_z;
// store in unused slots in NAV_SOL message
Temp2.BB = (int)mag_field_body_udb_x;
Store2LE(&NAV_SOL[30], Temp2);
Temp2.BB = (int)mag_field_body_udb_y;
Store2LE(&NAV_SOL[32], Temp2);
Temp2.BB = (int)mag_field_body_udb_z;
Store2LE(&NAV_SOL[46], Temp2);
CalculateChecksum(NAV_SOL);
SendToComPort(sizeof(NAV_SOL), NAV_SOL);
CalculateChecksum(NAV_DOP);
SendToComPort(sizeof(NAV_DOP), NAV_DOP);
CalculateChecksum(NAV_POSLLH);
SendToComPort(sizeof(NAV_POSLLH), NAV_POSLLH);
CalculateChecksum(NAV_VELNED);
SendToComPort(sizeof(NAV_VELNED), NAV_VELNED);
}
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
}