Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Chris Anderson 2012-01-15 07:43:01 -08:00
parent 49c04e46a7 456b532602
commit 3ab00b75c2
22 changed files with 991 additions and 1152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
View File

@ -247,6 +247,7 @@
<Compile Include="Controls\ImageLabel.Designer.cs">
<DependentUpon>ImageLabel.cs</DependentUpon>
</Compile>
<Compile Include="HIL\Utils.cs" />
<Compile Include="ResEdit.cs">
<SubType>Form</SubType>
</Compile>
@ -402,6 +403,7 @@
<DependentUpon>Splash.cs</DependentUpon>
</Compile>
<Compile Include="srtm.cs" />
<Compile Include="TCPConsole.cs" />
<Compile Include="temp.cs">
<SubType>Form</SubType>
</Compile>

14
Tools/ArdupilotMegaPlanner/ArdupilotMega.sln
View File

@ -5,6 +5,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ArdupilotMega", "ArdupilotM
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Updater", "Updater\Updater.csproj", "{E64A1A41-A5B0-458E-8284-BB63705354DA}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ArduinoCPP", "..\..\ArduinoCPP\ArduinoCPP.csproj", "{C38A02C5-3179-4047-8DC3-945341008A74}"
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Any CPU = Debug|Any CPU
@ -41,6 +43,18 @@ Global
{E64A1A41-A5B0-458E-8284-BB63705354DA}.Release|Win32.ActiveCfg = Release|x86
{E64A1A41-A5B0-458E-8284-BB63705354DA}.Release|x86.ActiveCfg = Release|x86
{E64A1A41-A5B0-458E-8284-BB63705354DA}.Release|x86.Build.0 = Release|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|Any CPU.ActiveCfg = Debug|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|Mixed Platforms.ActiveCfg = Debug|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|Mixed Platforms.Build.0 = Debug|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|Win32.ActiveCfg = Debug|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|x86.ActiveCfg = Debug|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Debug|x86.Build.0 = Debug|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Release|Any CPU.ActiveCfg = Release|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Release|Mixed Platforms.ActiveCfg = Release|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Release|Mixed Platforms.Build.0 = Release|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Release|Win32.ActiveCfg = Release|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Release|x86.ActiveCfg = Release|x86
{C38A02C5-3179-4047-8DC3-945341008A74}.Release|x86.Build.0 = Release|x86
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE

2
Tools/ArdupilotMegaPlanner/Common.cs
View File

@ -299,7 +299,7 @@ namespace ArdupilotMega
Console.WriteLine("a = {0}, the slope of the trend line.", Math.Round(a, 2));
Console.WriteLine("b = {0}, the intercept of the trend line.", Math.Round(b, 2));
Console.ReadLine();
//Console.ReadLine();
}
#if MAVLINK10

5
Tools/ArdupilotMegaPlanner/CurrentState.cs
View File

