mirror of https://github.com/ArduPilot/ardupilot
95 lines
3.3 KiB
C#
95 lines
3.3 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 Matrix3 dcm = new Matrix3();
|
|
|
|
public Vector3 gyro = new Vector3(0, 0, 0);
|
|
|
|
public Vector3 accel_body = new Vector3(0, 0, 0);
|
|
|
|
public Vector3 velocity = new Vector3(0, 0, 0); //# m/s, North, East, Up
|
|
public Vector3 position = new Vector3(0, 0, 0); //# m North, East, Up
|
|
public Vector3 accel = new Vector3(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.80665;//# m/s/s
|
|
public Vector3 accelerometer = new Vector3(0, 0, -9.80665);
|
|
|
|
public double roll = 0;
|
|
public double pitch = 0;
|
|
public double yaw = 0;
|
|
|
|
public Aircraft()
|
|
{
|
|
self = this;
|
|
}
|
|
|
|
public bool on_ground(Vector3 position = null)
|
|
{
|
|
// '''return true if we are on the ground'''
|
|
if (position == null)
|
|
position = self.position;
|
|
return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height;
|
|
}
|
|
|
|
public void update_position(double delta_time = 0)
|
|
{
|
|
//'''update lat/lon/alt from position'''
|
|
|
|
double bearing = degrees(atan2(self.position.y, self.position.x));
|
|
double distance = sqrt(self.position.x * self.position.x + self.position.y * self.position.y);
|
|
|
|
gps_newpos(self.home_latitude, self.home_longitude,
|
|
bearing, distance);
|
|
|
|
self.altitude = self.home_altitude - self.position.z;
|
|
|
|
Vector3 velocity_body = self.dcm.transposed() * self.velocity;
|
|
|
|
self.accelerometer = self.accel_body.copy();
|
|
|
|
}
|
|
|
|
public void gps_newpos(double lat, double lon, double bearing, double distance)
|
|
{
|
|
// '''extrapolate latitude/longitude given a heading and distance
|
|
// thanks to http://www.movable-type.co.uk/scripts/latlong.html
|
|
// '''
|
|
// from math import sin, asin, cos, atan2, radians, degrees
|
|
double radius_of_earth = 6378100.0;//# in meters
|
|
|
|
double lat1 = radians(lat);
|
|
double lon1 = radians(lon);
|
|
double brng = radians(bearing);
|
|
double dr = distance / radius_of_earth;
|
|
|
|
double lat2 = asin(sin(lat1) * cos(dr) +
|
|
cos(lat1) * sin(dr) * cos(brng));
|
|
double lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
|
|
cos(dr) - sin(lat1) * sin(lat2));
|
|
|
|
latitude = degrees(lat2);
|
|
longitude = degrees(lon2);
|
|
//return (degrees(lat2), degrees(lon2));
|
|
}
|
|
|
|
}
|
|
} |