mirror of https://github.com/ArduPilot/ardupilot
64 lines
2.2 KiB
C#
64 lines
2.2 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
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);
|
|
}
|
|
} |