@ -360,6 +360,9 @@ namespace ArdupilotMega
break;
case (byte)(Common.ac2modes.CIRCLE):
mode = "Circle";
break;
case (byte)(Common.ac2modes.LAND):
mode = "Land";
break;
default:
mode = "Unknown";
@ -392,7 +395,7 @@ namespace ArdupilotMega
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#else
#else
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
{

6
Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs
View File

@ -550,13 +550,13 @@ namespace ArdupilotMega.GCSViews
{
if (text.Length > 0)
{
if (sender.GetType() == typeof(NumericUpDown))
if (text[0].GetType() == typeof(NumericUpDown))
{
decimal option = (decimal)(float.Parse(Params[e.ColumnIndex, e.RowIndex].Value.ToString()));
((NumericUpDown)text[0]).Value = option;
((NumericUpDown)text[0]).BackColor = Color.Green;
}
else if (sender.GetType() == typeof(ComboBox))
else if (text[0].GetType() == typeof(ComboBox))
{
int option = (int)(float.Parse(Params[e.ColumnIndex, e.RowIndex].Value.ToString()));
((ComboBox)text[0]).SelectedIndex = option;
@ -685,7 +685,7 @@ namespace ArdupilotMega.GCSViews
Control[] text = this.Controls.Find(value, true);
if (text.Length > 0)
{
((NumericUpDown)text[0]).BackColor = Color.FromArgb(0x43, 0x44, 0x45);
((Control)text[0]).BackColor = Color.FromArgb(0x43, 0x44, 0x45);
}
}
catch { }

3
Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs
View File

@ -3,7 +3,6 @@ using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Text.RegularExpressions;
@ -591,7 +590,7 @@ namespace ArdupilotMega.GCSViews
UploadFlash(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"firmware.hex", board);
}
void UploadFlash(string filename, string board)
public void UploadFlash(string filename, string board)
{
byte[] FLASH = new byte[1];
StreamReader sr = null;

12
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs generated
View File

@ -8,8 +8,8 @@
{
this.components = new System.ComponentModel.Container();
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(FlightData));
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle7 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle8 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle();
System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle();
this.contextMenuStrip1 = new System.Windows.Forms.ContextMenuStrip(this.components);
this.goHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
this.pointCameraHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
@ -1217,8 +1217,8 @@
//
// dataGridViewImageColumn1
//
dataGridViewCellStyle7.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleCenter;
this.dataGridViewImageColumn1.DefaultCellStyle = dataGridViewCellStyle7;
dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleCenter;
this.dataGridViewImageColumn1.DefaultCellStyle = dataGridViewCellStyle1;
resources.ApplyResources(this.dataGridViewImageColumn1, "dataGridViewImageColumn1");
this.dataGridViewImageColumn1.Image = global::ArdupilotMega.Properties.Resources.up;
this.dataGridViewImageColumn1.ImageLayout = System.Windows.Forms.DataGridViewImageCellLayout.Stretch;
@ -1226,8 +1226,8 @@
//
// dataGridViewImageColumn2
//
dataGridViewCellStyle8.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleCenter;
this.dataGridViewImageColumn2.DefaultCellStyle = dataGridViewCellStyle8;
dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleCenter;
this.dataGridViewImageColumn2.DefaultCellStyle = dataGridViewCellStyle2;
resources.ApplyResources(this.dataGridViewImageColumn2, "dataGridViewImageColumn2");
this.dataGridViewImageColumn2.Image = global::ArdupilotMega.Properties.Resources.down;
this.dataGridViewImageColumn2.ImageLayout = System.Windows.Forms.DataGridViewImageCellLayout.Stretch;

1583
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx
View File

File diff suppressed because it is too large Load Diff

84
Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
View File

@ -270,9 +270,11 @@ namespace ArdupilotMega.GCSViews
System.Threading.Thread.Sleep(2000);
SITLSEND = new UdpClient(simIP, 5501);
}
SetupTcpJSBSim(); // old style
}
SetupUDPXplanes(); // fg udp style
SetupUDPMavLink(); // pass traffic - raw
}
@ -591,7 +593,7 @@ namespace ArdupilotMega.GCSViews
if (hzcounttime.Second != DateTime.Now.Second)
{
Console.WriteLine("SIM hz {0}", hzcount);
// Console.WriteLine("SIM hz {0}", hzcount);
hzcount = 0;
hzcounttime = DateTime.Now;
}
@ -660,84 +662,6 @@ namespace ArdupilotMega.GCSViews
MavLink = new UdpClient("127.0.0.1", 14550);
}
/// <summary>
/// From http://code.google.com/p/gentlenav/source/browse/trunk/Tools/XP_UDB_HILSIM/utility.cpp
/// Converts from xplanes to fixed body ref
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
/// <param name="alpha"></param>
/// <param name="beta"></param>
public static void FLIGHTtoBCBF(ref float x, ref float y, ref float z, float alpha, float beta)
{
float Ca = (float)Math.Cos(alpha);
float Cb = (float)Math.Cos(beta);
float Sa = (float)Math.Sin(alpha);
float Sb = (float)Math.Sin(beta);
float X_plane = (x * Ca * Cb) - (z * Sa * Cb) - (y * Sb);
float Y_plane = (z * Sa * Sb) - (x * Ca * Sb) - (y * Cb);
float Z_plane = (x * Sa) + (z * Ca);
x = X_plane;
y = Y_plane;
z = Z_plane;
}
void OGLtoBCBF(ref float x, ref float y, ref float z, float phi, float theta, float psi)
{
float x_NED, y_NED, z_NED;
float Cr, Cp, Cy;
float Sr, Sp, Sy;
//Accelerations in X-Plane are expressed in the local OpenGL reference frame, for whatever reason.
//This coordinate system is defined as follows (taken from the X-Plane SDK Wiki):
// The origin 0,0,0 is on the surface of the earth at sea level at some "reference point".
// The +X axis points east from the reference point.
// The +Z axis points south from the reference point.
// The +Y axis points straight up away from the center of the earth at the reference point.
// First we shall convert from this East Up South frame, to a more conventional NED (North East Down) frame.
x_NED = -1.0f * z;
y_NED = x;
z_NED = -1.0f * y;
// Next calculate cos & sin of angles for use in the transformation matrix.
// r, p & y subscripts stand for roll pitch and yaw.
Cr = (float)Math.Cos(phi);
Cp = (float)Math.Cos(theta);
Cy = (float)Math.Cos(psi);
Sr = (float)Math.Sin(phi);
Sp = (float)Math.Sin(theta);
Sy = (float)Math.Sin(psi);
// Next we need to rotate our accelerations from the NED reference frame, into the body fixed reference frame
// THANKS TO GEORGE M SIOURIS WHOSE "MISSILE GUIDANCE AND CONTROL SYSTEMS" BOOK SEEMS TO BE THE ONLY EASY TO FIND REFERENCE THAT
// ACTUALLY GETS THE NED TO BODY FRAME ROTATION MATRIX CORRECT!!
// CpCy, CpSy, -Sp | local_ax
// SrSpCy-CrSy, SrSpSy+CrCy, SrCp | local_ay
// CrSpCy+SrSy, CrSpSy-SrCy, CrCp | local_az
x = (x_NED * Cp * Cy) + (y_NED * Cp * Sy) - (z_NED * Sp);
y = (x_NED * ((Sr * Sp * Cy) - (Cr * Sy))) + (y_NED * ((Sr * Sp * Sy) + (Cr * Cy))) + (z_NED * Sr * Cp);
z = (x_NED * ((Cr * Sp * Cy) + (Sr * Sy))) + (y_NED * ((Cr * Sp * Sy) - (Sr * Cy))) + (z_NED * Cr * Cp);
}
double sin(double rad)
{
return Math.Sin(rad);
}
double cos(double rad)
{
return Math.Cos(rad);
}
float oldax =0, olday =0, oldaz = 0;
DateTime oldtime = DateTime.Now;
#if MAVLINK10

