ardupilot/Tools/ArdupilotMegaPlanner/HIL/Aircraft.cs

90 lines
3.0 KiB
C#
Raw Normal View History

2011-09-08 22:31:32 -03:00
using System;
using System.Collections.Generic;
using System.Text;
using YLScsDrawing.Drawing3d;
using ArdupilotMega.HIL;
2011-09-08 22:31:32 -03:00
namespace ArdupilotMega.HIL
{
public class Aircraft : Utils
2011-09-08 22:31:32 -03:00
{
Aircraft self;
2011-09-08 22:31:32 -03:00
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
2011-09-08 22:31:32 -03:00
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
2011-09-08 22:31:32 -03:00
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;
}
2011-09-08 22:31:32 -03:00
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);
}
2011-09-08 22:31:32 -03:00
}
}