double ProbeResult::altitude() const {
double latitude;
double longitdue;
double altitude;
XPLMLocalToWorld(x(), y(), z(), &latitude, &longitdue, &altitude);
return altitude;
}
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);
}