34
Tools/ArdupilotMegaPlanner/HIL/Aircraft.cs
View File

@ -2,11 +2,13 @@
using System.Collections.Generic;
using System.Text;
using YLScsDrawing.Drawing3d;
using ArdupilotMega.HIL;
namespace ArdupilotMega.HIL
{
public class Aircraft
public class Aircraft : Utils
{
Aircraft self;
public double home_latitude = 0;
public double home_longitude = 0;
public double home_altitude = 0;
@ -20,15 +22,27 @@ namespace ArdupilotMega.HIL
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()
{
@ -46,7 +60,8 @@ namespace ArdupilotMega.HIL
return angle;
}
public void update_position() {
public void update_position()
{
//'''update lat/lon/alt from position'''
double radius_of_earth = 6378100.0;// # in meters
@ -56,9 +71,20 @@ namespace ArdupilotMega.HIL
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);
}
const float rad2deg = (float)(180 / Math.PI);
const float deg2rad = (float)(1.0 / rad2deg);
}
}

164
Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs
View File

@ -3,15 +3,105 @@ using System.Collections.Generic;
using System.Linq;
using System.Text;
using YLScsDrawing.Drawing3d;
using ArdupilotMega.HIL;
namespace ArdupilotMega.HIL
{
public class Motor : Utils
{
const bool True = true;
const bool False = false;
public Motor self;
public double angle;
public bool clockwise;
public double servo;
public Motor(double angle, bool clockwise, double servo)
{
self = this;
self.angle = angle;
self.clockwise = clockwise;
self.servo = servo;
}
public static Motor[] build_motors(string frame)
{
Motor[] motors = new HIL.Motor[8];
frame = frame.ToLower();
if (frame.Contains("quad") || frame.Contains("quadx"))
{
motors = new HIL.Motor[] {
new Motor(90, False, 1),
new Motor(270, False, 2),
new Motor(0, True, 3),
new Motor(180, True, 4),
};
if (frame.Contains("quadx"))
{
foreach (int i in range(4))
motors[i].angle -= 45.0;
}
}
else if (frame.Contains("y6"))
{
motors = new HIL.Motor[] {
new Motor(60, False, 1),
new Motor(60, True, 7),
new Motor(180, True, 4),
new Motor(180, False, 8),
new Motor(-60, True, 2),
new Motor(-60, False, 3),
};
}
else if (frame.Contains("hexa") || frame.Contains("hexax"))
{
motors = new HIL.Motor[] {
new Motor(0, True, 1),
new Motor(60, False, 4),
new Motor(120, True, 8),
new Motor(180, False, 2),
new Motor(240, True, 3),
new Motor(300, False, 7),
};
}
else if (frame.Contains("hexax"))
{
motors = new HIL.Motor[] {
new Motor(30, False, 7),
new Motor(90, True, 1),
new Motor(150, False, 4),
new Motor(210, True, 8),
new Motor(270, False, 2),
new Motor(330, True, 3),
};
}
else if (frame.Contains("octa") || frame.Contains("octax"))
{
motors = new HIL.Motor[] {
new Motor(0, True, 1),
new Motor(180, True, 2),
new Motor(45, False, 3),
new Motor(135, False, 4),
new Motor(-45, False, 7),
new Motor(-135, False, 8),
new Motor(270, True, 10),
new Motor(90, True, 11),
};
if (frame.Contains("octax"))
{
foreach (int i in range(8))
motors[i].angle += 22.5;
}
}
return motors;
}
}
public class QuadCopter : Aircraft
{
const float ft2m = (float)(1.0 / 3.2808399);
const float rad2deg = (float)(180 / Math.PI);
const float deg2rad = (float)(1.0 / rad2deg);
const float kts2fps = (float)1.68780986;
QuadCopter self;
int framecount = 0;
DateTime seconds = DateTime.Now;
@ -21,6 +111,7 @@ namespace ArdupilotMega.HIL
double hover_throttle;
double terminal_velocity;
double terminal_rotation_rate;
Motor[] motors;
Vector3d old_position;
@ -31,16 +122,20 @@ namespace ArdupilotMega.HIL
DateTime last_time;
public QuadCopter()
public QuadCopter(string frame = "quad")
{
self = this;
motors = Motor.build_motors(frame);
mass = 1.0;// # Kg
frame_height = 0.1;
motor_speed = new double[] { 0.0, 0.0, 0.0, 0.0 };
motor_speed = new double[motors.Length];
hover_throttle = 0.37;
terminal_velocity = 30.0;
terminal_rotation_rate = 4 * 360.0;
thrust_scale = (mass * gravity) / (4.0 * hover_throttle);
thrust_scale = (mass * gravity) / (motors.Length * hover_throttle);
last_time = DateTime.Now;
}
@ -109,25 +204,53 @@ namespace ArdupilotMega.HIL
delta_time = new TimeSpan(0, 0, 0, 0, 20);
}
//# rotational acceleration, in degrees/s/s
double roll_accel = (m[1] - m[0]) * 5000.0;
double pitch_accel = (m[2] - m[3]) * 5000.0;
double yaw_accel = -((m[2] + m[3]) - (m[0] + m[1])) * 400.0;
// rotational acceleration, in degrees/s/s, in body frame
double roll_accel = 0.0;
double pitch_accel = 0.0;
double yaw_accel = 0.0;
double thrust = 0.0;
foreach (var i in range((self.motors.Length)))
{
roll_accel += -5000.0 * sin(radians(self.motors[i].angle)) * m[i];
pitch_accel += 5000.0 * cos(radians(self.motors[i].angle)) * m[i];
if (self.motors[i].clockwise)
{
yaw_accel -= m[i] * 400.0;
}
else
{
yaw_accel += m[i] * 400.0;
}
thrust += m[i] * self.thrust_scale; // newtons
}
// rotational resistance
roll_accel -= (self.pDeg / self.terminal_rotation_rate) * 5000.0;
pitch_accel -= (self.qDeg / self.terminal_rotation_rate) * 5000.0;
yaw_accel -= (self.rDeg / self.terminal_rotation_rate) * 400.0;
//Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
// rotational resistance
roll_accel -= (roll_rate / terminal_rotation_rate) * 5000.0;
pitch_accel -= (pitch_rate / terminal_rotation_rate) * 5000.0;
yaw_accel -= (yaw_rate / terminal_rotation_rate) * 400.0;
//# update rotational rates
roll_rate += roll_accel * delta_time.TotalSeconds;
pitch_rate += pitch_accel * delta_time.TotalSeconds;
yaw_rate += yaw_accel * delta_time.TotalSeconds;
//# update rotational rates in body frame
self.pDeg += roll_accel * delta_time.TotalSeconds;
self.qDeg += pitch_accel * delta_time.TotalSeconds;
self.rDeg += yaw_accel * delta_time.TotalSeconds;
// Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
// calculate rates in earth frame
var answer = BodyRatesToEarthRates(self.roll, self.pitch, self.yaw,
self.pDeg, self.qDeg, self.rDeg);
self.roll_rate = answer.Item1;
self.pitch_rate = answer.Item2;
self.yaw_rate = answer.Item3;
//self.roll_rate = pDeg;
//self.pitch_rate = qDeg;
//self.yaw_rate = rDeg;
//# update rotation
roll += roll_rate * delta_time.TotalSeconds;
pitch += pitch_rate * delta_time.TotalSeconds;
@ -143,7 +266,6 @@ namespace ArdupilotMega.HIL
//# normalise rotations
normalise();
double thrust = (m[0] + m[1] + m[2] + m[3]) * thrust_scale;//# Newtons
double accel = thrust / mass;
//Console.WriteLine("in {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", roll, pitch, yaw, accel);

104
Tools/ArdupilotMegaPlanner/HIL/Utils.cs Normal file
View File

@ -0,0 +1,104 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace ArdupilotMega.HIL
{
public class Utils
{
public const float rad2deg = (float)(180 / System.Math.PI);
public const float deg2rad = (float)(1.0 / rad2deg);
public const float ft2m = (float)(1.0 / 3.2808399);
public const float kts2fps = (float)1.68780986;
public static double sin(double val)
{
return System.Math.Sin(val);
}
public static double cos(double val)
{
return System.Math.Cos(val);
}
public static double radians(double val)
{
return val * deg2rad;
}
public static double degrees(double val)
{
return val * rad2deg;
}
public static double sqrt(double val)
{
return System.Math.Sqrt(val);
}
public static int[] range(int no)
{
int[] range = new int[no];
for (int a = 0; a < no; a++)
{
range[a] = a;
}
return range;
}
public static double fabs(double val)
{
return System.Math.Abs(val);
}
public static double tan(double val)
{
return System.Math.Tan(val);
}
public static Tuple<double,double,double> EarthRatesToBodyRates(double roll,double pitch,double yaw,
double rollRate,double pitchRate,double yawRate) {
//convert the angular velocities from earth frame to
//body frame. Thanks to James Goppert for the formula
//all inputs and outputs are in degrees
//returns a tuple, (p,q,r)
var phi = radians(roll);
var theta = radians(pitch);
var phiDot = radians(rollRate);
var thetaDot = radians(pitchRate);
var psiDot = radians(yawRate);
var p = phiDot - psiDot*sin(theta);
var q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
var r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;
return new Tuple<double,double,double> (degrees(p), degrees(q), degrees(r));
}
public static Tuple<double,double,double> BodyRatesToEarthRates(double roll, double pitch, double yaw,double pDeg, double qDeg,double rDeg){
//convert the angular velocities from body frame to
//earth frame.
//all inputs and outputs are in degrees
//returns a tuple, (rollRate,pitchRate,yawRate)
var p = radians(pDeg);
var q = radians(qDeg);
var r = radians(rDeg);
var phi = radians(roll);
var theta = radians(pitch);
var phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi));
var thetaDot = q*cos(phi) - r*sin(phi);
if (fabs(cos(theta)) < 1.0e-20)
theta += 1.0e-10;
var psiDot = (q*sin(phi) + r*cos(phi))/cos(theta);
return new Tuple<double,double,double> (degrees(phiDot), degrees(thetaDot), degrees(psiDot));
}
}
}

