Browse Source
fixup config highlighting mod log scanning increase calibation timeout add tcp console port 2300 add arduinoCPP - testmission-4.1.18
Michael Oborne
13 years ago
22 changed files with 1251 additions and 1412 deletions
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,104 @@
@@ -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)); |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,71 @@
@@ -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 { } |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,3 @@
@@ -0,0 +1,3 @@
|
||||
<?xml version="1.0"?> |
||||
<configuration> |
||||
<startup><supportedRuntime version="v2.0.50727"/></startup></configuration> |
||||
Loading…
Reference in new issue