Exemplo n.º 1
0
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);
}
Exemplo n.º 2
0
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);
}
Exemplo n.º 3
0
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;
}
Exemplo n.º 4
0
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
}