18
Tools/ArdupilotMegaPlanner/HUD.cs
View File

@ -27,6 +27,7 @@ namespace hud
object paintlock = new object();
object streamlock = new object();
MemoryStream _streamjpg = new MemoryStream();
[System.ComponentModel.Browsable(false)]
public MemoryStream streamjpg { get { lock (streamlock) { return _streamjpg; } } set { lock (streamlock) { _streamjpg = value; } } }
/// <summary>
/// this is to reduce cpu usage
@ -44,7 +45,10 @@ namespace hud
public HUD()
{
if (this.DesignMode)
return;
{
opengl = false;
//return;
}
InitializeComponent();
@ -237,7 +241,7 @@ namespace hud
{
e.Graphics.Clear(this.BackColor);
e.Graphics.Flush();
return;
//return;
}
if ((DateTime.Now - starttime).TotalMilliseconds < 30 && (_bgimage == null))
@ -277,7 +281,7 @@ namespace hud
if (DateTime.Now.Second != countdate.Second)
{
countdate = DateTime.Now;
Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count) + " gl " + opengl);
// Console.WriteLine("HUD " + count + " hz drawtime " + (huddrawtime / count) + " gl " + opengl);
if ((huddrawtime / count) > 1000)
opengl = false;
@ -1202,19 +1206,19 @@ namespace hud
if (gpsfix == 0)
{
gps = resources.GetString("GPS: No GPS.Text");
gps = ("GPS: No GPS");
}
else if (gpsfix == 1)
{
gps = resources.GetString("GPS: No Fix.Text");
gps = ("GPS: No Fix");
}
else if (gpsfix == 2)
{
gps = resources.GetString("GPS: 2D Fix.Text");
gps = ("GPS: 3D Fix");
}
else if (gpsfix == 3)
{
gps = resources.GetString("GPS: 3D Fix.Text");
gps = ("GPS: 3D Fix");
}
drawstring(graphicsObject, gps, font, fontsize + 2, whiteBrush, this.Width - 10 * fontsize, this.Height - 30 - fontoffset);

