public string doCommand(ArdupilotMega.Comms.ICommsSerial comPort, string cmd, int level = 0)
{
if (!comPort.IsOpen)
return "";
comPort.ReadTimeout = 1000;
// setup to known state
comPort.Write("\r\n");
// alow some time to gather thoughts
Sleep(100);
// ignore all existing data
comPort.DiscardInBuffer();
lbl_status.Text = "Doing Command " + cmd;
log.Info("Doing Command " + cmd);
// write command
comPort.Write(cmd + "\r\n");
// read echoed line or existing data
string temp;
try
{
temp = Serial_ReadLine(comPort);
}
catch { temp = comPort.ReadExisting(); }
log.Info("cmd " + cmd + " echo " + temp);
// delay for command
Sleep(500);
// get responce
string ans = "";
while (comPort.BytesToRead > 0)
{
try
{
ans = ans + Serial_ReadLine(comPort) + "\n";
}
catch { ans = ans + comPort.ReadExisting() + "\n"; }
Sleep(50);
if (ans.Length > 500)
{
break;
}
}
log.Info("responce " + level + " " + ans.Replace('\0', ' '));
Regex pattern = new Regex(@"^\[([0-9+])\]\s+",RegexOptions.Multiline);
if (pattern.IsMatch(ans))
{
Match mat = pattern.Match(ans);
ans = pattern.Replace(ans,"");
}
// try again
if (ans == "" && level == 0)
return doCommand(comPort, cmd, 1);
return ans;
}
/// <summary>
/// Recevied UDP packet, process and send required data to serial port.
/// </summary>
/// <param name="data">Packet</param>
/// <param name="receviedbytes">Length</param>
/// <param name="comPort">Com Port</param>
private void RECVprocess(byte[] data, int receviedbytes, ArdupilotMega.MAVLink comPort)
{
sitl_fdm sitldata = new sitl_fdm();
if (data[0] == 'D' && data[1] == 'A')
{
// Xplanes sends
// 5 byte header
// 1 int for the index - numbers on left of output
// 8 floats - might be useful. or 0 if not
int count = 5;
while (count < receviedbytes)
{
int index = BitConverter.ToInt32(data, count);
DATA[index] = new float[8];
DATA[index][0] = BitConverter.ToSingle(data, count + 1 * 4); ;
DATA[index][1] = BitConverter.ToSingle(data, count + 2 * 4); ;
DATA[index][2] = BitConverter.ToSingle(data, count + 3 * 4); ;
DATA[index][3] = BitConverter.ToSingle(data, count + 4 * 4); ;
DATA[index][4] = BitConverter.ToSingle(data, count + 5 * 4); ;
DATA[index][5] = BitConverter.ToSingle(data, count + 6 * 4); ;
DATA[index][6] = BitConverter.ToSingle(data, count + 7 * 4); ;
DATA[index][7] = BitConverter.ToSingle(data, count + 8 * 4); ;
count += 36; // 8 * float
}
bool xplane9 = !CHK_xplane10.Checked;
if (xplane9)
{
sitldata.pitchDeg = (DATA[18][0]);
sitldata.rollDeg = (DATA[18][1]);
sitldata.yawDeg = (DATA[18][2]);
sitldata.pitchRate = (DATA[17][0] * rad2deg);
sitldata.rollRate = (DATA[17][1] * rad2deg);
sitldata.yawRate = (DATA[17][2] * rad2deg);
sitldata.heading = ((float)DATA[19][2]);
}
else
{
sitldata.pitchDeg = (DATA[17][0]);
sitldata.rollDeg = (DATA[17][1]);
sitldata.yawDeg = (DATA[17][2]);
sitldata.pitchRate = (DATA[16][0] * rad2deg);
sitldata.rollRate = (DATA[16][1] * rad2deg);
sitldata.yawRate = (DATA[16][2] * rad2deg);
sitldata.heading = (DATA[18][2]); // 18-2
}
sitldata.airspeed = ((DATA[3][5] * .44704));
sitldata.latitude = (DATA[20][0]);
sitldata.longitude = (DATA[20][1]);
sitldata.altitude = (DATA[20][2] * ft2m);
sitldata.speedN = -DATA[21][5];// (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
sitldata.speedE = DATA[21][3];// (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));
sitldata.speedD = -DATA[21][4];
Matrix3 dcm = new Matrix3();
dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.heading * deg2rad);
// rad = tas^2 / (tan(angle) * G)
float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / (float)(9.808f * Math.Tan(sitldata.rollDeg * deg2rad)));
float gload = (float)(1 / Math.Cos(sitldata.rollDeg * deg2rad)); // calculated Gs
// a = v^2/r
float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad;
Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.808));
Vector3 centrip_accel = new Vector3(0, centripaccel * Math.Cos(sitldata.rollDeg * deg2rad), centripaccel * Math.Sin(sitldata.rollDeg * deg2rad));
// accel_body += centrip_accel;
Vector3 velocitydelta = dcm.transposed() * (new Vector3((sitldata_old.speedN - sitldata.speedN), (sitldata_old.speedE - sitldata.speedE), (sitldata_old.speedD - sitldata.speedD)));
Vector3 velocity = dcm.transposed() * (new Vector3((sitldata.speedN), (sitldata.speedE), (sitldata.speedD)));
//Console.WriteLine("vel " + velocity.ToString());
//Console.WriteLine("ved " + velocitydelta.ToString());
// a = dv / dt
// 50 hz = 0.02sec
Vector3 accel_mine_body = dcm.transposed() * (new Vector3((sitldata_old.speedN - sitldata.speedN) / 0.02, (sitldata_old.speedE - sitldata.speedE) / 0.02, (sitldata_old.speedD - sitldata.speedD) / 0.02));
// Console.WriteLine("G"+accel_body.ToString());
//Console.WriteLine("M"+accel_mine_body.ToString());
// sitldata.pitchRate = 0;
// sitldata.rollRate = 0;
// sitldata.yawRate = 0;
//accel_mine_body.x =0;// *= -1;
//accel_mine_body.y =0;//*= -1;
// accel_body -= accel_mine_body;
sitldata.xAccel = accel_body.x;// DATA[4][5] * 1;
sitldata.yAccel = accel_body.y;// DATA[4][6] * 1;
sitldata.zAccel = accel_body.z;// (0 - DATA[4][4]) * 9.808;
sitldata.xAccel = DATA[4][5] *9.808;
sitldata.yAccel = DATA[4][6] *9.808;
sitldata.zAccel = -DATA[4][4] *9.808;
// Console.WriteLine(accel_body.ToString());
// Console.WriteLine(" {0} {1} {2}",sitldata.xAccel, sitldata.yAccel, sitldata.zAccel);
}
else if (receviedbytes == 0x64) // FG binary udp
{
//FlightGear
/*
fgIMUData imudata2 = data.ByteArrayToStructureBigEndian<fgIMUData>(0);
if (imudata2.magic != 0x4c56414d)
return;
if (imudata2.latitude == 0)
return;
chkSensor.Checked = true;
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
imu.xmag = 0;
imu.yacc = ((Int16)(imudata2.accelY * 9808 / 32.2));
imu.ygyro = ((Int16)(imudata2.ratePitch * 17.453293));
imu.ymag = 0;
imu.zacc = ((Int16)(imudata2.accelZ * 9808 / 32.2)); // + 1000
imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
imu.zmag = 0;
gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
gps.lat = (int)(imudata2.latitude * 1.0e7);
gps.lon = (int)(imudata2.longitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);
//FileStream stream = File.OpenWrite("fgdata.txt");
//stream.Write(data, 0, receviedbytes);
//stream.Close();
*/
}
else if (receviedbytes == 662 || receviedbytes == 658) // 658 = 3.83 662 = 3.91
{
/*
// Model data in body frame coordinates (X=Right, Y=Front, Z=Up)
public float Model_fVel_Body_X; public float Model_fVel_Body_Y; public float Model_fVel_Body_Z; // m/s Model velocity in body coordinates
public float Model_fAngVel_Body_X; public float Model_fAngVel_Body_Y; public float Model_fAngVel_Body_Z; // rad/s Model angular velocity in body coordinates
public float Model_fAccel_Body_X; public float Model_fAccel_Body_Y; public float Model_fAccel_Body_Z; // m/s/s Model acceleration in body coordinates
*/
TDataFromAeroSimRC aeroin_last = aeroin;
aeroin = data.ByteArrayToStructure<TDataFromAeroSimRC>(0);
sitldata.pitchDeg = (aeroin.Model_fPitch * rad2deg);
sitldata.rollDeg = (aeroin.Model_fRoll * -1 * rad2deg);
sitldata.yawDeg = ((aeroin.Model_fHeading * rad2deg));
sitldata.pitchRate =aeroin.Model_fAngVel_Body_X * rad2deg;
sitldata.rollRate = aeroin.Model_fAngVel_Body_Y * rad2deg;
sitldata.yawRate = -aeroin.Model_fAngVel_Body_Z * rad2deg;
// calc gravity
Matrix3 dcm = new Matrix3();
dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.yawDeg * deg2rad);
Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.8)); // -9.8
sitldata.xAccel = aeroin.Model_fAccel_Body_Y / 9.808 + accel_body.x;// pitch - back forward-
sitldata.yAccel = aeroin.Model_fAccel_Body_X / 9.808 + accel_body.y; // roll - left right-
sitldata.zAccel = -aeroin.Model_fAccel_Body_Z /9.808 + accel_body.z;
// Console.WriteLine("2 {0,20} {1,20} {2,20}", aeroin.Model_fAccel_Body_X.ToString("0.000"), aeroin.Model_fAccel_Body_Y.ToString("0.000"), aeroin.Model_fAccel_Body_Z.ToString("0.000"));
sitldata.altitude = aeroin.Model_fPosZ;
sitldata.latitude = aeroin.Model_fLatitude;
sitldata.longitude = aeroin.Model_fLongitude;
sitldata.speedN = aeroin.Model_fVelY;
sitldata.speedE = aeroin.Model_fVelX;
sitldata.speedD = aeroin.Model_fVelZ;
float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;
sitldata.airspeed = ((float)Math.Sqrt((yvec * yvec) + (xvec * xvec)));
}
else if (receviedbytes == 408)
{
FGNetFDM fdm = data.ByteArrayToStructureBigEndian<FGNetFDM>(0);
lastfdmdata = fdm;
sitldata.altitude = (fdm.altitude);
sitldata.latitude = (fdm.latitude * rad2deg);
sitldata.longitude = (fdm.longitude * rad2deg);
sitldata.rollDeg = fdm.phi * rad2deg;
sitldata.pitchDeg = fdm.theta * rad2deg;
sitldata.yawDeg = fdm.psi * rad2deg;
sitldata.rollRate = fdm.phidot * rad2deg;
sitldata.pitchRate = fdm.thetadot * rad2deg;
sitldata.yawRate = fdm.psidot * rad2deg;
sitldata.speedN = fdm.v_north * ft2m;
sitldata.speedE = fdm.v_east * ft2m;
sitldata.speedD = fdm.v_down * ft2m;
sitldata.xAccel = (fdm.A_X_pilot * 9.808 / 32.2); // pitch
sitldata.yAccel = (fdm.A_Y_pilot * 9.808 / 32.2); // roll
sitldata.zAccel = (fdm.A_Z_pilot / 32.2 * 9.808);
sitldata.airspeed = fdm.vcas * 0.5144444f;// knots to m/s
// Console.WriteLine("1 {0} {1} {2} {3}",(float)sitldata.rollDeg,MainV2.comPort.MAV.cs.roll,sitldata.pitchDeg,MainV2.comPort.MAV.cs.pitch);
if (RAD_JSBSim.Checked)
sitldata.airspeed = fdm.vcas * ft2m;// fps to m/s
}
else
{
log.Info("Bad Udp Packet " + receviedbytes);
return;
}
if (sitldata.altitude < 0)
sitldata.altitude = 0.00001;
sitldata_old = sitldata;
// write arduimu to ardupilot
if (CHK_quad.Checked && !RAD_aerosimrc.Checked) // quad does its own
{
return;
}
if (RAD_JSBSim.Checked && chkSITL.Checked)
{
byte[] buffer = new byte[1500];
while (JSBSimSEND.Client.Available > 5)
{
int read = JSBSimSEND.Client.Receive(buffer);
// Console.WriteLine(ASCIIEncoding.ASCII.GetString(buffer,0,read));
}
sitldata.magic = (int)0x4c56414f;
byte[] sendme = StructureToByteArray(sitldata);
SITLSEND.Send(sendme, sendme.Length);
return;
}
if (chkSITL.Checked)
{
sitldata.magic = (int)0x4c56414f;
byte[] sendme = StructureToByteArray(sitldata);
SITLSEND.Send(sendme, sendme.Length);
return;
}
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
// add gps delay
if (gpsspan.TotalMilliseconds >= GPS_rate)
{
lastgpsupdate = DateTime.Now;
// save current fix = 3
sitl_fdmbuffer[gpsbufferindex % sitl_fdmbuffer.Length] = sitldata;
// Console.WriteLine((gpsbufferindex % gpsbuffer.Length) + " " + ((gpsbufferindex + (gpsbuffer.Length - 1)) % gpsbuffer.Length));
// return buffer index + 5 = (3 + 5) = 8 % 6 = 2
oldgps = sitl_fdmbuffer[(gpsbufferindex + (sitl_fdmbuffer.Length - 1)) % sitl_fdmbuffer.Length];
//comPort.sendPacket(oldgps);
gpsbufferindex++;
}
MAVLink.mavlink_hil_state_t hilstate = new MAVLink.mavlink_hil_state_t();
hilstate.time_usec = (UInt64)DateTime.Now.Ticks; // microsec
hilstate.lat = (int)(oldgps.latitude * 1e7); // * 1E7
hilstate.lon = (int)(oldgps.longitude * 1e7); // * 1E7
hilstate.alt = (int)(oldgps.altitude * 1000); // mm
// Console.WriteLine(hilstate.alt);
// Console.WriteLine("{0} {1} {2}", sitldata.rollDeg.ToString("0.0"), sitldata.pitchDeg.ToString("0.0"), sitldata.yawDeg.ToString("0.0"));
hilstate.pitch = (float)sitldata.pitchDeg * deg2rad; // (rad)
hilstate.pitchspeed = (float)sitldata.pitchRate * deg2rad; // (rad/s)
hilstate.roll = (float)sitldata.rollDeg * deg2rad; // (rad)
hilstate.rollspeed = (float)sitldata.rollRate * deg2rad; // (rad/s)
hilstate.yaw = (float)sitldata.yawDeg * deg2rad; // (rad)
hilstate.yawspeed = (float)sitldata.yawRate * deg2rad; // (rad/s)
hilstate.vx = (short)(sitldata.speedN * 100); // m/s * 100 - lat
hilstate.vy = (short)(sitldata.speedE * 100); // m/s * 100 - long
hilstate.vz = (short)(sitldata.speedD * 100); // m/s * 100 - + speed down
hilstate.xacc = (short)(sitldata.xAccel * 101.957); // (mg)
hilstate.yacc = (short)(sitldata.yAccel * 101.957); // (mg)
hilstate.zacc = (short)(sitldata.zAccel * 101.957); // (mg)
packetcount++;
if (comPort.BaseStream.BytesToWrite > 100)
return;
// if (packetcount % 2 == 0)
// return;
comPort.sendPacket(hilstate);
// comPort.sendPacket(oldgps);
//comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed } );
MAVLink.mavlink_raw_pressure_t pres = new MAVLink.mavlink_raw_pressure_t();
double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * sitldata.altitude, 5.25588)); // updated from valid gps
pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa
// comPort.sendPacket(pres);
}
string Serial_ReadLine(ArdupilotMega.Comms.ICommsSerial comPort)
{
StringBuilder sb = new StringBuilder();
DateTime Deadline = DateTime.Now.AddMilliseconds(comPort.ReadTimeout);
while (DateTime.Now < Deadline)
{
if (comPort.BytesToRead > 0)
{
byte data = (byte)comPort.ReadByte();
sb.Append((char)data);
if (data == '\n')
break;
}
}
return sb.ToString();
}
bool doConnect(ArdupilotMega.Comms.ICommsSerial comPort)
{
// clear buffer
comPort.DiscardInBuffer();
// setup a known enviroment
comPort.Write("\r\n");
// wait
Sleep(1100);
// send config string
comPort.Write("+++");
// wait
Sleep(1100);
// check for config responce "OK"
Console.WriteLine("Connect btr " + comPort.BytesToRead + " baud " + comPort.BaudRate);
string conn = comPort.ReadExisting();
Console.WriteLine("Connect first responce " + conn.Replace('\0', ' ') + " " + conn.Length);
if (conn.Contains("OK"))
{
//return true;
}
else
{
// cleanup incase we are already in cmd mode
comPort.Write("\r\n");
}
string version = doCommand(comPort, "ATI");
Console.Write("Connect Version: " + version.Trim() + "\n");
if (version.Contains("SiK") && version.Contains("on")) // should use a regex....
{
return true;
}
return false;
}
string doCommand(ArdupilotMega.Comms.ICommsSerial comPort, string cmd, int level = 0)
{
if (!comPort.IsOpen)
return "";
comPort.ReadTimeout = 1000;
// setup to known state
comPort.Write("\r\n");
// alow some time to gather thoughts
Sleep(100);
// ignore all existing data
comPort.DiscardInBuffer();
lbl_status.Text = "Doing Command " + cmd;
Console.WriteLine("Doing Command " + cmd);
// write command
comPort.Write(cmd + "\r\n");
// read echoed line or existing data
string temp;
try
{
temp = Serial_ReadLine(comPort);
}
catch { temp = comPort.ReadExisting(); }
Console.WriteLine("cmd " + cmd + " echo " + temp);
// delay for command
Sleep(500);
// get responce
string ans = "";
while (comPort.BytesToRead > 0)
{
try
{
ans = ans + Serial_ReadLine(comPort) + "\n";
}
catch { ans = ans + comPort.ReadExisting() + "\n"; }
Sleep(50);
if (ans.Length > 500)
{
break;
}
}
Console.WriteLine("responce " + level + " " + ans.Replace('\0', ' '));
// try again
if (ans == "" && level == 0)
return doCommand(comPort, cmd, 1);
return ans;
}
void frmProgressReporter_DoWork(object sender, ArdupilotMega.Controls.ProgressWorkerEventArgs e)
{
string dest = Port;
if (ArdupilotMega.MainV2.config["UDP_port"] != null)
dest = ArdupilotMega.MainV2.config["UDP_port"].ToString();
ArdupilotMega.Common.InputBox("Listern Port", "Enter Local port (ensure remote end is already sending)", ref dest);
Port = dest;
ArdupilotMega.MainV2.config["UDP_port"] = Port;
client = new UdpClient(int.Parse(Port));
while (true)
{
((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Waiting for UDP");
System.Threading.Thread.Sleep(500);
if (((ProgressReporterDialogue)sender).doWorkArgs.CancelRequested)
{
((ProgressReporterDialogue)sender).doWorkArgs.CancelAcknowledged = true;
try
{
client.Close();
}
catch { }
return;
}
if (BytesToRead > 0)
break;
}
if (BytesToRead == 0)
return;
try
{
client.Receive(ref RemoteIpEndPoint);
log.InfoFormat("NetSerial connecting to {0} : {1}", RemoteIpEndPoint.Address, RemoteIpEndPoint.Port);
client.Connect(RemoteIpEndPoint);
}
catch (Exception ex)
{
if (client != null && client.Client.Connected)
{
client.Close();
}
log.Info(ex.ToString());
System.Windows.Forms.CustomMessageBox.Show("Please check your Firewall settings\nPlease try running this command\n1. Run the following command in an elevated command prompt to disable Windows Firewall temporarily:\n \nNetsh advfirewall set allprofiles state off\n \nNote: This is just for test; please turn it back on with the command 'Netsh advfirewall set allprofiles state on'.\n");
throw new Exception("The socket/serialproxy is closed " + e);
}
}
public void update(ref double[] servos, ArdupilotMega.GCSViews.Simulation.FGNetFDM fdm)
{
for (int i = 0; i < servos.Length; i++)
{
var servo = servos[(int)self.motors[i].servo - 1];
if (servo <= 0.0)
{
motor_speed[i] = 0;
}
else
{
motor_speed[i] = scale_rc(i, (float)servo, 0.0f, 1.0f);
//servos[i] = motor_speed[i];
}
}
double[] m = motor_speed;
//# how much time has passed?
DateTime t = DateTime.Now;
TimeSpan delta_time = t - last_time; // 0.02
last_time = t;
if (delta_time.TotalMilliseconds > 100) // somethings wrong / debug
{
delta_time = new TimeSpan(0, 0, 0, 0, 20);
}
// rotational acceleration, in degrees/s/s, in body frame
Vector3 rot_accel = new Vector3(0, 0, 0);
double thrust = 0.0;
foreach (var i in range((self.motors.Length)))
{
rot_accel.x += -radians(5000.0) * sin(radians(self.motors[i].angle)) * m[i];
rot_accel.y += radians(5000.0) * cos(radians(self.motors[i].angle)) * m[i];
if (self.motors[i].clockwise)
{
rot_accel.z -= m[i] * radians(400.0);
}
else
{
rot_accel.z += m[i] * radians(400.0);
}
thrust += m[i] * self.thrust_scale; // newtons
}
// rotational air resistance
rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate;
rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate;
rot_accel.z -= self.gyro.z * radians(400.0) / self.terminal_rotation_rate;
// Console.WriteLine("rot_accel " + rot_accel.ToString());
// update rotational rates in body frame
self.gyro += rot_accel * delta_time.TotalSeconds;
// Console.WriteLine("gyro " + gyro.ToString());
// update attitude
self.dcm.rotate(self.gyro * delta_time.TotalSeconds);
self.dcm.normalize();
// air resistance
Vector3 air_resistance = -self.velocity * (self.gravity / self.terminal_velocity);
accel_body = new Vector3(0, 0, -thrust / self.mass);
Vector3 accel_earth = self.dcm * accel_body;
accel_earth += new Vector3(0, 0, self.gravity);
accel_earth += air_resistance;
// add in some wind (turn force into accel by dividing by mass).
accel_earth += self.wind.drag(self.velocity) / self.mass;
// if we're on the ground, then our vertical acceleration is limited
// to zero. This effectively adds the force of the ground on the aircraft
if (self.on_ground() && accel_earth.z > 0)
accel_earth.z = 0;
// work out acceleration as seen by the accelerometers. It sees the kinematic
// acceleration (ie. real movement), plus gravity
self.accel_body = self.dcm.transposed() * (accel_earth + new Vector3(0, 0, -self.gravity));
// new velocity vector
self.velocity += accel_earth * delta_time.TotalSeconds;
if (double.IsNaN(velocity.x) || double.IsNaN(velocity.y) || double.IsNaN(velocity.z))
velocity = new Vector3();
// new position vector
old_position = self.position.copy();
self.position += self.velocity * delta_time.TotalSeconds;
if (home_latitude == 0)
{
home_latitude = fdm.latitude * rad2deg;
home_longitude = fdm.longitude * rad2deg;
home_altitude = altitude;
}
// constrain height to the ground
if (self.on_ground())
{
if (!self.on_ground(old_position))
Console.WriteLine("Hit ground at {0} m/s", (self.velocity.z));
self.velocity = new Vector3(0, 0, 0);
// zero roll/pitch, but keep yaw
double r = 0;
double p = 0;
double y = 0;
self.dcm.to_euler(ref r, ref p, ref y);
self.dcm.from_euler(0, 0, y);
self.position = new Vector3(self.position.x, self.position.y,
-(self.ground_level + self.frame_height - self.home_altitude));
}
// update lat/lon/altitude
self.update_position(delta_time.TotalSeconds);
// send to apm
MAVLink.mavlink_hil_state_t hilstate = new MAVLink.mavlink_hil_state_t();
hilstate.time_usec = (UInt64)DateTime.Now.Ticks; // microsec
hilstate.lat = (int)(latitude * 1e7); // * 1E7
hilstate.lon = (int)(longitude * 1e7); // * 1E7
hilstate.alt = (int)(altitude * 1000); // mm
self.dcm.to_euler(ref roll, ref pitch, ref yaw);
if (double.IsNaN(roll))
{
self.dcm.identity();
}
hilstate.roll = (float)roll;
hilstate.pitch = (float)pitch;
hilstate.yaw = (float)yaw;
Vector3 earth_rates = Utils.BodyRatesToEarthRates(dcm, gyro);
hilstate.rollspeed = (float)earth_rates.x;
hilstate.pitchspeed = (float)earth_rates.y;
hilstate.yawspeed = (float)earth_rates.z;
hilstate.vx = (short)(velocity.y * 100); // m/s * 100
hilstate.vy = (short)(velocity.x * 100); // m/s * 100
hilstate.vz = 0; // m/s * 100
hilstate.xacc = (short)(accelerometer.x * 1000); // (mg)
hilstate.yacc = (short)(accelerometer.y * 1000); // (mg)
hilstate.zacc = (short)(accelerometer.z * 1000); // (mg)
MainV2.comPort.sendPacket(hilstate);
}
public bool doConnect(ArdupilotMega.Comms.ICommsSerial comPort)
{
try
{
// clear buffer
comPort.DiscardInBuffer();
// setup a known enviroment
comPort.Write("\r\n");
// wait
Sleep(1100);
// send config string
comPort.Write("+++");
// wait
Sleep(1100);
// check for config responce "OK"
log.Info("Connect btr " + comPort.BytesToRead + " baud " + comPort.BaudRate);
string conn = comPort.ReadExisting();
log.Info("Connect first responce " + conn.Replace('\0', ' ') + " " + conn.Length);
if (conn.Contains("OK"))
{
//return true;
}
else
{
// cleanup incase we are already in cmd mode
comPort.Write("\r\n");
}
doCommand(comPort, "AT&T");
string version = doCommand(comPort, "ATI");
log.Info("Connect Version: " + version.Trim() + "\n");
Regex regex = new Regex(@"SiK\s+(.*)\s+on\s+(.*)");
if (regex.IsMatch(version))
{
return true;
}
return false;
}
catch { return false; }
}
void getDatastream(ArdupilotMega.MAVLink.MAV_DATA_STREAM id, byte hzrate)
{
mavlink_request_data_stream_t req = new mavlink_request_data_stream_t();
req.target_system = sysid;
req.target_component = compid;
req.req_message_rate = hzrate;
req.start_stop = 1; // start
req.req_stream_id = (byte)id; // id
// send each one twice.
generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
}
/// <summary>
/// Recevied UDP packet, process and send required data to serial port.
/// </summary>
/// <param name="data">Packet</param>
/// <param name="receviedbytes">Length</param>
/// <param name="comPort">Com Port</param>
private void RECVprocess(byte[] data, int receviedbytes, ArdupilotMega.MAVLink comPort)
{
sitl_fdm sitldata = new sitl_fdm();
if (data[0] == 'D' && data[1] == 'A')
{
// Xplanes sends
// 5 byte header
// 1 int for the index - numbers on left of output
// 8 floats - might be useful. or 0 if not
int count = 5;
while (count < receviedbytes)
{
int index = BitConverter.ToInt32(data, count);
DATA[index] = new float[8];
DATA[index][0] = BitConverter.ToSingle(data, count + 1 * 4); ;
DATA[index][1] = BitConverter.ToSingle(data, count + 2 * 4); ;
DATA[index][2] = BitConverter.ToSingle(data, count + 3 * 4); ;
DATA[index][3] = BitConverter.ToSingle(data, count + 4 * 4); ;
DATA[index][4] = BitConverter.ToSingle(data, count + 5 * 4); ;
DATA[index][5] = BitConverter.ToSingle(data, count + 6 * 4); ;
DATA[index][6] = BitConverter.ToSingle(data, count + 7 * 4); ;
DATA[index][7] = BitConverter.ToSingle(data, count + 8 * 4); ;
count += 36; // 8 * float
}
bool xplane9 = !CHK_xplane10.Checked;
if (xplane9)
{
sitldata.pitchDeg = (DATA[18][0]);
sitldata.rollDeg = (DATA[18][1]);
sitldata.yawDeg = (DATA[18][2]);
sitldata.pitchRate = (DATA[17][0] * rad2deg);
sitldata.rollRate = (DATA[17][1] * rad2deg);
sitldata.yawRate = (DATA[17][2] * rad2deg);
sitldata.heading = ((float)DATA[19][2]);
}
else
{
sitldata.pitchDeg = (DATA[17][0]);
sitldata.rollDeg = (DATA[17][1]);
sitldata.yawDeg = (DATA[17][2]);
sitldata.pitchRate = (DATA[16][0] * rad2deg);
sitldata.rollRate = (DATA[16][1] * rad2deg);
sitldata.yawRate = (DATA[16][2] * rad2deg);
sitldata.heading = (DATA[18][2]);
}
sitldata.airspeed = ((DATA[3][5] * .44704));
sitldata.latitude = (DATA[20][0]);
sitldata.longitude = (DATA[20][1]);
sitldata.altitude = (DATA[20][2] * ft2m);
sitldata.speedN = DATA[21][3];// (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
sitldata.speedE = -DATA[21][5];// (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));
Matrix3 dcm = new Matrix3();
dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.yawDeg * deg2rad);
// rad = tas^2 / (tan(angle) * G)
float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / (float)(9.8f * Math.Tan(sitldata.rollDeg * deg2rad)));
float gload = (float)(1 / Math.Cos(sitldata.rollDeg * deg2rad)); // calculated Gs
// a = v^2/r
float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad;
Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.8));
Vector3 centrip_accel = new Vector3(0, centripaccel * Math.Cos(sitldata.rollDeg * deg2rad), centripaccel * Math.Sin(sitldata.rollDeg * deg2rad));
accel_body -= centrip_accel;
sitldata.xAccel = DATA[4][5] * 9.8;
sitldata.yAccel = DATA[4][6] * 9.8;
sitldata.zAccel = (0 - DATA[4][4]) * 9.8;
// Console.WriteLine(accel_body.ToString());
// Console.WriteLine(" {0} {1} {2}",sitldata.xAccel, sitldata.yAccel, sitldata.zAccel);
}
else if (receviedbytes == 0x64) // FG binary udp
{
//FlightGear
/*
fgIMUData imudata2 = data.ByteArrayToStructureBigEndian<fgIMUData>(0);
if (imudata2.magic != 0x4c56414d)
return;
if (imudata2.latitude == 0)
return;
chkSensor.Checked = true;
#if MAVLINK10
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
#else
imu.usec = ((ulong)DateTime.Now.ToBinary());
#endif
imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
imu.xmag = 0;
imu.yacc = ((Int16)(imudata2.accelY * 9808 / 32.2));
imu.ygyro = ((Int16)(imudata2.ratePitch * 17.453293));
imu.ymag = 0;
imu.zacc = ((Int16)(imudata2.accelZ * 9808 / 32.2)); // + 1000
imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
imu.zmag = 0;
#if MAVLINK10
gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
gps.lat = (int)(imudata2.latitude * 1.0e7);
gps.lon = (int)(imudata2.longitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);
#else
gps.alt = ((float)(imudata2.altitude * ft2m));
gps.fix_type = 3;
gps.hdg = ((float)Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg);
gps.lat = ((float)imudata2.latitude);
gps.lon = ((float)imudata2.longitude);
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m);
#endif
//FileStream stream = File.OpenWrite("fgdata.txt");
//stream.Write(data, 0, receviedbytes);
//stream.Close();
*/
}
else if (receviedbytes == 662 || receviedbytes == 658) // 658 = 3.83 662 = 3.91
{
aeroin = data.ByteArrayToStructure<TDataFromAeroSimRC>(0);
sitldata.pitchDeg = (aeroin.Model_fPitch * rad2deg);
sitldata.rollDeg = (aeroin.Model_fRoll * -1 * rad2deg);
sitldata.yawDeg = ((aeroin.Model_fHeading * rad2deg));
sitldata.pitchRate = aeroin.Model_fAngVel_Body_X * rad2deg;
sitldata.rollRate = aeroin.Model_fAngVel_Body_Y * rad2deg;
sitldata.yawRate = -aeroin.Model_fAngVel_Body_Z * rad2deg;
sitldata.xAccel = aeroin.Model_fAccel_Body_X; // pitch
sitldata.yAccel = aeroin.Model_fAccel_Body_Y; // roll
sitldata.zAccel = aeroin.Model_fAccel_Body_Z;
// YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(att.roll, att.pitch, 0, -9.8); //DATA[18][2]
sitldata.altitude = aeroin.Model_fPosZ;
sitldata.latitude = aeroin.Model_fLatitude;
sitldata.longitude = aeroin.Model_fLongitude;
sitldata.speedN = aeroin.Model_fVelX;
sitldata.speedE = aeroin.Model_fVelY;
float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;
sitldata.airspeed = ((float)Math.Sqrt((yvec * yvec) + (xvec * xvec)));
}
else if (receviedbytes == 408)
{
FGNetFDM fdm = data.ByteArrayToStructureBigEndian<FGNetFDM>(0);
lastfdmdata = fdm;
sitldata.rollDeg = fdm.phi * rad2deg;
sitldata.pitchDeg = fdm.theta * rad2deg;
sitldata.yawDeg = fdm.psi * rad2deg;
sitldata.rollRate = fdm.phidot * rad2deg;
sitldata.pitchRate = fdm.thetadot * rad2deg;
sitldata.yawRate = fdm.psidot * rad2deg;
sitldata.xAccel = (fdm.A_X_pilot * 9.808 / 32.2); // pitch
sitldata.yAccel = (fdm.A_Y_pilot * 9.808 / 32.2); // roll
sitldata.zAccel = (fdm.A_Z_pilot / 32.2 * 9.808);
sitldata.altitude = (fdm.altitude);
sitldata.latitude = (fdm.latitude * rad2deg);
sitldata.longitude = (fdm.longitude * rad2deg);
sitldata.speedN = fdm.v_east * ft2m;
sitldata.speedE = fdm.v_north * ft2m;
sitldata.airspeed = fdm.vcas * 0.5144444f;// knots to m/s
if (RAD_JSBSim.Checked)
sitldata.airspeed = fdm.vcas * 0.3048f;// fps to m/s
}
else
{
log.Info("Bad Udp Packet " + receviedbytes);
return;
}
// write arduimu to ardupilot
if (CHK_quad.Checked && !RAD_aerosimrc.Checked) // quad does its own
{
return;
}
if (RAD_JSBSim.Checked && chkSensor.Checked)
{
byte[] buffer = new byte[1500];
while (JSBSimSEND.Client.Available > 5)
{
int read = JSBSimSEND.Client.Receive(buffer);
}
byte[] sitlout = new byte[16 * 8 + 1 * 4]; // 16 * double + 1 * int
int a = 0;
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.latitude * rad2deg), a, sitlout, a, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.longitude * rad2deg), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.altitude), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.psi * rad2deg), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.v_north * ft2m), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.v_east * ft2m), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.A_X_pilot * ft2m), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.A_Y_pilot * ft2m), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.A_Z_pilot * ft2m), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.phidot * rad2deg), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.thetadot * rad2deg), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.psidot * rad2deg), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.phi * rad2deg), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.theta * rad2deg), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.psi * rad2deg), 0, sitlout, a += 8, 8);
Array.Copy(BitConverter.GetBytes((double)lastfdmdata.vcas * ft2m), 0, sitlout, a += 8, 8);
// Console.WriteLine(lastfdmdata.theta);
Array.Copy(BitConverter.GetBytes((int)0x4c56414e), 0, sitlout, a += 8, 4);
SITLSEND.Send(sitlout, sitlout.Length);
return;
}
if (RAD_softXplanes.Checked && chkSensor.Checked)
{
sitldata.magic = (int)0x4c56414e;
byte[] sendme = StructureToByteArray(sitldata);
SITLSEND.Send(sendme, sendme.Length);
return;
}
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
// add gps delay
if (gpsspan.TotalMilliseconds >= GPS_rate)
{
lastgpsupdate = DateTime.Now;
// save current fix = 3
sitl_fdmbuffer[gpsbufferindex % sitl_fdmbuffer.Length] = sitldata;
// Console.WriteLine((gpsbufferindex % gpsbuffer.Length) + " " + ((gpsbufferindex + (gpsbuffer.Length - 1)) % gpsbuffer.Length));
// return buffer index + 5 = (3 + 5) = 8 % 6 = 2
oldgps = sitl_fdmbuffer[(gpsbufferindex + (sitl_fdmbuffer.Length - 1)) % sitl_fdmbuffer.Length];
//comPort.sendPacket(oldgps);
gpsbufferindex++;
}
MAVLink.mavlink_hil_state_t hilstate = new MAVLink.mavlink_hil_state_t();
hilstate.time_usec = (UInt64)DateTime.Now.Ticks; // microsec
hilstate.lat = (int)(oldgps.latitude * 1e7); // * 1E7
hilstate.lon = (int)(oldgps.longitude * 1e7); // * 1E7
hilstate.alt = (int)(oldgps.altitude * 1000); // mm
// Console.WriteLine(hilstate.alt);
hilstate.pitch = (float)sitldata.pitchDeg * deg2rad; // (rad)
hilstate.pitchspeed = (float)sitldata.pitchRate * deg2rad; // (rad/s)
hilstate.roll = (float)sitldata.rollDeg * deg2rad; // (rad)
hilstate.rollspeed = (float)sitldata.rollRate * deg2rad; // (rad/s)
hilstate.yaw = (float)sitldata.yawDeg * deg2rad; // (rad)
hilstate.yawspeed = (float)sitldata.yawRate * deg2rad; // (rad/s)
hilstate.vx = (short)(sitldata.speedN * 100); // m/s * 100
hilstate.vy = (short)(sitldata.speedE * 100); // m/s * 100
hilstate.vz = 0; // m/s * 100
hilstate.xacc = (short)(sitldata.xAccel * 1000); // (mg)
hilstate.yacc = (short)(sitldata.yAccel * 1000); // (mg)
hilstate.zacc = (short)(sitldata.zAccel * 1000); // (mg)
comPort.sendPacket(hilstate);
// comPort.sendPacket(oldgps);
comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed } );
MAVLink.mavlink_raw_pressure_t pres = new MAVLink.mavlink_raw_pressure_t();
double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * sitldata.altitude, 5.25588)); // updated from valid gps
pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa
// comPort.sendPacket(pres);
}
public void requestDatastream(ArdupilotMega.MAVLink.MAV_DATA_STREAM id, byte hzrate)
{
double pps = 0;
switch (id)
{
case MAVLink.MAV_DATA_STREAM.ALL:
break;
case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS:
if (packetspersecondbuild[MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_SYS_STATUS];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.EXTRA1:
if (packetspersecondbuild[MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_ATTITUDE];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.EXTRA2:
if (packetspersecondbuild[MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_VFR_HUD];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.EXTRA3:
if (packetspersecondbuild[MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_AHRS];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.POSITION:
if (packetspersecondbuild[MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER:
if (packetspersecondbuild[MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.RAW_SENSORS:
if (packetspersecondbuild[MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_RAW_IMU];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.RC_CHANNELS:
if (packetspersecondbuild[MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_RC_CHANNELS_RAW];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
}
//packetspersecond[temp[5]];
if (pps == 0 && hzrate == 0)
{
return;
}
log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
getDatastream(id, hzrate);
}
public bool doCommand(ArdupilotMega.MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
{
// mavlink 10
throw new NotImplementedException();
}
internal void ProcessDeviceChanged(ArdupilotMega.MainV2.WM_DEVICECHANGE_enum dc)
{
if (DeviceChanged != null)
{
try
{
DeviceChanged(dc);
}
catch { }
}
}
/// <summary>
/// Recevied UDP packet, process and send required data to serial port.
/// </summary>
/// <param name="data">Packet</param>
/// <param name="receviedbytes">Length</param>
/// <param name="comPort">Com Port</param>
private void RECVprocess(byte[] data, int receviedbytes, ArdupilotMega.MAVLink comPort)
{
#if MAVLINK10
ArdupilotMega.MAVLink.__mavlink_hil_state_t hilstate = new ArdupilotMega.MAVLink.__mavlink_hil_state_t();
ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
#else
ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t();
#endif
ArdupilotMega.MAVLink.__mavlink_raw_imu_t imu = new ArdupilotMega.MAVLink.__mavlink_raw_imu_t();
ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
ArdupilotMega.MAVLink.__mavlink_vfr_hud_t asp = new ArdupilotMega.MAVLink.__mavlink_vfr_hud_t();
if (data[0] == 'D' && data[1] == 'A')
{
// Xplanes sends
// 5 byte header
// 1 int for the index - numbers on left of output
// 8 floats - might be useful. or 0 if not
int count = 5;
while (count < receviedbytes)
{
int index = BitConverter.ToInt32(data, count);
DATA[index] = new float[8];
DATA[index][0] = BitConverter.ToSingle(data, count + 1 * 4); ;
DATA[index][1] = BitConverter.ToSingle(data, count + 2 * 4); ;
DATA[index][2] = BitConverter.ToSingle(data, count + 3 * 4); ;
DATA[index][3] = BitConverter.ToSingle(data, count + 4 * 4); ;
DATA[index][4] = BitConverter.ToSingle(data, count + 5 * 4); ;
DATA[index][5] = BitConverter.ToSingle(data, count + 6 * 4); ;
DATA[index][6] = BitConverter.ToSingle(data, count + 7 * 4); ;
DATA[index][7] = BitConverter.ToSingle(data, count + 8 * 4); ;
count += 36; // 8 * float
}
att.pitch = (DATA[18][0] * deg2rad);
att.roll = (DATA[18][1] * deg2rad);
att.yaw = (DATA[18][2] * deg2rad);
att.pitchspeed = (DATA[17][0]);
att.rollspeed = (DATA[17][1]);
att.yawspeed = (DATA[17][2]);
TimeSpan timediff = DateTime.Now - oldtime;
float pdiff = (float)((att.pitch - oldatt.pitch) / timediff.TotalSeconds);
float rdiff = (float)((att.roll - oldatt.roll) / timediff.TotalSeconds);
float ydiff = (float)((att.yaw - oldatt.yaw) / timediff.TotalSeconds);
//Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]);
oldatt = att;
rdiff = DATA[17][1];
pdiff = DATA[17][0];
ydiff = DATA[17][2];
Int16 xgyro = Constrain(rdiff * 1000.0, Int16.MinValue, Int16.MaxValue);
Int16 ygyro = Constrain(pdiff * 1000.0, Int16.MinValue, Int16.MaxValue);
Int16 zgyro = Constrain(ydiff * 1000.0, Int16.MinValue, Int16.MaxValue);
oldtime = DateTime.Now;
YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1], DATA[18][0], 0, -9.8); //DATA[18][2]
float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704) * 1.943844) / (float)(11.26 * Math.Tan(att.roll))) * ft2m;
float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad;
//Console.WriteLine("old {0} {1} {2}",accel3D.X,accel3D.Y,accel3D.Z);
YLScsDrawing.Drawing3d.Vector3d cent3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1] - 90, 0, 0, centripaccel);
accel3D -= cent3D;
//Console.WriteLine("new {0} {1} {2}", accel3D.X, accel3D.Y, accel3D.Z);
oldax = DATA[4][5];
olday = DATA[4][6];
oldaz = DATA[4][4];
double head = DATA[18][2] - 90;
#if MAVLINK10
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
#else
imu.usec = ((ulong)DateTime.Now.ToBinary());
#endif
imu.xgyro = xgyro; // roll - yes
imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = ygyro; // pitch - yes
imu.ymag = (short)(Math.Cos(head * deg2rad) * 1000);
imu.zgyro = zgyro;
imu.zmag = 0;
imu.xacc = (Int16)(accel3D.X * 1000); // pitch
imu.yacc = (Int16)(accel3D.Y * 1000); // roll
imu.zacc = (Int16)(accel3D.Z * 1000);
//Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
#if MAVLINK10
gps.alt = (int)(DATA[20][2] * ft2m * 1000);
gps.fix_type = 3;
gps.cog = (ushort)(DATA[19][2] * 100);
gps.lat = (int)(DATA[20][0] * 1.0e7);
gps.lon = (int)(DATA[20][1] * 1.0e7);
gps.time_usec = ((ulong)0);
gps.vel = (ushort)(DATA[3][7] * 0.44704 * 100);
#else
gps.alt = ((float)(DATA[20][2] * ft2m));
gps.fix_type = 3;
gps.hdg = ((float)DATA[19][2]);
gps.lat = ((float)DATA[20][0]);
gps.lon = ((float)DATA[20][1]);
gps.usec = ((ulong)0);
gps.v = ((float)(DATA[3][7] * 0.44704));
#endif
asp.airspeed = ((float)(DATA[3][6] * 0.44704));
}
else if (receviedbytes == 0x64) // FG binary udp
{
//FlightGear
object imudata = new fgIMUData();
MAVLink.ByteArrayToStructureEndian(data, ref imudata, 0);
imudata = (fgIMUData)(imudata);
fgIMUData imudata2 = (fgIMUData)imudata;
if (imudata2.magic != 0x4c56414d)
return;
if (imudata2.latitude == 0)
return;
chkSensor.Checked = true;
#if MAVLINK10
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
#else
imu.usec = ((ulong)DateTime.Now.ToBinary());
#endif
imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
imu.xmag = 0;
imu.yacc = ((Int16)(imudata2.accelY * 9808 / 32.2));
imu.ygyro = ((Int16)(imudata2.ratePitch * 17.453293));
imu.ymag = 0;
imu.zacc = ((Int16)(imudata2.accelZ * 9808 / 32.2)); // + 1000
imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
imu.zmag = 0;
#if MAVLINK10
gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
gps.lat = (int)(imudata2.latitude * 1.0e7);
gps.lon = (int)(imudata2.longitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);
#else
gps.alt = ((float)(imudata2.altitude * ft2m));
gps.fix_type = 3;
gps.hdg = ((float)Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg);
gps.lat = ((float)imudata2.latitude);
gps.lon = ((float)imudata2.longitude);
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m);
#endif
//FileStream stream = File.OpenWrite("fgdata.txt");
//stream.Write(data, 0, receviedbytes);
//stream.Close();
}
else if (receviedbytes == 582)
{
aeroin = new TDataFromAeroSimRC();
object temp = aeroin;
MAVLink.ByteArrayToStructure(data, ref temp, 0);
aeroin = (TDataFromAeroSimRC)(temp);
att.pitch = (aeroin.Model_fPitch);
att.roll = (aeroin.Model_fRoll * -1);
att.yaw = (float)((aeroin.Model_fHeading));
att.pitchspeed = (aeroin.Model_fAngVelX);
att.rollspeed = (aeroin.Model_fAngVelY);
att.yawspeed = (aeroin.Model_fAngVelZ);
#if MAVLINK10
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
#else
imu.usec = ((ulong)DateTime.Now.ToBinary());
#endif
imu.xgyro = (short)(aeroin.Model_fAngVelX * 1000); // roll - yes
//imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = (short)(aeroin.Model_fAngVelY * 1000); // pitch - yes
//imu.ymag = (short)(Math.Cos(head * deg2rad) * 1000);
imu.zgyro = (short)(aeroin.Model_fAngVelZ * 1000);
//imu.zmag = 0;
YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(att.roll, att.pitch, 0, -9.8); //DATA[18][2]
imu.xacc = (Int16)((accel3D.X + aeroin.Model_fAccelX) * 1000); // pitch
imu.yacc = (Int16)((accel3D.Y + aeroin.Model_fAccelY) * 1000); // roll
imu.zacc = (Int16)((accel3D.Z + aeroin.Model_fAccelZ) * 1000);
Console.WriteLine("x {0} y {1} z {2}", imu.xacc, imu.yacc, imu.zacc);
#if MAVLINK10
gps.alt = ((int)(aeroin.Model_fPosZ) * 1000);
gps.fix_type = 3;
gps.cog = (ushort)(Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg * 100);
gps.lat = (int)(aeroin.Model_fLatitude * 1.0e7);
gps.lon = (int)(aeroin.Model_fLongitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX)) * 100);
#else
gps.alt = ((float)(aeroin.Model_fPosZ));
gps.fix_type = 3;
gps.hdg = ((float)Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg);
gps.lat = ((float)aeroin.Model_fLatitude);
gps.lon = ((float)aeroin.Model_fLongitude);
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX)));
#endif
float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;
asp.airspeed = ((float)Math.Sqrt((yvec * yvec) + (xvec * xvec)));
}
else if (receviedbytes > 0x100)
{
FGNetFDM fdm = new FGNetFDM();
object temp = fdm;
MAVLink.ByteArrayToStructureEndian(data, ref temp, 0);
fdm = (FGNetFDM)(temp);
lastfdmdata = fdm;
att.roll = fdm.phi;
att.pitch = fdm.theta;
att.yaw = fdm.psi;
#if MAVLINK10
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
#else
imu.usec = ((ulong)DateTime.Now.ToBinary());
#endif
imu.xgyro = (short)(fdm.phidot * 1150); // roll - yes
//imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000);
imu.ygyro = (short)(fdm.thetadot * 1150); // pitch - yes
//imu.ymag = (short)(Math.Cos(head * deg2rad) * 1000);
imu.zgyro = (short)(fdm.psidot * 1150);
imu.zmag = 0;
imu.xacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_X_pilot * 9808 / 32.2))); // pitch
imu.yacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_Y_pilot * 9808 / 32.2))); // roll
imu.zacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_Z_pilot / 32.2 * 9808)));
//Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc);
#if MAVLINK10
gps.alt = ((int)(fdm.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)((((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360) * 100);
gps.lat = (int)(fdm.latitude * rad2deg * 1.0e7);
gps.lon = (int)(fdm.longitude * rad2deg * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m * 100);
#else
gps.alt = ((float)(fdm.altitude * ft2m));
gps.fix_type = 3;
gps.hdg = (float)(((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360);
//Console.WriteLine(gps.hdg);
gps.lat = ((float)fdm.latitude * rad2deg);
gps.lon = ((float)fdm.longitude * rad2deg);
gps.usec = ((ulong)DateTime.Now.Ticks);
gps.v = ((float)Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m);
#endif
asp.airspeed = fdm.vcas * kts2fps * ft2m;
}
else
{
//FlightGear - old style udp
DATA[20] = new float[8];
DATA[18] = new float[8];
DATA[19] = new float[8];
DATA[3] = new float[8];
// this text line is defined from ardupilot.xml
string telem = Encoding.ASCII.GetString(data, 0, data.Length);
try
{
// should convert this to regex.... or just leave it.
int oldpos = 0;
int pos = telem.IndexOf(",");
DATA[20][0] = float.Parse(telem.Substring(oldpos, pos - 1), new System.Globalization.CultureInfo("en-US"));
oldpos = pos;
pos = telem.IndexOf(",", pos + 1);
DATA[20][1] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));
oldpos = pos;
pos = telem.IndexOf(",", pos + 1);
DATA[20][2] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));
oldpos = pos;
pos = telem.IndexOf(",", pos + 1);
DATA[18][1] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));
oldpos = pos;
pos = telem.IndexOf(",", pos + 1);
DATA[18][0] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));
oldpos = pos;
pos = telem.IndexOf(",", pos + 1);
DATA[19][2] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));
oldpos = pos;
pos = telem.IndexOf("\n", pos + 1);
DATA[3][6] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US"));
DATA[3][7] = DATA[3][6];
}
catch (Exception) { }
chkSensor.Checked = false;
att.pitch = (DATA[18][0]);
att.roll = (DATA[18][1]);
att.yaw = (DATA[19][2]);
#if MAVLINK10
gps.alt = ((int)(DATA[20][2] * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)(DATA[18][2] * 100);
gps.lat = (int)(DATA[20][0] * 1.0e7);
gps.lon = (int)(DATA[20][1] * 1.0e7);
gps.time_usec = ((ulong)0);
gps.vel = (ushort)((DATA[3][7] * 0.44704 * 100));
#else
gps.alt = ((float)(DATA[20][2] * ft2m));
gps.fix_type = 3;
gps.hdg = ((float)DATA[18][2]);
gps.lat = ((float)DATA[20][0]);
gps.lon = ((float)DATA[20][1]);
gps.usec = ((ulong)0);
gps.v = ((float)(DATA[3][7] * 0.44704));
#endif
asp.airspeed = ((float)(DATA[3][6] * 0.44704));
}
// write arduimu to ardupilot
if (CHK_quad.Checked) // quad does its own
{
return;
}
#if MAVLINK10
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
if (gpsspan.TotalMilliseconds >= GPS_rate)
{
lastgpsupdate = DateTime.Now;
oldgps = gps;
//comPort.sendPacket(gps);
}
hilstate.alt = oldgps.alt;
hilstate.lat = oldgps.lat;
hilstate.lon = oldgps.lon;
hilstate.pitch = att.pitch;
hilstate.pitchspeed = att.pitchspeed;
hilstate.roll = att.roll;
hilstate.rollspeed = att.rollspeed;
hilstate.time_usec = gps.time_usec;
hilstate.vx = (short)(gps.vel * Math.Sin(oldgps.cog / 100.0 * deg2rad));
hilstate.vy = (short)(gps.vel * Math.Cos(oldgps.cog / 100.0 * deg2rad));
hilstate.vz = 0;
hilstate.xacc = imu.xacc;
hilstate.yacc = imu.yacc;
hilstate.yaw = att.yaw;
hilstate.yawspeed = att.yawspeed;
hilstate.zacc = imu.zacc;
comPort.sendPacket(hilstate);
comPort.sendPacket(asp);
#else
if (chkSensor.Checked == false) // attitude
{
comPort.sendPacket( att);
comPort.sendPacket( asp);
}
else // raw imu
{
// imudata
comPort.sendPacket( imu);
#endif
MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t();
double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588)); // updated from valid gps
pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa
comPort.sendPacket(pres);
#if !MAVLINK10
comPort.sendPacket(asp);
}
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
if (gpsspan.TotalMilliseconds >= GPS_rate)
{
lastgpsupdate = DateTime.Now;
comPort.sendPacket(gps);
}
#endif
}
public void update(ref double[] servos, ArdupilotMega.GCSViews.Simulation.FGNetFDM fdm)
{
for (int i = 0; i < servos.Length; i++)
{
if (servos[i] <= 0.0)
{
motor_speed[i] = 0;
}
else
{
motor_speed[i] = scale_rc(i, (float)servos[i], 0.0f, 1.0f);
//servos[i] = motor_speed[i];
}
}
double[] m = motor_speed;
/*
roll = 0;
pitch = 0;
yaw = 0;
roll_rate = 0;
pitch_rate = 0;
yaw_rate = 0;
*/
// Console.WriteLine("\nin m {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", m[0], m[1], m[2], m[3]);
// Console.WriteLine("in vel {0:0.000000} {1:0.000000} {2:0.000000}", velocity.X, velocity.Y, velocity.Z);
//Console.WriteLine("in r {0:0.000000} p {1:0.000000} y {2:0.000000} - r {3:0.000000} p {4:0.000000} y {5:0.000000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate);
// m[0] *= 0.5;
//# how much time has passed?
DateTime t = DateTime.Now;
TimeSpan delta_time = t - last_time; // 0.02
last_time = t;
if (delta_time.TotalMilliseconds > 100) // somethings wrong / debug
{
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;
//Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
//# update rotational rates
roll_rate += roll_accel * delta_time.TotalSeconds;
pitch_rate += pitch_accel * delta_time.TotalSeconds;
yaw_rate += yaw_accel * delta_time.TotalSeconds;
// Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
//# update rotation
roll += roll_rate * delta_time.TotalSeconds;
pitch += pitch_rate * delta_time.TotalSeconds;
yaw += yaw_rate * delta_time.TotalSeconds;
// Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll);
//Console.WriteLine("r {0:0.0} p {1:0.0} y {2:0.0} - r {3:0.0} p {4:0.0} y {5:0.0} ms {6:0.000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate, delta_time.TotalSeconds);
//# air resistance
Vector3d air_resistance = -velocity * (gravity / terminal_velocity);
//# 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);
Vector3d accel3D = RPY_to_XYZ(roll, pitch, yaw, accel);
//Console.WriteLine("accel3D " + accel3D.X + " " + accel3D.Y + " " + accel3D.Z);
accel3D += new Vector3d(0, 0, -gravity);
accel3D += air_resistance;
Random rand = new Random();
int fixme;
//velocity.X += .02 + rand.NextDouble() * .03;
//velocity.Y += .02 + rand.NextDouble() * .03;
//# new velocity vector
velocity += accel3D * delta_time.TotalSeconds;
this.accel = accel3D;
//# new position vector
old_position = new Vector3d(position);
position += velocity * delta_time.TotalSeconds;
// Console.WriteLine(fdm.agl + " "+ fdm.altitude);
//Console.WriteLine("Z {0} halt {1} < gl {2} fh {3}" ,position.Z , home_altitude , ground_level , frame_height);
if (home_latitude == 0 || home_latitude > 90 || home_latitude < -90 || home_longitude == 0)
{
this.home_latitude = fdm.latitude * rad2deg;
this.home_longitude = fdm.longitude * rad2deg;
this.home_altitude = fdm.altitude * ft2m;
this.ground_level = this.home_altitude;
this.altitude = fdm.altitude * ft2m;
this.latitude = fdm.latitude * rad2deg;
this.longitude = fdm.longitude * rad2deg;
}
//# constrain height to the ground
if (position.Z + home_altitude < ground_level + frame_height)
{
if (old_position.Z + home_altitude > ground_level + frame_height)
{
// Console.WriteLine("Hit ground at {0} m/s", (-velocity.Z));
}
velocity = new Vector3d(0, 0, 0);
roll_rate = 0;
pitch_rate = 0;
yaw_rate = 0;
roll = 0;
pitch = 0;
position = new Vector3d(position.X, position.Y,
ground_level + frame_height - home_altitude + 0);
// Console.WriteLine("here " + position.Z);
}
//# update lat/lon/altitude
update_position();
// send to apm
#if MAVLINK10
ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t();
#else
ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t();
#endif
ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
ArdupilotMega.MAVLink.__mavlink_vfr_hud_t asp = new ArdupilotMega.MAVLink.__mavlink_vfr_hud_t();
att.roll = (float)roll * deg2rad;
att.pitch = (float)pitch * deg2rad;
att.yaw = (float)yaw * deg2rad;
att.rollspeed = (float)roll_rate * deg2rad;
att.pitchspeed = (float)pitch_rate * deg2rad;
att.yawspeed = (float)yaw_rate * deg2rad;
#if MAVLINK10
gps.alt = ((int)(altitude * 1000));
gps.fix_type = 3;
gps.vel = (ushort)(Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)) * 100);
gps.cog = (ushort)((((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360) * 100);
gps.lat = (int)(latitude* 1.0e7);
gps.lon = (int)(longitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
asp.airspeed = gps.vel;
#else
gps.alt = ((float)(altitude));
gps.fix_type = 3;
gps.lat = ((float)latitude);
gps.lon = ((float)longitude);
gps.usec = ((ulong)DateTime.Now.Ticks);
//Random rand = new Random();
//gps.alt += (rand.Next(100) - 50) / 100.0f;
//gps.lat += (float)((rand.NextDouble() - 0.5) * 0.00005);
//gps.lon += (float)((rand.NextDouble() - 0.5) * 0.00005);
//gps.v += (float)(rand.NextDouble() - 0.5) * 1.0f;
gps.v = ((float)Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)));
gps.hdg = (float)(((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360); ;
asp.airspeed = gps.v;
#endif
MainV2.comPort.sendPacket(att);
MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t();
double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588));
pres.press_diff1 = (short)(int)(calc); // 0 alt is 0 pa
MainV2.comPort.sendPacket(pres);
MainV2.comPort.sendPacket(asp);
if (framecount % 12 == 0)
{// 50 / 10 = 5 hz
MainV2.comPort.sendPacket(gps);
//Console.WriteLine(DateTime.Now.Millisecond + " GPS" );
}
framecount++;
}