mirror of https://github.com/ArduPilot/ardupilot
90 lines
3.0 KiB
C#
90 lines
3.0 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Text;
|
|
using YLScsDrawing.Drawing3d;
|
|
using ArdupilotMega.HIL;
|
|
|
|
namespace ArdupilotMega.HIL
|
|
{
|
|
public class Aircraft : Utils
|
|
{
|
|
Aircraft self;
|
|
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 double pDeg = 0.0; // degrees/s
|
|
public double qDeg = 0.0; // degrees/s
|
|
public double rDeg = 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 Vector3d accelerometer = new Vector3d(0, 0, -9.8);
|
|
|
|
public Aircraft()
|
|
{
|
|
self = this;
|
|
}
|
|
|
|
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;
|
|
|
|
// work out what the accelerometer would see
|
|
double xAccel = sin(radians(self.pitch)) * cos(radians(self.roll));
|
|
double yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch));
|
|
double zAccel = -cos(radians(self.roll)) * cos(radians(self.pitch));
|
|
double scale = 9.81 / sqrt((xAccel * xAccel) + (yAccel * yAccel) + (zAccel * zAccel));
|
|
xAccel *= scale;
|
|
yAccel *= scale;
|
|
zAccel *= scale;
|
|
self.accelerometer = new Vector3d(xAccel, yAccel, zAccel);
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
} |