2
Tools/ArdupilotMegaPlanner/Log.cs
View File

@ -347,6 +347,8 @@ namespace ArdupilotMega
if (items.Length == 11 && items[6] == "0.0000")
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
if (items.Length == 11 && items[6] == "0")
alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US"));
position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt));

5
Tools/ArdupilotMegaPlanner/MAVLink.cs
View File

@ -974,7 +974,7 @@ namespace ArdupilotMega
actionid == MAV_ACTION.MAV_ACTION_REBOOT)
{
retrys = 1;
timeout = 6000;
timeout = 20000;
}
while (true)
@ -2001,8 +2001,9 @@ namespace ArdupilotMega
if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync
{
if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n')
if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n' || temp[0] == '\r')
{
TCPConsole.Write(temp[0]);
Console.Write((char)temp[0]);
}
count = 0;

3
Tools/ArdupilotMegaPlanner/Program.cs
View File

@ -4,6 +4,7 @@ using System.Windows.Forms;
using System.Net;
using System.IO;
using System.Text;
using System.Threading;
namespace ArdupilotMega
@ -32,9 +33,7 @@ namespace ArdupilotMega
Application.SetCompatibleTextRenderingDefault(false);
try
{
Application.Run(new MainV2());
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }
}

4
Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
View File

@ -11,7 +11,7 @@ using System.Resources;
[assembly: AssemblyConfiguration("")]
[assembly: AssemblyCompany("Michael Oborne")]
[assembly: AssemblyProduct("ArdupilotMega Planner")]
[assembly: AssemblyCopyright("Copyright © Michael Oborne 2010-2011")]
[assembly: AssemblyCopyright("Copyright © Michael Oborne 2010-2012")]
[assembly: AssemblyTrademark("")]
[assembly: AssemblyCulture("")]
@ -34,5 +34,5 @@ using System.Resources;
// by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.0.0.0")]
[assembly: AssemblyFileVersion("1.1.20")]
[assembly: AssemblyFileVersion("1.1.21")]
[assembly: NeutralResourcesLanguageAttribute("")]

