ardupilot/Tools/ArdupilotMegaPlanner/HIL/Aircraft.cs

65 lines
2.2 KiB
C#

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using YLScsDrawing.Drawing3d;
namespace ArdupilotMega.HIL
{
public class Aircraft
{
public double home_latitude = 0;
public double home_longitude = 0;
public double home_altitude = 0;
public double ground_level = 0;
public double frame_height = 0.0;
public double latitude = 0;
public double longitude = 0;
public double altitude = 0;
public double pitch = 0.0; //# degrees
public double roll = 0.0; //# degrees
public double yaw = 0.0; //# degrees
public double pitch_rate = 0.0; //# degrees/s
public double roll_rate = 0.0; //# degrees/s
public double yaw_rate = 0.0; //# degrees/s
public Vector3d velocity = new Vector3d(0, 0, 0); //# m/s, North, East, Up
public Vector3d position = new Vector3d(0, 0, 0); //# m North, East, Up
public Vector3d accel = new Vector3d(0, 0, 0); //# m/s/s North, East, Up
public double mass = 0.0;
public double update_frequency = 50;//# in Hz
public double gravity = 9.8;//# m/s/s
public void normalise()
{
roll = norm(roll, -180, 180);
pitch = norm(pitch, -180, 180);
yaw = norm(yaw, 0, 360);
}
double norm(double angle, double min, double max)
{
while (angle > max)
angle -= 360;
while (angle < min)
angle += 360;
return angle;
}
public void update_position() {
//'''update lat/lon/alt from position'''
double radius_of_earth = 6378100.0;// # in meters
double dlat = rad2deg * (Math.Atan(position.X / radius_of_earth));
double dlon = rad2deg * (Math.Atan(position.Y / radius_of_earth));
altitude = home_altitude + position.Z;
latitude = home_latitude + dlat;
longitude = home_longitude + dlon;
}
const float rad2deg = (float)(180 / Math.PI);
const float deg2rad = (float)(1.0 / rad2deg);
}
}