mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
8001515b7e
add enable/disable to mavlinkcheckbox modify my button to curved add delay to progress reporter dialog. to ensure correct parent Fix Mount screen for AP Fix Hardware screen Text display roi difrently modify HIL/Quad Hil update dataflashlog format (thanks randy) update mavcmd format for roi
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));
|
|
}
|
|
|
|
}
|
|
} |