71
Tools/ArdupilotMegaPlanner/TCPConsole.cs Normal file
View File

@ -0,0 +1,71 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Net;
using System.Net.Sockets;
namespace ArdupilotMega
{
static class TCPConsole
{
static TcpListener listener;
static TcpClient client;
static bool started = false;
static void startup()
{
started = true;
try
{
listener = new TcpListener(IPAddress.Any, 2300);
listener.Start();
listener.BeginAcceptTcpClient(
new AsyncCallback(DoAcceptTcpClientCallback),
listener);
}
catch { Console.WriteLine("TCP Console fail: port 2300"); return; }
}
// Process the client connection.
static void DoAcceptTcpClientCallback(IAsyncResult ar)
{
// Get the listener that handles the client request.
TcpListener listener = (TcpListener)ar.AsyncState;
// End the operation and display the received data on
// the console.
client = listener.EndAcceptTcpClient(ar);
// setup for next listener
listener.BeginAcceptTcpClient(
new AsyncCallback(DoAcceptTcpClientCallback),
listener);
// Process the connection here. (Add the client to a
// server table, read data, etc.)
Console.WriteLine("Client connected completed");
client.Client.NoDelay = true;
}
public static void Write(byte thing)
{
if (!started)
startup();
if (client == null)
return;
if (!client.Connected)
return;
try
{
client.GetStream().WriteByte(thing);
}
catch { }
}
}
}

2
Tools/ArdupilotMegaPlanner/app.config
View File

@ -3,5 +3,5 @@
<configSections>
</configSections>
<startup useLegacyV2RuntimeActivationPolicy="true">
</startup>
<supportedRuntime version="v2.0.50727"/></startup>
</configuration>

3
Tools/ArdupilotMegaPlanner/bin/Release/ArduinoCPP.exe.config Normal file
View File

@ -0,0 +1,3 @@
<?xml version="1.0"?>
<configuration>
<startup><supportedRuntime version="v2.0.50727"/></startup></configuration>

2
Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.exe.config
View File

@ -3,5 +3,5 @@
<configSections>
</configSections>
<startup useLegacyV2RuntimeActivationPolicy="true">
</startup>
<supportedRuntime version="v2.0.50727"/></startup>
</configuration>

2
Tools/ArdupilotMegaPlanner/georefimage.cs
View File

@ -51,7 +51,7 @@ namespace ArdupilotMega
}
catch (JpegProcessingException e)
{
Console.Error.WriteLine(e.Message);
Console.WriteLine(e.Message);
return dtaken;
}